Apollo: Adaptive Polar Lattice-Based Local Obstacle Avoidance and Motion Planning for Automated Vehicles

Abstract

The motion planning module is the core module of the automated vehicle software system, which plays a key role in connecting its preceding element, i.e., the sensing module, and its following element, i.e., the control module. The design of an adaptive polar lattice-based local obstacle avoidance (APOLLO) algorithm proposed in this paper takes full account of the characteristics of the vehicle’s sensing and control systems. The core of our approach mainly consists of three phases, i.e., the adaptive polar lattice-based local search space design, the collision-free path generation and the path smoothing. By adjusting a few parameters, the algorithm can be adapted to different driving environments and different kinds of vehicle chassis. Simulations show that the proposed method owns strong environmental adaptability and low computation complexity.

1. Introduction

The paper aims to give an efficient local motion planning method for automated driving in complex environments. The planning module, which links the system perception module and the control module, plays a special and momentous role in the automated driving system. It needs to reasonably organize and formulate the sensing, positioning and routing information while considering the status of the vehicle. An eximious motion planner of an automated vehicle needs to consider the safety, feasibility, efficiency, environmental adaptability and real-time performance of the algorithm, especially in dynamic driving scenarios with humans.

1.1. Related Work

A hierarchical framework is typically adopted, in which the motion planning task is decomposed into the global path planning and the local path planning problems. The global path planning or deliberative path planning is typically map-based and often computed offline, while the local path planning or reactive planning is sensor-based and conducted online. Global planning algorithms for completely known topological or occupancy grid maps, such as A*, D*, jump point search (JPS), probabilistic roadmap (PRM), rapidly exploring random tree (RRT), Voronoi graph and their variants [1,2,3,4,5], are well studied. In practice, global path planning is often problematic due to the inaccuracy or unavailability of the global world model. The local path planning method is mainly used for real-time collision avoidance and global path tracking in unknown or semi-unknown environments via the onboard sensing information. Many local path planning algorithms are prone to falling into local minima or exhibiting oscillating behaviors. This paper proposes an adaptive polar lattice-based local obstacle avoidance and motion planning method (APOLLO) which is effective and computationally efficient in dynamic driving scenarios with humans. Without loss of generality, the vehicle’s sensing area is assumed to be fan-shaped. APOLLO is conducted on the scalable polar grids around the vehicle with elaborately designed probabilistic representations. A coarse collision-free path can then be generated by connecting grid nodes according to some optimization criteria. An efficient strategy is also proposed to adjust the selected path to a kinodynamic, plausible smooth trajectory. A local planner should track the global path while avoiding dynamic obstacles as well as taking into consideration the vehicle constraints. Several methods have been successfully used in some distinct driving scenarios, such as potential field methods [6], the dynamic windows approach (DWA) [7], the time-elastic band (TEB) [8,9,10], a state lattice [11,12,13], a numerical optimization approach [14,15,16,17], machine learning methods [18,19], etc. Paths planned by the potential field methods are generally smooth and safe, but methods of this kind have the problem of local optima, and it is difficult for the vehicle to approach the target point by using such methods when there are obstacles near the target point. The DWA algorithm consists of three main processes, i.e., the velocity sampling, the obstacle detection and the trajectory evaluation. This algorithm selects the path with the highest score and sends the associated speed to the vehicle, and it is also susceptible to local minima [20]. The TEB algorithm is formulated as a multi-objective nonlinear optimization problem with the initial trajectory generated by a global path planner, which can take into consideration the geometric and kinematic/dynamic constraints of the vehicle. Thanks to efficient optimization techniques, such as the “g2o” framework, the TEB algorithm can be implemented robustly and efficiently in a simple environment with relatively inexpensive hardware. However, this kind of algorithm may let the vehicle wander around the channel entrance when facing a narrow channel environment (Figure 1a). State lattice approaches, such as FALCO [11] (Figure 1b), which are based on pre-computed motion primitives and online path selection, can effectively reduce the computational complexity while needing relatively larger storage spaces, and they may produce oscillatory behavior in front of long obstructions. The receding horizon optimization (RHO) and model predictive control (MPC) methods transform the local trajectory generation problem into the numerical optimization problem, which can easily take into account the environment and vehicle constraints in the model and keep the ride smooth [21]. However, this kind of approach requires a deliberate balance between the model complexity and the computational complexity.

1.2. Contribution

A probability-based method to construct an adaptive local polar lattice search space with obstacles is proposed. Unlike an equidistant lattice, the proposed adaptive polar lattice based on sensor field of view (FOV) is closer to human search habits. The concise collision-free path generation and smoothing method has high computational efficiency. Compared with some state-of-the-art methods, the proposed obstacle avoidance and motion planning method shows stronger environmental adaptability and is effective in dynamic driving scenarios.

2. Local Search Space Design

An occupancy grid map is one of the most commonly used scene representation methods for automated vehicles. According to the size of the 2D grids, it can be divided into uniform grids, non-uniform grids and adaptive size grids. In practical applications, adjusting the local resolution of the grids adaptively according to the complexity of the environment and driving speed of the vehicle will be very helpful for motion planning. Despite the computational advantages, the variable-size grids can implicitly take the uncertainty of local sensing information into account. Apart from Cartesian occupancy grid maps, polar coordinate-based grid maps are also used especially in outdoor environments, having the advantages of easy adjusting of the angular resolution and integration of obstacles perceived by sensors, such as cameras and lidars [22].

2.1. Adaptive Polar Lattice

As shown in Figure 2, the angle range $\beta$ and radius L of the automated vehicle M are set adaptively according to the sensor horizontal FOV, the sensing range and the environment. Rays are built to divide the circumferential local search space, and the circular arcs with the automated vehicle as the center divide the radial direction of the space. The rays and circular arcs divide the local search area into $m\times n$ polar lattices with different sizes, where $m$ denotes the amount of radial dispersion and $n$ the amount of angle dispersion. Each polar lattice in Figure 2 can be assigned a probability. The magnitude of each grid’s probability value depends on the likelihood of the vehicle traversing that grid. Then, the overall probability distribution of all the polar lattices can be stored in an $m\times n$ matrix,

$$\Omega =\left[{\omega }_{ij}\right], i=1,\cdots ,m;j=1,\cdots ,n.$$

Suppose the size of the angle and radial interval, i.e., ${\delta }_a,{\delta }_r$, are constant, and the probability of any point $\left(x_k,y_k\right)$ in the local search space is assigned to be ${\omega }_{ij}$. Here, the grid index $i,j$ corresponding to the point $\left(x_k,y_k\right)$ can be calculated by Equations (1) and (2), when ${\delta }_a,{\delta }_r$ are constant.

$$i=\lfloor \frac{\sqrt{x_k^2+y_k^2}}{{\delta }_r}\rfloor ,$$

(1)

$$j=\lfloor \frac{\arctan \left(y_k/x_k\right)}{{\delta }_a}\rfloor .$$

(2)

Here, $\lfloor \cdot \rfloor$ means rounding down. In real applications, ${\delta }_a,{\delta }_r$ are tuned adaptively according to the geometric and physical parameters of the vehicle and the complexity of its working environment. Without a loss of generality, ${\delta }_a$ is set to be constant in the following discussion for simplicity. Considering that the motion planning of a vehicle should be more conscientious for the area closer to itself than the area far away, ${\delta }_r$ can be set as an arithmetic progression, a geometric progression or a logarithmic function. Similar to (1) and (2), the index $i,j$ of any point in the search area can also be easily calculated for the case that ${\delta }_r$ is an arithmetic or geometric progression. A hash table is used to store the mapping between each point’s coordinates and the corresponding index of $\Omega$. The complexity of the indexing time is O(1) (see Figure 2). Compared with the equidistant grid, the proper selection of the form and value of ${\delta }_a,{\delta }_r$ can reduce the computational complexity of the motion planning algorithm while ensuring the safety of the vehicle. We do not claim that an arithmetic progression or a geometric progression is the best choice for ${\delta }_r$. It needs to be determined according to the actual driving conditions.

2.2. Local Probability Distribution

An automated vehicle with nonholonomic constraints cannot move laterally. In order to make the vehicle ride smoother and reduce frequent turns, the probability should be set higher in the vehicle’s heading direction. As show in Figure 3, for a given initial state $x_0$, the vehicle can take different paths to the sensor frontier F. The 2D path first splits in 37 directions, and each path splits in another 12 directions. For each path generated as a cubic spline curve, the resulting paths group is symmetric. As the end points of the local search space are reachable, the probabilities at the edge of the search area should be approximately consistent.

Based on the above considerations, the initial probability distribution function of the local search space without obstacles and with a goal point is designed as (3), which is similar to a two-dimensional Gaussian distribution.

$$f_X\left(r,a\right)=\eta \frac{1}{\sqrt{{(2\pi )}^2|\Sigma |}}{e}^{-\frac{1}{2}{\left(X-\mu \right)}^T{\Sigma }^{-1}\left(X-\mu \right)}$$

(3)

where

$$X={\left(r,a\right)}^T, \mu ={\left({\mu }_r,{\mu }_a\right)}^T, \Sigma =\left|\begin{array}{cc}{\sigma }_r^2& \rho {\sigma }_r{\sigma }_a\\ \rho {\sigma }_r{\sigma }_a& {\sigma }_r^2\end{array}\right|.$$

Here, $r$ represents the distance between the point in the search range and the vehicle, and $a$ represents the polar angle of the proposed point in the vehicle’s coordinate system. $\mu$ is the expectation of $r$ and $a$. $\Sigma$ represents the covariance matrix. $\eta$ and $\rho$ are constants. Figure 4 illustrates the initial probability distribution of a 180° fan search space.

In order to make the vehicle move towards the target point, it is necessary to set high probabilities along the direction of the target point. As shown in Figure 5, we denote the distance and azimuth of the target point to be $r^{*}$ and $a^{*}$, respectively.

Then, the probability distribution between the vehicle and target point is set as:

$$g\left(r,a\right)=\frac{\zeta r}{r^{*}}\frac{1}{\sqrt{2\pi \left|\sigma \right|}}{e}^{-\frac{1}{2}{(a-a^{*})}^T{\sigma }^{-1}(a-a^{*})}$$

(4)

where $r\in [0,r^{*}]$ and $a\in [a^{*}-\Delta a,a^{*}+\Delta a]$. For a distinct distance $r$ from the original point, Formula (4) degenerates to a Gaussian distribution with respect to the angle $a$. Here, $a^{*}$ and $\sigma$ are respectively the mean and variance of the proposed Gaussian distribution.$\zeta$ is a constant, and $\Delta a$ is an adjustable parameter.

2.3. Obstacle Avoidance

It is always expected that the planned path of the vehicle will be as far away from obstacles as possible, so as to ensure safety. However, a too-conservative path planning strategy will inevitably reduce the planning efficiency and increase the length of the resulting path. As shown in Figure 6, the vicinity region of the obstacle point cloud $p=\left(x_k,y_k\right)$, $k=1,2,\cdots$, is divided into two areas, i.e., the strictly prohibited area and the expansion area. Here, $r_0$ denotes the strictly prohibited region, which is related to the shape and size of the vehicle.

The probability distribution around the obstacles is updated according to the following piecewise function:

$$\Omega \left(i,j\right)=\{\begin{array}{cc}-\gamma ,\hfill & \hfill \Vert p_0-p_{i,j}\Vert <r_0 \\ -\frac{\gamma }{\Vert p_0-p_{i,j}\Vert +\epsilon },\hfill & \hfill r_0\le \Vert p_0-p_{i,j}\Vert \le r_s\end{array}$$

(5)

Here, $p_0$ represents the coordinates of the obstacle point cloud. $r_s$ denotes the radius of the expanded obstacle point cloud, and $\gamma ,\epsilon$ are adjustable parameters. Figure 7 illustrates the probability distribution of the local search space with two static obstacles at coordinates (−1.0, 1.5) and (0.0, 0.7).

3. Collision-Free Path Generation

Based on the above local search space design, the problem of collision-free path planning can be transformed into the problem of node selection, i.e., the nodes are selected and connected one by one along the arcs that gradually expand from near to far in the search area and in the meantime ensure that the connecting lines do not intersect with obstacles (see Figure 8).

As shown in Figure 9, the azimuth angle of the goal point in the sector search area is first calculated. For a goal point inside the local search space, the node closest to the goal is chosen as the target. For the case that the goal point is outside the local search space, the center point of the vehicle is connected to the goal point, and the intersection point with the boundary of the search area is denoted as E. If E is collision-free, then its closest node is selected as the temporary target point. Otherwise, the nodes around E are selected as potential targets. The path weight of the vehicle to each potential target is calculated, and the path with the largest value is selected.

The specific path generation procedures are given in Algorithm 1.

Algorithm 1 Collision-free path planning

Initialize weight matrix $\Omega$ select the node $E\in S$ closest to the goal and calculate its corresponding index $\left(i,j\right)$ in matrix $\Omega$ If$\Omega \left(i,j\right)<0$ then   // indicates the lattice with index $\left(i,j\right)$ is occupied  find neighbor node $E^b$ of $E$, $b=1,2,\cdots$  generate corresponding paths $p^b$  calculate total weight ${\omega }^b={\displaystyle {\sum }_{i=1}^m{\omega }_i}$ of each path  select path $p^{*}=\left\{p^b\right|b=\arg \underset{b}{\max }{\omega }_b\}$  return $p^b$ else  generate path $p^0$ from the vehicle to node E  if $p^0!=\mathrm{NULL}$ then   return $p^0$  else    repeat steps 4–8  end if end if

4. Path Smoothing

In order to make the tracking process of the vehicle smoother and more stable, it is necessary to convert the polyline into a kinodynamic-friendly trajectory, especially at sharp turning points. The path transferring process should also be collision-free and can be adjusted automatically according to the speed of the vehicle.

4.1. Fast Point Sequence Generation

As shown in Figure 10, the unit heading vector of the vehicle at the position $A_i$ is denoted by $v^i,i=0,1,\cdots$. Point $B_i$ is on the vehicle’s current heading vector, and the distance between $A_i$ and $B_i$ is set to be $l_i$. With $B_i$ as the center point, find point $C_i$ on the polyline path so that $\Vert B_i{C}_i\Vert ={\kappa }_i$. Find point $A_{i+1}$ on vector $B_i{C}_i$ so that $\Vert B_i{A}_{i+1}\Vert ={l^{\prime }}_i$. Denote as $v_{i+1}$ the vector starting from $A_{i+1}$ and going in the direction of the extension line $A_i{A}_{i+1}$ (see the green vector in Figure 10). The above process is repeated until the vehicle reaches the vicinity of the target point, and the point sequence $A_i,i=0,1,\cdots$ generated in the process is smooth.

The point sequence can be kept collision-free by adjusting the parameters $\kappa ,l,l^{\prime }$. Algorithm 2 gives the specific path smoothing procedures.

Algorithm 2 Path smoothing

Initialize weight matrix $\Omega$ and $\kappa ,l,l^{\prime }$  // $\kappa ,l,l^{\prime }$ are adjustable parameters while $\left|A_{i}E\right|>\epsilon$ do   //$\epsilon$ is the margin    $B_i=A_i+l_i\cdot v^i$   // $v^i\int \mathrm{v}ccc$ is the orientation of the vehicle    find point $C_i$ so that $\left|B_i{C}_i\right|=\kappa$    choose test points $D_1,D_2,\cdots ,D_s$ on $B_i{C}_i$    for $k=1:s$ do     calculate index $\left(i,j\right)$ of point $D_k$ in $\Omega$     if $\Omega \left(i,j\right)<0$ then       $\kappa =\kappa -\Delta \kappa$       back to step 3     end if   end for    $A_{i+1}=B_i+{l^{\prime }}_i\cdot \overrightarrow{B_i{C}_i}/\kappa$   calculate angular velocity between $A_i$ and $A_{i+1}$ end while

As shown in Figure 10, a sequence of points $\left\{A_0,A_1,\cdots \right\}$ with directions can be generated by Algorithm 2. Figure 11 shows the path smoothing results of differently shaped polyline paths with different parameters $\kappa ,l,l^{\prime }$.

Figure 12 shows the path smoothing results of Algorithm 2 in a driving scenario with obstacles.

4.2. Speed Optimization

The smooth path generated can be followed by path tracking methods. Usually, the control module should be fed with the linear velocity and angular velocity at a given moment. Several approaches can be applied to transfer the discrete path into $v,\omega$ with limited motor torques, such as multi-objective optimization [8,9], polynomial interpolation and quadratic programming [14]. The optimization can also be performed on the SE(2) manifold, and then the resulting pose can be directly tracked by geometric control methods [10,23]. In this paper, it is assumed that the velocity between two adjacent configurations

$$g_0=\left(x_0,y_0,{\theta }_0\right), g_e=\left(x_e,y_e,{\theta }_e\right)\in {ℝ}^2⋉S^1\simeq SE(2)$$

is constant and that the direction of it has been obtained in the path smoothing process. Here, $\left(x_0,y_0\right),\left(x_e,y_e\right)$ denote the position of the vehicle, and ${\theta }_0,{\theta }_e$ denote the orientation of the vehicle. Divide the time interval $T=\left[t_0,t_e\right]$ between $g_0$ and $g_e$ into N equal subintervals with length $\delta t$.

As shown in Figure 13, the parameters selected in Algorithm 2, such as $l_i,l_i{}^{\prime }$, are closely related to the kinematics of the vehicle. For example, if $l_i=l_i{}^{\prime }$, the path between $A_i$ and $A_{i+1}$ is a circular arc. According to the geometric relation, the formula for solving the velocity can be obtained as:

$$v=\frac{\sqrt{{\left(x_e-x_0\right)}^2+{\left(y_e-y_0\right)}^2}}{2\sin \frac{{\theta }_e-{\theta }_0}{2}}\cdot \frac{{\theta }_e-{\theta }_0}{T}$$

(6)

Then, the angular velocity ${\omega }_i,i=1,\cdots ,N$ between the configurations $g_0\triangleq \left(p_0,{\theta }_0\right)$ and $g_e\triangleq \left(p_e,{\theta }_e\right)$ can be obtained by the following optimization model:

$$\begin{array}{l}\underset{{\omega }_i}{\min } J_{\theta }+J_p\\ \mathrm{s}.\mathrm{t}. {\omega }_{\min }\le {\omega }_i\le {\omega }_{\max }, i=1,\cdots ,N\\ \hspace{1em} \Delta {\omega }_{\min }\le {\omega }_{i+1}-{\omega }_i\le \Delta {\omega }_{\max }, i=1,\cdots ,N-1\end{array}$$

(7)

where

$$\begin{array}{l}{J}_{\theta }={\alpha }_1{\Vert \left({\theta }_0+{\displaystyle {\sum }_{i=1}^N{\omega }_i\delta t}\right)-{\theta }_e\Vert }_2\\ J_p={\alpha }_2{\Vert \left(p_0+{\displaystyle {\sum }_{i=1}^N{v}_i\delta t}\right)-p_e\Vert }_2\end{array}$$

(8)

Here, ${\alpha }_1,{\alpha }_2$ are weights corresponding to the terminal angle and position error. The vector $v$ in (8) consists of two elements, i.e., $v={\left[v_x,v_y\right]}^T$. The above objective function can also be represented by the distance metric of SE(2), which is $\Vert \log \left(g_0{}^{-1}{g}_e\right)\Vert$(here we abuse the notations $g_0,g_e$ to represent group elements in SE(2)). For N = 3, the above optimization formulation can be relaxed to the following algebraic equations:

$$\begin{array}{l}{\theta }_e={\theta }_0+\left(\Delta {\theta }_1+\Delta {\theta }_2+\Delta {\theta }_3\right)\\ x_e=x_0+\Delta s\left[\cos \left({\theta }_0+{\theta }_1\right)+\cos \left({\theta }_0+{\displaystyle \sum _{i=1}^2\Delta {\theta }_i}\right)+\cos \left({\theta }_0+{\displaystyle \sum _{i=1}^3\Delta {\theta }_i}\right)\right]\\ y_e=y_0+\Delta s\left[\sin \left({\theta }_0+{\theta }_1\right)+\sin \left({\theta }_0+{\displaystyle \sum _{i=1}^2\Delta {\theta }_i}\right)+\sin \left({\theta }_0+{\displaystyle \sum _{i=1}^3\Delta {\theta }_i}\right)\right]\end{array}$$

(9)

where $\Delta {\theta }_i={\omega }_i\delta t$ and $\Delta s=v\delta t$. It is obvious that Equation (9) has a unique analytical solution. As ${\theta }_i$ are often very small, especially with high control frequencies, in practice, the trigonometric functions in (9) can be approximated by the first-order Taylor expansion. Figure 14 and Figure 15 show the linear and angular velocity, i.e., $v,\omega$, generated in the long wall driving scenario.

It should be pointed out that the velocity and time can also be regarded as the decision variables in the optimization model, but this will consume much more time for solving the resulting optimization problem. Based on the consideration of real-time performance, the model is simplified, and the resulting model is verified to be effective and efficient. Due to the extremely low computational complexity of the proposed path planning and smoothing algorithms, the method can be effectively applied to human-involved dynamic driving scenarios without predicting the motion of the humans.

5. Simulations

The performance of the proposed method is evaluated in different simulated scenarios (see Figure 16): the forest scenario, the narrow channel scenario, the long wall scenario and the human-involved dynamic driving scenario. The algorithms are implemented in C++ and are executed on a laptop running Ubuntu 16.04 with an Intel i5-10200H CPU and 16 GB of RAM.

The TEB algorithm is also tested in the narrow channel scenario. Figure 17 shows the simulations of the TEB algorithm with different maximum global looking-ahead distances and the performance of the APOLLO algorithm in the narrow channel scenario. It can be seen that the vehicle is wandering around the gate and cannot follow the global path, while the APOLLO algorithm allows the vehicle to pass smoothly through the narrow channel of the same size. For the long wall scenario, the FALCO algorithm exhibits oscillatory behavior (Figure 1b), while the APOLLO algorithm can choose a relatively better direction without hesitation (Figure 16c).

Figure 18 records the CPU times of various planning algorithms in the long wall simulation environment. As we can see, APOLLO has the highest time efficiency and needs less than 2 ms on average to complete the local planning task. Compared with the state lattice-based FALCO local planner, the APOLLO planner does not need to pre-compute the path group for a distinct vehicle and only needs to change few parameters to adjust the size of the desired sector region, which makes APOLLO well adapted to different driving speeds of the automated vehicle.

Figure 19 shows the trajectory planning process of the automated vehicle in the human involved dynamic driving scenario. Figure 20 and Figure 21 show the linear and angular velocity, i.e., $v,\omega$, generated in this dynamic driving scenario.

Owning to its strong environmental adaptability and low computational complexity, the proposed APOLLO algorithm can also be used by the automated vehicle in a fleet to generate local collision-free trajectories. This algorithm can be applied in dynamic indoor environments, such as smart factories, shopping malls or airport stations, as well as high-speed outdoor transportation environments.

6. Conclusions

This paper presents APOLLO, a fast local smooth path generator for agile drives in unknown dynamic environments. The key advantage of the proposed local planner lies in its low computational complexity and its adaptivity. Therefore, this method can deal with human-involved dynamic driving scenarios and high-speed driving scenarios. Some of the variable parameters in the proposed algorithms can be adjusted manually to adapt to different driving scenarios.