METHOD FOR ROUTE OPTIMIZATION BASED ON DYNAMIC WINDOW AND REDUNDANT NODE FILTERING

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.

Claims

1. A method for route optimization based on dynamic window and redundant node filtering, comprising: S100: By using an existing raster map data set, the coordinate information of a starting position and a destination position of movement is determined, and a destination node and an obstacle node are marked in the raster map; S200: A* algorithm is used to plan a global route; S300: The global route planned by A* algorithm is globally optimized, filtering redundant nodes out; S400: By combining the dynamic window algorithm, the local optimization is performed section by section on the global route optimized in S300 so as to obtain a final global route.

2. The method for route optimization based on dynamic window and redundant node filtering according to claim 1, wherein the A* algorithm is used to plan a global route in S200, comprising the following steps: S210: Two empty tables OpenList and CloseList are built and initialized, and a starting position is set as a current node and stored into the OpenList list. If the current node is not a destination point, an extended node function is called to select all nodes adjacent to the current node and extensible, and information of all the extended nodes are stored into the OpenList list; S220: An insert node function is called to traverse all extensible nodes and calculate a cost function F corresponding to these nodes, which is expressed as:
F(n)=G(n)+H(n) In the formula, n represents the current node; F(n) is the cost function of the current node n; G(n) is the actual cost value of the mobile robot from an initial node to the node n; H(n) is the 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.sub.g−x.sub.n).sup.2+(y.sub.g−y.sub.n).sup.2)} Wherein (x.sub.g, y.sub.g) represents the coordinates of the destination node in the raster map and (x.sub.n, y.sub.n) represents the coordinates of the current node in the raster map; S230: The node corresponding to the minimum value of the cost function F(n) is selected, which is then deleted from the OpenList and stored into the CloseList. At the same time, this node is set as the current node connected to the previous node. S220 is repeated until the current node turns to be the destination point, and the global route is exported.

3. The method for route optimization based on dynamic window and redundant node filtering according to claim 2, wherein the global route planned by A* algorithm in S300 is globally optimized, and redundant nodes are filtered out, comprising: S310: The route node listed in the CloseList that is obtained in S200 is labeled as P{P.sub.i, 1≤j≤n}, wherein P.sub.1 is the starting point of the route and P.sub.n is the end point of the route. A key point set U{P.sub.1,P.sub.n} with the only initial values, P.sub.1 and P.sub.n, is created for storing key nodes after route optimization; S320: For the node set P{P.sub.i, 1≤j≤n}, if m=2, 3, 4, . . . n, the connection starts in sequence from P.sub.1 to P.sub.2, P.sub.3, . . . , and P.sub.m. It is determined whether any obstacle node exists between the straight line P.sub.1P.sub.m, and, if so, the node P.sub.m−1 must exist in the route. If the straight line P.sub.1P.sub.m does not pass through the obstacle node, it is determined that P.sub.m−1 is a redundant node; All nodes that must exist in the route are added to the set U. After the selection of key points is done, the set U{P.sub.1, P.sub.m−1, . . . , P.sub.m+k, P.sub.n} (2≤m≤n, 1≤k≤n−m) contains all key nodes. It is assumed that the number of nodes in U is r, namely U{P.sub.1, P.sub.2, . . . , P.sub.r}, (1≤r≤n); S330: All nodes in the set U are connected in sequence to complete the global optimization of the route.

4. The method for route optimization based on dynamic window and redundant node filtering according to claim 3, wherein in S400, by combining the dynamic window algorithm, the local optimization is performed section by section on the global route optimized in S300 so as to obtain a final global route, which specifically comprises: S410: As for the set U, all nodes starting from the starting point P.sub.1, except the end point P.sub.r, are taken as the starting points of local route planning in turn, with the starting points being labeled as {S.sub.1, S.sub.2, . . . , S.sub.r−1}. At the same time, {P.sub.2, P.sub.3, . . . , P.sub.r} starting from the second node P.sub.2 in the set U are labeled as the end points {D.sub.1, D.sub.2, . . . , D.sub.r−1} of local route planning in turn. As a result, the global route can be divided into a total of r−1 segments, S.sub.1D.sup.1, S.sub.2D.sub.2, . . . , S.sub.r−1D.sub.r−1, and the coordinate value of S.sub.1 in the raster map is (x.sub.S.sub.1,y.sub.S.sub.1), the coordinate value of D.sub.1 is (x.sub.D.sub.1,y.sub.D.sub.1), and so on; S420: A state parameter set L(l) of the mobile robot is initialized, 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); S430: A slope angle of S.sub.iD.sub.i is calculated with the following formula: ${\alpha }_i=\arctan \left(\frac{y_{D_i}-y_{S_i}}{x_{D_i}-x_{S_i}}\right)$ In the formula, x.sub.D.sub.i, y.sub.D.sub.i are the abscissa and ordinate of D.sub.i respectively, and x.sub.S.sub.i, y.sub.S.sub.i are the abscissa and ordinate of S.sub.i respectively; The slope angle of S.sub.iD.sub.i section route is converted into a radian value as an initial course angle of the mobile robot, and the formula for converting the slope angle into the radian value is as follows:
yaw.sub.i=α×180°÷π All state parameters l.sub.i(x.sub.i, y.sub.i, yaw.sub.i, v.sub.i, w.sub.i), 1≤i≤r of the robot in the S.sub.iD.sub.i section route are acquired, wherein x.sub.i, y.sub.i are the abscissa and ordinate of node S.sub.i respectively, v.sub.i is the linear velocity at the end point of the previous section route read in the S.sub.iD.sub.i section route, and w.sub.i is the angular velocity at the end point of the previous section route read in the S.sub.iD.sub.i section route; S440: The slope angle α.sub.i+1 of the Si.sub.+1D.sub.i+1 section route is calculated according to S430, and yaw.sub.i in the previous state parameter l.sub.i of the robot is converted into the angle value β, wherein the formula for converting the radian value into the angle value is as follows:
β=yaw.sub.i×π÷180° By comparing α.sub.i+1 and β, if |α.sub.i+1−β|<60°, the robot keeps the latest state l.sub.i+1(x.sub.i+1, y.sub.i+1, yaw.sub.i+1, v.sub.i+1, w.sub.i+1); while if |α.sub.i+1−β|≥60°, in order to avoid redundant route sections and unnecessary movement time due to the robot detouring and turning, the robot navigation angle is set to ${\mathrm{yaw}}_{i+1}=\frac{{\mathrm{yaw}}_i}{2};$ S450: S440 is repeated until the mobile robot reaches the end point D.sub.r−1 of the r−1.sup.th route section, and a set L(l.sub.1, l.sub.2, . . . , l.sub.i, l.sub.i+1, . . . , l.sub.r) recording all state parameters of the mobile robot is obtained, thus completing the optimization of the local routes.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0037] FIG. 1 is a process diagram of the method for route planning and optimization according to the present disclosure.

[0038] FIG. 2 is a procedure diagram of A* Algorithm and the global optimization.

[0039] FIG. 3 is a process diagram of the local optimization based on the idea of the dynamic window algorithm.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0040] Embodiments in the present disclosure will be further described with reference to accompanying drawings below.

[0041] By reference to FIGS. 1 to 3, a method for route optimization based on dynamic window and redundant node filtering is provided, including:

[0042] S100: By using an existing raster map data set, the coordinate information of a starting position and a destination position of movement is determined, and a destination node and an obstacle node are marked in the raster map. In this step, map data sets can be obtained by manual measurement, remote sensing measurement, photogrammetry, satellite data, and other manners, and can also be obtained by repeatedly collecting map information in an unknown environment by operating the mobile robot.

[0043] S200: A* algorithm is used to plan a global route, including:

[0044] S210: Two empty tables OpenList and CloseList are built and initialized, and a starting position is set as a current node and stored into the OpenList list. If the current node is not a destination point, an extended node function is called to select all nodes adjacent to the current node and extensible [all nodes excluding obstacle nodes], and information of all the extended nodes are stored into the OpenList list;

[0045] S220: An insert node function is called to traverse all extensible nodes and calculate a cost function F corresponding to these nodes, which is expressed as:

F(n)=G(n)+H(n)

[0046] In the formula, n represents the current node; F(n) is the cost function of the current node n; G(n) is the actual cost value of the mobile robot from an initial node to the node n; H(n) is the 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.sub.g−x.sub.n).sup.2+(y.sub.g−y.sub.n).sup.2)}

[0047] Wherein (x.sub.g,y.sub.g) represents the coordinates of the destination node in the raster map and (x.sub.n,y.sub.n) represents the coordinates of the current node in the raster map;

[0048] S230: The node corresponding to the minimum value of the cost function F(n) is selected, which is then deleted from the OpenList and stored into the CloseList. At the same time, this node is set as the current node connected to the previous node. S220 is repeated until the current node turns to be the destination point, and the global route is exported.

[0049] S300: The global route planned by A* algorithm is globally optimized, filtering redundant nodes out, including:

[0050] S310: The route node listed in the CloseList that is obtained in S200 is labeled as P{P.sub.i, 1≤j≤n}, wherein P.sub.1 is the starting point of the route and P.sub.n is the end point of the route. A key point set U{P.sub.1,P.sub.n} with the only initial values, P.sub.1 and P.sub.n, is created for storing key nodes after route optimization;

[0051] S320: For the node set P{P.sub.i, 1≤j≤n}, if m=2, 3, 4, . . . n, the connection starts in sequence from P.sub.1 to P.sub.2, P.sub.3, . . . , and P.sub.m. It is determined whether any obstacle node exists between the straight line P.sub.1P.sub.m, and, if so, the node P.sub.m−1 must exist in the route. If the straight line P.sub.1P.sub.m does not pass through the obstacle node, it is determined that P.sub.m−1 is a redundant node;

[0052] All nodes that must exist in the route are added to the set U. After the selection of key points is done, the set U{P.sub.1, P.sub.m−1, . . . , P.sub.m+k, P.sub.n} (2≤m≤n, 1≤k≤n−m) contains all key nodes. It is assumed that the number of nodes in U is r, namely U{P.sub.1, P.sub.2, . . . , P.sub.r}, (1≤r≤n).

[0053] S330: All nodes in the set U are connected in sequence to complete the global optimization of the route, which specifically includes:

[0054] S410: As for the set U, all nodes starting from the starting point P.sub.1, except the end point P.sub.r, are taken as the starting points of local route planning in turn, with the starting points being labeled as {S.sub.1, S.sub.2, . . . , S.sub.r−1}. At the same time, {P.sub.2, P.sub.3, . . . , P.sub.r} starting from the second node P.sub.2 in the set U are labeled as the end points {D.sub.1, D.sub.2, . . . , D.sub.r−1} of local route planning in turn. As a result, the global route can be divided into a total of r−1 segments, S.sub.1D.sub.1, S.sub.2D.sub.2, . . . , S.sub.r−1D.sub.r−1, and the coordinate value of S.sub.1 in the raster map is (x.sub.S.sub.1,y.sub.S.sub.1) the coordinate value of D.sub.1 is (x.sub.D.sub.1,y.sub.D.sub.1), and so on;

[0055] S420: A state parameter set L(l) of the mobile robot is initialized, 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);

[0056] S430: A slope angle of SiDi is calculated with the following formula:

[00003]${\alpha }_i=\arctan \left(\frac{y_{D_i}-y_{S_i}}{x_{D_i}-x_{S_i}}\right)$

[0057] In the formula, x.sub.D.sub.i, y.sub.D.sub.i are the abscissa and ordinate of D.sub.i respectively, and x.sub.S.sub.i, y.sub.S.sub.i are the abscissa and ordinate of S.sub.i respectively;

[0058] The slope angle of S.sub.iD.sub.i section route is converted into a radian value as an initial course angle of the mobile robot, and the formula for converting the slope angle into the radian value is as follows:

yaw.sub.i=α×180°÷π

[0059] All state parameters l.sub.i(x.sub.i, y.sub.i, yaw.sub.i, v.sub.i, w.sub.i), 1≤i≤r of the robot in the S.sub.iD.sub.i section route are acquired, wherein x.sub.i, y.sub.i are the abscissa and ordinate of node S.sub.i respectively, v.sub.i is the linear velocity at the end point of the previous section route read in the S.sub.iD.sub.i section route, and w.sub.i is the angular velocity at the end point of the previous section route read in the S.sub.iD.sub.i section route;

[0060] S440: The slope angle α.sub.i+1 of the S.sub.i+1D.sub.i+1 section route is calculated according to S430, and yaw.sub.i in the previous state parameter l.sub.i of the robot is converted into the angle value β, wherein the formula for converting the radian value into the angle value is as follows:

β=yaw.sub.i×π÷180°

[0061] By comparing α.sub.i+1 and β, if |α.sub.i+1−β|<60°, the robot keeps the latest state l.sub.i+1(x.sub.i+1, y.sub.i+1, yaw.sub.i+1, v.sub.i+1, w.sub.i+1); while if |α.sub.i+1−β|≥60°, in order to avoid redundant route sections and unnecessary movement time due to the robot detouring and turning, the robot navigation angle is set to

[00004]${\mathrm{yaw}}_{i+1}=\frac{{\mathrm{yaw}}_i}{2};$

[0062] S450: S440 is repeated until the mobile robot reaches the end point D.sub.r−1 of the r−1.sup.th route section, and a set L(l.sub.1, l.sub.2, . . . , l.sub.i, l.sub.i+1, . . . , l.sub.r) recording all state parameters of the mobile robot is obtained, thus completing the optimization of the local routes.

[0063] S400: By combining the dynamic window algorithm, the local optimization is performed section by section on the global route optimized in S300 so as to obtain a final global route.

[0064] The dynamic window algorithm is called DWA for short, and the core of DWA is to predict the future trajectory of the robot through different angular velocities and linear velocities. According to the present disclosure, the DWA method is used to optimize the real situation in the traveling process for optimization, which can be simply understood as: A* algorithm is used to plan a global route which becomes the movement route of the robot, and redundant nodes are filtered out, which is equivalent to preventing the robot from taking unnecessary paths. When the robot travels on the optimized global route, by DWA method, it avoids “pot holes” on the route, just like avoiding stone and pot holes on the road when driving. The route turn is wide enough that we turn from the inner track.

[0065] Experimental Comparison:

[0066] In view of the route planning and optimization methods applicable to the mobile robot mentioned in the present disclosure, in order to highlight the novelty thereof, this algorithm technology is put into a simulation experiment in comparison with traditional A* algorithm, Dijkstra algorithm and RRT algorithm as below, thus obtain the data listed in the following table. In the simulation experiment, three raster maps are constructed with the same size and randomly generated obstacles. They have 35 squares in the horizontal direction and 33 squares in the vertical direction, and a resolution of 10 cm*10 cm. The starting point coordinates are set to (0,0) and the end point coordinates are set as (31,28).

[0067] Comparison of length, calculation time and number of turning points produced by different algorithms are as shown in the following table.

TABLE-US-00001 Route Calculation Number of Map Algorithm length time turning points Map I Dijkstra Algorithm 466 0.0536 8 A* Algorithm 466 0.0503 5 RRT Algorithm 579 0.0056 15 This Algorithm 440 0.0459 4 Map II Dijkstra Algorithm 560 0.0612 16 A* Algorithm 516 0.0596 14 RRT Algorithm 521 0.0049 16 This Algorithm 506 0.0512 4 Map III Dijkstra Algorithm 482 0.0353 12 A* Algorithm 480 0.0344 11 RRT Algorithm 504 0.0081 30 This Algorithm 450 0.0251 8

[0068] It can be seen from the above table that in the three simulation experiments, all the 4 route planning algorithms can be used to plan the route, but they may result in some differences in aspects such as route length, calculation time, and number of turning points.

[0069] It can be seen from the table content that RRT algorithm provides the shortest computation time, but it produces too many turning points. This is because the route produced by this algorithm is composed of random tree nodes one after another, which may be containing edges and corners, not smooth enough, not an ideal smooth curve, and not conducive to the actual motion of the robot. Dijkstra algorithm, A* algorithm and this algorithm in the present disclosure all produce better global routes than RRT algorithm. However, compared with Dijkstra algorithm and A* algorithm, this algorithm is advantageous because of short route length, fewer turning points and better smoothness of route, which is suitable for the actual movement of the mobile robot and facilitates the robot to follow the route trajectory to reach the destination point.

[0070] Finally, it is noted that the above embodiments are only for the purpose of illustrating the technical scheme of the present disclosure without limiting it. Although a detailed specification is given for the present disclosure by reference to preferred embodiments, those of ordinary skills in the art should understand that the technical schemes of the present disclosure can be modified or equivalently replaced without departing from the purpose and scope of the technical schemes thereof, which should be included in the scope of claims of the present disclosure.