Dynamic Path Planning of AGV Based on Kinematical Constraint A* Algorithm and Following DWA Fusion Algorithms

In the field of AGV, a path planning algorithm is always a heated area. However, traditional path planning algorithms have many disadvantages. To solve these problems, this paper proposes a fusion algorithm that combines the kinematical constraint A* algorithm and the following dynamic window approach algorithm. The kinematical constraint A* algorithm can plan the global path. Firstly, the node optimization can reduce the number of child nodes. Secondly, improving the heuristic function can increase efficiency of path planning. Thirdly, the secondary redundancy can reduce the number of redundant nodes. Finally, the B spline curve can make the global path conform to the dynamic characteristics of AGV. The following DWA algorithm can be dynamic path planning and allow the AGV to avoidance moving obstacle. The optimization heuristic function of the local path is closer to the global optimal path. The simulation results show that, compared with the fusion algorithm of traditional A* algorithm and traditional DWA algorithm, the fusion algorithm reduces the length of path by 3.6%, time of path by 6.7% and the number of turns of final path by 25%.


Introduction
In recent years, due to the great market demands, broad application prospects and large potential development, mobile robots are widely used in transportation systems, e.g., for inspecting high-speed railway stations, sweeping roads and intelligently managing traffic [1,2]. Mobile robots are widely used in the coal mining industry, e.g., as a mining robot, coal robot and so on. Intelligent mining machinery can improve the scientific nature and efficiency of mining [3,4]. Mobile robots are widely used in the military field, e.g., as reconnaissance robots, supplies robots, attack robots and so on [5]. Intelligent military equipment can improve the combat capability and level of troops [6]. With the development of science, technology and the innovation of automobile technology [7], autonomous vehicles have gradually developed into intelligent networked vehicles, which continuously improve traffic safety and efficiency. Path planning is also an important bridge between the environment awareness module and the tracking control module of the autonomous vehicle [8]. With the modern agricultural production process moving towards intelligence, information, scale and refinement, agricultural machinery path planning technology is one of the basic technologies of intelligent agricultural equipment [9]. It not only can improve the quality of agricultural machinery operation, but also can make agricultural production standardization and normalization [10]. It promotes the efficient production of smart agriculture [11].
It is worth pointing out that most of the aforementioned path planning algorithms have a number of disadvantages, such as low search efficiency, discontinuous curvature and long path. Luo Q [12] shows that the unequal allocation of initial pheromones is constructed. The pseudo random states transition rule is used for route selection. Moreover, the proportion of determined or random selection is adjusted adaptively. Sang H [13] has shown that the (1) Improve the child node selection method and heuristic function of A* algorithm. This can improve the search efficiency of the algorithm. Reduce computing burden. Make the generated path more realistic; (2) For secondary redundant node removal, use B spline curve for smoothness constraint. This reduces the number of redundant points in the path. The generated path conforms to the dynamic constraints; (3) Intercept the key node information and apply improved DWA algorithm to make the local path planning follow the global path contour, so as to make the path smoother and achieve avoidance of local dynamic obstacle. It can prevent DWA algorithm from falling into local optimal. It allows the mobile robot to avoid moving obstacles.
The rest of this paper is organized as follows. Section 2 describes the disadvantages of traditional A* and improves the A* algorithm. Section 3 presents the improvement of DWA algorithm, the improved A* algorithm and the improved DWA algorithm fusion. Section 4 introduces the algorithm for simulation experiment and analysis of experimental values. Finally, Section 5 concludes our work and describes some future research issues.

Improved A* Algorithm
In the grid map, there is a heuristic search strategy based on the evaluation function of the traditional A* algorithm, which can find a long obstacle-free path of a static environment. However, there are a number of turn points and redundant points in this unsmooth path, which is unconformity kinematic. The car may not be able to go along this path, which is a tangent to the apex of obstacles. In this paper, we improve the traditional A* according to the motion characteristics and map information.

Node Optimization
In the mobile robot industry, the traditional A* algorithm has long been used for planning purposes, but there is still the difficulty of avoiding moving obstacle and temporary obstacle. The influence of node search strategy has shown a lot of redundant points and infection points. The traditional A* algorithm search principle is to add the starting point in the open list. Then, add the point as the father node to the close list. Next, search its neighboring children's nodes in 8 directions [3]. It visits nodes in a graph from the start node to the goal node. However, its simple search tactic is rigid when oriented to different environments. In most cases, the obstacle environment in which the mobile robot is located is not complex. The number of search directions can be reduced at this time. In this paper, we propose a five sub-nodes search method, which connects the current node with the target node and sets its angle with Y circumference as θ. θ can be obtained from Equation (1). With the node as the center, there is no obstacle to this region and the search direction will be reduced from 8 to 5, as shown in the Figure 1 and the search direction from Table 1.
Sensors 2023, 23, x FOR PEER REVIEW 3 of 16 this path, which is a tangent to the apex of obstacles. In this paper, we improve the traditional A* according to the motion characteristics and map information.

Node Optimization
In the mobile robot industry, the traditional A* algorithm has long been used for planning purposes, but there is still the difficulty of avoiding moving obstacle and temporary obstacle. The influence of node search strategy has shown a lot of redundant points and infection points. The traditional A* algorithm search principle is to add the starting point in the open list. Then, add the point as the father node to the close list. Next, search its neighboring children's nodes in 8 directions [3]. It visits nodes in a graph from the start node to the goal node. However, its simple search tactic is rigid when oriented to different environments. In most cases, the obstacle environment in which the mobile robot is located is not complex. The number of search directions can be reduced at this time. In this paper, we propose a five sub-nodes search method, which connects the current node with the target node and sets its angle with Y circumference as θ . θ can be obtained from Equation (1). With the node as the center, there is no obstacle to this region and the search direction will be reduced from 8 to 5, as shown in the Figure 1 and the search direction from Table 1.

Improved Heuristic Function
A* algorithm is a graph search algorithm depended on the heuristic selection of the heuristic function [26]. Heuristic information related to the characteristics of the problem is utilized to guide its performance. Calculate the cost of the nodes in the open list based on the evaluation function. Select the node with the lowest cost as the next parent node. Put it into the close list. Search the parent reachable node again. Calculate its cost and so

Improved Heuristic Function
A* algorithm is a graph search algorithm depended on the heuristic selection of the heuristic function [26]. Heuristic information related to the characteristics of the problem is utilized to guide its performance. Calculate the cost of the nodes in the open list based on the evaluation function. Select the node with the lowest cost as the next parent node. Put it into the close list. Search the parent reachable node again. Calculate its cost and so on until the parent node is the location of the target point. It is easy to see that h(n) has a great impact on the search efficiency: This is because the heuristic function is Euclidean distance. The estimated value is smaller than the actual surrogate value. This paper proposes increase the ratio of h(n) in f (n). The basic form of the evaluation function is as follows: The optimized form of the evaluation function is as follows: where g(n) is the actual cost from the starting point of the AGV path to the current node, r is the distance from the current point to the target point and R is the distance from the start point to the target point.

Redundancy Removal
The secondary removal of redundant points is improved on the basis of the ordinary removal of redundant points [27]. The algorithm of common redundancy removal takes the greedy algorithm as an example. The greedy algorithm selects the optimal parent node in the current condition. The parent node it selects is added with collision detection function. The final generated path will not collide with the obstacle. Connect the target point and the initial point in collision detection. If the collision means that it cannot be omitted, then the target node's previous father node is set as the target point to repeat the above process. If no collision occurs, this means that it can be omitted. The detection ends when the father node is the starting point. Moreover, the path point that does not collide will generate the de-redundant trajectory.
However, the normal redundancy removal is not sufficient and the path generated by it will be tangent to the vertices of the obstacles. In this case, the generated path is apparently collision-free. When the mobile robot travels along the path, it will collide with the obstacle when it reaches the vertex of the obstacle. Moreover, no space is reserved for the smoothness constraint on the subsequent path. Therefore, the common algorithm of removing redundant points is insufficient. This paper proposes a new algorithm-a redundant point secondary removal algorithm-when the distance between all nodes and the center point of the obstacle is greater than the preset safety distance. Then, the nodes between the two points that form a straight line are considered redundant nodes and can be removed. This can happen when there is any equilibrium point and the distance between the center point of the obstacle is less than the preset safety distance. The redundancy cannot be pointed removal operation. In this paper, the safety distance is the length of the edge of a grid.
where norm() function is used to find the Euclidean norm, and the ceil() function is used to find the nearest integer that is not smaller than the number in parentheses, with k 1 being any positive integer. As shown in Figure 2, the unremoved redundant path is X 1 → X 2 → X 3 → X 4 → X 6 . If removing this point can shorten the length of the path, it is not tangent to the vertex of the obstacle. It is regarded as a point that can be removed and the points on the path are judged. Finally, the redundancy removal path can be obtained

B Spline Curve Smoothing Constraints
Considering the kinematic characteristics of the mobile robot [28,29], it is necessary to constrain the smoothness of the path after the redundancy has been removed by a B spline curve, which can generate a smooth path to continuous curvature by using the path points put into the path after the redundancy has been removed by the greedy algorithm as the control points of the B spline curve basis function. This can be achieved by using a B spline curve to constrain the smoothness of the path after redundancy removal [30,31]. Not only can the order be specified, but also changing the control points will change the shape of part of the curve, which retains the advantages of the Bezier curve and optimizes the shortage of the Bezier curve that cannot be modified in the local path.
Let there exist 1 n + control points , ,..., n k T t t t + + = and k -order B spline curve can be defined as follows.
, 0 where ( ) This is how the equation can be expressed in matrix form: Ρ are control points, as shown in Figure 3.

B Spline Curve Smoothing Constraints
Considering the kinematic characteristics of the mobile robot [28,29], it is necessary to constrain the smoothness of the path after the redundancy has been removed by a B spline curve, which can generate a smooth path to continuous curvature by using the path points put into the path after the redundancy has been removed by the greedy algorithm as the control points of the B spline curve basis function. This can be achieved by using a B spline curve to constrain the smoothness of the path after redundancy removal [30,31]. Not only can the order be specified, but also changing the control points will change the shape of part of the curve, which retains the advantages of the Bezier curve and optimizes the shortage of the Bezier curve that cannot be modified in the local path.
where P(t) is B-spline curve function and B i,k (t) is k-order B-spline basis function. The paths with redundant points removed are smoothed using the cubic B spline curve constraint. Moreover, the cubic B spline curve equation are (A1)-(A3). This is how the equation can be expressed in matrix form: where P 0 , P 1 , P 2 , P 3 are control points, as shown in Figure 3.
In this paper, a B spline curve is used to smoothly fit the path after removing redundancy, and the smoothed path is shown in Figure 4. In this paper, a B spline curve is used to smoothly fit the path after removing redundancy, and the smoothed path is shown in Figure 4.
initial path B spline curve smooth

Improved DWA Algorithm
Dynamic window algorithm (DWA algorithm) is a common algorithm for solving local obstacle avoidance of mobile robots. The algorithm is designed to sample multiple sets of velocities in the velocity space based on the motion model of the mobile robot. Analyze and predict the trajectory of the mobile robot at each set of velocities over a period of time. Then, select the velocity corresponding to the optimal trajectory according to the In this paper, a B spline curve is used to smoothly fit the path after removing redundancy, and the smoothed path is shown in Figure 4.

Improved DWA Algorithm
Dynamic window algorithm (DWA algorithm) is a common algorithm for solving local obstacle avoidance of mobile robots. The algorithm is designed to sample multiple sets of velocities in the velocity space based on the motion model of the mobile robot. Analyze and predict the trajectory of the mobile robot at each set of velocities over a period of time. Then, select the velocity corresponding to the optimal trajectory according to the

Improved DWA Algorithm
Dynamic window algorithm (DWA algorithm) is a common algorithm for solving local obstacle avoidance of mobile robots. The algorithm is designed to sample multiple sets of velocities in the velocity space based on the motion model of the mobile robot. Analyze and predict the trajectory of the mobile robot at each set of velocities over a period of time. Then, select the velocity corresponding to the optimal trajectory according to the evaluation function of the algorithm. Drive mobile robot for local path planning according to velocity corresponding to the optimal trajectory.

Motion Model of Mobile Robot
The DWA algorithm samples the linear and angular velocities of the mobile robot in the window region, so the first step requires kinematic modeling of the mobile robot. Assume that the mobile robot moves in a uniform linear motion for a period of time (∆t). Denote the kinematic model of the mobile robot during that time period.

Speed Sampling for Mobile Robots
In the mobile robot velocity group space, there are theoretically infinite sets of velocity sets (υ, ω). However, the mobile robot is easily constrained by its own hardware and working environment. It is necessary to constrain the range of sampled velocity sets according to the actual situation after obtaining the robot motion model.
The maximum and minimum linear velocity υ max , υ min and angular velocity ω max , ω min ranges of a mobile robot constrained by its own conditions can be expressed as: The mobile robot is affected by its own motor. There is a deceleration constraint. Within the dynamic window display, the mobile robot is affected by the acceleration of the maximum. The minimum speed range is: where υ t is the current linear velocity, υ a is linear acceleration, ω t is the current angular velocity and ω a is angular acceleration. Mobile robot braking distance constraint. The realization of dynamic obstacle avoidance depends mainly on the braking distance constraint. The DWA algorithm looks for obstacles in the process of selecting the speed and trajectory evaluation. To ensure the reliability and safety of the mobile robot when it works, it needs its speed to be reduced to 0 under the maximum deceleration condition before colliding with the obstacle. The constraint Equation (12) is shown.

Optimization of Evaluation Function
The evaluation function in the DWA algorithm is used to select the optimal trajectory. The criteria for the regulation evaluation are: accurate avoidance of obstacles and the shortest time-consuming approach to the target point. The design evaluation function is: where head(υ, ω), dist(υ, ω) and vel(υ, ω) are the azimuth, distance, velocity evaluation sub-functions, respectively. σ is the smoothing function. α, β, γ are the weighting coefficients of each evaluation sub-function. However, the target points of path of DWA algorithm are proposed by the global path. This paper optimizes the evaluation function that change the angle difference between the end direction of the trajectory and the final target point to the angle difference between the current target point: where PHead(υ, ω) is the angular difference between the end direction of the trajectory and the current target point.

Algorithm Fusion
The improved A* algorithm obtains a smooth navigation path. However, if there was an obstacle to this route, the route would not be changed. The DWA algorithm has good local dynamic obstacle avoidance capability. However, it is easy to fall into local optimum or even no solution. Therefore, this algorithm fuses the improved A* algorithm with the DWA algorithm. Extract the key nodes in the global path generated by the improved A* algorithm. Use them as temporary target points of the DWA algorithm so that the local path planning follows the contour of the global path planning. By combining the two, the mobile robot is able to avoid dynamic obstacles while not colliding with static obstacles in the path planning process.
First, initialize the raster map, select the starting point and target point and generate an initial path using the improved A* algorithm. As the path may slice the obstacle fixed point, paths are de-redundant twice. Then, use the B spline as a smoothness constraint to obtain the global path. Extract the key nodes in the path. Thirdly, DWA algorithm receives the extracted key nodes as each target point of the DWA algorithm. The DWA algorithm kinematically models the mobile robot, samples the velocity and simulates the moving trajectory of each velocity. Fourthly, combine the evaluation function to select the optimal simulated moving trajectory. Move along the globally planned path with the optimal trajectory, while avoiding local dynamic obstacles. Finally, reach the target point. The flow chart of fusion algorithm is shown in Figure 5.

Experimental and Results
In order to test the feasibility and effectiveness of the improved algorithm in this paper, the running environment used is: MATLAB R2022a; Windows 10 64bit; processor (Intel) i5-12400F 12th generation; memory 16GB.

Experimental and Results
In order to test the feasibility and effectiveness of the improved algorithm in this paper, the running environment used is: MATLAB R2022a; Windows 10 64bit; processor (Intel) i5-12400F 12th generation; memory 16GB.
In order to verify the validity of the improved child nodes search strategy, set the starting coordinates to (6,7) and the ending coordinates to (17,16). Then, conduct 8direction search and 5-direction search, respectively, to obtain the 8-direction search node schematic and 5-direction search node schematic, which can obviously find the search node reduction. In the map, the black squares are obstacles. The triangle is the starting point, the circle is the goal point. The gray grid is the child node of the search. The black dotted line is the path. The number of children in 5-search direction decreased by 7.6% compared to the number in 8-search direction, as shown in Figure 6. In the figure, the black squares are obstacles. The gray squares are the children of the search. The triangle with coordinates (6, 7) is the starting point. The coordinate (17,16) is the end point. In order to verify the effectiveness of secondary de-redundancy, set the starting coordinates to (4,3) and the ending coordinates to (17,16). Then, a path with the redundancy points not removed and a path with the redundancy points removed are generated, respectively, which can obviously find that the number of turns is less. The length of path is shorter and the path is not tangent to the vertex of the obstacle. The number of turns in a path with the redundancy points removed decreased by 66.7%, as shown in Figure 7 In order to verify the effectiveness of secondary de-redundancy, set the starting coordinates to (4,3) and the ending coordinates to (17,16). Then, a path with the redundancy points not removed and a path with the redundancy points removed are generated, respectively, which can obviously find that the number of turns is less. The length of path is shorter and the path is not tangent to the vertex of the obstacle. The number of turns in a path with the redundancy points removed decreased by 66.7%, as shown in Figure 7. In the figure, the black squares are obstacles. The black dotted line is the path. The triangle with coordinates (4, 3) is the starting point. The coordinate (17,16) is the end point.
In order to verify the effectiveness of the B spline curve, set the starting coordinates to (4, 3) and the ending coordinates to (17,16). Then, an unsmoothed path and a smoothed B spline path are generated, respectively, which can obviously find that the smooth path of the B spline curve conforms to the kinematic characteristics of AGV, as shown in Figure 8. In the figure, the black squares are obstacles. The black dotted line is the path. The triangle with coordinates (4, 3) is the starting point. The coordinate (17,16) is the end point.
In the map, the black squares are obstacles. The triangle is the starting point, the circle is the goal point. The circle with the green arc is the mobile robot. The yellow square is the moving obstacle, the gray square is the temporary static obstacle. The blue dashed line is the global path, and the blue solid line is the final generated path. To verify the global path planning and the ability to avoid local dynamic obstacles of the improved A* fusion DWA algorithm, a 21 m × 21 m raster map is selected for the experiment. A movable obstacle is set in the path of the robot. The simulation experiment results are shown in Figure 9.
points not removed and a path with the redundancy points removed are generated, respectively, which can obviously find that the number of turns is less. The length of path is shorter and the path is not tangent to the vertex of the obstacle. The number of turns in a path with the redundancy points removed decreased by 66.7%, as shown in Figure 7. In the figure, the black squares are obstacles. The black dotted line is the path. The triangle with coordinates (4, 3) is the starting point. The coordinate (17,16) is the end point. In order to verify the effectiveness of the B spline curve, set the starting coordinates to (4,3) and the ending coordinates to (17,16). Then, an unsmoothed path and a smoothed B spline path are generated, respectively, which can obviously find that the smooth path of the B spline curve conforms to the kinematic characteristics of AGV, as shown in Figure  8. In the figure, the black squares are obstacles. The black dotted line is the path. The triangle with coordinates (4, 3) is the starting point. The coordinate (17,16) is the end point. In the map, the black squares are obstacles. The triangle is the starting point, the circle is the goal point. The circle with the green arc is the mobile robot. The yellow square is the moving obstacle, the gray square is the temporary static obstacle. The blue dashed line is the global path, and the blue solid line is the final generated path. To verify the global path planning and the ability to avoid local dynamic obstacles of the improved A* fusion DWA algorithm, a 21 m × 21 m raster map is selected for the experiment. A movable obstacle is set in the path of the robot. The simulation experiment results are shown in Figure  9. In the map, the black squares are obstacles. The triangle is the starting point, the circle is the goal point. The circle with the green arc is the mobile robot. The yellow square is the moving obstacle, the gray square is the temporary static obstacle. The blue dashed line is the global path, and the blue solid line is the final generated path. To verify the global path planning and the ability to avoid local dynamic obstacles of the improved A* fusion DWA algorithm, a 21 m × 21 m raster map is selected for the experiment. A movable obstacle is set in the path of the robot. The simulation experiment results are shown in Figure  9. To verify the improved evaluation function of DWA algorithm, the linear velocity, angular velocity and pose of AGV were, respectively, generated by traditional DWA algorithm and improved DWA algorithm. It can be obviously found that the linear velocity fluctuation of AGV is reduced, the angular velocity fluctuation is smaller and the posture is better. The simulation experiment results are shown in Figures 10-12. To verify the improved evaluation function of DWA algorithm, the linear velocity, angular velocity and pose of AGV were, respectively, generated by traditional DWA algorithm and improved DWA algorithm. It can be obviously found that the linear velocity fluctuation of AGV is reduced, the angular velocity fluctuation is smaller and the posture is better. The simulation experiment results are shown in Figures 10-12.    . Improved algorithm and traditional algorithm for AGV linear velocity. Figure 11. Improved algorithm and traditional algorithm for AGV linear velocity.  To verify the fitting of local path to global path, the linear velocity, the paths generated by the hybrid algorithm of traditional A* algorithm and traditional DWA algorithm and the paths generated by the fusion algorithm that combines the kinematical constraint A* algorithm and the following DWA algorithm are generated, respectively. The path in red is the global path. The blue path is the local path. The yellow grid at coordinate (9,4) is the termination position of the movable obstacle. There is a temporary stationary obstacle at coordinate (14,15). It can be obviously found that the path of the fusion algorithm is better. The simulation experiment results are shown in Figure 13.  To verify the fitting of local path to global path, the linear velocity, the paths generated by the hybrid algorithm of traditional A* algorithm and traditional DWA algorithm and the paths generated by the fusion algorithm that combines the kinematical constraint A* algorithm and the following DWA algorithm are generated, respectively. The path in red is the global path. The blue path is the local path. The yellow grid at coordinate (9, 4) is the termination position of the movable obstacle. There is a temporary stationary obstacle at coordinate (14,15). It can be obviously found that the path of the fusion algorithm is better. The simulation experiment results are shown in Figure 13. To verify the fitting of local path to global path, the linear velocity, the paths generated by the hybrid algorithm of traditional A* algorithm and traditional DWA algorithm and the paths generated by the fusion algorithm that combines the kinematical constraint A* algorithm and the following DWA algorithm are generated, respectively. The path in red is the global path. The blue path is the local path. The yellow grid at coordinate (9,4) is the termination position of the movable obstacle. There is a temporary stationary obstacle at coordinate (14,15). It can be obviously found that the path of the fusion algorithm is better. The simulation experiment results are shown in Figure 13.  The running results of Tables 2-4 show that the global path length has been reduced 3.571% by improved A* algorithm. The time of the local path has been reduced 50% by an improved DWA algorithm. Compared with the fusion algorithm of traditional A* algorithm and traditional DWA algorithm, the hybrid algorithm proposed in this paper can plan an optimal path, which can reduce the number of turns of the final path by 25%.

Conclusions
This paper introduces a fusion algorithm that combines the kinematical constraint A* algorithm and the following dynamic window approach. Firstly, optimize the search strategy for child nodes. Secondly, improve the heuristic function of the A* algorithm. Thirdly, remove redundant points of path. Fourthly, create a B spline curve smooth path. Finally, improve the heuristic function of the DWA algorithm. Compared with the fusion algorithm of traditional A* algorithm and traditional DWA algorithm, the fusion algorithm reduces length of path by 3.6%, time of path by 6.7% and the number of turns of the final path by 25%. The local path is closer to the global path. AGV has lower linear velocity, smaller angular velocity and better pose angle. AGV can avoid dynamic obstacles.
The fusion algorithm proposed in this paper is better than other traditional algorithms, but it still has the following deficiencies: (1) In fusion algorithm, the kinematic characteristics of the mobile robot are considered.
The energy efficiency of the mobile robot is not considered. This may not save energy on mobile robots; (2) The fusion algorithm in this paper does not integrate environmental awareness. It only carries out path planning on the established map. There is a need to integrate efficient perception methods.
However, there are still a lot of future studies required in the field of robot path planning: (1) The fusion algorithm is applied to a real mobile robot. Verify the feasibility of fusion algorithm in a real mobile robot. The fusion algorithm is tested on different mobile robots. Verify the compatibility of the fusion algorithm. Test the fusion algorithm in different environments. Some coefficients in the fusion algorithm are adjusted for different environments; (2) Coordinate and control robot clusters to prevent conflicts. Multi-machine collaboration can expand the working radius of the mobile robots. Autonomously assign multiple tasks. Multi-task assignment can make the mobile robots work more efficiently. Research is a hot topic.