ASL-DWA: An Improved A-Star Algorithm for Indoor Cleaning Robots

The traditional A-star algorithm has many search nodes, and the obtained path has polylines and cannot avoid local unknown obstacles. In response to these problems, this paper proposes a new improved A-star algorithm suitable for indoor cleaning robots, called ASL-DWA (A Star Leading Dynamic Window Approach). First, to solve the problem of many search nodes in the A-star algorithm, a new hybrid heuristic function that combines Euclidean distance and point-to-line distance is proposed, thereby reducing the number of search nodes. Then, to solve the problem that the A-star algorithm has a polyline path and cannot avoid local unknown obstacles, this paper designs the global path yaw angle according to the relationship between the real-time position of the robot and the global path, which is added as a score item to the traditional score function. A decay coefficient with prediction function is also added to the score function to reduce the risk of the algorithm falling into local optima. Finally, a mechanism to adaptively adjust the coefficient according to the distance between the robot and the target point is designed, thereby realizing ASL-DWA. The ASL-DWA algorithm is tested in three indoor environments and compared with traditional algorithms. The experimental results show that ASL-DWA can meet the path planning requirements of mobile robots in indoor environments, and has obvious advantages over traditional algorithms.

into the local optimum, resulting in the inability to reach the 104 target point. 105 Aiming at the cleaning robot in the indoor environ-106 ment, this paper proposes an improved A-star path plan-107 ning method, called ASL-DWA. In the first stage, the A-star 108 algorithm with a new hybrid heuristic function is used to 109 obtain the global path, during which the safety distance 110 between the robot and the obstacle is taken into account. 111 In the second stage, the global path yaw angle is designed, 112 which describes the relationship between the robot's real-113 time pose and the global path. In the third stage, the global 114 path yaw angle is added to the traditional scoring function 115 as a scoring term. In the fourth stage, attenuation coefficients 116 with prediction function are used. Finally, an adaptive mecha-117 nism that autonomously adjusts the various score coefficients 118 according to the distance between the robot and the target 119 point is used to realize ASL-DWA. 120 The contributions of this paper are as follows: 121

1) A hybrid heuristic function based on Euclidean dis-122
tance and point-to-line distance is proposed, thereby 123 reducing the number of search nodes in the A-star 124 algorithm.

125
2) The global path yaw angle is proposed, which com-126 bines the global path information with the real-time 127 information of the robot so that the robot can avoid 128 unknown obstacles and is not easy to fall into the local 129 optimum. At the same time, the method also avoids 130 the appearance of polyline paths, thereby reducing the 131 movement time of the robot from the starting point to 132 the target point.

133
3) A decay coefficient with a prediction function is pro-134 posed. The score is intervened by predicting the closest 135 distance between the robot and the obstacle in the next 136 few cycles, thereby further avoiding the algorithm from 137 falling into a local optimum.
138 4) Propose an adaptive weighting mechanism. By judging 139 the distance between the robot and the target point, 140 the weights of each score are adaptively adjusted. This 141 mechanism enables the robot to avoid obstacles effec-142 tively and reach the target point accurately.

143
The rest of this paper is organized as follows. The second 144 section introduces the A-star algorithm based on the new 145 hybrid heuristic function, including environment modeling, 146 the design principle of the hybrid heuristic function, and the 147 selection of the expansion threshold. Section III introduces 148 the design principles of the global path yaw angle, attenuation 149 coefficient, and adaptive mechanism. Then a new scoring 150 function is introduced on this basis, and finally, the algorithm 151 flow of ASL-DWA is introduced. The fourth section is the 152 experiment and results in analysis. First, the proposed hybrid 153 heuristic function is compared with the other four traditional 154 heuristic functions, and then ASL-DWA is compared with 155 several other algorithms. Section 5 summarizes the perfor-156 mance advantages of the ASL-DWA algorithm and the short-157 comings of current research, and looks forward to several 158 directions for future research. 159 VOLUME 10, 2022

II. HYBRID HEURISTIC FUNCTION BASED A-STAR
160 ALGORITHM 161 A. ENVIRONMENT MODELING 162 The cleaning robot used in this paper is shown in Fig 1. The 163 robot has a circular shape and a diameter of 34cm.   in this environment. In Fig 2.b, each grid corresponds to an 172 area of 5cm * 5cm. The black grid represents the obstacle area; 173 the cyan grid represents the blank area; the smoke-white grid 174 represents the unknown area, that is, the area that has not been 175 detected by the robot. 176 The A-star algorithm performs path planning in a grid map. 177 The resulting path should allow the cleaning robot to pass 178 safely, so the robot's outer dimensions should be considered. 179 At the same time, to ensure the safety of the robot, the outside 180 of the robot should maintain a sufficient safety distance from 181 obstacles. For these reasons, this paper identifies the area near 182 the obstacle on the map, which is called the warning area, that 183 is, the area that is not recommended for robots to pass through 184 although there are no obstacles.

185
As shown in Fig. 3(a), the area with the obstacle net P as the 186 center and r+d as the radius is marked as the warning area, 187 where r is the radius of the robot and d is the minimum safe 188 distance between the outside of the robot and the obstacle. 189 The final result is shown in Fig 3.b. In the figure, cyan 190 indicates the safe passage area; black indicates obstacles; 191 green indicates the warning area.
Among them, f(n) is the cost value of node P n , g(n) is the 198 actual path value of node P n , and h(n) is the heuristic value. 199 The A-star algorithm based on the hybrid heuristic function 200 includes the following steps.

201
Step 1: Put the starting point into the open-list, and put 202 these blank grids adjacent to the starting point into the 203 open-list.

204
Step 2: Take a node p(n) from the open-list as the cur-205 rent node. Calculate the cost function f(n+k) of its adjacent 206 node p(n+k), where k= [1,2,. . . ,8]. Calculate the difference 207 between the cost function of the current node and the adjacent 208 node, the calculation formula is The Chebyshev distance is shown in Equation 3 .
In the formula, (x n , y n ) is the coordinate of the node P n , 233 (x N , y N ) is the coordinate of the target point P N , and d is the 234 weighting coefficient. Chebyshev calculates the x-direction 235 distance and y-direction distance between the node and the 236 target point and selects the larger absolute value as the output. 237 The Manhattan distance is shown in Equation 4 .
It calculates the x-direction distance and y-direction dis-240 tance between the node and the target point and uses the sum 241 of their absolute values as the node's heuristic value.

242
The diagonal distance is shown in Equation 5.
is 245 the shorter one the distance between the node and the tar-246 get point in the x-direction and the y-direction. h 2 (n) = 247 d * (|x n − x N | + |y n − y N |) is the Manhattan distance of two 248 points. h(n) is the diagonal distance, which roughly approx-249 imates the Euclidean distance, avoiding the squaring and 250 exploiting operation.

251
The Euclidean distance is shown in Equation 6.
To reduce the number of search nodes of the A * algorithm 254 in the indoor environment, a new hybrid heuristic function is 255 proposed here, as shown in Equation 7.
Among them, h 1 (n) = |x n * a + y n * b + c| /sqrt (a * a + b * b) 259 is the distance from the node p n (x n , y n ) to the line a * x + 260 b * y + c = 0. The line crosses the start point and the 261 taget point. h 2 (n) = d * sqrt((x n − x N ) 2 + (y n − y N ) 2 ) is the 262 Euclidean distance from the node p n (x n , y n ) to the target point 263 p N (x N , y N ).
Among them, f(n) is the cost value of the current node     The above calculation process is implemented on Matlab7. 326 The distribution of the solutions of the difference function is 327 shown in Figure 6. In Figure 6, points of the same color constitute a set of 329 solutions for the different functions. The vertical axis is the 330 number of solutions, and the horizontal axis is the numerical 331 value of the solutions.    center of the robot is R 2 = 12cm, and the distance between 392 the guide wheel and the center of the robot is R 3 = 11.5cm. 393 In the figure, v is the linear velocity of the robot center, ω is 394 VOLUME 10, 2022 .
Among them, t is the sampling interval, (x t , y t, θ t ) is the . .  The difference between the robot's orientation θ 0 and the 427 global path's orientation θ sk is the global path yaw angle, 428 denoted as θ path , which can be obtained by Equation 12. The black dot represents the robot center p 0 . The red square 439 represents the robot center at the closest point P s of the global 440 path, and the orange square represents the forward point P k . 441 Fig 10.a shows the most ideal situation, where the robot 442 center is on the global path, and the robot is oriented in 443 the same way as the global path. In this case, the point 444 P s (x s , y s ) closest to the robot center on the global path coin-445 cides with the robot center p 0 (x 0 , y 0 , θ 0 ), and the forward 446 path in front of the robot, so θ sk = 0, the global yaw angle 472 In Fig 10.e, the robot is located near the turning point 474 of the global path, the global path forward direction before 475 turning is 0 • , the global path forward direction after turning 476 is −45 • , and the robot's heading angle is 0 • . At this time, 477 the point P s (x s , y s ) closest to the robot center on the global 478 path coincides with the robot center, and the forward point 479 P k (x k , y k ) is located on the global path after turning, so θ sk = 480 −32 • , the global yaw The angle θ path = |θ 0 − θ sk | = 32 • .

481
In Fig 10.f, the robot is located near the turning point of the 482 global path, with a certain distance from both global paths. 483 The forward direction of the global path before turning is 0 • , 484 the forward direction of the global path after turning is −45 • , 485 and the orientation angle of the robot is 0 • . At this time, the 486 point P s (x s , y s ) that is closest to the robot center on the global 487 path is the vertical foot of the robot center on the global path 488 before turning, and the forward point P k (x k , y k ) is located on 489 the global path after turning, so θ sk = −35 • , global yaw angle 490 In the formula, (v, ω) are the linear and angular veloci-502 ties of the robot. (α, β, γ ) are fixed weighting coefficients. 503 target_heading(v, ω) is the score obtained by measuring the 504 angle between the robot's orientation and the target's ori-505 entation, called the target orientation score: obs_dist(v,ω) is 506 the score obtained by measuring the distance between the 507 robot and the obstacle, which is called the obstacle distance 508 score. velocity(v,ω) is the score obtained by measuring the 509 difference between the robot's speed and the optimal speed, 510 called the speed score.

511
The target orientation score can be obtained from 512 Equation 14.
Among them, θ 0 is the orientation angle of the robot, and 515 θ tar is the orientation angle of the robot to the target point.

516
The obstacle distance score can be obtained by Equation15. 517 The speed score can be obtained by Equation 16.
Among them, v max is the maximum linear velocity that 534 the robot is allowed to achieve, v min is the minimum linear 535 velocity that the robot is allowed to achieve, and v 0 is the 536 desired optimal linear velocity.

538
Among them, ω max is the maximum angular velocity that 539 the robot is allowed to achieve, ω min is the minimum angular 540 velocity that the robot is allowed to achieve, and ω 0 is the 541 desired optimal angular velocity.

561
The global path distance score is obtained according to 562 the distance between the robot center and the global path, 563 as shown in Equation 20. The global yaw score and the global path distance score are 568 added to the traditional score function shown in Equation 13 569 to obtain a score function with a fixed coefficient, as shown 570 in Equation 21.

577
Equation 21 obtains the optimal speed to drive the robot to 578 the next location. However, when the robot reaches the new 579 location, it may find that it cannot proceed with the next path 580 planning, that is, it is stuck in a local optimum. To reduce the 581 probability of this phenomenon, the prediction coefficient is 582 introduced in this paper, as shown in Equation 22.
Among them, t is the number of forecast periods and k is 585 the decay coefficient. The robot moves at the current speed for 586 t cycles, and the closest distance to the obstacle in each cycle 587 is denoted as d i . Min i=t i=1 (d i ) is the closest distance between the 588 robot and the obstacle in t cycles. pre min is the set minimum 589 distance.

590
As shown in formula 22, the working principle of the 591 prediction coefficient is: Assuming that the robot continues 592 to move at the current speed, predict the closest distance 593 between the robot and the obstacle in the next few cycles, 594 if the distance is less than the threshold, multiply the score of 595 the current speed by an attenuation factor k, thus reducing the 596 probability of this speed is selected.

597
The score function with prediction coefficients is shown in 598 Equation 23. Considering the kinematic characteristics of the mobile 656 robot, this paper selects T=0.8S. The ideal motion speed 657 of the cleaning robot used is v=25cm/s, and the distance 658 traveled in one sampling period is s=T * v=20cm. The safety distance means that the robot maintains a suffi-661 cient distance from the obstacle so that the robot can avoid 662 the obstacle in time. It should be noted that the mentioned 663 obstacles refer to the obstacles in the forward direction of the 664 robot.  In the formula, r is the outer radius of the robot, v is 670 the optimal linear velocity, and ω is the maximum angular 671 velocity. For the cleaning robot used in this article: r=17cm, 672 v=25cm/s, ω max = 40 • /s. Calculation result: dmin=35.8cm. 673

674
As shown in Equation 22, the algorithm intervenes in the 675 current speed score by predicting the closest distance between 676 the robot and the obstacle in the next few cycles.

677
The effects of different forecast periods on path planning 678 are shown in Figure 13. In the figure, the black dot is the 679 starting point, the black square is the target point, the red dot 680 is the moving obstacle, and the red line is the path.

681
See Figure 13.a, when the prediction period is 0 or too 682 small, the algorithm may get stuck in a local optimum.

683
See Figure 13.c, when the prediction period is too large, 684 the algorithm may get erroneous results at relatively narrow 685 channels. 686 VOLUME 10, 2022      In Fig 15.b, the robot center coincides with the global path, 719 but the robot orientation is not consistent with the global path. 720 At this time, if the global path obtained by A star is followed, 721 the robot needs to stop and rotate in the same direction as the 722 global path. The path obtained by the adaptive weighted score 723 function enables the robot to avoid pauses through arc motion 724 and improve the action efficiency.

725
In Fig 15.c, the robot is located near the turning point of the 726 global path. The forward direction of the global path before 727 the turning is 0 • , the forward direction of the global path after 728 the turning is −45 • , and the robot heading angle is 0 • . If the 729 global path obtained by A star is followed, the robot needs to 730 stop and turn in the same direction as the global path. Driven 731 by the global yaw angle, the new score function obtains a 732 smooth path, which enables the robot to move more smoothly 733 to the global path after turning.    In the four cases shown in Fig. 16 In Fig 17.a the three curves marked with τ represent the 761 relationship between the weighting coefficient (1-σ (d)) * τ 762 and the distance corresponding to the global path yaw score 763 when τ = 0.5, 0.4, and 0.3, respectively. The three curves 764 marked with α represent the relationship between the weight-765 ing coefficient σ (d) * α corresponding to the target orientation 766 score and the distance when α = 0.8, 0.6, and 0.4, respec-767 tively. As shown in the figure, when the distance between the 768 robot and the target point is far, the weighting coefficient cor-769 responding to the target orientation score is very small, while 770 the weighting coefficient corresponding to the global path 771 yaw score is close to τ . As the robot gradually approaches 772 the target point, the weighting coefficient corresponding to 773 the target orientation score gradually increases, while the 774 weighting coefficient corresponding to the global path yaw 775 score gradually decreases.  In the case of using a fixed coefficient score function, 787 the scores corresponding to the 3-speed combinations are 788 obtained by formula 21: s 1 = 82.1, s 2 = 87.7, s 3 = 78.0. 789 It can be seen that (v 2 , ω 2 ) corresponds to the highest score, 790 and the robot will execute at this speed. The final planned 791 path is shown in the purple circle in Figure 17.c and the path 792 fall into a local optimum.

793
In the case of using the adaptive weighted score function, 794 the scores corresponding to the 3-speed combinations are 795 obtained by formula 24: s 1 = 57.1, s 2 = 50.0, s 3 = 42.8. 796 It can be seen that (v 1 , ω 1 ) corresponds to the highest score, 797 VOLUME 10, 2022 and the robot will execute at this speed. The final planned 798 path is shown in the blue circle in Figure 17.c and the path 799 reach the target smoothly.

847
(2) Global path planning. According to the search result of 848 A star, a global path from the out point to the target point is 849 obtained, denoted as Path{p 0 , . . . , p M }.

850
(3) Calculate the score corresponding to the sampling 851 speed (v i , ω i ). Firstly, the global path yaw score, target head-852 ing angle score, obstacle distance score, and speed score are 853 calculated respectively, and then the final score is obtained 854 according to the adaptive weighted score function.

855
(4) Run ASL-DWA. That is, traverse the entire sampling 856 space to obtain the optimal speed combination (v, ω) as the 857 output result.  The number of search nodes for the five heuristic functions 912 is shown in Table 2.  Table 2, compared with the heuristic function 914 based on Chebyshev distance, the proposed hybrid heuristic 915 function reduces the number of nodes in the three maps by 916 25.17%, 26.5%, and 16.6% respectively, with an average 917 reduction of 22.7%. Compared with the heuristic function 918 based on Manhattan distance, the number of nodes is reduced 919 by 29.6%, 18.8%, and 1.6%, and the average is 16.7; com-920 pared with the heuristic function based on diagonal distance, 921 the number of nodes is reduced by 16.5%, 13.7 %, -3%, 922 an average reduction of 9%; compared with the heuristic 923 function based on Euclidean distance, the number of nodes 924 is reduced by 13.4%, 11%, -2.4%, and an average reduction 925 of 7.3%.  Table 3 shows the number of search nodes for the five 936 heuristic functions in the three grid maps. We use several indicators to quantify the pathfinding effect of 953 the algorithm: the number of static times, the rotation angle 954 in situ, the path length, the movement time, and whether the 955 target is reached.

956
Static times refer to the number of times the robot stops 957 moving, which is recorded as N static .

958
The in-situ rotation angle refers to the rotation angle of the 959 robot around the center of the machine, denoted as A turn .

960
The path length refers to the movement distance of the 961 robot from the starting point to the target point, denoted as 962 L path . To simplify the calculation, we store the path as N 963 discrete points at certain distance intervals. The path length 964 can be obtained by Equation 27.
Among them, N is the number of points, and (x i , y i ) is the 967 coordinates of the i-th point on the path. Among them, s i is the arc length of the robot in the i-th 974 sampling period, v i is the linear velocity of the robot in 975 the i-th sampling period, and M is the number of sampling 976 periods. For the traditional A * algorithm, the linear speed 977 of the robot is v=25cm/s. For the ASL-DWA algorithm, the 978 optimal linear speed of the robot is also 25cm/s, but the actual 979 movement speed is determined by the real-time calculation 980 results. N static is the number of times the robot stops, and t static 981 is the time for each stop. Taking into account the braking time, 982 the elimination of inertia time, and other factors, the stopping 983 time of the robot is t static = 500ms. A turn is the angle which 984 the robot turns around its center. ω turn is the angular velocity 985 when rotating in place, and the angular velocity of the robot 986 rotating in place is ω = 40 • /S.

987
To compare the pathfinding effect of the ASL-DWA algo-988 rithm and the traditional A-star algorithm, we tested both 989 algorithms in the three environments shown in Figure 20. In the absence of unknown obstacles, the real-time map 992 during robot movement is the same as the static map used 993 in A-star path planning. The path planning results of the 994 traditional A-star algorithm and the proposed ASL-DWA 995 algorithm in three environments are shown in Figure 23.  of the mobile robot when using ASL-DWA and the blue line 1022 is the walking path using other algorithms.

1023
As shown in Fig 24.a-c, when there are unknown obstacles 1024 near the global path, traditional algorithms cannot guide the 1025 robot to avoid these obstacles, resulting in an early stop of the 1026 path planning.

1027
As shown in Fig 24.d, when the ASL-DWA algorithm is 1028 adopted, the robot can avoid moving obstacles and reach the 1029 target smoothly.

1030
The parameters of the paths obtained by the two algorithms 1031 are shown in Table 5.  Table 6 shows the parameters of the paths obtained by 1041 several algorithms.

1042
As shown in Table 6

1049
This paper introduces a new path planning algorithm called 1050 the ASL-DWA algorithm. It is suitable for indoor cleaning 1051 robots, which can reduce the number of search nodes of 1052 the traditional A * algorithm, eliminate polyline paths, guide 1053 robots to avoid unknown obstacles, and avoid falling into 1054 local optimum. Experiments show that the proposed ASL-1055 DWA algorithm has obvious advantages compared with other 1056 algorithms.

1057
There are several directions for future work. (1) In the 1058 indoor environment, not all of the ground is on one plane, 1059 and the path planning based on 3D sensor information is a 1060 direction worthy of research. (2) The working goal of the 1061 cleaning robot is to achieve full coverage of the working 1062 environment. How to combine the coverage algorithm with 1063 the path planning algorithm will be an interesting topic; 1064 (3) Multi-machine collaboration can make the cleaning robot 1065 work more efficiently, Research in this area is a hot topic.