Development of Local Path Planning Using Selective Model Predictive Control, Potential Fields, and Particle Swarm Optimization

Abstract

This paper focuses on the real-time obstacle avoidance and safe navigation of autonomous ground vehicles (AGVs). It introduces the Selective MPC-PF-PSO algorithm, which includes model predictive control (MPC), Artificial Potential Fields (APFs), and particle swarm optimization (PSO). This approach involves defining multiple sets of coefficients for adaptability to the surrounding environment. The simulation results demonstrate that the algorithm is appropriate for generating obstacle avoidance paths. The algorithm was implemented on the ROS platform using NVIDIA’s Jetson Xavier, and driving experiments were conducted with a steer-type AGV. Through measurements of computation time and real obstacle avoidance experiments, it was shown to be practical in the real world.

1. Introduction

Autonomous driving vehicles are a subject of active research worldwide, particularly in the fields of indoor and outdoor robots, automobiles, space robot, and the military. The primary objectives of this research are efficient exploration, saving time, and safety. Various ideas are being researched and presented to achieve these goals.

Autonomous driving consists of three main components: environmental awareness, path planning, and driving control [1]. Environmental awareness involves the use of sensors, such as cameras and LiDAR, to perceive surroundings. Path planning incorporates various methodologies, including graph-search-based, sampling-based, interpolation-based, and optimization-based methods [1]. These techniques aim to formulate a safe driving path based on the information gathered from environmental awareness.

Graph-search-based methods create nodes within the domain and construct a graph [2], which includes algorithms like Dijkstra [3] for finding the shortest path and A-Star [4] for determining the optimal path, using heuristic techniques. Sampling-based approaches randomly sample and connect points within the surrounding space [5]. Examples of such methods include the probabilistic roadmap method [6] and the rapidly explored random tree method [7]. Interpolation-based methods define a function using multiple coefficients and determine these coefficients [8]. Optimization-based methods formulate an optimization problem with constraints and cost functions to find a path that minimizes the cost. Examples of such methods include the potential field algorithm [9,10] and model predictive control (e.g., [11]).

The potential field algorithm is widely employed for real-time collision-free path planning [12]. In this approach, an attractive force is generated in the direction of the goal, while a repulsive force emanates from obstacles. Despite its frequent use due to its mathematical simplicity, issues such as local minima and oscillations around obstacles have been encountered [12]. Sfeir [13] introduced new formulas to mitigate these oscillations and prevent conflicts when obstacles are situated near a target. Kim [14,15] proposed a novel framework for escaping local minima in a robot’s path using the potential field method.

Path planning using model predictive control (MPC) is being studied as one of the important technologies in mobility systems such as autonomous vehicles and robots. MPC-based path planning solves an optimization problem for a set of future control inputs, creating a stable, collision-free, and efficient path.

However, there are still issues with path generation using MPC. The first issue is how to describe obstacles, and the second is the calculation of the optimization problem in MPC [16]. Recently, attempts have been made to integrate Artificial Potential Fields (APFs) into MPC. Potential fields are used to identify obstacles instead of using inequality constraints [17,18]. To tackle the second issue, optimization techniques have been employed, including the use of genetic algorithms (GAs) [19] or particle swarm optimization (PSO) [20,21,22]. Zuo [16] conducted a study on vehicle lane changes using MPC, potential fields, and PSO.

When generating a path using MPC and potential field, the decision must be made as to whether to choose a safe or fast path. This is typically adjusted by establishing coefficients for the potential field and MPC’s cost function. However, it is not possible to change these assigned coefficients while driving. This paper proposes a method for defining multiple sets of weight coefficients when using MPC. We aim to guide path selection through an additional evaluation using a weight coefficient set appropriate for the surrounding environment.

Additionally, using multiple sets of weight coefficients may cause issues with real-time performance. Our objective is to demonstrate the practical applicability of this method in real-world driving. This will be accomplished by measuring computation times and conducting experiments.

This paper is structured as follows: Section 2 describes the selective MPC-PF-PSO algorithm, including the system configuration, potential field, model predictive path planning, particle swarm optimization, and the cost function used. Section 3 presents the simulation results, which demonstrate the algorithm’s effectiveness in autonomous driving scenarios. Section 4 delves into the experimental setup, showcasing the algorithm’s performance in real-world driving scenarios.

2. Selective MPC-PF-PSO Algorithm

2.1. System Configuration

By simplifying the steering-type vehicle model to a bicycle model, we can obtain the following dynamic equations:

$$\dot{x_V}=v,\dot{\theta }=\frac{v\mathit{tan}\delta }{L}$$

(1)

$$v=R\dot{\theta },R=\frac{L}{\mathit{tan}\delta }$$

(2)

$${\overrightarrow{dP}}_V={\left[\begin{array}{c}dx\\ dy\end{array}\right]}_V=\left[\begin{array}{c}R\mathit{sin}d\theta \\ R\mathit{cos}d\theta \end{array}\right]$$

(3)

$${\overrightarrow{dP}}_0=R_0^v{\overrightarrow{dP}}_V=\left[\begin{array}{cc}\mathit{cos}\theta & -\mathit{sin}\theta \\ \mathit{sin}\theta & \mathit{cos}\theta \end{array}\right]\left[\begin{array}{c}{dx}_V\\ {dy}_V\end{array}\right]$$

(4)

x₀ and y₀ refer to the global frame, while x_v and y_v refer to the frame fixed to the vehicle. P₀ is the displacement vector from the global frame to the vehicle frame, while $\theta$ represents the rotation angle of the vehicle frame in relation to the global frame. The vehicle has a wheelbase of L, a speed of v, and a front wheel steering angle of delta, which affects the rotation angle $\theta$ and results in a turning radius of R as shown in Figure 1.

2.2. Potential Field

The potential field algorithm is a method that creates a virtual potential field for obstacles or a goal point, and the robot determines its movement based on this potential field. It was initially proposed by Oussama Khatib [23] in 1986. Due to its simple and intuitive design, it is widely used in the fields of robotics and autonomous driving, and research is continuously being conducted to apply it to actual vehicles [9,24].

Obstacle information in real vehicles is obtained by integrating Lidar and IMU data. The location of the obstacle is then used to calculate the potential field of the area surrounding the vehicle. The potential of each point (U) is calculated based on its distance from the obstacle, using the following equation [25]:

$$U=\left\{\begin{array}{cc}0& d>D_2\\ {\left(\frac{1}{d}-\frac{1}{D_2}\right)}^2& if D_1<d<D_2\\ U_{max}& d<D_1\end{array}\right.$$

(5)

$d$ is the distance between the vehicle and the obstacle, $D_1$ is the distance at which the vehicle collides with the obstacle, and the potential U is calculated only for obstacles within distance $D_2$. The potential field, calculated by placing obstacles around the vehicle, is shown in the figure below.

The distance between the vehicle and the obstacle is represented by $d$, while $D_1$ represents the distance at which the vehicle collides with the obstacle. The potential is only calculated for obstacles within a distance of $D_2$. The Figure 2 shows the potential field obtained by placing obstacles around the vehicle.

2.3. Model Predictive Path Planning

Model predictive control (MPC) is generally used to analytically determine the inputs that optimize cost when planning a path to a destination. However, in real-time systems, numerical optimization techniques are preferred over finding a global optimal input. This paper utilizes MPC to apply the vehicle model and input/output limits, rather than finding the optimal path.

In model predictive path planning, the path is planned by predicting up to T in time intervals dt. The variables at the k-th time step and k + 1-th time step can be expressed using the formulas below, as shown in Equations (1)–(4):

$$\theta \left(k+1\right)=\theta \left(k\right)+\frac{v\mathit{tan}\delta }{L}dt$$

(6)

$$x\left(k+1\right)=x\left(k\right)+\frac{L}{\mathit{tan}\delta }\left(\mathit{cos}\theta \mathit{sin}\frac{v\mathit{tan}\delta }{L}dt-\mathit{sin}\theta \mathit{cos}\frac{v\mathit{tan}\delta }{L}dt\right)$$

(7)

$$y\left(k+1\right)=y\left(k\right)+\frac{L}{\mathit{tan}\delta }\left(\mathit{sin}\theta \mathit{sin}\frac{v\mathit{tan}\delta }{L}dt+\mathit{cos}\theta \mathit{cos}\frac{v\mathit{tan}\delta }{L}dt\right)$$

(8)

$${\delta }_{min}<\delta <{\delta }_{max}$$

(9)

$${\dot{\delta }}_{min}<\dot{\delta }<{\dot{\delta }}_{max}$$

(10)

where $\mathrm{x},\mathrm{y},\mathrm{a}\mathrm{n}\mathrm{d} \theta$ represent the position and angle of the vehicle. The speed of the vehicle, denoted by v, is assumed to be constant, while the steering angle, $\delta$, is treated as an input to the system.

2.4. Particle Swarm Optimization

PSO is a population-based optimization method that uses a bionic intelligent algorithm. It aims to find the optimal value quickly, using few parameters. The PSO is a method of optimization based on population, where a group of candidate solutions, known as particles, iteratively search for the best solution (fitness) by exchanging information about their discoveries in the search space [26].

Each particle has a position and velocity, and these equations are as follows:

$$V_i^{j+1}=\gamma V_i^j+c_1{rand}_1\left({pB}_i^j-X_i^j\right)+c_2{rand}_2\left({gB}^j-X_i^j\right)$$

(11)

$$X_i^{j+1}=X_i^j+V_i^{j+1}$$

(12)

$X_i^j$ is the position of the i-th particle in the j-th iteration and $V_i^j$ is the velocity of the i-th particle in the j-th iteration. c₁ and c₂ are positive constants, defined as the acceleration coefficients, $\gamma$ is the inertia weight factor, and rand₁ and rand₂ are chosen as uniform random values in the range [0:1].

The position of the i-th particle in the j-th iteration is represented by $X_i^j$, and its velocity is represented by $V_i^j$. The acceleration coefficients, c₁ and c₂, are positive constants. The inertia weight factor is represented by $\gamma$, while rand₁ and rand₂ are chosen as uniform random values in the range [0:1].

For model predictive path planning, particles are defined for the time range [0:dt:T]. The global best particles are found through iteration. The best particle is expressed as $X_{ik}^j$, which represents the steering angle of the i-th particle at the k-th time step in the j-th iteration. Similarly, $V_{ik}^j$ denotes the angular rate of the steering of the i-th particle at the k-th time step in the j-th iteration [27].

2.5. Cost Function for Selectiveness

The cost function for obtaining an optimal output through PSO is typically defined in order to find the shortest path while avoiding collisions or to follow a given path as closely as possible while avoiding collisions. Optimization can be achieved by using the cost function and cost map obtained from the potential field to balance safe driving and the shortest route. The equation for the cost function f can be expressed as follows:

$$f\left(X_i^j\left(t\right)\right)=w_{s}U\left(X_i^j\left(t\right)\right)+w_{d}D\left(X_i^j\right(t\left)\right)+w_{u}u\left(t\right)$$

(13)

where the weights for safety, distance from the global path, and input are represented by $w_s$, $w_d$, and $w_u$ respectively. By adjusting these weights, a balance can be achieved between distance from obstacles, adherence to the global path, and the magnitude of steering input changes.

This approach prevents unnecessary avoidance in sparsely populated obstacle spaces and reduces the risk of generating dangerous paths in areas with numerous obstacles. To ensure optimal obstacle avoidance, it is recommended that multiple sets of coefficients $\mathrm{W}=\left[w_s, w_d, w_u\right]$ are defined and optimized accordingly.

The global best ($gB$) is obtained for each coefficient set W. The cost function can be formulated as follows, utilizing the r-th $gB$(${gB}_r$), the maximum potential value $\mathrm{M}\mathrm{a}\mathrm{x}\left(U\right){|}_{{gB}_r}$, and the distance between the final position of ${gB}_r$ and the destination point $D_{goal}\left({gB}_r\right)$.

$$g\left({gB}_r\right)=k_1\mathrm{M}\mathrm{a}\mathrm{x}\left(U\right){|}_{{gB}_r}+k_2{D}_{goal}\left({gB}_r\right)$$

(14)

where choosing the ${gB}_r$ with a minimum $g\left({gB}_r\right)$ as the final path will allow us to avoid the problems mentioned above.

3. Simulation

To achieve autonomous driving using the MPC-PF-PSO algorithm, it is crucial to prioritize obstacle avoidance and path maintenance by adjusting the tunable variables which are shown in

Table 1. Simulation variables.

Variables
$\gamma$	Inertia of $V_i^j$ in PSO
$c_1$	Weight of particle best in PSO
$c_2$	Weight of global best in PSO
$w_s$	Weight of safety in cost function
$w_d$	Weight of distance from global path in cost function
$w_u$	Weight of input in cost function

.

Pseudo code of the MPC-PF-PSO algorithm is as shown as Algorithm 1. When the vehicle is located at position (5, 25) and its destination is set at (30, 25), the resulting graphs for M = 1, 10, and 30 are shown in the Figure 3. The parameters used were $\gamma =0.95$, $c_1=0.333$, $c_2=0.5$, T = 3, dt = 0.2, and N = 40. The bold red line is the optimal path, and other colored lines are path candidates.

Algorithm 1: Pseudo code of the MPC-PF-PSO algorithm

initialize cost map using Potential Field as shown in Figure 2 costmap(x, y) = U(d) T—prediction time dt—time step N—Number of Particles M—Maximum number of iteration for all ${\mathrm{W}}_r={\left[w_s, w_d, w_u\right]}_r$,   ${gB}^j$ = Max value   ${pB}^j$ = Max value   initialize particles randomly   for i = 1: N $X_i^0\leftarrow$ a random vector     $V_i^0\leftarrow$ a random vector     apply Equations (6)–(8) to calc $\mathrm{x},\mathrm{y},\theta$     Calc cost $f\left(X_i^0\right)$     if $f\left(X_i^0\right)<f\left({pB}_i^0\right),{pB}_i^0\leftarrow X_i^0$     if $f\left(X_i^0\right)<f\left({gB}^0\right),{gB}^0\leftarrow X_i^0$   end for   while j < M     for i = 1: N       apply Equations (11) and (12) to calc $X_i^j, V_i^j$       apply Equations (6)–(8) to calc $\mathrm{x},\mathrm{y},\theta$       Calc cost $f\left(X_i^0\right)$       if $f\left(X_i^j\right)<f\left({pB}_i^j\right),{pB}_i^j\leftarrow X_i^j$       if $f\left(X_i^j\right)<f\left({gB}^j\right),{gB}^j\leftarrow X_i^j$     end for   end while   Calc cost $g\left({gB}_r\right)$ end for Choose $\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}\left(g\left({gB}_r\right)\right)$ as a final path

If the number of iterations is set to 1, particles are randomly generated and the optimal path is then selected from them. As the number of iterations increases, particles converge to the optimal path through PSO, and the optimal path is selected from them.

When an obstacle is encountered like Figure 4, the path generated will vary depending on the values of ${\mathrm{W}}_r={\left[w_s, w_d, w_u\right]}_r$. The resulting paths when applying ${\mathrm{W}}_1=\left[0.2, 0.5, 0\right]$ and ${\mathrm{W}}_2=\left[1.0, 0.5, 0\right]$ are shown Figure 5. The red line is the optimal path when W1 is applied, and the blue line is the optimal path when W2 is applied. Other colored lines are path candidates.

As $w_s$ increases, the algorithm generates a path that avoids obstacles further away. Conversely, decreasing $w_s$ results in a path that approaches obstacles more closely. When there are two obstacles, the algorithm creates a path as shown Figure 6.

Five sets of W were defined using ${\mathrm{W}}_r=\left[1.5-0.2r, 0.5, 0\right]$, each with different $w_s$ values. The generated paths are illustrated in the Figure 7, where $\mathrm{K}=\left[\begin{array}{cc}{k}_1& k_2\end{array}\right]$ in Equation (11) is defined as $\mathrm{K}=\left[\begin{array}{cc}1.5& 0.5\end{array}\right]$ and $\mathrm{K}=\left[\begin{array}{cc}0.5& 0.5\end{array}\right]$.

The figure below shows the r values of the final selected Wr, and the difference in $\mathrm{M}\mathrm{a}\mathrm{x}\left(U\right){|}_{{gB}_r}$ and $D_{goal}\left({gB}_r\right)$, when r = 1 and r = 5 at each step until the vehicle reaches its destination.

In the Figure 8, variations occur in the values of $\mathrm{M}\mathrm{a}\mathrm{x}\left(U\right){|}_{{gB}_r}$ and $D_{goal}\left({gB}_r\right)$ when encountering the first and second obstacles, and the coefficient K begins to influence the path selection. In case (a), a long-distance route is chosen for safety when encountering an obstacle. Once the obstacle has been passed, the shortest route to the destination is chosen. In case (b), although a safer path is selected when an obstacle is encountered, there is a higher tendency to choose the shortest path to the global path.

This simulation was performed on a PC equipped with an Intel^® Core™ i7-8559U CPU @ 2.7 GHz and 16 GB RAM. 124 time steps were calculated before the vehicle reached its destination. The constants related to MPC that have a significant impact on computation time were set as follows: T = 3, dt = 0.2, N = 40, and M = 30. After calculating the average time taken, it was found that one time step took approximately 33 ms.

When driving with only one set of W in an area with complex obstacles, the vehicle may excessively avoid obstacles, as shown in (a) of Figure 9, or avoidance may occur due to it being too close to obstacles, even in non-dense areas, as shown in (b). However, by defining five sets of W and creating paths through adjusting the constant K, it is possible to avoid obstacles that are far away in less dense areas, as shown in (c) and (d). Additionally, it is possible to achieve a close avoidance of obstacles in dense areas during path generation.

4. Experiment

A steering-type vehicle was equipped with an RTK GPS for localization, LiDAR for obstacle recognition, and NVIDIA’s Jetson AGX Xavier for executing driving control. The appearance of the vehicle is as shown in Figure 10.

Using LiDAR, the point cloud information of the surrounding obstacles was acquired. Subsequently, a 2D grid map was generated from that point cloud information, and a grid map with a calculated potential field was obtained as shown in Figure 11. After that, a global path was created, in a straight line, to the destination, and a local path was generated using the MPC-PF-PSO algorithm.

Computation time is a crucial factor in obstacle avoidance during navigation. We have implemented the MPC-PF-PSO algorithm and a package that computes the potential field of an obstacle point cloud in an ROS Melodic environment. We measured the computation time used by each package.

The MPC-PF-PSO-related constants were defined as T = 3, dt = 0.2, N = 40, M = 30, and 5 sets of W. It took 7 ms to generate a 2D grid map containing potential field information, and the MPC-PF-PSO algorithm’s operation took 24 ms as shown in Figure 12.

A driving experiment was performed by applying the MPC-PF-PSO algorithm. Obstacles were placed, using bricks, and a global path was established in a straight line to the destination point as shown in Figure 13. Experiments were conducted by altering the constant K. When the coefficient $k_{1 }$ (for the maximum potential $\mathrm{M}\mathrm{a}\mathrm{x}\left(U\right){|}_{{gB}_r}$) was relatively larger than the coefficient $k_2$ (for the distance to the destination $D_{goal}\left({gB}_r\right)$), a path was generated that avoided the obstacle further, as confirmed in the simulation.

The driving vehicle and data visualization for the cases of K = [0.5 0.5] and K = [1.5 0.5] are shown in Figure 14. We measured the distance using log data to see how far the vehicle traveled from the obstacle, and the results are shown in the Figure 15. For K = [0.5, 0.5] and K = [1.5, 0.5], the shortest distances between the obstacle and the global path were 0.44 m and 0.62 m, respectively. The shortest distances between the obstacle and the vehicle were 1.03 m and 1.25 m, respectively. For K = [0.5, 0.5], even though a distance from the global path to the obstacle were closer than that of K = [1.5, 0.5], the vehicle drove closer to the obstacle.

5. Discussion

In this paper, our focus was on developing an algorithm that can be applied in practical scenarios, not only for obstacle avoidance but also for real-time decision making to ensure safe navigation. We applied a potential field to assess safety, used MPC to consider the characteristics of the actual moving vehicle, and employed the PSO algorithm to ensure real-time responsiveness. Additionally, we introduced a method for utilizing various coefficient sets to determine whether to select a safe path or a fast path according to the surrounding environment. The algorithm was implemented through ROS on Jetson Xavier, which was released by NVIDIA. Through computational time measurements and experiments, we demonstrated that this algorithm is suitable for practical use.

While the proposed method is suitable for real-time applications, a challenge lies in the low consistency of its path generation, attributed to the utilization of randomness at each step of finding the optimal path. Future research will focus on enhancing path consistency, considerations for dynamic obstacles, and developing path generation methods for irregular terrains.