Method for route optimization based on dynamic window and redundant node filtering

Patent metadata

Field	Value
Publication number	US-11747826-B2
Application number	US-202117403243-A
Country	US
Kind code	B2
Filing date	Aug 16, 2021
Priority date	Jun 18, 2021
Publication date	Sep 5, 2023
Grant date	Sep 5, 2023

Abstract

The present disclosure discloses a method for route optimization based on dynamic window and redundant node filtering, comprising using an existing raster map data set to determine the coordinate information of a starting position and a destination position of movement, and to mark a destination node and an obstacle node in the raster map; using A* algorithm to plan a global route; globally optimizing the global route planned by A* algorithm, and filtering redundant nodes out; combining a dynamic window algorithm to perform the local optimization section by section on the optimized global route so as to obtain a final global route. According to the present disclosure, the combination of algorithms reduces a single movement duration of a mobile robot and improves the smoothness of the movement route curve. At the same time, the problems of the robot occurring on the route during the static driving are alleviated.

First claim

What is claimed is:

1. A method for route optimization based on dynamic window and redundant node filtering, performed by a processor, comprising:
S 100 : by using an existing raster map data set, determining coordinate information of a starting position and a destination position of movement, and marking a destination node and an obstacle node in the existing raster map;
S 200 : using A* algorithm to plan a global route, to obtain a route node set;
S 300 : performing a global optimization to the global route planned by the A* algorithm, to filter redundant nodes;
S 400 : by combining the dynamic window algorithm, performing a local optimization section by section on the global route optimized in S 300 so as to obtain a final global rout; and
S 500 : controlling a mobile robot to move from the starting position to the destination position based on the final global route; wherein
performing the global optimization to the global route planned by the A* algorithm to filter the redundant nodes in S 300 comprises:
S 310 : labeling the route node set that is obtained in S 200 as P{P i , 1≤j≤n}, wherein P 1 is the starting point of the route and P n is the end point of the route; creating a key point set U{P 1 , P n }for storing key nodes after route optimization;
S 320 : for the node set P{P i , 1≤j≤n}, setting m=2, 3, 4, . . . n, connecting in sequence from P 1 to P 2 , P 3 , . . . , and P m , when any obstacle node exists between a straight line P 1 P m , taking the node P m− 1 as a required node of the route; when the straight line P 1 P m does not pass through the obstacle node, determining that the node P m−1 is a redundant node;

adding all of the required nodes to the set U; wherein after the selection of key points is done, U{P 1 , P m−1 , . . . , P m+k , P n }(2≤m≤n, 1≤k≤n−m) contains all key nodes; when a number of nodes in U is r, the set U is represented as U{P 1 , P 2 , . . . ,P r }, (1≤r≤n);
S 330 : connecting all nodes in the set U in sequence to complete the global optimization of the route.

2. The method for route optimization based on the dynamic window and the redundant node filtering according to claim 1 , wherein the A* algorithm is used to plan a global route in S 200 , comprising the following steps:
S 210 : building and initializing two empty tables OpenList and CloseList, setting the starting position as a current node, and storing the current node into the OpenList list; when the current node is not a destination point, calling an extended node function to select all extended nodes adjacent to the current node and storing information of all the extended nodes into the OpenList list;
S 220 : calling an insert node function to traverse all the extended nodes, and calculating a cost function F corresponding to these nodes, wherein the cost function F is expressed as:
F ( n )= G ( n )+ H ( n )

wherein, n represents the current node; F(n) is the cost function of the current node n; G(n) is an actual cost value of the mobile robot from an initial node to the current node n; H(n) is a cost value of the mobile robot reaching the destination point from the current node n, which selects the Euclidean distance between these two nodes to represent H(n), and the function of H(n) is expressed as:
H ( n )=√{square root over (( x g −x n ) 2 +( y g −y n ) 2 )}

wherein (x g , y g ) represents the coordinates of the destination node in the existing raster map and (x n , y n ) represents the coordinates of the current node in the existing raster map;
S 230 : selecting and deleting a node corresponding to a minimum value of the cost function F(n) from the OpenList and storing the node corresponding to the minimum value of the cost function F(n) into the CloseList; at the same time, setting the node corresponding to the minimum value of the cost function F(n) as the current node connected to the previous node; repeating S 220 when the current node does not turn to be the destination point, and exporting the global route when the current node turns to be the destination point.

3. The method for route optimization based on the dynamic window and the redundant node filtering according to claim 1 , wherein in S 400 , by combining the dynamic window algorithm, the local optimization is performed section by section on the global route optimized in S 300 so as to obtain a final global route, which specifically comprises:
S 410 : as for the set U, taking all nodes starting from the starting point P 1 , excepting the end point P r , as the starting points of local route planning in turn, wherein the starting points are labeled as {S 1 , S 2 , . . . , S r−1 }; taking {P 2 , P 3 , . . . , P r }starting from the second node P 2 in the set U as the end points {D 1 , D 2 , . . . , D r−1 }of local route planning in turn; and dividing the global route into a total of r−1 segments, S 1 D 1 , S 2 D 2 , . . . , S r−1 D r−1 , wherein a coordinate value of S i in the existing raster map is (x S i ,y S i ), and a coordinate value of D i is (x D 1 ,y D 1 );
S 420 : initializing a state parameter set L(l) of the mobile robot, wherein l(x, y, yaw,v,w) records state parameters of the robot movement in the planned route, including position, course angle, linear velocity and angular velocity; as for the robot, its initial linear velocity is set to v(m/s), its initial angular velocity is set to w(rad/s) and its initial navigation angle is set to yaw(rad);
S 430 : calculating a slope angle of S i D i with the following formula:

α
i

=

arctan
⁡
(

y

D
i

-

y

S
i

x

D
i

-

x

S
i

)

wherein x D 1 ,y D i are an abscissa and an ordinate of D i respectively, and x s i , y s i are an abscissa and an ordinate of S i respectively;
converting the slope angle of S i D i section route into a radian value as an initial course angle of the mobile robot, wherein a formula for converting the slope angle into the radian value is as follows:
yaw i =α×180°÷π

acquiring all state parameters l i (x s i , y s i , yaw i , v i , w i ), 1≤i≤r of the robot in the S i D i section route, wherein x s i , y s i are the abscissa and the ordinate of the node S i respectively, v i is the linear velocity at the end point of the previous route in the DS i D i section route, and w i is the angular velocity at the end point of the previous section route read in the S i D i section route;
S 440 : calculating the slope angle α i+1 of the S i+1 D i+1 section route according to S 430 , and converting yaw i in the previous state parameter l i of the robot into the angle value β, wherein a formula for converting the radian value into the angle value is as follows:
β=yaw i ×π÷180°

Assignees

• Univ Chongqing
• Star Inst Of Intelligent Systems
• Dibi Chongqing Intelligent Tech Research Institute Co Ltd

Inventors

• Song Yongduan
• Zhang Congyi
• Tan Lihui
• Lai Junfeng
• Wang Saiyu
• Zhang Yankai

Classifications

• G05D1/0217 Primary

  in accordance with energy consumption, time reduction or distance reduction criteria · CPC title

• G05D1/0274 Primary

  using mapping information stored in a memory device (navigation using map-matching G01C21/30) · CPC title

• G05D1/0221

  involving a learning process · CPC title

• G05D1/0223

  involving speed control of the vehicle (vehicle fittings for automatically controlling, i.e. preventing speed from exceeding an arbitrarily established velocity or maintaining speed at a particular velocity, as selected by the vehicle operator B60K31/00) · CPC title

• Y02D30/70

  in wireless communication networks · CPC title