一种面向泊车场景智能车辆轨迹规划方法

图1 混合A^*算法节点扩展示意图

Fig.1 Node expansion diagram of hybrid A^* algorithms

1.2 启发函数

对于启发式算法,启发函数的设计至关重要,在复杂环境下,如果以欧氏距离或者曼哈顿距离作为启发值,由于未考虑障碍物导致搜索效率不高.混合A^*算法定义两种启发值,并取两者中最大值为节点启发值.

首先考虑障碍物的约束,而不考虑车辆运动约束来计算启发值,其考虑的是允许全向运动情况下节点坐标 $n (x, y, φ) (φ$ 为航向角)到终点坐标n_end(x_end, y_end, φ_end)的最短距离,简记为h₁(n, n_end).

其次考虑车辆运动约束,而不考虑障碍物的启发值计算,其考虑的是无视障碍情况下节点n(x, y, φ)到终点n_end(x_end, y_end, φ_end)满足非完整约束情况下的最短路径,通常可以通过计算Reeds-Shepp曲线得到,简记为h₂(n, n_end).

最终,该节点的启发值可表示为

$h (n, n_{e n d}) = m a x (h_{1} (n, n_{e n d}) + h_{2} (n, n_{e n d}))$

(1)

2 基于改进混合A^*算法的路径搜索

传统混合A^*算法虽然解决了A^*算法无法满足车辆非完整性约束的问题,然而在实际应用中仍然存在诸多问题,例如规划时间长、路径平滑性差、安全性与求解效率难以平衡等缺陷.针对上述问题本文通过优化解析扩展,引入风险评估函数,进一步提升算法搜索效率同时保障搜索路径的安全性.

2.1 解析扩展

在进行节点扩展的过程中,无法精确地扩展至终点,因此在靠近终点时,需要考虑构造曲线等解析扩展的方式完成节点与终点的连接.传统混合A^*算法通过构造Reeds-Shepp曲线,进一步进行碰撞检测,若不存在碰撞情况则完成连接.然而在狭窄泊车场景中,该曲线构造连接方式大多难以通过碰撞检测,即使在空旷场景下不会出现该问题,多数Reeds-Shepp曲线会存在1~2个行进方向档位切换点,这并不是合理的驾驶行为.

为解决上述问题,本文尝试在节点坐标n(x, y, φ)与终点坐标n_end(x_end, y_end, φ_end)之间采用一段曲率小于车辆可行驶的最大曲率的圆弧衔接直线实现.具体解析以终点为原点的笛卡尔坐标系下展开,记连接终点的直线段长为l₀,衔接的圆弧长为l₁,如图2所示.其中:红色箭头为终点位姿;绿色箭头为连接的节点位姿.

图2

图2 终点连接示意图

Fig.2 Diagram of destination link

记以终点为原点的笛卡尔坐标系下节点 $n 坐标为 n' (x', y', φ'), 并记圆弧的曲率为 κ,$ 可得到以下关系式:

$\begin{matrix} \begin{array}{l} x' = l_{0} + \frac{s i n (κ l_{1})}{κ} \\ y' = \frac{1 - c o s (κ l_{1})}{κ} \\ φ' = κ l_{1} \end{array}\} \end{matrix}$

(2)

经过变换后可以根据n'(x', y', φ')求得l₀,l₁和κ的表达式:

$\begin{matrix} \begin{array}{l} κ = \frac{1 - c o s φ'}{y'} \\ l_{1} = \frac{φ' y'}{1 - c o s φ'} \\ l_{0} = x - \frac{y' s i n φ'}{1 - c o s φ'} \\ κ \leq κ_{m a x}, l_{0} \geq 0, l_{1} \geq 0 \end{array}\} \end{matrix}$

(3)

式中:κ_max为路径允许的最大曲率.

相比于Reeds-Shepp曲线,该种构造方式更加符合人类驾驶行为,更加适用于泊车场景,能够使得算法容易通过终点连接的碰撞检测,从而加快算法收敛.

2.2 风险评估函数

多数情况下由混合A^*算法搜索得到的路径比较靠近障碍物,解决这个问题的一种方式是进行膨胀处理和更严格的碰撞检测,但在狭窄环境下这样的处理可能会导致无法搜索出可行解.

为了在最大保证安全限度的同时保证可解性,对节点扩展的优先级设计进行优化.原算法对于节点n(x,y,φ)将(n,f(n))放入优先级队列,根据评估函数值f(n)最小的节点进行扩展.本文对于(n,f(n))进行扩展为(n,f(n),d(n))放入优先级队列,d(n)为风险评估函数,可表示为

$\begin{matrix} d (n) = \{\begin{array}{l} d_{m a x} - d_{o}, & d_{o} \leq d_{m a x} \\ 0, & 其他 \end{array} \end{matrix}$

(4)

式中:d_o为节点距离最近障碍物的距离值;d_max为安全距离.

优先级方面优先考虑d(n)最小,在d(n)相近时考虑f(n)最小的节点进行扩展.这样设计保证规划过程能与障碍物保持安全距离,同时当面临狭窄区域时不会阻碍规划.

3 基于二次规划的轨迹生成

由于运动基元根据前轮偏角进行离散采样得到,所以通过上述方法得到的初始路径仍然存在路径曲率不连续的问题,需要进行相应的路径后处理来生成轨迹.

3.1 路径平滑

在泊车场景中,搜索路径存在档位切换的情况,需要对路径按照行进方向进行分段平滑处理,每段路径的平滑处理如下.

在初始路径上等距采样m个路径点,记为 $p_{0}^{r e f} (x_{0}^{r e f}, y_{0}^{r e f})$ , $p_{1}^{r e f} (x_{1}^{r e f}, y_{1}^{r e f})$ ,…, $p_{m - 1}^{r e f} (x_{m - 1}^{r e f}, y_{m - 1}^{r e f})$ ,处理后得到 $p_{0} (x_{0}, y_{0})$ , $p_{1} (x_{1}, y_{1})$ ,…, $p_{m - 1} (x_{m - 1}, y_{m - 1})$ ,所形成的路径需具备一定平滑性.考虑离散点一阶差分近似相等即 $‖ p_{i + 1} - p_{i} ‖ \approx ‖ p_{i} - p_{i - 1} ‖$ 的情况下,离散路径点的二阶差分量 $‖ p_{i + 1} + p_{i - 1} - 2 p_{i} ‖$ 会随着路径曲率增大而增大,且当3点位于一条直线上时取得最小,因而该项可从一定程度上反映路径的平滑性,可以以此建立优化函数如下:

$\begin{array}{l} J_{p a} = ω_{1} \overset{m - 1}{\sum_{i = 1}} [(x_{i} - x_{i - 1})^{2} + (y_{i} - y_{i - 1})^{2}] + \\ ω_{2} \overset{m - 2}{\sum_{i = 1}} [(x_{i + 1} + x_{i - 1} - 2 x_{i})^{2} + \\ (y_{i + 1} + y_{i - 1} - 2 y_{i})^{2}] \end{array}$

(5)

式中:ω₁,ω₂均为优化目标的权重系数;第1项优化目标使得离散点一阶差分尽可能相近同时路段长度尽可能短;第2项优化目标使得路径平滑性增强.

考虑优化过程中起始点和终止点的位置以及朝向均不变,可以得出等式约束:

$\begin{array}{l} \begin{matrix} x_{i} = x_{i}^{r e f}, y_{i} = y_{i}^{r e f} \end{matrix} \\ i = 0, 1, m - 2, m - 1 \end{array}$

(6)

考虑优化过程中避免与障碍物发生碰撞,需要限定离散路径点坐标,具体数值可由初始路径点距离最近障碍物的距离值作为参考确定,该不等式约束可表示为

$\begin{array}{l} \begin{matrix} l_{x i} \leq x_{i} \leq l'_{x i}, l_{y i} \leq y_{i} \leq l'_{y i}, \end{matrix} \\ i = 2, 3, \dots, m - 3 \end{array}$

(7)

式中: $l_{x i}, l'_{x i}, l_{y i} 和 l'_{y i}$ 为优化变量的边界.

考虑车辆行驶的运动学性质,路径曲率存在一个最大曲率的限制,假定离散点一阶差分近似相等,构建曲率约束如下:

$\begin{array}{l} ‖ p_{i + 1} + p_{i - 1} - 2 p_{i} ‖ = \\ ‖ (p_{i + 1} - p_{i}) - (p_{i} - p_{i - 1}) ‖ \approx \frac{‖ p_{i} - p_{i - 1} ‖^{2}}{R} \\ ‖ p_{i + 1} + p_{i - 1} - 2 p_{i} ‖ \leq ‖ p_{i} - p_{i - 1} ‖^{2} κ_{m a x} \end{array}$

(8)

其中:R为p_i点对应的曲率半径.

式(8)两边同时平方,展开得到:

$\begin{array}{l} (x_{i + 1} + x_{i - 1} - 2 x_{i})^{2} + (y_{i + 1} + y_{i - 1} - 2 y_{i})^{2} < \\ κ_{m a x}^{2} [(x_{i} - x_{i - 1})^{2} + (y_{i} - y_{i - 1})^{2}]^{2} \\ i = 1,2, \dots, m - 2 \end{array}$

(9)

综上,路径平滑问题可以建模成以式(5)为优化目标,式(6)~(7)和(9)为约束的二次规划问题.约束项式(9)为非线性约束,导致原问题难以求解,对此可以采用序列二次规划的方式进行求解,按照当前迭代点将原问题处理成凸二次规划问题,不断求解迭代直至收敛.

3.2 速度规划

在进行速度规划时,对于存在档位切换的路径同样进行分段处理的方式,在档位切换处,车辆处于静止状态.对正向行驶的速度规划问题予以阐述,反向行驶的速度规划问题可以类推之.考虑限定时间t_k内车辆行驶路径长度为s的路径,起始点的车辆处于静止状态.

在时域 $[0, t_{k}] 上以等时间间隔 Δ t 采样 k 个离散时间点 {t_{z} | t_{z} = z Δ t}, 其中 z = 1, 2, \dots, k . 假定在 t_{z} 到 t_{z + 1}$ 时间段内车辆的加加速度近似不变,则可以列出如下等式:

$\begin{matrix} \begin{array}{l} s_{z + 1} = s_{z} + {\overset{\cdot}{s}}_{z} Δ t + \frac{1}{2} {\ddot{s}}_{z} Δ t^{2} + \frac{1}{6} {\overset{\dots}{s}}_{z} Δ t^{3} \\ {\overset{\cdot}{s}}_{z + 1} = {\overset{\cdot}{s}}_{z} + {\ddot{s}}_{z} Δ t + \frac{1}{2} {\overset{\dots}{s}}_{z} Δ t^{2} \\ {\ddot{s}}_{z + 1} = {\ddot{s}}_{z} + {\overset{\dots}{s}}_{z} Δ t \end{array}\} \end{matrix}$

(10)

式中: $s_{z} 为 t_{z} 时刻下车辆行驶的距离; {\overset{\cdot}{s}}_{z} 为 t_{z} 时刻下车辆的速度; {\ddot{s}}_{z} 为 t_{z} 时刻下车辆的加速度; {\overset{\dots}{s}}_{z} 为 t_{z}$ 时刻下车辆的加加速度.

为了使得车辆能够尽可能快地行驶目标距离,需要建立优化目标 $\overset{k}{\sum_{z = 1}} (s - s_{z})^{2}$ 使得终端状态以及过程状态与s尽可能接近.除此之外,出于驾驶舒适以及平滑性的角度考虑,还需添加有关加速度以及加加速度的优化目标,具体的优化函数如下:

$\begin{array}{l} J_{s p e} = ω_{s} \overset{k}{\sum_{z = 1}} (s - s_{z})^{2} + ω_{\ddot{s}} \overset{k}{\sum_{z = 1}} {\ddot{s}}_{z}^{2} + ω_{\overset{\dots}{s}} \overset{k - 1}{\sum_{z = 1}} {\overset{\dots}{s}}_{z}^{2} \\ s_{z} \in [0, s], {\overset{\cdot}{s}}_{z} \in [0, v_{m a x}], \\ {\ddot{s}}_{z} \in [- a_{m a x}, a_{m a x}], {\overset{\dots}{s}}_{z} \in [- j_{m a x}, j_{m a x}] \end{array}$

(11)

式中: $ω_{s}, ω_{\ddot{s}} 和 ω_{\overset{\dots}{s}}$ 均为优化目标的权重系数;v_max为车辆所允许行驶的最大速度;a_max为车辆所允许行驶的最大加速度;j_max为车辆所允许行驶的最大加加速度.

定义状态量 $S_{z} = {[\begin{array}{l} s_{z} & {\overset{\cdot}{s}}_{z} & {\ddot{s}}_{z} \end{array}]}^{T}$ 以及控制量 $u_{z} = {\overset{\dots}{s}}_{z},$ 式(11)可写为

$\begin{array}{l} J_{spe} = \overset{k}{\sum_{z = 1}} {(S_{z} - S_{ref})}^{T} Q (S_{z} - S_{ref}) + \overset{k - 1}{\sum_{z = 0}} u_{z} R u_{z} \\ S_{r e f} = [\begin{array}{l} s \\ 0 \\ 0 \end{array}], Q = [\begin{array}{l} ω_{s} & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & ω_{\ddot{s}} \end{array}], R = ω_{\overset{⃛}{s}} \end{array}$

(12)

根据式(12),状态量S_z以及控制量u_z的约束可表示为

$\begin{array}{l} \begin{matrix} L \leq S_{z} \leq U, - j_{m a x} \leq u_{z} \leq j_{m a x} \end{matrix} \\ L = [\begin{array}{l} 0 \\ 0 \\ - a_{m a x} \end{array}], U = [\begin{array}{l} s \\ v_{m a x} \\ a_{m a x} \end{array}] \end{array}$

(13)

将式(10)以状态量S_z以及控制量u_z表示,可以得到:

$\begin{array}{l} S_{z + 1} = A S_{z} + B u_{z} \\ A = [\begin{array}{l} 1 & Δ t & \frac{1}{2} Δ t^{2} \\ 0 & 1 & Δ t \\ 0 & 0 & 1 \end{array}], B = [\begin{array}{l} \frac{1}{6} Δ t^{3} \\ \frac{1}{2} Δ t^{2} \\ Δ t \end{array}] \end{array}$

(14)

由于车辆在起始点处于静止状态,所以可以得到 $S_{0} = {[\begin{array}{l} 0 & 0 & 0 \end{array}]}^{T}$ ,则显然 $S_{1}$ , $S_{2}$ ,…, $S_{k}$ 可被 $u_{0}$ , $u_{1}$ ,…, $u_{k - 1}$ 以及 $S_{0}$ 线性表示,优化函数 $J_{s p e}$ 是关于 ${u_{z}}_{z = 0}^{k - 1}$ 的二次型函数,求解该线性约束二次规划问题得到 ${u_{z}^{*}}_{z = 0}^{k - 1}$ ,由此可相应求出 ${S_{z}^{*}}_{z = 1}^{k}$ ,完成速度规划.

4 实验结果与分析

通过设计仿真和实车试验,验证本文所提方法的有效性和实用性.实验软件环境为Ubuntu 16.04,并基于机器人操作系统(ROS)进行C++算法开发,所使用的计算平台CPU型号为i7-10510U,内存大小为8 GB.

4.1 仿真实验

4.1.1 路径搜索

路径搜索实验所涉及算法的参数如表1所示.

表1 路径搜索算法相关参数

Tab.1 Parameters of search-based path planning algorithms

参数	取值
位置离散分辨率/m	0.5
角度离散分辨率/(°)	7.5
前轮转角离散数	5.0
步长/m	0.8

本次仿真实验设置了狭窄环境泊车入位的场景,仿真车辆长度为5 m,车辆宽度为2.2 m,车辆最小转向半径为6 m,车位宽度为2.5 m,过道宽度为5 m.在该场景下,实现文献[15]所述方法以及本文所述方法,进行多次实验并对比实验数据,结果如图3所示.

图3

图3 路径搜索实验结果

Fig.3 Results of search-based path planning experiment

在该实验场景下,文献[15]所述方法相较于本文方法存在碰撞风险,如图3(a)中所示红圈处,本文方法所采用的风险评估函数会优先扩展安全距离外的节点,提高结果的安全性.处理该结果得到的数据如表2所示.

表2 路径搜索实验数据

Tab.2 Data of search-based path planning experiment

算法	路径长度/m	拓展节点	档位切换	搜索时间/ms
文献[15]算法	13.5	1769	3	84
本文算法	13.7	478	3	18

由表2可知,本文方法相较于文献[15]所述方法能够显著降低所需拓展节点的数量,从而减少搜索时间,路径搜索时间缩短了78.6%.此外,本文还选取不同的泊车起点进行仿真实验,结果显示平均路径搜索时间减少56.3%,进一步验证本文方法的实时性.

4.1.2 路径平滑

路径平滑实验所涉及算法的参数如表3所示.

表3 路径平滑算法相关参数

Tab.3 Parameters of path smoothing algorithm

参数	取值
离散状态点数	50
一阶差分权重	1
最大曲率	5
二阶差分权重	1000

场景及车辆参数同上述实验,从路径搜索的实验结果中选取多组数据,分别运用文献[16]所述方法以及本文方法进行测试,发现本文所述路径平滑方法耗时5~20 ms,满足实时性的要求.在实验结果中选取两组路径平滑的效果图如图4所示.

图4

图4 路径平滑实验结果

Fig.4 Results of path smoothing experiment

图5

图5 路径曲率结果图

Fig.5 Results of path curvature

从图4中可以看出,当原路径段为恒定曲率的曲线时,平滑效果比较有限,而当原路径段存在曲率变化的情况时,路径平滑会取得一定效果如图4中红色虚线框内区域所示.为比较文献[16]所述方法与本文方法效果,计算离散路径点的曲率信息,如图5所示.

对比图5中平滑前后的曲率曲线可知,平滑处理后的路径曲率的变化更加合理.由于路径平滑工作会根据路径档位的变化而分段进行,所以在档位切换点的曲率突变并没有得到优化,但事实上,档位切换点时的车辆也需要处于静止状态,因而此时曲率的突变带来的影响相对较小.通过对比本文方法与文献[16]所述方法的曲率曲线可以看出, 两种方法在曲率的平滑处理上均有效果,但是文献[16]由于未考虑曲率约束,结果存在超出曲率约束的情况,这使得某些情景下路径的可行性受到影响.对于路径整体的平滑性,本文应用指标J_κ评估:

$\begin{matrix} J_{κ} = \frac{\int_{0}^{s_{p a}} κ^{2}}{s_{p a}} \approx \frac{\overset{w}{\sum_{j = 1}} κ_{j}^{2}}{w} \end{matrix}$

(15)

式中:s_pa为路径总长度;w为将路径离散化后点的个数;κ_j为各个离散点处对应的曲率.

路径处理前后平滑性指标J_κ如表5所示.

表5 路径平滑前后平滑性指标 $J_{κ}$ 对比

Tab.5 Comparison of $J_{κ}$ between original path and smoothed path

实验组	平滑前的J_κ	本文方法的J_κ	文献[16]的J_κ
实验一	0.01707	0.01383	0.01372
实验二	0.02042	0.01434	0.01545

通过表5可以看出,路径平滑性指标在平滑处理过后具有较为明显的下降,本文方法与文献[16]对于路径整体的平滑效果并无较大差异.但是本文方法对于路径曲率的约束较为显著,相较文献[16]更具可行性.

4.1.3 速度规划

在路径平滑后进行速度规划,关于速度规划中二次规划问题的各参数如表6所示.

表6 速度规划算法相关参数

Tab.6 Parameters of speed planning algorithms

参数	取值
离散状态点数	24
最大速度/(m·s^-1)	1.0
最大加速度/(m·s^-2)	1.0
最大加加速度/(m·s^-3)	0.5
距离权重	10000
加速度权重	10
加加速度权重	10

以一段路径为例:路径长度s为9.25 m,限定时间t_k取12 s,规划耗时为6 ms.规划结果如图6所示.

图6

图6 速度规划实验结果

Fig.6 Results of speed planning experiment

在该速度规划的问题中,将离散点间加加速度作为定值来构建优化问题.由图6可知,加加速度作为状态方程式(15)中的控制量存在一定程度跳变,根据状态方程式(15)可以递推得到连续的状态量,即加速度、速度以及路程,同时速度及路程还具备较好的平滑性.另外,该速度规划算法可对速度、加速度以及加加速度加以限制,图6中所示的各变量曲线均能被限定在给定值,进一步说明该规划算法的可行性.

4.2 实车试验

为进一步验证本文方法的实用性,本文在实际的泊车场景开展实车验证,如图7所示.首先搭建实车平台,以Kinect V2作为主传感器获取环境信息,完成场景下感知以及定位工作并进行规划控制,并利用机器人操作系统平台上的可视化工具Rviz实现实验结果的可视化.

图7

图7 实车试验

Fig.7 Test of real-world vehicle

图7(a)所展示的为本次实车试验的车辆平台以及实验环境,实验在一处地下停车场进行,背景中的车位为本次实验所计划停驻的车位.图7(b)则是在ROS平台上的可视化工具Rviz下显示的实验结果,黑色区域是通过建图得到由点云表示的环境障碍物信息;灰色线条为可视化平台中表示尺度的栅格,每个栅格边长为1 m;红色框则辅助标出了本次实验环境下的3个并排车位,其中中间方框即为本次实验需停泊的车位;绿色曲线为车辆进行规划控制后,车辆所行驶的路径结果.

在该环境下,文献[15]所述方法的搜索节点为 1324 个,总耗时为76 ms,而本文方法搜索节点数为218个,初始路径搜索以及轨迹生成总耗时为 54 ms.可以看出,虽然本文方法相较于文献[15]增添了路径平滑和速度规划等轨迹生成的工作,但得益于本文方法在路径搜索方面的改进,并不会影响规划结果的实时性.规划结果经控制执行后,文献[15]所述方法实验结果在终止处有0.143 m的横向误差,1.537° 的角度误差,本文方法实验结果在终止处有0.078 m的横向误差,0.946° 的角度误差.可以得到,本文方法所述的路径平滑和速度规划这些轨迹生成的工作提升了规划结果的可行性,对车辆控制更加友好,进一步验证了方法的实用性.

5 结语

本文提出了一种面向泊车场景智能车辆轨迹规划方法,该方法包括初始路径搜索以及轨迹生成两个步骤.在基于混合A^*算法实现的初始路径搜索中,本文优化解析扩展,加快算法收敛,同时引入风险评估函数,保证路径的安全性.在基于二次规划的轨迹生成中,本文分别将离散点路径平滑以及速度规划建立为二次规划问题进行求解.最后,设计仿真实验进行验证,验证本文方法的有效性,并通过实车实验说明可行性.结果表明,本文方法提升了路径搜索实时性与安全性,路径平滑与速度规划都具备良好的平滑性.下一步工作将致力于研究动态环境下车辆泊车轨迹规划的问题.

参考文献

原文顺序

文献年度倒序

文中引用次数倒序

被引期刊影响因子

[1]

胡兵, 杨明, 郭林栋, 等.

基于地面快速鲁棒特征的智能车全局定位方法

[J]. 上海交通大学学报, 2019, 53(2): 203-208.

Bing

, YANG

Ming

, GUO

Lindong

, et al.

Global localization for intelligent vehicles using ground SURF

[J]. Journal of Shanghai Jiao Tong University, 2019, 53(2): 203-208.

[2]

WERLING

, ZIEGLER

, KAMMEL

, et al.

Optimal trajectory generation for dynamic street scenarios in a Frenét Frame

[C]// 2010 IEEE International Conference on Robotics and Automation. Anchorage, AK, USA: IEEE, 2010: 987-993.

[3]

CHEN

S T

, JIAN

Z Q

, HUANG

Y H

, et al.

Autonomous driving: Cognitive construction and situation understanding

[J]. Science China Information Sciences, 2019, 62(8): 1-27.

[4]

, YANG

, ZHANG

A two-level path planning method for on-road autonomous driving

[C]// 2012 Second International Conference on Intelligent System Design and Engineering Application. Sanya, China: IEEE, 2012: 661-664.

DOI:10.1016/j.cad.2009.12.007 URL [本文引用: 1]

[5]

MAEKAWA

, NODA

, TAMURA

, et al.

Curvature continuous path generation for autonomous vehicle using B-spline curves

[J]. Computer-Aided Design, 2010, 42(4): 350-359.

[6]

PIVTORAIKO

, KNEPPER

R A

, KELLY

Differentially constrained mobile robot motion planning in state lattices

[J]. Journal of Field Robotics, 2009, 26(3): 308-333.

DOI:10.1002/rob.v26:3 URL [本文引用: 1]

[7]

DOLGOV

, THRUN

, MONTEMERLO

, et al.

Path planning for autonomous vehicles in unknown semi-structured environments

[J]. The International Journal of Robotics Research, 2010, 29(5): 485-501.

DOI:10.1177/0278364909359210 URL [本文引用: 2]

We describe a practical path-planning algorithm for an autonomous vehicle operating in an unknown semi-structured (or unstructured) environment, where obstacles are detected online by the robot’s sensors. This work was motivated by and experimentally validated in the 2007 DARPA Urban Challenge, where robotic vehicles had to autonomously navigate parking lots. The core of our approach to path planning consists of two phases. The first phase uses a variant of A* search (applied to the 3D kinematic state space of the vehicle) to obtain a kinematically feasible trajectory. The second phase then improves the quality of the solution via numeric non-linear optimization, leading to a local (and frequently global) optimum. Further, we extend our algorithm to use prior topological knowledge of the environment to guide path planning, leading to faster search and final trajectories better suited to the structure of the environment. We present experimental results from the DARPA Urban Challenge, where our robot demonstrated near-flawless performance in complex general path-planning tasks such as navigating parking lots and executing U-turns on blocked roads. We also present results on autonomous navigation of real parking lots. In those latter tasks, which are significantly more complex than the ones in the DARPA Urban Challenge, the time of a full replanning cycle of our planner is in the range of 50—300 ms.

[8]

KUWATA

, FIORE

G A

, TEO

, et al.

Motion planning for urban driving using RRT

[C]// 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice, France: IEEE, 2008: 1681-1686.

[9]

BANZHAF

, NIENHÜSER

, KNOOP

, et al.

The future of parking: A survey on automated valet parking with an outlook on high density parking

[C]// 2017 IEEE Intelligent Vehicles Symposium. Los Angeles, CA, USA: IEEE, 2017: 1827-1834.

[10]

王旭东, 王春香, 杨明, 等.

一种基于环视相机的自动泊车方法

[J]. 上海交通大学学报, 2013, 47(7): 1077-1081.

WANG

Xudong

, WANG

Chunxiang

, YANG

Ming

, et al.

Automatic parking based on bird’s eye view cameras

[J]. Journal of Shanghai Jiao Tong University, 2013, 47(7): 1077-1081.

[11]

LIN

L T

, ZHU

J J

Path planning for autonomous car parking

[C]// Proceedings of ASME 2018 Dynamic Systems and Control Conference. Atlanta, Georgia, USA: ASME, 2018: 1-10.

[12]

MOON

, BAE

, CHA

J G

, et al.

A trajectory planning method based on forward path generation and backward tracking algorithm for Automatic Parking Systems

[C]// 17th International IEEE Conference on Intelligent Transportation Systems. Qingdao, China: IEEE, 2014: 719-724.

[13]

齐尧, 徐友春, 李华, 等.

一种基于改进混合A^*的智能车路径规划算法

[J]. 军事交通学院学报, 2018, 20(8): 85-90.

Yao

, XU

Youchun

, LI

Hua

, et al.

Hybrid A^* algorithm-based path planning for intelligent vehicle

[J]. Journal of Military Transportation University, 2018, 20(8): 85-90.

[14]

陈鑫鹏, 徐彪, 胡满江, 等.

一种基于等步长分层拓展的混合A^*路径规划方法

[J]. 控制与信息技术, 2021(1): 17-22.

CHEN

Xinpeng

, XU

Biao

, HU

Manjiang

, et al.

A hybrid-A^* path planning method based on equal step hierarchical expansion

[J]. Control and Information Technology, 2021(1): 17-22.