CN111811517A - A dynamic local path planning method and system - Google Patents

Abstract

The invention relates to a dynamic local path planning method, which comprises the following steps: acquiring an initial pose and an end pose of a moving body, updating a cost map in real time, judging a planned path to be a route of several orders, and representing the planned path through a mathematical expression; discretizing the planned path represented by the mathematical expression to obtain discrete path points, converting the discrete path points into grid coordinates, judging whether the planned path is an infeasible area or not according to the grid coordinates, if so, discarding the planned path, and otherwise, taking the planned path as a possible path; and evaluating all possible paths through an evaluation function, and selecting one path with the highest score for smoothing. The invention also relates to a dynamic local path planning system. The invention ensures that the curvature of the planned path is continuous, the curve is smooth and the movement characteristics of movable equipment such as a robot or an AGV are met.

Description

A dynamic local path planning method and system

technical field

The invention relates to the field of robotics technology, in particular to a dynamic local path planning method and system.

Background technique

Path planning refers to a mobile device such as a robot or AGV searching for an optimal or sub-optimal path from the starting state to the target state according to certain performance indicators (such as distance, time, etc.). According to the degree of mastery of environmental information, it can be divided into two types: (1) global path planning based on a priori complete information of the environment, also known as static or offline planning; (2) local path planning based on sensor information, also known as dynamic or offline planning. Online path planning. In the application scenario of automatic parking, general local path planning methods such as dynamic window algorithm and fuzzy logic algorithm cannot consider the attitude of the car at the end of the planned path, and can only achieve the effect of dynamic obstacle avoidance, while the dubin algorithm and reeds-sheep Although the algorithm can consider the end pose problem, it can only plan the path without obstacles. For another example, in a logistics yard, if you want to insert a pallet with a known pose, the existing algorithm cannot dynamically avoid obstacles based on the consideration of the end pose.

At present, the more mature algorithm is the A* algorithm, and its map format also uses a rasterized cost map, but the A* algorithm A* algorithm fixedly adopts the 8-neighbor expansion method when selecting nodes, and can only select at most 8 moving directions around. , and the movement angle is limited to an integer multiple of π/4, which is not conducive to the steering of the robot, resulting in the generation of more redundant nodes, and the redundant nodes are not optimized, so that the final path has many turning points and is not smooth.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide a dynamic local path planning method and system, so that the curvature of the planned path is continuous, the curve is smooth and conforms to the motion characteristics of movable equipment such as robots or AGVs.

The technical solution adopted by the present invention to solve the technical problem is: a dynamic local path planning method is provided, comprising the following steps:

(1) Obtaining the starting pose and ending pose of the moving body, and updating the cost map in real time, judging that the planned path is a several-order route, and expressing the planned path by mathematical expressions;

(2) Discretize the planned path represented by the mathematical expression to obtain discrete path points, convert the discrete path points into grid coordinates, and judge whether the planned path is an infeasible area according to the grid coordinates, and if it is an infeasible area then discard the planned path, otherwise use the planned path as a possible path;

(3) Evaluate all possible paths through the evaluation function, and select the one with the highest score for smoothing.

In the step (1), it is judged that the planned path is a several-order route, and before the planned path is represented by a mathematical expression, it is necessary to convert the global map under the original coordinate system into the position of the termination pose as the origin and the pose direction as: The global map in the set coordinate system of the Y axis, the conversion method is (x ₁ ,y ₁ )=([x·cos(θ)+y·sin(θ)+X],[y·cos(θ)- x sin(θ)+Y]), where (x, y) is the coordinate in the original coordinate system, θ is the angle between the X axis of the original coordinate system and the X axis of the set coordinate system, and (X, Y) is The difference between the origin of the original coordinate system and the origin of the set coordinate system.

In the step (1), when judging that the planned path is a several-order route, it is necessary to analyze the definition, constraint conditions and corresponding actions of each order, wherein the first order analysis is as follows:

First-order path definition: the moving body can reach the end position through a direction change from the current pose;

First-order path constraints:

(a) d1>R _min and d2>R _min , where d1 and d2 are the distances from the moving body to (R _min ,0) and (-R _min ,0) respectively, and R _min is the minimum turning radius; Constraint (a), then perform third-order analysis, and if satisfied, verify constraint (b);

L型路径或

R型路径，其中，O为原点，坐标为(0,0)；C_c为X轴上上圆心，坐标为(x_c,0)，x_c∈(-∞,-R_min)∪(R_min,∞)；V为运动体的位姿，坐标为(x_v,y_v,θ_v)，其中，x_v∈(-∞,-R_min)∪(R_min,∞),y_v＞0,θ_v∈[0,2π]；β为

和

的夹角，β∈[0,π]，R型表示顺时针，L型表示逆时针；若满足约束条件(b)则跳出阶的分析，否则进入二阶分析。(b)

L-path or

R-type path, where O is the origin and the coordinates are (0,0); C _c is the center of the circle on the X axis, and the coordinates are (x _c ,0), x _c ∈(-∞,-R _min )∪(R _min ,∞); V is the pose of the moving body, and the coordinates are (x _v ,y _v ,θ _v ), where x _v ∈(-∞,-R _min )∪(R _min ,∞),y _v ＞ 0,θ _v ∈[0,2π]; β is

and

The included angle of β∈[0,π], R type means clockwise, L type means counterclockwise; if the constraint condition (b) is satisfied, it will jump out of the first-order analysis, otherwise it will enter the second-order analysis.

The second-order analysis is as follows:

Second-order path definition: The moving body cannot reach the target point through the first-order analysis, and it is necessary to adjust the pose once so that the pose of the moving body at the next moment can pass the first-order analysis;

Second-order path constraints:

(A) d1>R _min and d2>R _min , where d1 and d2 are the distances from the moving body to (R _min ,0) and (-R _min ,0) respectively, and R _min is the minimum turning radius; If the constraint (A) is satisfied, a third-order analysis is performed, and if it is satisfied, the constraint (B) is verified;

(B) C ₁ C ₂ =R ₁ +R ₂ or C ₁ C ₂ =|R ₁ -R ₂ |, where C ₁ is the center of a circle with a radius of R ₁ and the coordinates are (R ₁ ,0), R ₁ >R _min ; make a straight line with point V, and make the vertical line of the straight line through the point V, C ₂ is the center of the circle with R ₂ as the radius on the vertical line, and the coordinates are (x _c2 , y _c2 );

θ₁为

和

的夹角，θ₂为

和

的夹角，RR型表示第一段路径为顺时针，第二段路径为顺时针；LR型表示第一段路径为逆时针，第二段路径为顺时针；RL型表示第一段路径为顺时针，第二段路径为逆时针；LL型表示第一段路径为逆时针，第二段路径为逆时针。(C)

_θ1 is

and

The included angle, θ ₂ is

and

The included angle of , the RR type means that the first section of the path is clockwise, the second section of the path is clockwise; the LR type means that the first section of the path is counterclockwise, and the second section of the path is clockwise; Clockwise, the second path is counterclockwise; LL type means that the first path is counterclockwise, and the second path is counterclockwise.

The third-order analysis is as follows:

Third-order path definition: If the path is a third-order path, it means that the position of the moving body does not meet the constraints, d1 < R _min or d2 < R _min , and it is necessary to find another arc that can be tangent to the second-order route and satisfy the third-order path. path constraints;

其中，θ₃为第三段圆弧转过的角度，RRR型表示第一段路径为顺时针，第二段路径为顺时针，第三段路径为顺时针；LRR型表示第一段路径为逆时针，第二段路径为顺时针，第三段路径为顺时针；RLR型表示第一段路径为顺时针，第二段路径为逆时针，第三段路径为顺时针；LLR型表示第一段路径为逆时针，第二段路径为逆时针，第三段路径为顺时针；RRL型表示第一段路径为顺时针，第二段路径为顺时针，第三段路径为逆时针；LRL型表示第一段路径为逆时针，第二段路径为顺时针，第三段路径为逆时针；RLL型表示第一段路径为顺时针，第二段路径为逆时针，第三段路径为逆时针；LLL型表示第一段路径为逆时针，第二段路径为逆时针，第三段路径为逆时针。Third-order path constraints:

Among them, θ ₃ is the angle that the third arc turns, RRR type means that the first segment path is clockwise, the second segment path is clockwise, and the third segment path is clockwise; LRR type means that the first segment path is clockwise Counterclockwise, the second path is clockwise, and the third path is clockwise; RLR type indicates that the first path is clockwise, the second path is counterclockwise, and the third path is clockwise; LLR type indicates that the first path is clockwise. One path is counterclockwise, the second path is counterclockwise, and the third path is clockwise; RRL type means that the first path is clockwise, the second path is clockwise, and the third path is counterclockwise; LRL type means that the first section of the path is counterclockwise, the second section of the path is clockwise, and the third section of the path is counterclockwise; RLL type means that the first section of the path is clockwise, the second section of the path is counterclockwise, and the third section of the path is counterclockwise; the LLL type indicates that the first path is counterclockwise, the second path is counterclockwise, and the third path is counterclockwise.

根据栅格序号求出栅格坐标：

其中，INT表示取整，G_size表示最小栅格宽度，x_max为代价地图x轴宽度，(x_i,y_i)为第i个点的坐标，％表示取余运算符。In the step (2), the discrete path points are converted into grid coordinates through a mapping relationship, wherein the mapping relationship is: the grid serial number of the i-th point:

Find the grid coordinates according to the grid number:

Among them, INT means rounding, G _size means the minimum grid width, x _max is the x-axis width of the cost map, (x _i , y _i ) is the coordinate of the ith point, and % means the remainder operator.

In the step (2), judging whether the planned path is an infeasible area according to the grid coordinates is specifically: traversing the grid coordinates transformed by all discrete path points, comparing the cost value of the grid coordinates with the threshold, if If there are grid coordinates greater than the threshold, it means that the path is an infeasible area.

The scoring standard of the evaluation function in the step (3) includes the ratio of the radius of the planned arc and the distance of the nearest obstacle, wherein the closer the ratio of the radius of the planned arc is to 1, the higher the score, and the farther the distance of the nearest obstacle is. The higher the score; the two scoring criteria are normalized during evaluation and then added, and the path with the highest score is selected for smoothing.

The technical solution adopted by the present invention to solve the technical problem is as follows: a dynamic local path planning system is further provided, which includes a control module, an obstacle avoidance module and an operation module, and the control module is used for receiving the planning path data and controlling the robot to walk precisely; The obstacle avoidance module is used to receive external information through an external sensor and output the position of the obstacle; the calculation module is used to plan an optimal path by using the above dynamic local path planning method.

beneficial effect

Compared with the prior art, the present invention has the following advantages and positive effects due to the adoption of the above-mentioned technical solution: the present invention determines that the planned route is a several-order route, and uses a mathematical expression to represent the planned route, through discretization The mathematical expression converts the planned path into the grid position in the cost map, thereby discarding the path with obstacles, and finally evaluates the possible paths, and selects one for smooth processing, so that the final planned path has continuous curvature and smooth curve, And it conforms to the motion characteristics of mobile devices such as robots or AGVs.

Description of drawings

Fig. 1 is the flow chart of the present invention;

Fig. 2 is the flow chart of embodiment 1 in the present invention;

Fig. 3 is the schematic diagram of embodiment 1 in the present invention;

Fig. 4 is the flow chart of embodiment 2 in the present invention;

Fig. 5 is the schematic diagram of embodiment 2 in the present invention;

Fig. 6 is the flow chart of embodiment 3 in the present invention;

Fig. 7 is the schematic diagram of embodiment 3 in the present invention;

FIG. 8 is a system block diagram of the present invention.

Detailed ways

The present invention will be further described below in conjunction with specific embodiments. It should be understood that these examples are only used to illustrate the present invention and not to limit the scope of the present invention. In addition, it should be understood that after reading the content taught by the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent forms also fall within the scope defined by the appended claims of the present application.

Embodiments of the present invention relate to a dynamic local path planning method, as shown in FIG. 1 , including the following steps:

(1) Obtain the starting pose and ending pose of the moving body, update the cost map in real time, determine the order of the planned path, and express the planned path through mathematical expressions.

In this embodiment, the starting pose is obtained through its own real-time positioning, and the positioning methods may include but are not limited to laser positioning, visual positioning, and inertial navigation positioning. Timestamp, each timestamp corresponds to a different pose; the information of the starting pose includes: 1. The position of the moving body in the global map; 2. The posture of the moving body, which is defined as treating the moving body as a rigid body , the direction of the rigid body is used as attitude information; 3. Timestamp.

The manners of obtaining the termination pose in this embodiment include, but are not limited to, the following manners: inputting through human measurements, obtaining UWB positioning, obtaining Bluetooth positioning, and the like. The information of the termination pose includes: 1. the position in the global map when the moving body stops; 2. the pose in the global map when the moving body stops.

The cost map in this embodiment is a superimposed obstacle map, an inflated map, etc. on top of the original static map; wherein, the obstacle map is the cost value of updating the map with the obstacle information dynamically recorded by the sensor through the Bayesian formula ;Inflated map is inflated on the above two-layer superimposed map, that is, the cost value of the grid around the obstacle is increased, so as to avoid the robot's shell from hitting the obstacle. Among them, the cost value is the probability that the grid has obstacles.

In this embodiment, the order of the path is defined as the adjustment times of the direction. If the direction is not adjusted, the planned path is a first-order path. It can be seen from the dubin algorithm that the moving body can plan a planned path consisting of two curves and a straight line. It can be seen that the maximum order of space is the third-order path.

Before judging that the planned path is a several-order route, and expressing the planned path through mathematical expressions, it is necessary to convert the global map in the original coordinate system into a set coordinate system with the position of the termination pose as the origin and the orientation direction as the Y-axis The global map below, the conversion method is (x ₁ ,y ₁ )=([x·cos(θ)+y·sin(θ)+X],[y·cos(θ)-x·sin(θ)+ Y]), where (x, y) is the coordinates in the original coordinate system, θ is the angle between the X-axis of the original coordinate system and the X-axis of the set coordinate system, (X, Y) is the origin of the original coordinate system and the setting The difference between the origin of the coordinate system.

(2) Discretize the planned path represented by the mathematical expression to obtain discrete path points, convert the discrete path points into grid coordinates, and judge whether the planned path is an infeasible area according to the grid coordinates, and if it is an infeasible area then discard the planned path, otherwise use the planned path as a possible path;

(3) Evaluate all possible paths through the evaluation function, and select the one with the highest score for smoothing. In this embodiment, the scoring criteria of the evaluation function include the planning arc radius ratio ratio and the distance dist _{obstacle of the nearest obstacle} . For the planned arc radius ratio ratio, in order to reduce the mechanical wear of the moving body, the planned path should be as smooth as possible. The curvature of the arc is related to the turning radius, so the closer the ratio of the turning radius of the two arcs is to 1, the smaller the path curvature, and the higher the score. high. For the distance dist _obstacle of the nearest obstacle, the farther the planned path is from the obstacle, the better, and the farther away from the obstacle, the higher the score.

The present invention is further illustrated by several embodiments below.

Example 1:

This embodiment provides a path planning method and system for a forklift to automatically insert and extract irregular goods. Further, the hardware of the system is mainly composed of: a forklift, a computer, and a camera. It should be noted that the obstacle avoidance module includes but is not limited to vision sensors.

As shown in Figure 2, the dynamic local path planning method specifically includes the following steps:

Step 1: Obtain the starting pose, the ending pose and the new cost map in real time, judge the planned route as a few-order route, and express the path with a mathematical expression.

When judging the planned route as a few-order route, first-order path analysis is performed first:

I. Judging the order of the path

1.1 First judge whether d1 and d2 meet the constraints d1> _Rmin and d2>_Rmin;

In this embodiment, d1=(x _v +R _min ) ² +y _v ² , d2=(x _v +R _min ) ² +y _v ² , as shown in FIG. 3 , d1 and d2 respectively represent the current location of the forklift The distance from the position to (R _min ,0) and (-R _min ,0), (x _v , y _v ) are the coordinates of the current position of the AGV forklift, and R _min is the minimum turning radius. In this embodiment, d1> _Rmin , d2> _Rmin , so the constraint condition is satisfied.

L型路径或

R型路径，其中，O为原点，坐标为(0,0)；C_c为X轴上上圆心，坐标为(x_c,0)，x_c∈(-∞,-R_min)∪(R_min,∞)；V为运动体的位姿，坐标为(x_v,y_v,θ_v)，其中，x_v∈(-∞,-R_min)∪(R_min,∞),y_v＞0,θ_v∈[0,2π]；β为

和

的夹角，β∈[0,π]，R型表示顺时针，L型表示逆时针。1.2 Judgment Constraints

L-path or

R-type path, where O is the origin and the coordinates are (0,0); C _c is the center of the circle on the X axis, and the coordinates are (x _c ,0), x _c ∈(-∞,-R _min )∪(R _min ,∞); V is the pose of the moving body, and the coordinates are (x _v ,y _v ,θ _v ), where x _v ∈(-∞,-R _min )∪(R _min ,∞),y _v ＞ 0,θ _v ∈[0,2π]; β is

and

The included angle of β∈[0,π], R-type means clockwise, L-type means counterclockwise.

本实施例中，符合约束条件

此时确定规划路径为一阶路径。The judging steps are as follows: x _c is obtained by calculating according to OC _c =C _c V, for the L-type path, x _c >0, and for the R-type path, x _c <0. Judgment Constraints

In this embodiment, the constraints are met

At this time, the planned path is determined to be a first-order path.

II Expressing the planned path with mathematical expressions

Here, various mathematical expressions can be used to represent the planned path, such as equations, or polar coordinates. The expression of this embodiment is:

Step 2: Discretize the path, convert it into grid coordinates, and judge the infeasible area.

1. Discrete path

Sampling γ with a sampling interval of σ to obtain discrete path points

2. Convert to grid coordinates

2.1 Find the grid number of the i-th point:

2.2 Find the grid coordinates according to the grid number:

Among them, INT means rounding, G _size means the minimum grid width, x _max is the x-axis width of the cost map, (x _i , y _i ) is

The coordinates of the i-th point, % indicates the remainder operator.

3. Determine the infeasible area

Each grid area will store occupancy information to indicate whether the current grid has obstacles, that is, the cost value. Therefore, after obtaining the grid coordinates, traverse the cost value of all grid coordinates. If the cost value exceeds the threshold, it indicates that there are obstacles on the path, and the planned path is discarded and selected again. Otherwise, the path is generated.

Step 3. The first-order path has strict requirements on the initial attitude of the AGV, so only one planned path is output. During the entire running process, the computing module detects in real time whether the obstacle avoidance module outputs obstacle information. If there is an emergency stop, return to step 1, otherwise no action.

If obstacle information is received after outputting the path, perform the following steps:

1. The obstacle avoidance module outputs two-dimensional coordinates (x _obstacle , y _obstacle );

2. Convert the two-dimensional coordinates into grid coordinates;

3. Compare the grid coordinates of the obstacle with the mapped (x _N , y _N ) to see if the grid coordinates of the obstacle are on the planned path, if so, discard the path and re-plan.

Example 2:

This embodiment provides a path planning method and system for reversing a car into a warehouse. Further, the hardware of the system is mainly composed of: a car, an industrial computer, and several laser radars.

As shown in Figure 4, the dynamic local path planning method specifically includes the following steps:

Step 1: Obtain the starting pose, the ending pose and the new cost map in real time, judge the planned route as a few-order route, and express the path with a mathematical expression.

When judging the planned route as a few-order route, first-order path analysis is performed first:

I. Judging the order of the path

1.1 First judge whether d1 and d2 meet the constraints d1> _Rmin and d2>_Rmin;

In this embodiment, d1=(x _v +R _min ) ² +y _v ² , d2=(x _v +R _min ) ² +y _v ² , as shown in FIG. 5 , d1 and d2 respectively represent the current location of the car. The distance from the position to (R _min ,0) and (-R _min ,0), (x _v , y _v ) are the coordinates of the current position of the car, and R _min is the minimum turning radius. In this embodiment, d1> _Rmin , and d2> _Rmin .

L型路径或

R型路径，如图5所示，O为原点，坐标为(0,0)；C₁为X轴上上圆心，坐标为(x_c1,0)，x_c∈(-∞,-R_min)∪(R_min,∞)；V为运动体的位姿，坐标为(x_v,y_v,θ_v)，其中，x_v∈(-∞,-R_min)∪(R_min,∞),y_v＞0,θ_v∈[0,2π]；β为

和

的夹角，β∈[0,π]，R型表示顺时针，L型表示逆时针。1.2 Judgment Constraints

L-path or

R-type path, as shown in Figure 5, O is the origin, and the coordinates are (0, 0); C ₁ is the center of the circle on the X-axis, and the coordinates are (x _c1 ,0), and x _c ∈(-∞,-R _min )∪(R _min ,∞); V is the pose of the moving body, and the coordinates are (x _v ,y _v ,θ _v ), where x _v ∈(-∞,-R _min )∪(R _min ,∞) ,y _v ＞0,θ _v ∈[0,2π]; β is

and

The included angle of β∈[0,π], R-type means clockwise, L-type means counterclockwise.

本实施例中，不符合约束条件

The judging steps are as follows: x _c is obtained by calculating according to OC ₁ =C ₁ V, for the L-type path, x _c1 >0, and for the R-type path, x _c1 <0. Judgment Constraints

In this embodiment, the constraints are not met

1.3 Second-order path analysis

The moving body cannot reach the target point through the first-order analysis. It is necessary to adjust the pose once so that the pose of the moving body at the next moment can pass the first-order analysis, that is, it can be tangent to the first-order arc at the end of the planned arc. .

1.3.1 Judging the constraint condition C ₁ C ₂ =R ₁ +R ₂ (circumscribed) or C ₁ C ₂ =|R ₁ -R ₂ | (inscribed), where C ₁ is the radius of the circle with radius R ₁ The center of the circle, the coordinates are (R ₁ , 0), R ₁ >R _min ; take the point V as a straight line, and make the vertical line of the straight line through the point V, C ₂ is the center of the circle with R ₂ as the radius on the vertical line, The coordinates are (x _c2 , y _c2 ).

The judgment steps are as follows: the radius selection method is to perform sampling at an interval of d _x on the vertical line between the X axis and the vector (x, y, θ), and the sampling range is the above constraint until C ₁ C ₂ =R ₁ +R ₂ (external) or C ₁ C ₂ = |R ₁ -R ₂ | (intro).

遍历结束时，所得结果是一个二阶路径的集合。The center C ₁ (x _c1 ,y _c1 ) is (R ₁ ,0) or (-R ₁ ,0); the center C ₂ (x _c2 ,y _c2 ) is (x _m +r ₂ *cos(θ-90°) ), y _m +r ₂ *sin(θ-90°)); the tangent point of circle C ₁ and circle C ₂ is

At the end of the traversal, the result is a collection of second-order paths.

其中，θ_v为汽车的姿态，θ₁为

和

的夹角

θ₂为

和

的夹角

RR型表示第一段路径为顺时针，第二段路径为顺时针；LR型表示第一段路径为逆时针，第二段路径为顺时针；RL型表示第一段路径为顺时针，第二段路径为逆时针；LL型表示第一段路径为逆时针，第二段路径为逆时针。1.3.2 Judgment Constraints

where θ _v is the attitude of the car, and θ ₁ is

and

angle

_θ2 is

and

angle

RR type means the first segment path is clockwise, the second segment path is clockwise; LR type means the first segment path is counterclockwise, and the second segment path is clockwise; RL type means the first segment path is clockwise, The second path is counterclockwise; the LL type indicates that the first path is counterclockwise, and the second path is counterclockwise.

第二段路径

R-type L-type judgment: the first segment of the path

second path

Calculate the constraints imposed by θ ₁ and θ ₂ , and filter out the set to be optimized.

II. Express the path in mathematical form

圆心C₂的圆弧表达式为：The arc expression of the circle center C ₁ in this embodiment is:

The arc expression of the circle center C ₂ is:

Step 2: Discretize the path, convert it into grid coordinates, and judge the infeasible area.

1. Discrete path

Sampling γ with a sampling interval of σ to obtain discrete waypoints.

2. Convert to grid coordinates

2.1 Find the grid number of the i-th point:

2.2 Find the grid coordinates according to the grid number:

Among them, INT means rounding, G _size means the minimum grid width, x _max is the x-axis width of the cost map, (x _i , y _i ) is the coordinate of the ith point, and % means the remainder operator.

3. Determine the infeasible area

Each grid area will store occupancy information to indicate whether the current grid has obstacles, that is, the cost value. Therefore, after obtaining the grid coordinates, traverse the cost value of all grid coordinates. If the cost value exceeds the threshold, it indicates that there are obstacles on the path, and the planned path is discarded and selected again. Otherwise, the path is generated.

Step 3: Evaluate all possible paths and smooth the one with the highest score.

1.1 Normalization:

其中，n为所有路径的条数，i为

Among them, n is the number of all paths, i is

Normal(i)=Normal_ratio(i)+Normal_dist _obstacle (i)

The current number of paths, Normal_ratio is the normalized planning arc radius ratio score, Normal_dist _obstacle is the normalized distance score of the nearest obstacle, and a path with the highest score is selected.

1.2 Smoothing

其中，W_ob,W_s和W_k为各项所占权重，权重由多次试验方式获得。The specific method of smoothing the planned path is as follows: establishing a multi-objective function, using the conjugate gradient descent method to obtain an extreme value of the objective function to obtain a relatively smooth path, and the algorithm generates a series of nodes X _i =( _xi , y _i ), i is composed of [0, N]; let ΔX _i =X _i -X _i-1 represent a vector composed of two nodes; O _i represents the position of the closest obstacle point to the node. The established objective function is:

Among them, W _ob , W _s and W _k are the weights occupied by each item, and the weights are obtained by multiple trials.

The first item of the objective function constrains the safe distance between the node and the obstacle, d _max is the minimum safe distance between the node and the obstacle point when smoothing, the size is the vehicle axis length, when |X _i -0|>d _max , the first item effect, otherwise the gradient is 0; the second term smoothes the path; the third term constrains the curvature of any node, the curvature value must be less than km _max , the maximum curvature km _max is determined by the kinematic constraints of the vehicle, when When _ki is less than or equal to k _max , the gradient of the third term takes the value 0.

使用共轭梯度下降的方法对目标函数求极值，得到了较为平滑的路径。The gradients of the objective function are

Using the conjugate gradient descent method to find the extreme value of the objective function, a smoother path is obtained.

After obtaining the second-order path, real-time detection of whether the obstacle avoidance module outputs obstacle information. If there is an emergency stop, return to step 1, otherwise no action.

If obstacle information is received after outputting the path, perform the following steps:

1. The obstacle avoidance module outputs two-dimensional coordinates (x _obstacle , y _obstacle );

2. Convert the two-dimensional coordinates to grid coordinates;

3. Compare the obstacle grid coordinates with the mapped (x _N , y _N ) to see if the obstacle grid coordinates are on the planned path, if so, discard this path and re-plan.

Example 3:

This embodiment provides an embodiment of a third-order path.

If the path is a third-order path, it means that the position of the moving body does not meet the constraints, d1 < R _min or d2 < R _min , and it is necessary to find another arc that can be tangent to the second-order route and satisfy the third-order path constraints.

其中，θ₃为第三段圆弧转过的角度，RRR型表示第一段路径为顺时针，第二段路径为顺时针，第三段路径为顺时针；LRR型表示第一段路径为逆时针，第二段路径为顺时针，第三段路径为顺时针；RLR型表示第一段路径为顺时针，第二段路径为逆时针，第三段路径为顺时针；LLR型表示第一段路径为逆时针，第二段路径为逆时针，第三段路径为顺时针；RRL型表示第一段路径为顺时针，第二段路径为顺时针，第三段路径为逆时针；LRL型表示第一段路径为逆时针，第二段路径为顺时针，第三段路径为逆时针；RLL型表示第一段路径为顺时针，第二段路径为逆时针，第三段路径为逆时针；LLL型表示第一段路径为逆时针，第二段路径为逆时针，第三段路径为逆时针。Third-order path constraints:

Among them, θ ₃ is the angle that the third arc turns, RRR type means that the first segment path is clockwise, the second segment path is clockwise, and the third segment path is clockwise; LRR type means that the first segment path is clockwise Counterclockwise, the second path is clockwise, and the third path is clockwise; RLR type indicates that the first path is clockwise, the second path is counterclockwise, and the third path is clockwise; LLR type indicates that the first path is clockwise. One path is counterclockwise, the second path is counterclockwise, and the third path is clockwise; RRL type means that the first path is clockwise, the second path is clockwise, and the third path is counterclockwise; The LRL type indicates that the first section of the path is counterclockwise, the second section of the path is clockwise, and the third section of the path is counterclockwise; RLL type indicates that the first section of the path is clockwise, the second section of the path is counterclockwise, and the third section of the path is counterclockwise; the LLL type indicates that the first path is counterclockwise, the second path is counterclockwise, and the third path is counterclockwise.

As shown in Figure 6, the dynamic local path planning method specifically includes the following steps:

Step 1: Obtain the starting pose, the ending pose and the new cost map in real time, judge the planned route as a few-order route, and express the path with a mathematical expression.

I. Judging the order of the path

1.1 First judge whether d1 and d2 meet the constraints d1> _Rmin and d2> _Rmin .

In this embodiment, d1=(x _v +R _min ) ² +y _v ² , d2=(x _v +R _min ) ² +y _v ² , as shown in FIG. 7 , d1 and d2 respectively represent the current location of the robot The distance from the position to (R _min ,0) and (-R _min ,0), (x _v ,y _v ) is the current position coordinate of the robot, and R _min is the minimum turning radius. In this embodiment, the starting pose of the robot does not meet the constraints, and the third-order path planning steps are as follows:

a. Establish a coordinate system, take the current coordinate M of the robot as the origin, the vector direction is the positive direction of the y-axis, the x-axis is the vertical line of the y-axis, the right direction is the positive direction, and the conversion matrix of the original coordinate system is H;

b. Select a point A(x _a , y _a , θ _a ) on the first-order path in this coordinate system, point A satisfies the constraints, and converts A to the original coordinate system to get A'(x _a ',y _a ', θ _a '), judge whether A' satisfies the constraints;

c. If satisfied, find the M->A discrete path point in the new coordinate system, convert it to the original coordinate system through H ^-1 , and then use A' as the new starting point to carry out the second-order path planning as in Embodiment 2. If it is not satisfied, select the appropriate point A again.

As shown in FIG. 8 , this embodiment further provides a dynamic local path planning system, which includes an obstacle avoidance module, an arithmetic module, and a control module. The control module is used to receive the planned path data and control the robot to walk accurately; the obstacle avoidance module is used to receive external information through external sensors and output the position of the obstacle; the calculation module is used to adopt the above-mentioned dynamic local path planning method plan the best path

Among them, the obstacle avoidance module includes the functions of mapping and detecting obstacles, and the external sensors used in this module include but are not limited to lidar, ultrasonic sensors, visual sensors, etc.

The operation module includes the functions of updating grid map data, the operation of planning paths and basic logic operations, and the control module includes the function of merging the data of the operation module and outputting control signals. The realization devices of these two modules include but are not limited to personal computers, Industrial computers, servers.

The main thing is that the above computing module first obtains the starting pose, ending pose and real-time update cost map, analyzes the path order, and calculates the mathematical expression of the path; then it can evaluate the infeasible area, and the path needs to be discretized , converted into grid coordinates, the grid information stores the information of whether the grid is occupied; the calculated path is a set of feasible paths, so the operation module needs to evaluate all possible paths and smooth the one with the highest score. . The above-mentioned obstacle avoidance module also needs to detect external obstacle information in real time. If there is an obstacle, the computing module transmits an emergency stop signal to the control module and re-plans the route. Otherwise, the control module outputs control information to follow the generated route.

Claims (9)

1. a dynamic local path planning method, is characterized in that, comprises the following steps: (1) Obtaining the starting pose and ending pose of the moving body, and updating the cost map in real time, judging that the planned path is a several-order route, and expressing the planned path by mathematical expressions; (2) Discretize the planned path represented by the mathematical expression to obtain discrete path points, convert the discrete path points into grid coordinates, and judge whether the planned path is an infeasible area according to the grid coordinates, and if it is an infeasible area then discard the planned path, otherwise use the planned path as a possible path; (3) Evaluate all possible paths through the evaluation function, and select the one with the highest score for smoothing. 2. dynamic local path planning method according to claim 1, is characterized in that, in described step (1), it is judged that planning path is several-order route, and before the described planning path is represented by mathematical expression, it is necessary to put original coordinate The global map under the system is converted to the global map under the set coordinate system with the position of the termination pose as the origin and the attitude direction as the Y axis. The conversion method is (x ₁ ,y ₁ )=([x·cos(θ) +y·sin(θ)+X],[y·cos(θ)-x·sin(θ)+Y]), where (x,y) is the coordinates in the original coordinate system, and θ is the original coordinate system The angle between the X axis and the X axis of the set coordinate system, (X, Y) is the difference between the origin of the original coordinate system and the origin of the set coordinate system. 3. dynamic local path planning method according to claim 1, is characterized in that, when judging in described step (1) that planning path is several order route, need to the definition of each order, constraint condition and corresponding action are analyzed ,in, The first-order analysis is as follows: First-order path definition: the moving body can reach the end position through a direction change from the current pose; First-order path constraints: (a) d1>R _min and d2>R _min , where d1 and d2 are the distances from the moving body to (R _min ,0) and (-R _min ,0) respectively, and R _min is the minimum turning radius; Constraint (a), then perform third-order analysis, and if satisfied, verify constraint (b); (b)

L-path or

R-type path, where O is the origin and the coordinates are (0,0); C _c is the center of the circle on the X axis, and the coordinates are (x _c ,0), x _c ∈(-∞,-R _min )∪(R _min ,∞); V is the pose of the moving body, and the coordinates are (x _v ,y _v ,θ _v ), where x _v ∈(-∞,-R _min )∪(R _min ,∞),y _v ＞ 0,θ _v ∈[0,2π]; β is

and

The included angle of β∈[0,π], R type means clockwise, L type means counterclockwise; if the constraint condition (b) is satisfied, it will jump out of the first-order analysis, otherwise it will enter the second-order analysis. 4. The dynamic local path planning method according to claim 3, wherein the second-order analysis is as follows: Second-order path definition: The moving body cannot reach the target point through the first-order analysis, and it is necessary to adjust the pose once so that the pose of the moving body at the next moment can pass the first-order analysis; Second-order path constraints: (A) d1>R _min and d2>R _min , where d1 and d2 are the distances from the moving body to (R _min ,0) and (-R _min ,0) respectively, and R _min is the minimum turning radius; If the constraint (A) is satisfied, a third-order analysis is performed, and if it is satisfied, the constraint (B) is verified; (B) C ₁ C ₂ =R ₁ +R ₂ or C ₁ C ₂ =|R ₁ -R ₂ |, where C ₁ is the center of a circle with a radius of R ₁ and the coordinates are (R ₁ ,0), R ₁ >R _min ; make a straight line with point V, and make the vertical line of the straight line through the point V, C ₂ is the center of the circle with R ₂ as the radius on the vertical line, and the coordinates are (x _c2 , y _c2 ); (C)

_θ1 is

and

The included angle, θ ₂ is

and

The included angle of , the RR type means that the first section of the path is clockwise, the second section of the path is clockwise; the LR type means that the first section of the path is counterclockwise, and the second section of the path is clockwise; Clockwise, the second path is counterclockwise; LL type means that the first path is counterclockwise, and the second path is counterclockwise. 5. The dynamic local path planning method according to claim 4, wherein the third-order analysis is as follows: Third-order path definition: If the path is a third-order path, it means that the position of the moving body does not meet the constraints, d1 < R _min or d2 < R _min , and it is necessary to find another arc that can be tangent to the second-order route and satisfy the third-order path. path constraints; Third-order path constraints:

Among them, θ ₃ is the angle that the third arc turns, RRR type means that the first segment path is clockwise, the second segment path is clockwise, and the third segment path is clockwise; LRR type means that the first segment path is clockwise Counterclockwise, the second path is clockwise, and the third path is clockwise; RLR type indicates that the first path is clockwise, the second path is counterclockwise, and the third path is clockwise; LLR type indicates that the first path is clockwise. One path is counterclockwise, the second path is counterclockwise, and the third path is clockwise; RRL type means that the first path is clockwise, the second path is clockwise, and the third path is counterclockwise; LRL type means that the first section of the path is counterclockwise, the second section of the path is clockwise, and the third section of the path is counterclockwise; RLL type means that the first section of the path is clockwise, the second section of the path is counterclockwise, and the third section of the path is counterclockwise; the LLL type indicates that the first path is counterclockwise, the second path is counterclockwise, and the third path is counterclockwise. 6. The dynamic local path planning method according to claim 1, wherein in the step (2), discrete path points are converted into grid coordinates through a mapping relationship, wherein the mapping relationship is: the i-th point Grid number:

Find the grid coordinates according to the grid number:

Among them, INT means rounding, G _size means the minimum grid width, x _max is the x-axis width of the cost map, (x _i , y _i ) is the coordinate of the ith point, and % means the remainder operator. 7. The dynamic local path planning method according to claim 1, wherein in the step (2), judging whether the planned path is an infeasible area according to grid coordinates is specifically: traversing all discrete path points to transform The grid coordinates of , compare the cost value of the grid coordinates with the threshold value, and if there are grid coordinates greater than the threshold value, it means that the path is an infeasible area. 8 . The dynamic local path planning method according to claim 1 , wherein the scoring criterion of the evaluation function in the step (3) includes a planning arc radius ratio and a distance to the nearest obstacle, wherein the planning circle The closer the arc radius ratio is to 1, the higher the score, and the farther the distance from the nearest obstacle is, the higher the score; during the evaluation, the two scoring criteria are normalized, and then added together, and the path with the highest score is selected for the evaluation. Smooth processing. 9. A dynamic local path planning system, comprising a control module, an obstacle avoidance module and an arithmetic module, wherein the control module is used to receive planning path data and control the robot to walk precisely; the obstacle avoidance module is used to pass the external The sensor receives external information and outputs the position of the obstacle; the computing module is used to plan an optimal path by using the dynamic local path planning method according to any one of claims 1-8.

Images