An RRT-Dijkstra-Based Path Planning Strategy for Autonomous Vehicles

Abstract

It is challenging to plan paths for autonomous vehicles on half-structured roads because of the vast planning area and complex environmental constraints. This work aims to plan optimized paths with high accuracy and efficiency. A two-step path planning strategy is proposed. The classic planning problem is divided into two simpler planning problems: reduction problems for a vast planning area and solving problems for weighted directed graphs. The original planning area is first reduced using an RRT (Rapidly Exploring Random Tree) based guideline planner. Second, the path planning problem in the smaller planning region is expanded into a weighted directed graph and transformed into a discrete multi-source cost optimization problem, in which a potential energy field based discrete cost assessment function was designed considering obstacles, lanes, vehicle kinematics, and collision avoidance performances, etc. The output path is then obtained by applying a Dijkstra optimizer. Comparative simulations are conducted to assess the effectiveness of the proposed strategy. The results shows that the designed strategy balances efficiency and accuracy with enough planning flexibility and a 22% improvement in real-time performance compared to the classic Lattice planner, without significant loss of accuracy.

1. Introduction

1.1. Background

Autonomous vehicles are revolutionary products that improve people’s lives. Without onboard human intervention, it transports cargo and passengers, and travels across hazardous terrain. Autonomous express delivery vehicles, autonomous logistics vehicles, autonomous sweepers, and autonomous taxis are already in commercial operation in some cities; these vehicles improve people’s quality of life [1]. The path planning module is an essential part of an autonomous driving system. It constrains the driving behavior of the vehicle [2] and generates flexible collision-free paths to guide the vehicle to its destination [3].

1.2. Related Works

To find feasible collision-free paths, the path planning strategy incorporates environmental data, vehicle location, and driving behavior. According to intelligence or flexibility, there are two categories of path planning for autonomous vehicles: rule-based and search-based.

Rule-based path planning strategies are widely used in autonomous driving systems [4] and the planning results are strictly constrained by the planning process, i.e., if the inputs are unchanged, the output path remains unchanged [5,6]. The driving path can be obtained using the curve-fitting-based planning strategies through fitting functions. It is a fundamental rule-based planning strategy. The common path fitting techniques are B-spline curves, quintic polynomials, and cubic polynomials [7]. Sampling-based path planning strategies are more flexible and adaptable than fitting-based strategies. The expectation maximum (EM) planner [8] and Lattice planner [9] are applied to the vehicle and have been shown to be efficient. The path planning problem can also be transformed into minimum reachable set problems in node matrices [10], enabling global planning approaches such as the Dijkstra algorithm. The outcomes of the rule-based path planning strategies are strictly constrained, and quantitatively evaluated, and controllable. However, this pre-defined planning process is not flexible or intelligent enough, and might not be appropriate for narrow scenarios or scenarios with multiple obstacles.

Search-based path planning strategies are novel for autonomous driving systems that are capable of adapting to changing scenarios. The outcomes of the planning were more flexible and adaptable. Search-based strategies can be divided into numerical optimization-based strategies and graph search-based strategies. (1) Numerical solution-based strategies are a class of optimization algorithms. Artificial potential field strategies [11], quadratic programming (QP) strategies, model predictive control (MPC) strategies [12], and elastic band strategies are typical numerical optimization-based strategies. These strategies combine the discrete environment model, multiple constraints, and output reference paths that satisfy different preferences. However, the calibration of the discrete environment model that incorporates many constraints is challenging, and the optimization procedure requires a significant amount of computer work. (2) Path planning strategies based on graph searching algorithms are known as “graph search-based strategies”, which are improved over the motion planning approaches of the robots and have strong scenario adaptability and flexibility. The A-Star (A*) [13], rapidly exploring random tree (RRT) [14] are typical graph search-based strategies that use a random searching process to produce flexible, adaptable, and variable planning outcomes. However, it is difficult to guarantee that the suggested courses will be easy to navigate and adhere to the continuity requirements.

1.3. Motivations and Contributions

This work aims to provide a flexible, accurate, and efficient path planning strategy for autonomous vehicles traveling on half-structured roads: (1) bringing higher efficiency compared to traditional sampling strategies, especially in vast areas [4]; (2) bringing more flexibility compared to state machine-based planning strategies and avoiding unintended hazards due to fault transmission [15]; and (3) bringing a higher degree of interpretability compared to numerical optimization strategies [16], where the output paths are filtered from within the path candidates.

Half-structured roads include, but are not limited to, rural roads, urban non-main roads, and industrial district roads. Similar to the structured road scenario, there will be a recommended route on the map and there will be road markings. There are some distinctions and difficulties to be aware of while comparing structured road scenarios. (1) The traffic constraints are soft constraints on half-structured roads, more like mobile robot scenarios, where the movements of the vehicle are not strictly constrained by lanes. It is impossible to separate the path planning problem into straightforward driving planning issues like adaptive cruise control, lane change, or vehicle following [17]. The scenarios cannot be used with the behavior-based planning strategy [18]. (2) In half structured road scenarios, the planning area may be vast, which requires the planning strategy to be efficient and flexible enough to obtain feasible paths from the vast area. By generating flexible paths, path planning strategies with small planning steps may handle narrow gap collision avoidance scenarios; however, it is difficult for them to handle wide area scenarios. There should be a balance between planning efficiency and planning flexibility.

It is now evident how to solve the path planning issue on the half-structured road. (1) Issues with simplification for vast planning areas. (2) Issues in balancing flexibility and efficiency in trajectory planning.

In this work, the path planning problem on vast half-structured roads is successfully solved by combining the discrete multi-source cost optimization problem with a straightforward guideline finding problem. (1) The RRT-based guideline planner reduces the wide planning area into smaller areas, generating smaller passable areas for vehicles to avoid obstacles. Compared to traditional driving behavior-based strategies [2], this offers flexibility and great efficiency. (2) In the resulting reduced planning area, the path planning problem is converted into a discrete multi-source cost optimization problem. A weighted directed graph is created, after which a Dijkstra algorithm optimizer is used, enhancing the path planning for obstacle avoidance with high accuracy.

1.4. Organization

The architecture of our strategy is shown in Figure 1. The rest of this paper is structured as follows: Section 2 concerns the formulation of a path planning problem. Then, the RRT-based guideline planner is established in Section 3. The path optimization problem is solved in Section 4, followed by Section 5, where the simulation results are presented. Finally, conclusions are drawn in Section 6.

2. Problem Statement

2.1. Planning Problem Basement

The autonomous vehicle path planning problem in vast half-structured roads is formulated as a discrete optimal control problem on finding the minimum cost path, which is subject to the planning problem basement, collision avoidance constraints, and vehicle kinematic constraints.

The vehicle will travel on half-structured roads. There should always be a recommended path indicating the vehicle’s direction of movement; that is, the planning process can be applied to a Frenet coordinate system [19]. The path planning problem is finding the minimum-cost node sequence that satisfies multiple constraints connecting the start and goal points.

The path planning problem can be defined after the start point and the goal points are identified. To that end, the vehicle location $\mathit{L}\mathit{o}=\left[x, y\right]$ in the Cartesian coordinate system [10] should be converted into $\mathit{L}{\mathit{o}}_{fre}\left(s_s,d_s\right)$ in the Frenet coordinate system, and this point can be identified as the start point. The goal points ${\mathit{p}}_{gk}\left(s_g,{\mathit{d}}_{\mathit{g}}\left(k_g\right)\right)$ are a series of points along the vertical direction of the road, defined by equations:

$${\mathit{s}}_g=\left\{\begin{array}{l}{\mathit{s}}_s+\Delta s_{\min }\left(v{t}_{pre}<\Delta s_{\min }\right)\\ {\mathit{s}}_s+v{t}_{pre}\left(v{t}_{pre}\ge \Delta s_{\min }\right)\end{array}\right.$$

(1)

$${\mathit{d}}_g\left(k_{g1}\right)={\xi }_g{k}_{g1}\left(\left|d_{el}\right|+\left|d_{er}\right|\right)/N+d_{er}$$

(2)

where $\Delta s$ is the distance between the goal point and the vehicle along axle s, $\Delta s_{\min }$ is the minimum of $\Delta s$, $v$ is the vehicle speed, $t_{pre}$ is the preview time threshold, $N$ is a numerical constant, ${\zeta }_g\ge 1$ is a gain factor, $d_{el}$ and $d_{er}$ are the distance between the road reference line and the road edge at $s_g$. An example of goal points is shown in Figure 2, where ${\mathit{d}}_g\left(k_g\right)$ is a vector used to represent all possible locations, the collision-free ${\mathit{d}}_g\left(k_g\right)$ with the minimum value can be chosen as a feasible goal point, which is ${\mathit{d}}_g\left(3\right)$ in the figure.

2.2. Collision Avoidance Constraints

Collision avoidance constraints are the most complicated part of the path planning problem. In this work, an occupancy grid map is used to represent the environment, and the collision between the planned path and the obstacles can be evaluated. Here, we present two types of collision avoidance constraints: a binary occupancy grid map-based constraint and a cost occupancy map-based collision avoidance preference.

(1)

Binary occupancy grid map. The binary occupancy grid map ${\mathit{M}}_E$ is a grid map that contains information about road edges, passable areas, and obstacles to represent the environment [20]. In a binary occupancy map, each grid contains the probability of believing that there is an obstacle or not. So, the possible states of each grid are $0 for Free$ or $1 for Occupied$. The grid occupancy of static obstacles can be directly projected onto the occupancy grid map by setting the corresponding grid to 1. Before projecting a dynamic obstacle onto a map, its grid occupancy should be evaluated. A simple and efficient time to collision (TTC) based approach is used to evaluate dynamic obstacles.

For a given dynamic obstacle located at ${\mathit{X}}_{\mathbf{o}}=\left[x_{\mathbf{o}}, y_{\mathbf{o}}\right]$ on the road, its motion velocity is $v_{\mathrm{o}}$, velocity direction is ${\theta }_{\mathrm{o}}$. Its location in the Frenet coordinate system is ${\mathit{p}}_{\mathrm{o}}\left(s_{\mathrm{o}},d_{\mathrm{o}}\right)$. $v$ represents the speed of the ego vehicle at the current time $t_0$. If the TTC (time to collision) time between the vehicle and the obstacle is $\Delta t$, $\Delta t$ can be obtained through Equation (3):

$$\Delta t=\frac{s_{\mathrm{o}}-s_0-l_o}{v-v_o}$$

(3)

where $l_o$ is the distance from the obstacle location point to the closest point on the obstacle to the ego vehicle. If $\Delta t\le 0$, there will be no collision. All the dynamic obstacles on the road should be evaluated. Those $\Delta t>0$ are considered threatening, and those with the minimum $\Delta t$ are considered the most threatening one. $\Delta t_{min}$ represents the minimum $\Delta t$ from those obstacles with $\Delta t>0$. All the locations of dynamic obstacles should be transformed into virtual locations at time $t+\Delta t_{min}$ and then projected onto the occupancy grid map. The virtual obstacle location of the dynamic obstacle under the Cartesian system at time $t+\Delta t_{min}$ can be calculated by

$${\mathit{X}}_o^{\mathbf{\prime }}=\left[x_{\mathrm{o}}+v_o\Delta t_{min}\sin {\theta }_o, y_{\mathrm{o}}+v_o\Delta t_{min}\cos {\theta }_o\right]$$

(4)

As shown in Figure 3b, the red square is the predicted location of virtual dynamic obstacle at time $t_0+\Delta t_{min}$.

After all the elements (road edges, static obstacles, dynamic obstacles, etc.) are projected onto the occupancy grid map, the environmental model is established. Similarly, the occupancy relationship brought about by the path can also be projected onto the occupancy map. The vehicle projection ${\mathit{M}}_V \left(t\right)$ on the planned path at time $t$ can be obtained. The result of $\parallel {\mathit{M}}_E\circ {\mathit{M}}_V\left(t\right)\parallel$ ($°$ is an operator that multiplies the elements at the corresponding position in two matrices) can be used for identifying whether the vehicle projection ${\mathit{M}}_V \left(t\right)$ on the planned path at time $t$ intersects the non-passable area in ${\mathit{M}}_E$. If the result equals to zero, there will be no collision; otherwise, a collision occurs.

(2)

Cost occupancy map. The cost occupancy grid map ${\mathit{M}}_C$ is a grid map with different cost values, it is used for describing the cost value brought by multiple factors. An example of ${\mathit{M}}_C$ is shown in Figure 4 [21]. It is quite like ${\mathit{M}}_E$. The difference between ${\mathit{M}}_C$ and ${\mathit{M}}_E$ is that the cost value ${\mathit{E}}_C$ of each grid in ${\mathit{M}}_C$ belongs to $\left(0, 1\right)$. The cost around the obstacle varies linearly, as does the cost of traffic rules. The calculation approach for ${\mathit{E}}_C$ will be introduced in detail later in this paper.

3. RRT-Based Guideline Planner

3.1. RRT-Based Guideline Searching Process

The guideline is generated by an RRT-based guideline planner with multiple constraints considered. This guideline is responsible for two factors: defining virtual driving behavior and reducing the wide planning area. The RRT-based planning process is shown in Algorithm 1.

Algorithm 1. RRT searching process

Input: Vehicle model, environment model ${\mathit{M}}_E$, start point $\mathit{L}{\mathit{o}}_{fre}$, and goal points ${\mathit{p}}_{gk}$.

Output: A collision-free, dynamical driving path.

1.  $\Gamma$ ← InitializeTree( );

2.  while ExecutionCount $\le$ MaxIterationNumber

3.    ${\mathit{\tau }}_{new}$ ← SampleNewNode( );

4.    if Assessment(${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) $\ge$ Constraints

5.       CONTINUE; // If ${\mathit{\tau }}_{new}$ is not a valid new node

6.    end if

7.    Insert ${\mathit{\tau }}_{new}$ to $\Gamma$ // Add ${\mathit{\tau }}_{new}$ into RRT-Tree if its valid

8.    if Distance(${\mathit{\tau }}_{new}$, ${\mathit{p}}_{gk}$) $\le$ $th{r}_{end}$

9.       BREAK;

10.   end if

11.  BackRRTPath( );

12.  return ${\mathit{P}}_{\mathit{R}}$;

Definition 1.

Function Initialize Tree ( ) is a function used for generating a basic RRT-tree$\Gamma$with all the node being set to zero. Especially, the first node of$\Gamma$should be updated to the location$\mathit{L}{\mathit{o}}_{fre}$.

Definition 2.

Function Sample New Node ( ) is a function that generates a random new node${\mathit{\tau }}_{new}=\left(s_{new}, d_{new}\right)$in${\mathit{M}}_E$through a random sampling method [22].

$${\mathit{\tau }}_{new}=\left\{\begin{array}{c}{\mathit{\tau }}_{new}\in M_E \left(randva{l}_0\le th{r}_{\tau }\right)\\ {\mathit{p}}_{gk} \left(randva{l}_0>th{r}_{\tau }\right)\end{array}\right.$$

(5)

where$randva{l}_0$ is a random number in $\left[0, 1\right]$. $th{r}_{\tau }$ is a switching threshold that determines the convergence speed. Since this process is a rough search process, $th{r}_{\tau }$ can be 0.3.

Definition 3.

Function Distance (${\mathit{\tau }}_{new}$, ${\mathit{p}}_{gk}$) is a function that calculates the Euclidean distance between the two input points.

Definition 4.

Function Assessment (${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) is a function to assess whether the new sampled point meets the constraints or not. The point that meets all the constraints will be adapted and added to$\Gamma$.

Definition 5.

Function BackRRTPath ( ) is a function that sorts$\Gamma$and obtains the final feasible path${\mathit{P}}_R$.

Constraints that are mentioned in Definition 4:

(1)

Maximum distance constraint. The value of function Distance (${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) should meet a distance threshold $th{r}_{dis}$. If Distance (${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) $\ge th{r}_{dis}$, ${\mathit{\tau }}_{new}$ should be resampled on the connection between ${\mathit{\tau }}_{new}$ and ${\mathit{\tau }}_{prev}$ at where Distance (${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) $=th{r}_{dis}$. The RRT process in this work is a rough search process. And our initial planning area is larger than $60m\times 20m$. in this work, $th{r}_{dis}=2.5m$.

(2)

Minimum distance constraint. The value of function Distance (${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) should be bigger than a minimum distance threshold $th{r}_{same}$. That is, if Distance (${\mathit{\tau }}_{new}$, ${\mathit{\tau }}_{prev}$) $\le th{r}_{same}$, ${\mathit{\tau }}_{new}$ locates too close to the points on $\Gamma$, ${\mathit{\tau }}_{new}$ should be abandoned. $th{r}_{same}$ can be choose as $th{r}_{same}=0.2th{r}_{dis}$.

(3)

Process termination constraint. This constraint is used to determine whether $\Gamma$ have reached ${\mathit{p}}_{gk}$ or not. If yes, the while loop should be terminated. Basically, it is a distance defined threshold, if the distance between the new sampled node and ${\mathit{p}}_{gk}$ is smaller than $0.5th{r}_{dis}$, $\Gamma$ reached.

(4)

Kinematic constraint. The yaw angle can reflect this constraint, which can be used to evaluate whether the newly sampled node can be reached by the vehicle. If the yaw angle between ${\mathit{\tau }}_{prev}$ and ${\mathit{\tau }}_{prev2}$ (the point before ${\mathit{\tau }}_{prev}$) is ${\Theta }_{prev}$. The yaw angle between ${\mathit{\tau }}_{new}$ and ${\mathit{\tau }}_{prev}$ is ${\Theta }_{new}$. $\left({\Theta }_{new}-{\Theta }_{prev}\right)$ should be less than a yaw angle threshold $th{r}_{\Theta }$. $th{r}_{\Theta }=20°$.

(5)

Collision constraint. The paths should be collision-free. That is, the connection between ${\mathit{\tau }}_{new}$ and ${\mathit{\tau }}_{prev}$ should be collision-free. $\parallel {\mathit{M}}_E\circ {\mathit{M}}_V\parallel$ should equals zero. If not, ${\mathit{\tau }}_{new}$ should be abandoned.

An example $\Gamma$, and an example ${\mathit{P}}_R$ are shown in Figure 5.

3.2. Guideline Simplification

The guideline ${\mathit{P}}_R$ is a basic guideline, it is not smooth and even contains some unnecessary nodes. ${\mathit{P}}_R$ should be optimized into a guideline with fewer nodes and be smoother. An example of a simplified guideline ${\mathit{P}}_S$ is shown in Figure 6, it is smoother, has fewer nodes, and is collision-free, marking a smaller passable area for the vehicle to travel. For a given path sequence ${\mathit{P}}_R=\left[{\mathit{p}}_1, {\mathit{p}}_2,{\mathit{p}}_3,\dots ,{\mathit{p}}_{n-1}, {\mathit{p}}_n\right]$, a simplified guideline ${\mathit{P}}_S=\left[{\mathit{p}}_{p1}, {\mathit{p}}_{p2},{\mathit{p}}_{p3},\dots ,{\mathit{p}}_{p{n}_p-1}, {\mathit{p}}_{p{n}_p}\right]$ contains $n_p$ nodes can be obtained through the process represented in Algorithm 2.

.

Algorithm 2. Path simplification

Input: ${\mathit{P}}_R=\left[{\mathit{p}}_1, {\mathit{p}}_2,{\mathit{p}}_3,\dots ,{\mathit{p}}_{n-1}, {\mathit{p}}_n\right]$, ${\mathit{M}}_E$

Output: ${\mathit{P}}_S$

1.  ${\mathit{p}}_{now}$←${\mathit{p}}_n$;

2.  while ${\mathit{p}}_{now}\ne {\mathit{p}}_1$

3.     ${\mathit{p}}_{next}$←${\mathit{p}}_{now-1}$;

4.     $M_V$←${\mathit{p}}_{next}$, ${\mathit{p}}_{now}$; //Update $M_V$

5.     if ∥$M_E°M_V$∥ ≥ 1 //Identify collision

6.      ${\mathit{p}}_{next}$←${\mathit{p}}_{next-1}$; //Update the current node

7.     else

8.      ${\mathit{p}}_{now}$←${\mathit{p}}_{next}$; //Update the current node

9.      ${\mathit{P}}_S$←${\mathit{p}}_{now}$; //Update simplified path

10.     end if

11.  return ${\mathit{P}}_S$

4. Dijkstra-Based Path Optimizer

4.1. Guideline Expansion

In this section, the simplified guideline ${\mathit{P}}_s$ will be expanded into a small planning area. In fact, the smaller passable area (or the smaller planning area) is generated at this step, ${\mathit{P}}_s$ obtained in Section 3 is just a guideline instead of an area. ${\mathit{P}}_E$ represents a smaller planning area constructed from discrete nodes. It can be obtained after ${\mathit{P}}_s$ is processed via a two-step expansion process, in both the s direction and the d direction (under the Frenet coordinate system). The expansion process is made up of the following two aspects.

(1)

Expansion along s-axle direction.

An isometric sampling strategy is introduced to expand ${\mathit{P}}_s$. The nodes expect the first node ${\mathit{p}}_1\in {\mathit{P}}_S$ (start point) and the last node ${\mathit{p}}_{n_{\mathit{p}}}\in {\mathit{P}}_S$ (goal point) should be expanded. For a given simplified path ${\mathit{P}}_S=\left[{\mathit{p}}_{p1}, {\mathit{p}}_{p2}, {\mathit{p}}_{p3},\dots ,{\mathit{p}}_{p\left(n_p-1\right)},{\mathit{p}}_{p{n}_p}\right] \left({\mathit{p}}_{k\left(k=1,2,3,\dots ,n_p\right)}=\left[s_k, d_k\right]\right)$, the expanded node sequence ${\mathit{P}}_{ESk}$ at the node ${\mathit{p}}_k\in {\mathit{P}}_S$ can be calculated.

$${\mathit{P}}_{ESk}=\left[{\mathit{p}}_{ES1}, {\mathit{p}}_{ES2}, {\mathit{p}}_{ES3},\dots ,{\mathit{p}}_{ES\left(k_s-1\right)},{\mathit{p}}_{ES{n}_{ES}}\right]$$

(6)

$${\mathit{p}}_{ES{k}_s}=\left\{\begin{array}{c}\mathit{L}{\mathit{o}}_{fre} \left(k=1\right)\\ {\mathit{p}}_{p{n}_p} \left(k=n_p\right)\\ \left[s_n+k_s\times th{r}_s,d_n\right]\left(k=2,3,\dots ,n_p-1\right)\end{array}\right.$$

(7)

where ${\mathit{P}}_{ESn}=\left[s_{ESn},d_n\right]$ represents the node in ${\mathit{P}}_{ES}$, $th{r}_s$ is a distance threshold between adjacent nodes, $k$ represents the serial number of node ${\mathit{p}}_k\in {\mathit{P}}_S$, $k_s=\left(1,2,3,4,\dots ,n_{ES}\right)$, $n_{ES}$ is the number of nodes expanded at ${\mathit{p}}_k$. An example of ${\mathit{P}}_{ES}$ is shown by the red dots in Figure 7. Thus, the expanded node sequence ${\mathit{P}}_{ES}$ along s-axle can be obtained, that is:

$${\mathit{P}}_{ES}=\left[\mathit{L}{\mathit{o}}_{fre},{\mathit{P}}_{ES2},{\mathit{P}}_{ES3},\dots ,{\mathit{P}}_{ES\left(n-1\right)},{\mathit{p}}_{p{n}_p}\right]$$

(8)

(2)

Expansion Along d-axle Direction.

For a given path ${\mathit{P}}_{ES}$, the nodes expect the first node ${\mathit{P}}_{ES1}$ (start point) and the last node ${\mathit{P}}_{ESn}$ (goal point) should be expanded. If ${\mathit{P}}_{ES}$ contains $n_s$ nodes, the node sequence ${\mathit{P}}_{Ed{k}_d}$ at node ${\mathit{p}}_{ES{k}_s \left(k_s=1,2,3,\dots ,n_s\right)}\in {\mathit{P}}_{ES}$ can be calculated

$${\mathit{P}}_{Ed{k}_d}=\left[{\mathit{p}}_{Ed1}, {\mathit{p}}_{Ed2}, {\mathit{p}}_{Ed3},\dots ,{\mathit{p}}_{Ed\left(k_d-1\right)},{\mathit{p}}_{ES{k}_d}\right]$$

(9)

$${\mathit{p}}_{ES{k}_d}=\left\{\begin{array}{c}\mathit{L}{\mathit{o}}_{fre} \left(k_s=1\right))\\ {\mathit{p}}_{p{n}_p} \left(k=n_p\right)\\ \left[s_k{}_s ,d_{k_d}\right]\left(k=2,3,4,\dots ,n-1\right)\end{array}\right.$$

(10)

$$d_{k_d}=\left\{\begin{array}{c}\frac{k_d\left|d_{el}-d_{er}\right|}{th{r}_d}+d_{er} \left(\left|d_{el}-d_{er}\right|\le th{r}_{er}\right)\\ \frac{k_d\times th{r}_{er}}{th{r}_d}+d_{er} (\left|d_{el}-d_{er}\right|>th{r}_{er})\end{array}\right.$$

(11)

$${\mathit{P}}_E=\left[\mathit{L}{\mathit{o}}_{fre},{\mathit{P}}_{Ed2},{\mathit{P}}_{Ed3},\dots ,{\mathit{p}}_{p{n}_p}\right]$$

(12)

where the road edge at node ${\mathit{P}}_S$ is $d_{el}$ and $d_{er}$, the threshold distance for sampling is $th{r}_d$, $k_d=\left(\frac{1,2, 3, \dots ,\max \left(\left|d_{el}-d_{er}\right|,th{r}_{er}\right)}{th{r}_e}\right)$ represents the number of expansions nodes along d-axle, $th{r}_{er}=d_{el}-d_{er}$ is the sampling range threshold at node ${\mathit{p}}_{ES{k}_s}$. An example of ${\mathit{P}}_E$ is shown by the red dots in Figure 8.

4.2. Path Candidate Generation

Nodes can make up paths, and various arrangements of nodes can create carious paths. As shown in Figure 8, ${\mathit{p}}_{c{k}_c \left(k_c=1,2,3,\dots ,n_s\right)}$ represents the node cluster at one node in ${\mathit{P}}_{ESk}$, and there is ${\mathit{p}}_{ES{k}_d}\in {\mathit{p}}_{c{k}_c}$. For example, the second node in ${\mathit{p}}_{c2}$ can be represented by ${\mathit{p}}_{c2K2}$. The node map can be obtained after all the connection relationships of nodes are evaluated. The connection relationship can be evaluated in two different plans: Plan A and Plan B.

(1)

Plan A

In each cluster ${\mathit{p}}_{c{k}_c \left(k_c=1,2,3,\dots ,n_s\right)}$, there is one node that will be selected and be added to the path candidate ${\mathit{P}}_c$. An example path candidate (blue line) ${\mathit{P}}_c=\left[{\mathit{p}}_{c1},{\mathit{p}}_{c2K3},{\mathit{p}}_{c3K3},\dots ,{\mathit{p}}_{c13K3},{\mathit{p}}_{c14}\right]$ is shown in Figure 9.

(2)

Plan B

Different from Plan A, Plan B divides node clusters based on the node ${\mathit{p}}_{p{n}_p}\in {\mathit{P}}_S$. That is, the node number of each path candidate is the same as the node number in ${\mathit{P}}_S$. An example path candidate (blue line) ${\mathit{P}}_c=\left[{\mathit{p}}_{c1},{\mathit{p}}_{c2K2},{\mathit{p}}_{c7K2},{\mathit{p}}_{c12K4},{\mathit{p}}_{c14}\right]$ is shown in Figure 10.

4.3. Path Optimization

The smaller area will be transformed into a node map ${\mathit{P}}_E$, and the costs between each node in ${\mathit{P}}_E$ will be assessed. Therefore, the path optimization problem is modeled and transformed into a neighborhood cost assessment and discrete minimum cost sequence optimization problem. The minimum cost node sequence will be obtained via a Dijkstra-based optimizer. After the minimum cost node sequence is smoothed, the optimal path can be obtained. To that end, there are two path optimizers: Plan A and Plan B.

Every ${\mathit{P}}_c$ needs to be assessed. That is, every cost value between each two nodes in ${\mathit{P}}_E$ should be assessed. The cost node map ${\mathit{M}}_m$ can be obtained. So as the optimized path ${\mathit{P}}_{bt}$ can be obtained through a Dijkstra process [23]. ${\mathit{P}}_{bt}$ should be smoothed before it can be tracked by the vehicle. A Bezier spline curve is introduced to fit ${\mathit{P}}_{bt}$, making it smooth to meet the steering characteristics of the vehicle. An example smoothed path ${\mathit{P}}_b$ is shown in Figure 11.

Here is an example of cost value assessment of connections between two nodes. The cost between each two nodes ${\mathit{P}}_c$ can be calculated.

$$E=E_{road}+E_{dynamic}+E_{\mathit{p}re}$$

(13)

(1)

$E_{road}$ is the cost of road preference. In our scenario, the autonomous vehicle is a level-four vehicle driving in specific area, there is a map marked reference path (or a reference path generated by the mission planner) with constant 50 nodes ${\mathit{P}}_{ref}=\left[{\mathit{p}}_{r1}, {\mathit{p}}_{r2},{\mathit{p}}_{r3},\dots ,{\mathit{p}}_{r49},{\mathit{p}}_{r50}\right]$ to guide the vehicle. The path planner will perform local path planning based on ${\mathit{P}}_{ref}$, thus, the vehicle should not deviate too far from ${\mathit{P}}_{ref}$. For a given connection line ${\mathit{P}}_c$ between two of nodes in ${\mathit{P}}_E$, ${\mathit{P}}_c$ should be discrete into a path ${\mathit{P}}_{cd}$ according to the distance threshold $th{r}_{E_{road}}$.

$${\mathit{P}}_{cd}=\left[{\mathit{p}}_{cd1},{\mathit{p}}_{cd2},{\mathit{p}}_{r3},\dots ,{\mathit{p}}_{cd{k}_{cd}}\right]$$

(14)

where ${\mathit{p}}_{cd{k}_{cd}}$ represents the $k$th node, $k_{cd}$ is the number of nodes included. If the length of ${\mathit{P}}_c$ in the s-axle direction is $l_{P_c}$.

$$k_{cd}=\left\{\begin{array}{c}1\hspace{0.17em} (\left[\frac{l_{P_c}}{th{r}_{E_{road}}}\right]<1)\\ \left[\frac{l_{P_c}}{th{r}_{E_{road}}}\right]\left(\left[\frac{l_{P_c}}{th{r}_{E_{road}}}\right]\ge 1\right)\end{array}\right.$$

(15)

If the offset between the node $p_{cdk}\in {\mathit{P}}_{cd}$ can be represented by $l_k$. $E_{road}$ of ${\mathit{P}}_c$ can be obtained

$$E_{road}=\sum \left\{\begin{array}{c}\left|{\zeta }_{p1}{l}_k\right|(l_k<R_{p1})\\ \left|{\zeta }_{p2}{l}_k\right|\left(R_{p1}\le l_k\le R_{p2}\right)\\ \left|{\zeta }_{p3}{l}_k\right|(l_k>R_{p2})\end{array}\right.$$

(16)

where ${\zeta }_{p1}$, ${\zeta }_{p2}$, and ${\zeta }_{p3}$ are gain coefficients, $R_{p1}$ and $R_{p2}$ are offset thresholds, $k=\left[1,2,3,\dots ,k_{cd}\right]$. An example $E_{road}$ is shown in Figure 4a.

(2)

$E_{dynamic}$ is the cost of vehicle dynamics.

Vehicle dynamics constraints cannot be ignored. Generally, the vehicle’s lateral acceleration can be used to calculate the cost: the smaller the cost, the smoother the path. However, the lateral acceleration cannot be directly estimated in our node map. Road curvature and relative lateral offset (under the Frenet coordinate) are chosen to calculate the cost of lateral acceleration.

$$E_{dynamic}={\zeta }_{la1}{E}_{curv}+{\zeta }_{la2}{E}_{lat}$$

(17)

where ${\zeta }_{la1}$ and ${\zeta }_{la2}$ are two gain factors, $E_{curv}$ is the cost caused by the road curvature, $E_{lat}$ if the cost of road offset value.

Assuming that the connection between two nodes can be divided into serval parts, the curvature (under the Cartesian coordinate system) of each part can be represented by $C_{road} \left(k\right)$.

$$E_{curv}=\sum \frac{1}{C_{road}\left(k\right)} \left(k=\left[1,2,3,\dots ,k_{cd}\right]\right)$$

(18)

$E_{lat}$ can be calculated by analyzing the relative offset value between the current point and the previous node along the d-axle. The offset value at each point on the connection between two nodes can be represented by $D_{offset}$. An example $D_{offset }\left(k_{cd}\right)$ between node ${\mathit{p}}_{c7K2}$ and the $k_{cd}$th point is shown in Figure 10.

$$E_{lat}=\sum \left|D_{offset}\left(k\right)\right| \left(k=\left[1,2,3,\dots ,k_{cd}\right]\right)$$

(19)

(3)

$E_{\mathit{p}re}$ is the cost of other path preferences.

For a given connection ${\mathit{P}}_{cd}=\left[{\mathit{p}}_{cd1},{\mathit{p}}_{cd2},{\mathit{p}}_{r3},\dots ,{\mathit{p}}_{cd{k}_{cd}}\right]$ between two nodes, the cost of preference should be assessed. Figure 4b shows an example cost of collision risk preference. That is, the vehicle should keep a gap from obstacles (as shown in Figure 12). The grid value in preference cost map ${\mathit{M}}_C$ of collision risk example can be calculated by:

$$E_{C ij}=\left\{\begin{array}{c}inf \left(r_{ij}\le 0\right)\\ 1-\frac{\left|r_{ij}\right|}{r_{max}} (0<r_{ij}\le r_{max})\\ 0 \left(r_{ij}\ge r_{max}\right)\end{array}\right.$$

(20)

where $i$, $j$ is the map size, $r_{ij}$ represent the distance from a grid to an obstacle, if $r_{ij}\le 0$ the grid inside the obstacle, else the grid is outside the obstacle. $E_{C_{ij}}\in \left[0,1\right]$ represent the risks to a grid. $r_{\max }$ is the maximum value of the influence range.

If ${\mathit{M}}_V$ represents the projection of ${\mathit{P}}_{cd}$ on a binary occupancy map. ${\mathit{M}}_C$ can be obtained.

$${\mathit{M}}_C=\parallel {\mathit{E}}_{pre}\circ {\mathit{M}}_V\parallel$$

(21)

4.4. Speed Profile Generation

The speed profile generation is an essential part of autonomous driving systems, it generates a feasible profile based on the speed limit, and collision probability with dynamic or static obstacles, which cannot be ignored. This work focuses on the path planning process for autonomous vehicles running on half structured roads with a maximum speed limit $v_l=25 \mathrm{km}/\mathrm{h}$. A simple rule-based speed profile generation approach is functional and applied to our vehicle, as shown in Figure 13. The initial target speed of the vehicle is set to $v_T=v_l$. If there is no new path while the vehicle reaches the end of ${\mathit{P}}_b$, the target speed is set to 0, the vehicle stops. If there is a collision on ${\mathit{P}}_b$, the planned path is no longer feasible, and the target speed will be set to 0. This rule-based speed profile generation approach is also applicable to dynamic obstacles. The vehicle will stop if there are dynamic obstacles occupying the driving area, the current path has no collisions, and new paths cannot be obtained. For a dynamic obstacle following scenario, our approach theoretically allows the vehicle to follow the obstacle behind it at a distance of $s_g$.

5. Simulation

5.1. Simulation Statement

Simulations are conducted with our hardware-in-loop (HIL) simulator, with the Lattice planning techniques in reference [4] as a control group. Parameters are shown in

Table 1. Simulation parameters.

Item	Model/Value
Rapid Autopilot Controller	NI CompactRIO 9047
Real-time Computer	NI PXIe 8840
Host Computer	HP Pavilion (I7-8550U, 16G RAM)
Scenario Generator	Lenovo Legion (i7-11800H, 16G RAM)
Vehicle length	2.3 m
Vehicle width	1.2 m
Vehicle wheelbase	1.5 m
Planning resolution	0.25 m

, and the simulation architecture is shown in Figure 14.

5.2. Scenarios with Static Obstacles

Two scenarios with static obstacles are conducted, focusing on evaluating the performance of the planning strategy. All the scenario data is collected by an autonomous vehicle platform and reproduced on our HIL simulation platform. The ego vehicle travels at a constant speed on city roads, with obstacles in front of and around it. The simulation results are shown in Figure 15, all the paths can successfully guide vehicles to avoid obstacles. In more detail. The average results of 50 simulations under the same scenario, including Plan A, Plan B, and a classic Lattice planning strategy [4], are shown in

Table 2. Comparison of two planning plans.

	Time Consumption	Success Rate	Real-Time Performance
Plan A	27 ms	98.3%	22% improvement
Plan B	19 ms	92.5%	45% improvement
Lattice	35 ms	99.8%	Control group

.

The path shapes of Plan A and Lattice is similar. Due to the fact that Plan A is an approximate Lattice sampling process, in which the path candidate generation and the optimal path selection is similar to those of the control group [4]. To this end, the planning accuracy can be aligned with Lattice, and the computational time has been reduced by 8 ms (22% real-time performance improvement) thanks to the introduction of a planning area reduction process in Step 1.

The path shape of Plan B is smoother and has less curvature variation than Plan A and Lattice. In particular, in Figure 15a, the path of Plan B is more in line with the human driver’s driving habits, with a gentle yaw to avoid obstacles and then a gentle return to the map path [24]. In principle, Plan B has fewer nodes than Plan A, it is more suitable for planning comfortable smooth paths in scenarios with fewer obstacles [25]. For this reason, it has the lowest planning success rate (92.5%) of the three planning strategies, but has the best real-time performance, which takes 19 ms to run once.

5.3. Scenario with Dynamic Obstacles

A scenario with dynamic obstacles is conducted to evaluate the adaptation of our strategies to dynamic scenarios. The ego vehicle runs on the road with a maximum speed of $v_l=3 \mathrm{m}/\mathrm{s}$. The red point clouds represent the position ${\mathit{X}}_{\mathit{o}}^{\mathbf{\prime }}$ of the dynamic obstacles, and the red arrows represent the motion of the dynamic obstacles. Simulation results are shown in Figure 16a.

The planning result at time $t=0 \mathrm{s}$ is shown in Figure 16a. The point clouds of dynamic obstacles are projected onto the map, where the planning task for a dynamic scenario is transformed into the planning task for a static scenario. Similar to the results in Section 5.2., the planning results for Plan A and Lattice [4] are similar, and the result for Plan B is smoother and more easily executed by the vehicle [26].

The planning result at time $t=3 \mathrm{s}$ is shown in Figure 16b, where it is interesting to note that the Lattice strategy did not yield any feasible paths, but Plan A and Plan B are still functional. Yet, for the Lattice planner, there is no feasible collision-free path for the vehicle to travel, as the predicted point clouds of the dynamic obstacles occupied all feasible areas, and as the Lattice planner is not flexible enough [25]. The target speed is set to zero $v_t=0$, the vehicle stops. For Plan A and Plan B, thanks to the search strategy in Step 1, the planning area reduction process, which brings a stochastic process with more flexibility, where the RRT process in Step 1 refreshes until the boundary constraints are reached if there is no feasible path.

The planning result at time $t=6 \mathrm{s}$ is shown in Figure 16c. The dynamic obstacle has passed the ego vehicle and been assessed as no threat, where $\Delta t=0$. The planning task for a dynamic scenario regress into normal planning task for a static scenario.

6. Conclusions

This paper presented a path planning strategy for autonomous vehicles traveling on half-structured roads. The challenge of path planning was split into two issues. (1) An RRT-based planner, which turned the large-scale planning problem into a small-scale planning problem, lowered the planning area. Due to its random sampling feature, the RRT approach increased process flexibility. (2) By turning the smaller planning area into a discrete multi-source cost optimization problem, the planning problem was solved. Two strategies were established to address the issue: with Plan A it was more flexible; Plan B was more efficient. (3) Simulation results proved that our proposal was efficient and effective in both static and dynamic scenarios. Although the planned path was not optimal, it consumed less computing power (20% real-time performance improvement) and there was no significant loss of accuracy (compared with a Lattice planner).

In our future work, numerical optimization methods incorporating additional vehicle kinematic constraints, dynamic constraints, and other constraints, will be applied to this two-step strategy to enhance search completeness. Instead of obtaining discrete suboptimal paths, optimal paths could be obtained.