Research on APF-Dijkstra Path Planning Fusion Algorithm Based on Steering Model and Volume Constraints

Abstract

For the local oscillation phenomenon of the APF algorithm in the face of static U-shaped obstacles, the path cusp phenomenon caused by the vehicle corner and path curvature constraints is not taken into account, as well as the low path safety caused by ignoring the vehicle volume constraints. Therefore, an APF-Dijkstra path planning fusion algorithm based on steering model and volume constraints is proposed to improve it. First, perform an expansion treatment on the obstacles in the map, optimize the search direction of the Dijkstra algorithm and its planned global path, ensuring that the distance between the path and the expanded grid is no less than 1 m, and use the path points as temporary target points for the APF algorithm. Secondly, a Gaussian function is introduced to optimize the potential energy function of the APF algorithm, and the U-shaped obstacle is ellipticized, and a virtual target point is used to provide the gravitational force. Again, the three-point arc method based on the steering model is used to determine the location of the predicted points and to smooth the paths in real time while constraining the steering angle. Finally, a 4.5 m × 2.5 m vehicle rectangle is used instead of the traditional mass points to make the algorithm volumetrically constrained. Meanwhile, a model for detecting vehicle collisions is established to cover the rectangle boundary with 14 envelope circles, and the combined force of the computed mass points is transformed into the combined force of the computed envelope circles to further improve path safety. The algorithm is validated by simulation experiments, and the results show that the fusion algorithm can avoid static U-shaped obstacles and dynamic obstacles well; the curvature change rate of the obstacle avoidance path is 0.248, 0.162, and 0.169, and the curvature standard deviation is 0.16, which verifies the smoothness of the fusion algorithm. Meanwhile, the distances between the obstacles and the center of the rear axle of the vehicle are all higher than 1.60 m, which verifies the safety of the fusion algorithm.

1. Introduction

For the last few years, unmanned driving technology has become the focus topic of the society, which makes path planning as the core of intelligent driving more and more critical. People have explored a lot about path planning and invented many methods [1]. Common global algorithms include the A* algorithm [2], the RRT algorithm [3], the ant colony algorithm [4], etc. A number of local planning algorithms are in common use, including the DWA [5] and the APF [6], etc. The APF algorithm moves towards the target point by constructing a potential field [7], but it is easy to fall into local minima, and the path is not smooth enough, so it has been studied by a wide range of scholars. Zhang L. et al. [8] improved the APF method and overcame the tendency to fall into local minima. However, this method relies on the pre-drawn map and does not consider environmental changes, such as the addition of new obstacles. Qian Y. et al. [9] integrated the simulated annealing algorithm with APF to optimize the local minimal problem, which can achieve obstacle avoidance. Chu Y. et al. [10] propose the concepts of virtual pendant points and temporary forces to provide a path deviation-based attraction for USVs and to break the local minima dilemma, but there are limitations in the generalization of the algorithms, which do not take into account factors such as the size of the USVs. Zeng Z. et al. [11] proposed an optimization algorithm combining deep reinforcement learning and APF, which improved the path planning ability in complex environments. However, in the experiment, obstacles of different shapes were not considered, especially irregular obstacles such as U-shaped obstacles, and the generalization ability of the algorithm was not fully verified. Meng F. et al. [12] propose a two-way A*-APF fusion algorithm to improve the repulsive field function for the local optimal solution and oscillation problem of the artificial potential field method. However, the algorithm generalization is limited, and the volume factor of the algorithm carrier is not considered. Gao J. et al. [13] found that combining the simulated annealing algorithm and adding angle and distance constraints can effectively avoid dynamic obstacles. However, it does not consider how to avoid U-shaped obstacles when they are randomly generated, nor does it consider the volume factor of the carrier. Guo T. et al. [14] proposed an adaptive step size adjustment method, and the step size can be adjusted according to the distance between the robot and the obstacle as well as the number of iterations. However, the planned paths have cusps, are not smoothed, do not consider the dynamic obstacles, and do not take the volume of the carrier into account. Lee H.J. et al. [15] achieved the optimization of the local optimal problem by measuring the distance to the obstacles to reduce unnecessary walking. However, this simulation experiment is relatively simple, and there are no randomly occurring obstacles. Zheng L. et al. [16] combine the APF algorithm with an improved PSO algorithm. However, the planned path is too close to the obstacles, and there is even a risk of diagonal penetration of the obstacle. Also, moving obstacles are not considered. Chen J. et al. [17], irregular obstacles are wrapped with rectangular regions to avoid the robot searching for invalid rasters. However, this method is only applicable to static obstacles with known maps. Szczepanski R. et al. [18], virtual walls and virtual obstacles are created to guide the robot around the local minimum region. However, the curvature of the planned path is large, and no smoothing is performed. Zhai S. et al. [19] use a fifth-degree polynomial to fit discrete path points. However, this smoothing treatment is post-smoothing, and the post-smoothing treatment will be detached from the dynamic variability of the real situation.

Based on the common problems of APF algorithms in the above studies, this article is aimed at (1) Localized oscillatory phenomena due to random generation of U-shaped obstacles; (2) When the target point is reached, the combined potential energy of the potential field is not zero, and the total potential field changes by a large amount; (3) The vehicle obstacle avoidance process produces the phenomenon of path cusps as well as the planned path does not conform to the vehicle steering constraints; (4) No real-time smoothing, not based on real-time dynamic feedback; (5) The volume influence of the algorithm carrier was not taken into account, and other defects to carry out the following work, the flowchart of the fusion algorithm in this paper is shown in Figure 1.

(1)

When a U-shaped obstacle appears randomly and causes a local oscillation phenomenon, a virtual target point is generated near the U-shaped obstacle by ellipticizing the U-shaped obstacle to provide temporary gravitational force for the vehicle.

(2)

Introducing the Gaussian function and target distance factor in the combined potential energy function, when away from the target point, the repulsive potential energy will gradually equal the original potential energy, and the magnitude of the change of the total potential energy field will be smoother, and located in the vicinity of the target point, the value of the Gaussian function tends to be close to 0, which can ensure that the target point is the lowest point of the total potential energy of the whole field.

(3)

For the problems of path cusp phenomenon, more path folds, and planned paths not conforming to the vehicle steering constraints, the three-point arc method based on the steering model is used to determine the prediction points, and real-time smoothing is performed according to the vehicle model parameters.

(4)

The vehicle carrier is equated to a rectangle of 4.5 m × 2.5 m, and 14 envelope circles are covered on the rectangle to increase the volume effect, and at the same time, the vehicle collision detection model is established to calculate the force of the envelope circles, which can be more accurate to determine whether a collision occurs.

2. Improved APF Algorithm

The improvement of this paper will be carried out in the following five parts: Section 2.1 introduces the principle of the APF algorithm. Section 2.2 Optimize the global algorithm Dijkstra by changing its search method and removing path redundancy to preprocess the raster map. Section 2.3 A Gaussian function and a target distance factor are introduced to smooth the amplitude of the potential field oscillations and to minimize the combined potential energy at the target point for the combined potential field. Expand the U-shaped obstacles to obtain virtual target points and avoid local oscillation phenomena. Section 2.4 For the phenomenon of unsmooth paths and sharp points, the theory of the three-point arc method based on the steering model is used to smooth the paths in real time, limit the turning angle to a controllable range, and use different step sizes for different obstacle densities. Section 2.5 Establish a vehicle collision detection model, convert the mass into a 4.5 m × 2.5 m vehicle rectangle, and use 3 rows of 14 envelope circles to cover the body, changing the previous way of calculating the combined force.

2.1. Principle of APF Algorithm

The APF algorithm, as a kind of virtual composite force field, superimposes the attractive force from the target point and the repulsive force from the obstacle together to form a resultant force vector, which acts on the vehicle for local obstacle avoidance. The gravitational field, repulsive field, and combined potential field of the APF algorithm are shown in Figure 2.

As can be seen from the figure, the gravitational potential field is equivalent to a mountain slope, which will attract the ball to roll towards the bottom of the slope, while the repulsive potential field is like a high mountain, providing repulsive force to ensure that the ball will not be too close to it, and the superposition of the two together will result in the red ball rolling all the way around the high mountain to the bottom of the mountain slope, which is the effect of the APF.

In the traditional APF, the magnitude of the gravitational potential energy at the current point position increases as the distance and position of the target point increase. Similarly, the potential energy of the repulsive force decreases as the distance and position from the obstacle increase. The calculation formulas of the potential field functions for the gravitational potential field and the repulsive potential field are shown in Equations (1) and (2) as follows [20]:

$$U_{att}={\displaystyle \frac{k_{att}\rho {\left(q,q_{goal}\right)}^2}{2}}$$

(1)

$$U_{req} =\left\{\begin{array}{ll}{\displaystyle \frac{k_{rep}{\left(\frac{1}{\rho \left(q,q_{obs}\right)}-\frac{1}{{\rho }_0}\right)}^2}{2}}\hfill & \rho \left(q,q_{obs}\right)\le {\rho }_0\hfill \\ 0\hfill & \rho \left(q,q_{obs}\right)>{\rho }_0\hfill \end{array}\right.$$

(2)

where $\rho \left(q,q_{goal}\right)$ is the Euclidean distance from the current point to the target point. $\rho \left(q,q_{obs}\right)$ is the Euclidean distance from the current point to the obstacle. ${\rho }_0$ is the repulsive influence range. katt is the gravitational coefficient, and krep is the repulsive coefficient.

$F_{att}$ and $F_{rep}$ can be derived by taking the negative gradients of $U_{att}$ and $U_{rep}$. The calculation formulas are shown as Equations (3) and (4) [21]:

$$F_{att}(q)=-\nabla U_{att}(q)=-k_{att}\rho \left(q,q_{goal}\right)$$

(3)

$$F_{rep} (q)=-\nabla U_{rep} (q)=\left\{\begin{array}{l}{k}_{rep}\left({\displaystyle \frac{1}{\rho \left(q,q_{obs}\right)}}-{\displaystyle \frac{1}{{\rho }_0}}\right){\displaystyle \frac{1}{\rho {\left(q,q_{obs}\right)}^2}}\hfill \\ 0\hfill \end{array}\right.$$

(4)

2.2. Dijkstra’s Algorithm Optimization and Map Preprocessing

In this paper, the APF algorithm is applied to raster maps, but the traditional raster maps and Dijkstra’s algorithm have the following defects: firstly, although the child node generated by the algorithm is not in the range of the obstacle, the connecting line of the child node and the parent node is easy to be close to the obstacle, and secondly, in the actual path planning, there are some concave obstacles as well as some extremely narrow roads, which cause the algorithm to waste unnecessary resource area to explore these unnecessary grids, as shown in Figure 3.

From Figure 3, we can see that in the raster map without preprocessing, due to the characteristics of Dijkstra’s algorithm, the global path will be close to or even diagonally through the obstacles when planning. This is because the single optimization of the cost function only takes the “shortest distance” as the optimization goal; even if there is no collision, the path will be too close to the obstacle and ignore the safety of the path. Moreover, due to the limitation of the search method, each node in the path must be adjacent to the previous node and is not allowed to enter the obstacle grid. As a result, the path can only be selected from the passable grids around the obstacle, resulting in the effect of “near but not through.” In this paper, the following two methods are used to improve this, and the improved effect is shown in Figure 4.

(1)

Change the searching method of Dijkstra’s algorithm by removing the nodes in the search direction to the left and right of the neighborhood if there are obstacles near the searching neighborhood.

(2)

Directly expand the obstacles to increase their effective range and avoid the global path being too close to the obstacles.

As can be seen in Figure 4, when the pre-processed obstacles appear near the search neighborhood, the optimized Dijkstra’s algorithm filters out the 1 and 7 search neighborhoods to prevent the phenomenon of diagonal penetration of obstacles and selects Neighborhood 2 for planning. At the same time, preprocessing the obstacles can ensure the safety of the path.

In this paper, the obstacle grid is inflated with a width of 1 m. This inflation threshold is for the safety margin provided by the global planning algorithm (Dijkstra). At the same time, if a larger distance expansion is chosen, it will result in a situation where the passable area is too small, which is additionally burdensome. In practice, the expansion threshold is not necessarily fixed and can be adjusted according to the situation.

In this paper, the proposed two measures are simulated and verified. Among them, the green grid, red grid, and black grid represent the starting point, arrival point, and obstacles, respectively; the gray grid is the extended grid; and the blue line is the path planned after the Dijkstra algorithm optimizes the search direction, as shown in Figure 5.

From Figure 5, it can be observed that the optimized raster map and the distance between the path planned by Dijkstra’s algorithm and the obstacles are enlarged by at least one raster width, which improves the quality of the path and provides a good basis for the subsequent APF algorithm.

2.3. Resolving Localized Oscillations Caused by U-Shaped Obstacles

Map preprocessing can only address known obstacles, but when the algorithm is run, the method is not applicable to obstacles that appear suddenly, because map preprocessing requires real-time modeling of the environment, which can lead to inefficiency in path planning if it is used during the planning process, and the appearance of these obstacles is unpredictable and their morphology may change. When a U-shaped obstacle suddenly appears on the trajectory of the APF, a phenomenon of local oscillation occurs, and this phenomenon with its repulsive potential field energy is shown in Figure 6.

From the figure, it can be seen that the traditional APF, when faced with the sudden appearance of a U-shaped obstacle, will experience the phenomenon of localized oscillation, i.e., the current point will stall at a certain position and reach a force equilibrium point near the U-shaped obstacle, thus being unable to exit the area. In real vehicle navigation, the vehicle may be trapped by the repulsive force of the obstacle and fall into a localized region of minimum potential field, which makes it difficult to continue driving towards the target, while the U-shaped obstacle causes the computation of the potential field to become more complicated and the computational efficiency of the vehicle controller to increase linearly.

Therefore, this paper solves this problem by integrating global and local path planning strategies. The Dijkstra algorithm is used to generate the approximate target path for the APF algorithm, and then the APF algorithm is used to adjust the robot’s traveling route in real time to avoid the local optimal trap. However, the global path planned by the optimized Dijkstra’s algorithm can guarantee the distance to the obstacles, but there are more inflection points in the path, which will lead to the APF algorithm showing an increase in the computational complexity as well as a decrease in the algorithm’s real-time performance.

This paper optimizes the global path, removes redundant path points, and ensures that the distance from the remaining path points to the center point of the obstacle is not less than the distance to the grid, as shown in Equation (5).

$$t={\displaystyle \frac{(p3-p1)\cdot (p_{obs}-p1)}{{(p3-p1)}^2}}$$

(5)

where t is the projection scale. p1 is the first point of the selected path. p3 is the third point, and $p_{obs}$ is the center of the obstacle.

The relative position of the center point in this line segment is represented by the projection scale t while restricting it to the range (0,1), and then the distance dist is calculated by Equation (6).

$$dist=|p_{obs}-[p1+t\cdot (p3-p1)]|$$

(6)

The steps to implement the method are shown below:

(1)

The start point of the path is taken as the first point of the optimization step, called p1, and the subsequent two points, p2 and p3, of this point are selected to connect the path point p1 and the path point p3 and to check whether the connecting line intersects with the obstacle.

(2)

If they intersect, the intermediate point p2 is retained, and points like p2 are called necessary points; then the next test starts directly from p2, and the three points are selected in turn for the cycle.

(3)

If they do not intersect, then calculate the shortest distance dist between the center point of the obstacle and p1p3. If the distance of dist is less than the distance of a grid, then p2 is retained for the cycle of step 2, and vice versa, p2 is removed, and p4 is selected at this time as the intermediate point, and the algorithm cycles through the judgments until the last point is the target point.

The necessary points of the paths calculated by Formulas (5) and (6) are taken as the temporary target points of the APF algorithm to provide attraction. Use the final target point and the temporary target point, respectively, to exert gravitational force on the current point. Compare the two ways and calculate the combined potential energy. The comparison graph and the combined potential energy graph are shown in Figure 7.

It can be observed that the paths planned by the traditional APF algorithm show a greater variation in the resultant force and a poorer match with the global path than the APF using temporary target points. Due to the existence of obstacles near the target point, there is a large difference between the combined potential energy and the zero potential energy point. This will lead to not accurately reaching the target point, so in this paper, the potential energy function of the APF is improved. Based on Equation (2), the improved potential energy function is shown in Equation (7).

$$U_{rep}=\left\{\begin{array}{ll}{U}_{rep}={\displaystyle \frac{1}{2}}\times k_{rep}\times {\left({\displaystyle \frac{1}{\rho \left(q,q_{obs}\right)}}-{\displaystyle \frac{1}{{\rho }_0}}\right)}^2\times \left(1-\exp (-\rho {\left(q,q_{goal}\right)}^2)\right)\times \rho {\left(q,q_{goal}\right)}^2\hfill & \rho \left(q,q_{obs}\right)\le {\rho }_0\hfill \\ 0\hfill & \rho \left(q,q_{obs}\right)>{\rho }_0\hfill \end{array}\right.$$

(7)

Split the original repulsive force $F_{rep}$ into two repulsive forces, $F_{rep1}$ and $F_{rep2}$. $F_{rep1}$ is still pointing from the obstacle to the current point, and $F_{rep2}$ is pointing from the current point to the target point. Solve for the negative gradient for $\rho \left(q,q_{obs}\right)$ and $\rho \left(q,q_{goal}\right)$, respectively, as shown in Equations (8)–(10).

$$F_{rep1}=k_{rep}\times {\left({\displaystyle \frac{1}{\rho \left(q,q_{obs}\right)}}-{\displaystyle \frac{1}{{\rho }_0}}\right)}^2\times \left(1-\exp \left(-\rho {\left(q,q_{goal}\right)}^2\right)\right)\times {\displaystyle \frac{\rho {\left(q,q_{goal}\right)}^2}{\rho {\left(q,q_{obs}\right)}^2}}$$

(8)

$$F_{rep2}=k_{rep}\times {\left({\displaystyle \frac{1}{\rho \left(q,q_{obs}\right)}}-{\displaystyle \frac{1}{{\rho }_0}}\right)}^2\times \left(2\times \rho \left(q,q_{goal}\right)\times \exp \left(-\rho {\left(q,q_{goal}\right)}^2\right)\times \rho {\left(q,q_{goal}\right)}^2+N\right)$$

(9)

$$N=2\times \rho \left(q,q_{goal}\right)\times \left(1-\exp \left(-\rho {\left(q,q_{goal}\right)}^2\right)\right)$$

(10)

A distance-dependent target distance factor is introduced into the potential energy function $\rho {\left(q,q_{goal}\right)}^2$, After performing the square operation on the distance result, it is proportionally magnified to emphasize the influence of distance change on the potential energy value. At the same time in the function introduced Gaussian function, the repulsive potential energy will gradually equal the original potential energy, and the change of the total potential energy field will be smoother. If the current position is near the final arrival point, the value of the Gaussian function tends to 0, which ensures that the final point is the lowest point of the total potential energy of the entire field. The combined potential energy of the APF with the modified repulsion function is compared with that of the APF, and the comparison plot is shown in Figure 8, and the path comparison with the lowest combined potential energy is shown in Figure 9.

It can be observed that the paths planned by the APF using the improved repulsion function have less variation, fit the global path better, incorporate the distances between the target and the obstacles well, and pass the distance factor as well as the Gaussian decay term so as to realize a nonlinear and adaptive potential field, and the minimum value of the combined potential energy point is 0.0006, which is almost converging to 0, which will provide a basis for the subsequent localized paths for the APF.

The APF with the above modified repulsion function is fused with the optimized Dijkstra’s algorithm, and the simulation is shown in Figure 10 when facing a U-shaped obstacle.

As can be seen from the figure, when facing the appearing U-shaped obstacles, the fusion algorithm of APF can provide the path inflection point as a temporary target point with the help of Dijkstra’s algorithm, which avoids the local oscillation phenomenon caused by the force balance due to the U-shaped obstacles.

However, this type of approach is not applicable to all cases. If the U-shaped obstacle suddenly appears after the global algorithm has finished planning, and it happens to appear in the path of Dijkstra’s planning, it will still cause the phenomenon of local oscillation. Therefore, the algorithm can not always rely on Dijkstra’s path inflection points but directly in the vicinity of the U-shaped obstacles directly form a gravitational point to guide the APF algorithm to “escape.” Based on the above situation, this paper continues to improve.

Since U-shaped obstacles usually have complex shapes and may lead to unstable potential fields, in this paper, U-shaped obstacles are ellipticized because ellipses have better mathematical descriptions for obstacles with different sizes and shapes (e.g., U- or L-shaped obstacles), which can be adapted to different obstacles by ellipticization and can be more easily dealt with when dealing with the ensemble potential field by constructing the ellipticization formula shown in Equation (11).

$${\displaystyle \frac{{[x-\frac{(u_{{\max }_x}+u_{{\min }_x})}{2}]}^2}{{\left\{\frac{(u_{{\max }_x}+u_{{\min }_x})}{2}+[(u_{{\max }_x}-u_{{\min }_x})+\frac{width}{2}]\cos \theta \right\}}^2}}+{\displaystyle \frac{{[y-\frac{(u_{{\max }_y}+u_{{\min }_y})}{2}]}^2}{{\left\{\frac{(u_{{\max }_y}+u_{{\min }_y})}{2}+[(u_{{\max }_y}-u_{{\min }_y})+\frac{3width}{2}]\cos \theta \right\}}^2}}=1$$

(11)

where $u_{{\max }_x}$, $u_{{\min }_x}$, $u_{{\max }_y}$, and $u_{{\min }_y}$ are the U-shaped obstacle boundary coordinates. $\theta$ is the angle in polar coordinates, and $width$ is the vehicle width, respectively.

The approach begins by determining the center of the U-shaped obstacle, $({\displaystyle \frac{(u_{{\max }_x}+u_{{\min }_x})}{2}},{\displaystyle \frac{(u_{{\max }_y}+u_{{\min }_y})}{2}})$, as the center of the ellipse and then determines the long and short axes of the ellipse based on the length of the boundary of the U-shaped obstacle plus the width of the vehicle. The long and short axes of the ellipse are determined by adding the width of the vehicle to the length of the boundary of the U-shaped obstacle, which is the “dilated” half-axis of the x- and y-directions, respectively, and taking the width of the vehicle (${\displaystyle \frac{width}{2}}$) into account. The $\cos \theta$ is used for reorientation when the obstacle is not directly in the Cartesian coordinate system. Secondly, the two endpoints of the long axis are selected as the temporary virtual target points. At this time, the size of the combined potential energy of the two virtual target points is calculated, and the temporary virtual target point with the smallest combined potential energy is selected as the final virtual target point. According to Equation (3), the virtual target point $q_{\mathrm{goal}}$ is used as the attraction for the APF algorithm to provide the improvement of the simulation, as shown in Figure 11.

As can be seen from the figure, the U-shaped obstacle is ellipticized, and two temporary virtual target points are found through the elliptic function. Finally, the green point with the lowest potential energy is selected as the final virtual target point to provide attraction. When facing U-shaped obstacles, it can be directly bypassed, avoiding the occurrence of local oscillation phenomena. And based on the improved repulsive force function, the local path can basically fit the path planned by the Dijkstra algorithm. The improvements in Section 2.3 are shown in Figure 12.

2.4. Path Optimization Based on Steering Model

Since the APF algorithm essentially calculates the location of the next path point by using the combined force, if the parameters of the force field are not set appropriately, it may lead to discontinuous changes in the path. Meanwhile, if there are multiple obstacles in the environment, cusps or sharp turns in the path can be formed since the formulas for the calculation of the attraction and repulsion forces usually use forms such as the inverse square or the exponential function. Paths are too tortuous, and path cusps occur. It can lead to frequent steering in real-world situations, as well as not meeting the mechanical constraints of the vehicle and leading to very poor comfort. Moreover, the overly zigzag path cannot be used as a reference path for subsequent tracking control studies.

Therefore, according to the above problems, this paper adopts the three-point arc method based on the steering model to make the APF algorithm able to make the path more continuous and smooth when performing local obstacle avoidance and path planning. Since the application carrier of the algorithm is a vehicle model, and the vehicle model has the characteristic of a minimum turning radius, in order to comply with the vehicle kinematics constraints, not only are curvature limitations needed on the planned paths, but also the planned paths cannot be less than the minimum turning radius of the vehicle radius, so it is necessary to add the corner constraints in the algorithm.

Steering sensitivity is an important index for evaluating the steady-state effect of the vehicle [22], and its formula is shown in Equation (12).

$${\left.{\displaystyle \frac{\omega }{{\delta }_f}}\right)}_s={\displaystyle \frac{\frac{u}{L}}{1+\frac{m}{L^2}\times \left(\frac{a}{k_2}-\frac{b}{k_1}\right)\times u^2}}$$

(12)

where $\omega$ is the angular velocity of the pendulum. ${\delta }_f$ is the angular velocity of the pendulum. s represents the current state. $u$ is the component of velocity in the x-axis. L is the vehicle wheelbase. m is the mass. a is the front wheelbase, and b is the rear wheelbase.

In the case of not considering the tire side deflection, and at the same time the speed state in the obstacle avoidance process is low speed, the vehicle is considered to be in the neutral steering state, so the maximum front wheel angle and the minimum turning radius relationship formula is shown in Equation (13), and the path curvature is shown in Equation (14).

$$R_{\min }={\displaystyle \frac{L}{{\delta }_{f\max }}}$$

(13)

$$\rho ={\displaystyle \frac{1}{R_{\min }}}={\displaystyle \frac{{\delta }_{f\max }}{L}}$$

(14)

The three-point arc method used in this paper will first determine whether the turning radius meets the requirements. According to the coordinates of the previous point $\left(x_0,y_0\right)$, the current point $\left(x_1,y_1\right)$ and the coordinates of the predicted point $\left(x_2,y_2\right)$ planned by the APF algorithm to calculate the distance between the three points, the calculation formula is shown in Equation (15).

$$\left\{\begin{array}{l}s01=\sqrt{{(x_1-x_0)}^2+{(y_1-y_0)}^2}\\ s02=\sqrt{{(x_2-x_1)}^2+{(y_2-y_1)}^2}\\ s03=\sqrt{{(x_2-x_0)}^2+{(y_2-y_0)}^2}\end{array}\right.$$

(15)

where $s01$, $s02$, $s03$ are the Euclidean distance between the current point and the previous point, the Euclidean distance between the predicted point and the current point and the Euclidean distance between the predicted point and the previous point, respectively.

After obtaining the Euclidean distances of the three points, the angle formed by the three points is calculated using the cosine theorem, and the angle is restricted to [−1,1] as shown in Equation (16).

$$-1\le \cos (\theta )={\displaystyle \frac{s{01}^2+s{12}^2-s{02}^2}{2\times s01\times s12}}\le 1$$

(16)

Based on the angle formed by the three points and the distance between the three points, the radius R of the three-point arc can be found, and the formula is shown in Equation (17).

$$R={\displaystyle \frac{s02}{2\times \sqrt{1-{\left(\cos \theta \right)}^2}}}$$

(17)

The path planned by the APF algorithm using the theory of the three-point arc method is shown in Figure 13, and the curvature of this smooth path segment is derived and represented in Figure 14.

As can be seen from Figure 11 and Figure 12, the use of the three-point arc method reduces the path cusp situation while making the path very smooth. However, the path curvature varies greatly, and there are cases of abrupt changes in curvature over a wide range. The curvature reflects the degree of curvature of the path, and the greater the curvature, the sharper the turn of the path. If the curvature of the path is too large, it means that the vehicle needs a smaller turning radius, which is not possible to drive on this path because the vehicle model has the limitations of steering angle and minimum turning radius; therefore, this paper proposes a three-point arc method based on the steering model for path optimization.

Firstly, according to the vehicle’s three-point arc radius R and vehicle wheelbase L, and ignoring the steering difference between the left and right wheels and mechanical factors. Based on the simplified steering model, to find out the steering angle, the formula is shown in Equation (18).

$$direction=\arctan \left({\displaystyle \frac{L}{R}}\right)\times {\displaystyle \frac{180}{\pi }}$$

(18)

where L is the wheelbase of the vehicle, and R is the turning radius found by the three-point arc method.

When the APF algorithm plans the location of the predicted points, the coordinates of the three points are used with other parameters of the vehicle model to calculate the arc radius R. If $R\ge R_{\min }$ also calculates the steering angle based on the steering model, then it means that the planned path angle $direction\le direction0$, meets the front wheel angle constraints, and the position of the next point can be planned in accordance with the ensemble of the APF algorithm; If $R<R_{\min }$ also calculates the steering angle $direction>direction0$, based on the steering model, that is, if the corner change caused by the resultant force direction is not within the angle constraint range, then the driving direction is re-determined according to Equation (19).

$$angle=\pi -2\times \arccos \left({\displaystyle \frac{s01}{2\times R_{\min }}}\right)$$

(19)

Equation (18) is based on the geometric characteristics of the vehicle and is used to calculate the front wheel angle of the vehicle for a given turning radius R. The front wheel angle is calculated based on the geometric characteristics of the vehicle. If the calculated front wheel angle does not meet the actual constraints of the vehicle, the limit angle of the vehicle is calculated based on Equation (19), the minimum turning radius of the vehicle, R_min, and the Euclidean distance between the first two points. The direction of travel at the limit is shown in Figure 15.

The three-point arc method using the steering model-based method will be compared with the APF algorithm, and the comparison plot is shown in Figure 16, while the curvature and steering angle of the path will be calculated and plotted as shown in Figure 17 and Figure 18.

From the figure, we can see that the three-point arc method based on the steering model can well smooth the path planned by APF, and the maximum value of the curvature of the smoothed path is not more than 0.4, and there is no curvature mutation phenomenon, which indicates that the path curvature is not particularly drastic at any point, and it is relatively smooth, and at the same time, the maximum value of the steering angle is 37.7°, and the average value is 7.5°, which is all in the range of the steering angle limitation.

Therefore, the three-point arc method based on the steering model proposed in this paper can largely improve the phenomenon of path cusps produced by APF due to the uneven ensemble force and, at the same time, limit the steering angles to the steering constraints of the vehicle.

Since the traditional step size is too small, the step size is optimized according to the density of obstacles in order to improve the efficiency and accuracy of path planning, as well as to ensure smooth paths and avoid large turns. The optimization used in this paper is shown below:

(1)

The base step length is set, and a threshold for the number of obstacles is set. When the number of obstacles in a certain range is less than the current threshold, the base step is enlarged, and in this paper, we choose to directly enlarge it 4 times. When the number of obstacles is greater than the threshold, directly shrink 0.8 times.

(2)

When the number of obstacles is between the maximum and minimum number, a linear difference is calculated for the step size so that the step size can be dynamically adjusted.

In this paper, the maximum number of obstacles is set to 6, the minimum number of obstacles is set to 1, and the basic step size in this paper is set to 1. A dynamic step size is used to balance speed and obstacle avoidance. These values are based on many experiments. If the step size is fixed and too large, the algorithm may skip local details, resulting in an unsmooth path, especially in complex environments with many obstacles.

Through the adaptive step size, the algorithm can dynamically adjust the step size according to the actual situation to ensure a smoother path and avoid generating large turns or unnatural paths; meanwhile, in the more open area, the gradient change of the potential field is small, and the step size can be increased appropriately to accelerate the search speed, while in the complex environment, the gradient change of the potential field is large, and the step size is reduced to precisely adjust the position to avoid collision or path deviation from the target.

Simulation is carried out using MATLAB to add randomly appearing static U-shaped obstacles on the path and two reciprocating dynamic circular obstacles, and the improved algorithm in this paper is compared with the fusion algorithms of A* and DWA, and the comparison graph is shown in Figure 19, and the steering angle is shown in Figure 20.

In the figure, the gray grid is the grid after the expansion process. The purpose is to expand the range of obstacles to provide a safe distance for the global path and does not provide repulsive force. The green curve is the path after the APF planning, and the blue curve is the path after the planning of A* and DWA algorithms. When static U-shaped obstacles and dynamic obstacles suddenly appear on the map, the improved APF algorithm can avoid the local oscillation phenomenon by virtue of the virtual target point to bypass the U-shaped obstacles, and at the same time, it can smoothly avoid dynamic obstacles, and the curvature of the planned path meets the requirements, and the steering angle is within the limitation; while the combination of A* and DWA algorithms is directly trapped in the local optimal phenomenon when facing the U-shaped obstacles.

The three-point arc method based on the steering model used in this paper can realize the smoothing of the generated paths in real time during the path planning process, changing the predicted point positions according to random and dynamic obstacles and adjusting the direction angle based on the vehicle model parameters. The real-time smoothed path is compared with the three-times B-spline smoothing, and the comparison graph is shown in Figure 21. The rate of change of curvature of the two will also be compared, as shown in Figure 22.

As can be seen from the figure, the path planned by APF is smoothed by using three times B-spline in path post-processing, and its effect is not good at the corners of the path and in avoiding dynamic obstacles, and there are sharp points in the path and the corners are not silky smooth, while the three-point arc method based on the steering model adopted in this paper still has a very good smoothing effect at the corners, and the angle meets the requirements.

Meanwhile, the curvature change of the smoothing method used in this paper is better than the three-times B-spline smoothing. In the use of B-spline for smoothing, if the number of path points is too many, it will lead to unstable curvature calculation, very sensitive to small perturbations, while the smoothing method used in this paper is essentially based on the steering model, which can ensure that the vehicle will not have a drastic steering change, and the front wheel steering angle is continuous, which ensures that the curvature change on the path is smooth.

Several papers consider the carrier of the algorithm as a mass, which is unrealistic, because the vehicle carrier usually exists in volume when avoiding obstacles. Although the mass was successfully avoided, if the volume exists, it will also collide, so this paper continues to optimize the algorithm to incorporate the vehicle volume into the algorithm and take into account the volume constraints of the vehicle in performing the obstacle avoidance process. The flowchart of the algorithm in Section 2.4 is shown in Figure 23.

2.5. Vehicle Collision Detection Model

When planning the APF algorithm, the center point of the rear axle of the vehicle, i.e., the prime point $O_{\mathrm{r}}=\left(x_r,y_r,\theta \right)$, is usually chosen to represent the current position of the vehicle. Therefore, the planned paths are paths consisting of a series of rear axle centers, and it is not possible to determine whether a vehicle is in a collision only at the rear axle center point, because vehicles have volume and width, and collision detection with obstacles usually uses the outer contour of the vehicle as a reference. Since the surface of the vehicle has bumps and also the vehicle itself is irregularly shaped, the outer contour of the vehicle is simplified to a rectangle, and a plurality of envelope circles are used to cover the surface of the rectangle in response to the presence of bumps.

Given that the APF algorithm is more sensitive to the combined force, in order to improve the safety of the planned path, and at the same time, in order to approximate the expansion of each contour of the vehicle body, to ensure that the vehicle still has enough expansion circle area when avoiding obstacles. If fewer expansion circles are used, it will lead to the model area not being expanded. At the same time, if the number of expansion circles is too high, it will lead to an increase in the computational complexity, so this paper selects three rows of a total of 14 envelope circles to cover the body. In order to improve the accuracy of the vehicle collision detection, the vehicle collision detection model is shown in Figure 24.

From Figure 17, Or is the center of the rear axle, L1, the rear axle, L2, the front axle, d1, the distance from the rear axle to the edge of the vehicle, d2, the distance from the front axle to the edge of the vehicle, W, the width of the vehicle, $R_{\mathrm{c}}$ the radius of the collision detection, and the radius of each envelope the same as that of the center of the two envelope circles. By using the envelope circle, the vehicle collision problem can be converted into a problem of the distance between the 14 envelope circles and the surrounding obstacles, so it is only necessary to calculate the distance between the center of each envelope circle and the obstacles and compare it with the collision detection radius $R_{\mathrm{c}}$ to determine whether a collision occurs or not. The formula for calculating the collision detection radius $R_{\mathrm{c}}$ and the distance between the center of the envelope circle is shown in Equations (20) and (21).

$$R_{\mathrm{c}}=k\times \sqrt{{\displaystyle \frac{L^2}{{(2\times n)}^2}}+{\displaystyle \frac{W^2}{36}}}$$

(20)

$$d=2\times \sqrt{R_{\mathrm{c}}^2-{\displaystyle \frac{W^2}{36}}}$$

(21)

where n is the maximum number of envelope circles in a row, which is 6 in this paper. L is the axis distance, and k is the envelope circle radius expansion factor.

According to the coordinates of the center point of the rear axis $O_{\mathrm{r}}=\left(x_r,y_r,\theta \right)$, to calculate the center coordinates of the 14 envelope circles, collision detection can be carried out. In this paper, we give the eight center coordinates of O1, O2, O3, O4, O10, O12, O13, and O14, and the calculation formula of the other center coordinates is similar. The calculation formula is shown in Equations (22)–(29).

$$\left\{\begin{array}{l}O{2}_{\mathrm{x}}=x_r-L1\times \cos \theta -d1\times \cos \theta \hfill \\ O{2}_{\mathrm{y}}=y_r-L1\times \sin \theta -d1\times \cos \theta \hfill \end{array}\right.$$

(22)

$$\left\{\begin{array}{l}O{1}_{\mathrm{x}}=O{2}_{\mathrm{x}}-d1\times \sin \theta \hfill \\ O{1}_{\mathrm{y}}=O{2}_{\mathrm{y}}+d1\times \cos \theta \hfill \end{array}\right.$$

(23)

$$\left\{\begin{array}{l}O{3}_{\mathrm{x}}=O{2}_{\mathrm{x}}+d1\times \sin \theta \hfill \\ O{3}_{\mathrm{y}}=O{2}_{\mathrm{y}}-d1\times \cos \theta \hfill \end{array}\right.$$

(24)

$$\left\{\begin{array}{l}O{4}_{\mathrm{x}}=O{1}_{\mathrm{x}}+d1\cos \theta \hfill \\ O{4}_{\mathrm{y}}=O{1}_{\mathrm{y}}+d1\sin \theta \hfill \end{array}\right.$$

(25)

$$\left\{\begin{array}{l}O{13}_{\mathrm{x}}=x_r+L2\cos \theta +d2\cos \theta \hfill \\ O{13}_{\mathrm{y}}={\mathrm{y}}_r+L2\sin \theta +d2\sin \theta \hfill \end{array}\right.$$

(26)

$$\left\{\begin{array}{l}O{12}_{\mathrm{x}}=O{13}_{\mathrm{x}}-d2\sin \theta \hfill \\ O{12}_{\mathrm{y}}=O{13}_{\mathrm{y}}+d2\cos \theta \hfill \end{array}\right.$$

(27)

$$\left\{\begin{array}{l}O{14}_{\mathrm{x}}=O{13}_{\mathrm{x}}+d2\sin \theta \hfill \\ O{14}_{\mathrm{y}}=O{13}_{\mathrm{y}}-d2\cos \theta \hfill \end{array}\right.$$

(28)

$$\left\{\begin{array}{l}O{10}_{\mathrm{x}}=O{12}_{\mathrm{x}}-d2\cos \theta \hfill \\ O{10}_{\mathrm{y}}=O{12}_{\mathrm{y}}-d2\sin \theta \hfill \end{array}\right.$$

(29)

According to the above formula to establish a good vehicle collision detection model, in the collision detection, you only need to judge the center of each envelope circle, and the distance between the obstacle and the obstacle is greater than that is to say, when the obstacle in the environment is denser, the passable area is narrower, and if there is also a random emergence of dynamic obstacles near the passable area, it will lead to the distance between the vehicle and obstacle being smaller, which is not conducive to the vehicle’s passage. Therefore, this paper adopts the temporary target point bias strategy, which combines the temporary target point provided by the Dijkstra algorithm with the combined potential energy in the APF algorithm and selects a safer temporary target point to attract vehicles to move. The temporary target point bias strategy is shown in Figure 25.

The red dashed line in the figure is the global path planned by Dijkstra’s algorithm, the blue circle is the temporary target point, and its distance from the gray grid is dist = 1. The calculation formula is shown in Equation (6). The red dashed circle is the target point bias circle used to test the distance between the obstacles to ensure that there is enough safe distance. R is the test radius of the bias circle; its size is equal to the width of a grid. $\mathrm{M}{\mathrm{U}}_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}$ is the bias circle within the range of the smallest point of the combined potential energy. The bias strategy process is shown below:

(1)

Iterate over all temporary target points, calculate the distance dist between them and the gray grid, and generate a target point bias circle with the temporary target point as the center and $\sqrt{2}\times dist$ as the radius.

(2)

If $dist\le \sqrt{2}$, the point is moved in all four directions, up, down, left, and right.

(3)

The final direction of movement is selected based on the combined potential energy values of the four directions, and the direction with the lowest combined potential energy is chosen as the target bias point.

(4)

The move distance is d. If after the move it is found that its bias circle still intersects with the gray grid’s, recalculate the combined potential energy and continue to choose the optimal direction for the move until to satisfy $dist>\sqrt{2}$.

The above improvements are simulated, and the simulation is shown in Figure 26, and the steering angle plots and smoothing comparison plots are shown in Figure 27 and Figure 28.

As can be seen from the figure, the vehicle collision detection model ensures the distance between the vehicle model and the dynamic and static obstacles and has good results in avoiding the dynamic obstacles.

The gray grid in the figure is the expanded obstacle, but there is a phenomenon that the vehicle carrier covers the gray grid because this type of obstacle does not provide repulsive force, and if the same size of repulsive force is still applied to the carrier, it will cause the vehicle carrier to be overstressed and the steering effect will not be obvious, so the grid is only for expanding the distance with the original obstacle to provide a certain safe distance base for local path planning; at the same time, the target point bias strategy further expands the distance between the temporary target point and the obstacles to prevent the vehicle from colliding with the obstacles when steering; the steering angles of the vehicles are all limited to ±40°, the maximum angle of the left turn is 28.35°, the maximum angle of the right turn is 36.98°, and the average steering angle is 8.38°; comparing the planned paths with the three-times B-spline, the three-times B-spline curves cannot deal well with the phenomenon of sudden corner change caused by vehicle turning, while the three-point arc method based on the steering model used in this paper is still smooth in the turning stage.

The APF algorithm with the added collision detection model is compared with the unadded collision detection model, and the comparison results are shown in

Table 1. Comparison of time cost after adding a collision detection model.

Search Method	Algorithm Time/s	Minimum Distance from Obstacles/m	Time Variation/%	Distance Variation/%
No collision detection model added	20.5400	0.9	--------	-----------
Add collision detection model	23.0526	1.5	24.33	66.67

.

As can be seen from Table 1, the APF algorithm with the addition of three rows of 14 envelope circles improves the minimum distance between the planned path and the obstacle by 66.67%, although the algorithm running time increases slightly (by 12.23%). This is because too close a distance triggers the safety mechanism of the expansion detection model, and the obstacle provides repulsive force in advance to ensure that it will not touch the obstacle, so the relative increase in time to improve the safety of the path is tolerable.

Although the comparison of the algorithm running time was conducted, the improved method still needs to be analyzed for theoretical cost complexity, including time complexity and space complexity.

After theoretical analysis, the time complexity of the method is O(N), where N is the number of obstacles in the environment. Although the introduction of the multi-envelope circle model increases the computation of the constant factor, the method improves the accuracy of vehicle collision detection and demonstrates a good balance of performance and accuracy. In each iteration, we compute 14 fixed-position envelope circle centers. These circle centers are distributed in three alignments to form a comprehensive coverage of the vehicle body. The computation is independent of the number of obstacles and has a time complexity of O(1).

For each envelope circle, we need to calculate its distance to each obstacle in the environment, and a total of 14 × N distance calculations are performed. Although the computation is 14 times more than the single-point model, the constant coefficient can be ignored from the asymptotic analysis point of view, and the complexity is still O(N). When an obstacle enters the detection range, we compute the distance-based repulsive potential field. This process has O(N) complexity for the worst case. The space complexity, also O(N), is mainly used to store the distance and force value information associated with the N obstacles. 14 The storage requirement for the circle center coordinates is a fixed value and has a negligible effect on the space complexity.

The envelope circle model does not change the asymptotic complexity O(N) of the algorithm, although it increases the computational effort by a constant factor. This increase in computational cost is fully justified considering that the method significantly improves the accuracy of collision detection by meticulously modeling the vehicle shape, especially in complex environments and narrow passages.

In summary, the improvement of this paper for the APF algorithm is shown below:

(1)

Firstly, in order to circumvent the planned trajectory from intersecting with any obstacles and thereby mitigate the risk of a collision, the obstacles are augmented by one raster width for the purpose of collision detection.

(2)

Secondly, the optimal path planned by Dijkstra’s algorithm is used as the global path of the APF algorithm, and the search direction of Dijkstra’s algorithm is improved due to the fact that there will be a situation where the distance to the obstacle is too close, and the path is optimized for the more inflection points of the global path in order to reduce the computational complexity and ensure that the distance between the path and the obstacle is not less than dist.

(3)

Again, in order to ensure that the combined potential energy is not equal to zero when the final point is reached, the APF is augmented by adding a Gaussian function and a distance factor, and it can be ascertained that the final value of the combined potential energy tends to 0. In addition, for the localized oscillation phenomenon induced by U-shaped obstacles, these are configured as ellipticals, and coupled with the gravitational force provided by the virtual target point, it is possible for the vehicle to bypass the U-shaped obstacles voluntarily.

(4)

In order to conform the local paths to the kinematic constraints of the vehicle, we use a three-point arc method based on the steering model. This method smoothes the path in real time, and the steering angles are all in compliance with the requirements. In addition, the planning step size is optimized based on the number of obstacles in order to improve efficiency.

(5)

Finally, the prime is instantiated as a vehicle model, and the vehicle collision detection model is used to wrap the vehicle with an envelope circle, which can be used to further improve the path safety by considering the vehicle volume constraints when performing collision detection and by using the target point bias strategy.

3. Simulation Verification

In this paper, different raster maps are constructed, and random static U-shaped obstacles and dynamic obstacles are added as a way to validate the effectiveness of the Dijkstra + APF fusion algorithm in this paper. The running environment of the simulation experiment is MATLAB R2022b, the graphics card is GTX 1650, the CPU is i5-9300HF. The GTX 1650 graphics card is manufactured by NVIDIA Corporation, Santa Clara, CA, USA, and the i5-9300HF processor is manufactured by Intel Corporation, headquartered in Santa Clara, California, USA. And the parameters of the vehicle model used in this paper are shown in

Table 2. Vehicle model parameters.

Length of the Vehicle Body L/mm	Width of the Vehicle Body W/mm	Wheelbase L/mm	Maximum Rotation Angle δ/°	Envelope Circle Expansion Coefficient k
4500	2500	2600	40	15

.

3.1. Working Condition 1

The simulation environment uses a 60 × 60 raster map with obstacle coverage (including inflated obstacles) $p=31.1\%$. Two static U-shaped obstacles are randomly generated in the vicinity of the global path, as well as two dynamic obstacles (reciprocating motion), and the A* + DWA algorithm, the traditional APF algorithm, and the improved fusion algorithm in this paper are selected for comparison, and the path comparison diagram is shown in Figure 29, and the geometric features of path diagrams are in Figure 30, and the minimum distance graph between path points and obstacles is shown in Figure 31.

3.2. Working Condition 2

The simulation environment uses a 50 × 50 raster map with obstacle coverage (including inflated obstacles) $p=29.5\%$. Three static U-shaped obstacles are randomly generated in the vicinity of the global path, as well as two dynamic obstacles (reciprocating motion), and the A* + DWA algorithm, the traditional APF algorithm, and the improved fusion algorithm in this paper are selected for comparison, and the path comparison diagram is shown in Figure 32, and the geometric features of path diagrams are in Figure 33, and the minimum distance graph between path points and obstacles is shown in Figure 34.

3.3. Working Condition 3

The simulation environment uses a 70 × 70 raster map with obstacle coverage (including inflated obstacles) $p=30.8\%$. Three static U-shaped obstacles are randomly generated in the vicinity of the global path, as well as two dynamic obstacles (reciprocating motion), and the A* + DWA algorithm, the traditional APF algorithm, and the improved fusion algorithm in this paper are selected for comparison, and the path comparison diagram is shown in Figure 35, and the geometric features of path diagrams are in Figure 36, and the minimum distance graph between path points and obstacles is shown in Figure 37.

3.4. Analysis of Results

As can be seen from the simulation diagram, both APF algorithm and A* + DWA algorithm do not complete the path planning from the starting point to the target, so only the improved fusion algorithm of this paper is analyzed for its results, and the results are shown in

Table 3. Simulation data results of the fusion algorithm in this paper.

Obstacle Rate P/%	Map Size	Path Length/m	Average Steering Angle/°	Rate of Change of Average Curvature	Standard Deviation of Curvature	Minimum Distance from Obstacles/m
29.50	50 × 50	67.21	12.57	0.248	0.159	1.79
31.10	60 × 60	88.13	7.98	0.162	0.158	1.82
30.78	70 × 70	99.74	8.28	0.169	0.158	1.67

.

Based on the above two sets of working condition experiments, it can be seen that when random U-shaped obstacles appear on the global path, the fusion algorithm of A* + DWA as well as the traditional APF algorithm are unable to perform avoidance and are caught in the localized oscillations of the U-shaped obstacles.

In this paper, the fusion algorithm inflates the U-shaped obstacle ellipsoidization and relies on the virtual target point provided by the ellipse to attract the vehicle for avoidance; at the same time, when dynamic obstacles appear around and the vehicle model passes through the narrow channel, the fusion algorithm establishes an envelope circle model of the vehicle, which is regarded as a whole and can prevent the outer profile from touching the obstacle, compared with A* + DWA’s fusion algorithm in Case 2. In contrast, the A* + DWA fusion algorithm in Case 2 bypasses the first U-shaped obstacle, but its planned path touches the boundary of the obstacle. As can be seen from Table 3, the minimum distance between the obstacle and the center of the rear axle of the vehicle is 1.79 m, 1.82 m, and 1.66 m, respectively, which are all higher than the minimum distance of 1.60 m set in this paper, which in turn proves that the method can improve the safety of the path.

And the average steering angles under the three sets of working conditions are 12.57°, 7.98°, and 8.28°, respectively, and the maximum steering angle does not exceed the set value of 40°. For the problem that the algorithm is prone to sharp corners of the path due to uneven joint forces while avoiding obstacles, the fusion algorithm in this paper can well smooth the path in real time. As can be seen from Table 3, the average curvature change rates under the two conditions are 0.248, 0.162, and 0.169, respectively, and the standard deviation of curvature is about 0.16, which indicates that the overall path curvature change is relatively smooth, and the path smoothing effect is good.

4. Discussion

This paper presents a study of the APF-Dijkstra fusion algorithm based on volume and steering model constraints. Relying on the optimality of the global path Dijkstra algorithm, whose gravity comes from the path points of the optimized global path, the main improvements of the fusion algorithm are as follows:

(1)

The map preprocessing of the raster map of the algorithm’s operating environment was carried out, known obstacles were inflated with an expansion width of 1 raster distance, and the search direction of the global path Dijkstra algorithm was optimized by removing 45° of the search direction when obstacles appeared near the search neighborhood. Simulation experiments show that the distance between the path that can be planned and the obstacle is increased by at least 1 m.

(2)

Aiming at the problem that the path planned by the traditional APF algorithm cannot fit the global path well and the combined potential energy cannot converge to 0 when reaching the target point, the path inflection point in the optimized global path is used as the temporary target point of the APF algorithm, and the potential energy function of the APF algorithm is improved by introducing a Gaussian function and distance factor. Simulation experiments show that through the distance factor and Gaussian decay term, a nonlinear and adaptive potential field is realized, and the minimum value of the combined potential energy point is 0.0006, which almost converges to 0.

(3)

Aiming at the local oscillation phenomenon of the APF algorithm caused by static U-shaped obstacles, by ellipsing the U-shaped obstacles and selecting the point with the lowest resultant potential energy as the virtual target point to provide attraction, the U-shaped obstacles can be avoided in time, and the algorithm step size is optimized based on the obstacle density.

(4)

As the traditional APF algorithm suffers from uneven combined force when avoiding obstacles, it is prone to problems such as path cusps and unrestricted steering angle. Based on the vehicle steering model, using the three-point arc method to smooth the path in real time is more reasonable than post-processing the path. Simulation experiments show that the average steering angle is 12.57°, 7.98°, and 8.28°; the maximum turning angle does not exceed the set 40°, while the average curvature rate of change is 0.248, 0.162, and 0.169, which can show that the planned path smoothness is better.

(5)

Traditional fusion algorithms usually consider the carrier as a material point; the planned path seems to avoid obstacles, but in fact, if the volume is considered, it will still collide with the obstacles. Therefore, in this paper, we instantiate the algorithm carrier as a rectangle with a length of 4.5 m and a width of 2.5 m and use three rows of 14 envelope circles to wrap the body to further expand the radius of the collision detection, which combines with the map preprocessing in (1) and adopts a temporary target point bias strategy to further expand the distance between the vehicle and the obstacles. This method is combined with the map preprocessing in (1), and a temporary target point bias strategy is used to further expand the distance between the vehicle and the obstacle. Simulation experiments show that the minimum distance between the obstacle and the center of the rear axle of the vehicle is 1.79 m, 1.82 m, and 1.66 m, respectively, which are higher than the minimum distance of 1.60 m set in this paper.

5. Conclusions

In this study, the theory of the APF-Dijkstra fusion algorithm with volume and steering model constraints is proposed to improve the algorithm in response to the local oscillation phenomenon of the APF algorithm in the face of static U-shaped obstacles, the phenomenon of sharp corners of the path when avoiding obstacles, and the lack of consideration of corner constraints, curvature limitations, and the volume of carriers in the planning of the path. This fusion algorithm theory not only avoids static and dynamic obstacles on the basis of ensuring the safety of the path but also smoothly avoids U-shaped obstacles compared to the local oscillation problem that occurs in the A* + DWA algorithm and the traditional APF algorithm. In response to most of the literature, local planning is carried out first, and then smoothing (post-processing) is performed on it. This paper adopts the three-point arc smoothing method on the basis of the steering model, and the smoothing process is carried out in real time to ensure the smoothness of the paths, and the paths planned by using this theory can be used as the reference paths for the subsequent tracking and control, but the algorithm also suffers from the following defects:

(1)

Although the steering angles of the fusion algorithms all comply with the limitations, there are cases where the turning angles are too large.

(2)

Although dynamic obstacles are considered, the speed and future position of dynamic obstacles are not taken into account.

This theory can be considered for experiments in warehouse logistics vehicles. Since there are usually many goods (static obstacles), irregular obstacles (U-shaped obstacles), narrow driving spaces, and randomly appearing people (dynamic obstacles) in warehouse depots, traditional logistics vehicles may be difficult to cope with flexibly in such environments. The improvement in this paper can ensure the safety and smoothness of the path by real-time calculation of the obstacle avoidance path and optimization of the moving trajectory so that the logistics vehicle can navigate smoothly in the complex environment. In future research, we can consider how to plan the optimal path without affecting the local obstacle avoidance on the basis of maximizing the step size, and at the same time, the speed factor needs to be taken into consideration, and we should focus on how to introduce the speed information of the obstacles and then optimize the dynamic path planning algorithm.