Path planning for obstacle avoidance of mobile robot based on optimized A*and DWA algorithm

Yi CAO，Yi ZHOU，Ya-bin ZHANG

（College of Electrical Engineering，Henan University of Technology，Zhengzhou 450000，China）

Abstract：A path planning method combining with an improved A*and Dynamic Window Approach（DWA）is proposed to address the problems of discontinuous path curvature and inefficient obstacle avoidance in the path planning of mobile robots.Firstly，based on the traditional A*algorithm，the conventional eight search directions are deduced to five for improving the search efficiency.Secondly，the Floyd algorithm is introduced into the A*algorithm，and a new heuristic search function is designed to achieve no oblique through the vertex of the obstacle，and to increase the smoothness of the path.Finally，a novel evaluation function is constructed by fusing the improved algorithm with the DWA to avoid barriers while ensuring the global optimization of the planned path.The simulation results indicate that this research makes some practical sense for the implementation of autonomous navigation of mobile robots.

Key words：Mobile robot，Path planning，A*algorithm，DWA algorithm，Global optimization

1 Introduction

Path planning is one of the critical issues in the field of mobile robotics research，which can be defined as an optimal route from the desired starting point to a target with the shortest path and obstacles avoidance，according to path planning algorithms.Path planning has two categories：one is global path planning，where the information including obstacles and scenarios in the workspace is all known［1］；the other is local path planning，where the information about obstacles is unknown.In recent years，there has been much research on robot path planning，including sample-based Voronoi graph methods and Rapidly-exploring Random Tree algorithms；model-based Artificial Potential Field methods［2］and Dynamic Window approach［3］；biologically-inspired Neural Networks，i.e.，Genetic and Ant Colony algorithms［4］；nodebased Dijkstra［5］，A*algorithm，and D*algorithm，etc.

The A*algorithm has been widely used for autonomous path planning for mobile robots.However，the curvature of the planned path by this algorithm is discontinuous with more sharp turning points and longer path length，which is not conducive to the smooth operation of the robot.Wang et al.［6］proposed an improved strategy for the A*algorithm that simplifies the path，but the algorithm only boosts the path curvature instead reduce the path length.Also，the cost to therobot for execution does not much deduce，and the robot does not have dynamic avoidance capabilities.In a grid environment，Dong et.al［7］put the planning information of artificial potential field method as the basis of the ant colony optimization algorithm by introducing the potential field force as part of the heuristic information for ant searching path point.This present method solves the disadvantage of the artificial potential field method prone to fall into extreme value points，but trades the calculation efficient off.

Lu et al.［8］fused the A*algorithm with the Floyd algorithm by removing redundant nodes and smoothing the path，but with the loss of real-time obstacle avoidance in real-time.Cheng et al.［9］adopted the DWA（Dynamic Window Approach）algorithm for real-time path planning，which has good obstacle avoidance ability and is suitable for autonomous navigation of mobile robots but is prone to fall into local optimization.

Considering the shortcomings mentioned in［8-9］，such as longer planning time，irregular path curvature，prone to fall into the local optimum，and cannot avoid obstacles in real-time，this paper proposes an improved A*.First，the search direction is screened to enhance the search efficiency，then the A*cost function is improved by the introduction of the Floyd algorithm［10］，so that the developed A*algorithm could generate the path with less turning point，shorter length，and smoother curvature.Finally，the hybrid algorithm is fused with the DWA algorithm to construct an evaluation function for the globally optimal path，which enables the robot search for the globally optimal path and avoids obstacles in real-time.

2 Principle of A*algorithm

The A*algorithm is one of the most popular means in path planning since its quite flexible and wide variety of scenarios，with a typical heuristic feature.Its principal expression is：

Where f（n）denotes the path length from the starting point through the current node to the target node；g（n）the path measure from the starting point to the current node n（g（n）which is already the shortest distance from the starting point to the current point）；h（n）is the estimated cost function distance from the state node n to the target state.Since g（n）is the actual distance，it is essential to choose the type of cost h（n）to search for the optimal solution on a static map，and the value of f（n）depends on the size of h（n）.The search node is shown in Fig.1.The blue grid indicates the starting point position，the yellow grid the target point position，and the 8 grids around the blue one imply the candidate child nodes that can be set as the current state space.

Fig.1 A*algorithm search node

3 Improvements to A*algorithm

While the traditional A*algorithm has become a widely used path planning algorithm for mobile robot navigation，the paths planned by the algorithm suffer from a high number of turns and sharp turn angles，which have a significant impact on the mobile robot during operation.Therefore，an improved A*algorithm suitable for mobile robots is proposed，which consists of the following three aspects：

3.1 Improve search directions

The traditional A*algorithm expands all the candidate nodes each time developing the children of the current node，so that it may have a certain amount of invalid search in complex environments and a cost of a certain amount of time.Therefore，based on the traditional A*algorithm，with adding the directional judgment，the eight search nodes decline to five by eliminating the three expansion nodes that are opposite to the direction of the target point，meanwhile improving the search efficiency.The principle is as follows.

Connect the current extension node to the target point in a straight line and measure the angleαbetween the line and the X-axis of the map in the forward direction.

When（xt-xs）＞0 and（yt-ys）＞0，θ＝0°，ω＝1；When（xt-xs）＞0 and（yt-ys）＜0，θ＝180°，ω＝-1；

When（xt-xs）＜0 and（yt-ys）＜0，θ＝180°，ω＝1；

When（xt-xs）＜0 and（yt-ys）＞0，θ＝360°，ω＝-1；

Where（xt，yt）is the endpoint coordinate，（xs，ys）is the current expansion point coordinate，theθandω are constants.The comparison yields eight alpha values，leaving out the three with the largest ones and remaining the other five，thus improving search efficiency.As shown in Fig.2，S is the starting point，T is the end point，red ones are the discarded nodes and the grays are the valid search node.

Fig.2 lmproved A*algorithm search nodes

3.2 Smooth path

The Floyd algorithm is able to use the idea of dynamic planning to find the shortest distances between every pair of vertices in a given edge-weighted directed Graph.The algorithm’s goal is to find the shortest path from point i to point j.This paper introduces the Floyd algorithm into the A*algorithm，removing redundant nodes，reducing path length and developing the path smoother.The optimization principle is shown in Fig.3.

Fig.3 Optimization path principle

The black parts of Fig.3 represent the obstacle，the white part the available blank area，and the black solid line segments represent the unoptimized path.S stands for the starting point，T the target point，S1through S4the different nodes on the path，and the dashed lines the alternative paths that the robot can travel through.Expanding the range of the neighborhood，it can obtain the optimal paths in the figure are S1-S4.The process pseudocode is as follows：

3.3 Improve the cost function

The A*algorithm is a heuristic search approach where the cost function h（n）directly affects the path planning performance of the algorithm.The most common cost functions of the classical A*algorithm are the Manhattan distance function and the Euclidean distance function.

Where nx，ny，gx，gyare the current nodes where the robot is presently located and target nodes.Considering that the robot is a two-wheeled differential robot without omnidirectional mobility and that the improved A*algorithm searches the current node’s 5-neighborhood grid each time，however，neither of the above cost functions is optimal.Therefore，a new cost function based on the Euclidean distance is applied

Where d is the distance from the current point to the target point，and D is the distance from the starting point to the target.

4 Dynamic window approach

The principle of the Dynamic Window Approach isto sample multiple sets of velocities in the velocity space according to its motion model，predict the possible motion trajectory of the robot in the future period at different speeds of each set，and meanwhile use a specific evaluation function to assess the trajectory of the sets and select one set with the highest evaluation to perform until the next execution time.

4.1 Robotic motion model

Considering the actual implementation of the robot，the motion model should first be established，assuming that the trajectory of the robot in adjacent time is supposed to be circular，while the radius of its circular motion is：

When the rotation speed“ω”is not equal to 0，the robotic model coordinates are

Where x is the displacement of the robot in the x-axis direction，y the displacement of the robot in the y-axis，v the linear velocity of the robot’s motion，ωthe angular velocity of the robot’s motion，andθ（t）the angle between the robot’s motion direction and the xaxis in the adjacent timeΔt.

4.2 Robotic speed sampling

There exist infinitely many sets of velocity pairs（v，ω）in the two-dimensional velocity space，but the velocity range can be constrained according to the robot’s constraints and environmental factors.This paper makes the following provision for robots：

1）The maximum and minimum speed constraint of the robot is：

2）Motor acceleration and deceleration constraint：The maximum and minimum velocities due to acceleration present in the time interval during which the simulated robot advances.

Where vc、ωcis the current velocity；the maximum acceleration；the maximum deceleration.

3）Raking distance constraint：for the safety of the robot under maximum deceleration conditions，the current speed should be able to decelerate to zero before violating an obstacle，then

Where dist（v，ω）is the nearest distance to the obstacle on the corresponding trajectory（v，ω）.

4.3 The evaluation function

If several sets of sampling speeds are feasible in the velocity space，the optimal trajectory needs to be selected using an evaluation function.The original evaluation function is：

Where：head（v，ω）is the azimuth evaluation function，representing the angular difference between the orientation of the robot and the target when it reaches the end of the simulated trajectory at the current speed；dist（v，ω）the distance between the robot and the nearest obstacle on the current trajectory；vel（v，ω）the evaluation function of the size of the current speed；σthe smoothing function；α，β，γare the weighting factors of the 3 terms.

5 Hybrid algorithm

Although the DWA is of good obstacle avoidance，this algorithm cannot estimate the global optimum during the application process，which can lead to the robot falling into the local optimum during the operation process.Consequently，this paper chooses to fuse the A*algorithm with this algorithm to ensure the global optimum of the dynamic planning path.

Avoiding the DWA falling into local optimization，a dynamic window evaluation function takes into account the globally optimal path as below：

Where N head（v，ω）is the directional angular deviation between the endpoint direction of the simulated trajectory and the current target point.The current target point is the one in the globally optimal path sequence closest to the current point in the heading direction of the robot.This evaluation function enables local path planning to conform to the globally optimalpath planning direction，thus ensuring global optimization.The algorithm flow shows in Fig.4.

Fig.4 Flowchart of the fusion algorithm

6 Simulation tests

These simulation scenarios were carried out under a grid map（21 m ×21 m with a grid spacing of 1 m）with starting coordinates（4.5 m，16.5 m）and target coordinates（19.5 m，2.5 m）in each set of experiments.Scenario 1 is the global path planning of the traditional A*algorithm in the absence of obstacles；Scenario 2 is the global path planning using the improved A*algorithm based on 1；No.3 is the global path planning using the improved A*algorithm and DWA algorithm based on scenario 2.

The traditional A*algorithm，the improved A*algorithm，and the hybrid algorithm are used for global path planning，respectively.The path planning results of various algorithms in the test experiment show in Fig.5.

In Fig.5，the solid green line refers to the path from the traditional A*algorithm，and the red dashed line is the trajectory from the improved A*algorithm.The improved algorithm significantly boosts the path，not only on removing the redundant nodes by the traditional A*algorithm，which make the path more smooth，but also on shortening the path length r compared to the traditional A*algorithm.In addition，in Fig.5，the path（denoted by a solid bule curve）from the fusion one is not only good at shortening the search time but also adds the expansion factor so that the robot can avoid collisions in the maneuver.The comparative data are shown in Tables 1 and 2.

Fig.5 Simulation results of three algorithms

Table 1 Simulation data for the three algorithms

Based on scenario 3，the fusion algorithm’s obstacle avoidance capability is validated.The experimental parameters are set as follows：maximum robot speed：2.0 m／s，maximum rotational speed：40.0（°）／s，acceleration：0.4 m／s2，rotational acceleration：100.0（°）／s2，speed resolution：0.02 m／s，speed resolution：2.0（°）／s，azimuthal deviation coefficientα：20，obstacle distance coefficientβ：30，current speed size coefficientγ：30，predicted systemtime：3.0 s，extra obstacle point（grid）（10.5 m，14.5 m）.The results of the experiment were as follows.It can be seen from Fig.6 that the global path from the improved A*algorithm shown in the dotted curve cannot avoid the sudden appearance of obstacles.When the DWA is incorporated，it not only prevents the extra obstruction but also reaches the target point under global optimization，achieving the intended purpose of the hybrid algorithm.

Fig.6 Dynamic obstacle avoidance simulation results

7 Conclusion

For the mobile robot path planning，this paper proposes a path planning method that combines the optimization A*method and the DWA.The proposed method removes redundant points of the path sequence obtained by the traditional A*algorithm，improves the path planning performance，and makes the moving path smoother.Based on the DWA algorithm，an evaluation function that takes into account the globally optimal path is constructed，which can avoid the DWA falling into the local optimal，and enable the globally optimal path planning.The comparison to the traditional A*algorithm shows this improved one is good at the path smooth，real-time obstacle avoidance，and feedback control of the mobile robot.

Since the A*algorithm is required for global planning，and the computational complexity of the algorithm increases exponentially with the dimension，the hybrid method of this paper needs to be further improved for real-time path planning of highly complicated scenarios.Besides，since the DWA algorithm is planned according to the location of obstacles，the choice of the proportion of parameters in the evaluation function is particularly important.Due to improper parameter selection，there are still cases of falling into the local optimum during the experiment.Therefore，this proposed method also needs to improve the adaptability of parameters in the future.