面向动态障碍物场景的自主代客泊车路径规划

张家旭 王志伟 郭 崇 赵 健

(1吉林大学汽车仿真与控制国家重点实验室, 长春 130022)(2中国第一汽车集团有限公司智能网联研发院, 长春 130011)

泊车入位一直被视为最具挑战性的驾驶环节，随着城市车辆保有量增加，每辆车所分配到的车位更加紧凑，进一步增加了泊车难度[1].从20世纪90年代开始，相应的泊车辅助功能研究逐渐增多，目前被动泊车辅助功能已得到广泛应用，近年来逐渐出现了自动泊车等主动泊车辅助功能.代客泊车系统是自动驾驶和自动泊车技术的优势结合[2-3]，其将停车位搜索和泊车操作集成，能够代替驾驶员完成下车点到泊车位之间的驾驶任务[4].

与传统自动泊车系统相比，代客泊车系统需应对的场景更加复杂，其路径规划更具挑战性.代客泊车系统路径规划可分为全局路径规划、局部路径优化和泊车路径规划3个阶段.其中，全局路径规划和局部路径规划是代客泊车系统路径规划的核心.全局路径规划首先在已知静态地图的基础上规划出一条引导车辆从下车点到期望泊车起始位置的全局路径，其核心问题是地图构建和路径搜索，全局路径规划多以无向图建立停车场静态地图，以启发式算法实现路径搜索.局部路径优化着重于路径的可跟随性优化，基于全局路径生成一条符合车辆运动学原理的路径.

目前代客泊车系统路径规划的研究多集中于基于先验停车场地图信息的全局规划及局部路径优化.文献[5]针对自主代客泊车场景中汽车转弯运动过程，采用栅格搜索方法规划该场景自主代客泊车路径.文献[6]首先利用A*算法规划全局自主代客泊车路径，随后利用改进动态窗口算法规划局部自主代客泊车路径.文献[7]以栅格化自主代客泊车地图为基础，采用优化方法规划全局和局部自主代客泊车路径.文献[8]融合人工势场法与随机采样树算法来规划全局自主代客泊车路径.文献[9]首先利用批处理先验知识树算法构建全局自主代客泊车路径，再利用滚动优化算法构建局部自主代客泊车路径.文献[10]基于Dijkstra算法规划动态场景的全局自主代客泊车路径.文献[11]以多车协同自主代客泊车场景为基础，将自主代客泊车路径规划问题转化为最优控制问题，并提出了一种基于初始化的计算框架来降低计算负担.

本文针对存在动态障碍物场景的自主代客泊车路径规划问题，提出一种基于D*算法和动态窗口法的自主代客泊车路径规划方法.首先，利用栅格扫描算法构建自主代客泊车场景的静态环境地图，并采用Dijkstra算法实时更新动态障碍物影响的局部静态环境地图信息.随后，利用D*算法将自主代客泊车场景静态环境地图转化为静态路径场，得到原始全局自主代客泊车路径.最后，采用动态窗口法和圆弧-直线组合方式规划最优的局部自主代客泊车路径.

1 环境地图构建

基于外部环境感知传感器信息准确实时地构建动态环境地图是规划自主代客泊车路径的重要基础.本节首先利用栅格扫描算法构建自主代客泊车场景的静态环境地图，再结合自主代客泊车场景中的动态障碍物信息，利用Dijkstra算法实时更新静态环境地图，实现自主代客泊车场景动态环境地图的快速构建.栅格扫描算法利用图1所示的节点邻域模板对自主代客泊车场景的栅格地图从左到右逐行正向扫描和从右到左逐行反向扫描后来获得任意可行节点到其最近障碍物节点的欧氏距离[12-13].

图1 节点p的邻域模板

定义如图1所示的节点p与其相邻节点q1,q2,…,q8间的平方欧氏距离偏差d(p,q)和相对坐标偏差M(p,q)分别为

(1)

(2)

式中，C(q)=(Cx(q),Cy(q))为节点p的相邻节点q∈{q1,q2,…,q8}的相对坐标.

①从左到右逐行正向扫描节点p.设置f(p)=∞，对于任意的相邻节点q={q1,q2,q3,q4}，如果f(p)>f(q)+d(p,q)，更新f(p)=f(q)+d(p,q)，C(p)=C(q)+M(p,q).

②从右到左逐行反向扫描节点p.对于任意的相邻节点q={q5,q6,q7,q8}，如果f(p)>f(q)+d(p,q)，更新f(p)=f(q)+d(p,q)，C(p)=C(q)+M(p,q).

当自主代客泊车场景中出现动态障碍物时，将障碍物占据的节点插入到Dijkstra算法的开放列表中，并且从障碍物节点开始逐层更新自主代客泊车场景的静态环境地图，直到扩展到出现的动态障碍物节点不为当前节点的最近障碍物节点为止.自主代客泊车场景下动态环境地图构建流程如下：

①将障碍物节点插入到开放列表中，将其欧氏距离设置为0.

②重复以下过程直到开放列表为空.

③将开放列表中最小欧氏距离的节点取出作为当前节点，将其移至封闭列表.

④检查当前节点的8个相邻节点.若相邻节点为障碍物节点或已在封闭列表中，不操作；若相邻节点不在开放列表中且当前节点能减小其欧氏距离，将其加入开放列表；若相邻节点在开放列表中且当前节点能减小相邻节点欧氏距离，更新相邻节点的欧氏距离.

2 全局路径规划

全局自主代客泊车路径规划的任务是根据动态环境地图信息规划出从起始点到目标点的无碰撞路径.本节采用对动态环境具有自适应能力的D*算法规划全局自主代客泊车路径.采用开放列表存储待扩展的节点，K(X)和H(X)分别表示节点X的最小成本和当前成本，将从未列入开放列表中的节点状态设置为NEW，将历史列入开放列表且当前不在开放列表中的节点状态设置为CLOSED，将当前在开放列表中的节点状态设置为OPEN，将满足K(X)

图2 D*算法主要流程

该算法通过5个模块维护一个开放列表，实现动态环境下的全局自主代客泊车路径规划.模块1通过相邻节点优化当前节点X的H(X)；模块2为内嵌的Dijkstra算法，用于将自主代客泊车场景静态环境地图转化为一个静态路径场；模块3用于将父节点当前成本变化信息传递给其子节点，逐渐形成的传递路径与静态路径场包含的原始规划路径方向相反；模块4和模块5用于处理当前节点X相邻且不相关节点，通过将LOWER状态的节点插入到开放列表中来激活模块2，使插入到开放列表中的节点影响程度形成波浪传播到其相邻节点，而不是仅影响某个节点.

如图2所示，D*算法首先采用模块2描述的Dijkstra算法，从目标节点开始对自主代客泊车场景静态环境地图进行反向搜索，确定出每个节点的父节点，从而将自主代客泊车场景静态环境地图转化为一个静态路径场.如图3所示，利用模块2描述的Dijkstra算法可规划出从节点31到目标节点22的最短原始路径为：节点31→节点25→节点20→节点21→节点22.当自主代客泊车场景静态环境地图中原始路径节点20变为障碍物节点时，需要在已得到的静态路径场基础上，采用D*算法重新规划从节点31到目标节点22的路径.

图3 基于D*算法的原始路径规划示意图

如图4所示，由于节点25的父节点20变为障碍物节点，节点25状态变为RAISE并且被重新插入到开放列表中，从开放列表中取出节点25来激活模块1，使得其父节点由障碍物节点20变为节点19.对于重规划路径节点19，其父节点20变为障碍物节点，节点19状态变为RAISE并且被重新插入到开放列表中，从开放列表中取出节点19来激活模块3和模块5，分别将节点19的子节点25和节点19的不相关节点13插入到开放列表中.随后，从开放列表中取出不相关节点13来激活模块2，使得节点19的父节点变为节点13.对于重规划路径节点13，其父节点20变为障碍物节点，节点13状态变为RAISE并且被重新插入到开放列表中，从开放列表中取出节点13来激活模块3，将节点13的子节点7状态变为RAISE并插入到开放列表中.对于重规划路径节点7，采用节点13的处理方式将节点2状态变为RAISE并插入到开放列表中，由此原始规划的子路径节点2→节点7→节点13→节点20上的节点状态均变为RAISE.从开放列表中取出节点2来激活模块5，将节点3插入到开放列表中.从开放列表中取出节点3来激活模块2，将节点2父节点变为节点3.随后，采用节点3的处理方式将节点7的父节点变为节点2，将节点13的父节点变为节点7，从而得到重规划的路径为：节点31→节点25→节点19→节点13→节点7→节点2→节点3→节点4→节点5→节点12→节点17→节点22.

图4 基于D*算法的重规划路径示意图

通过上述分析可知，D*算法运行过程需要反复执行开放列表插入节点操作和提取最小节点操作，这2类操作的运行时间是影响D*算法搜索效率的重要因素.如图5所示，设N为数组元素数量，采用节点结构体数组存储自主代客泊车场景静态环境地图中的节点信息，并采用指针数组快速高效地操作节点结构体数组元素.为了减小指针数组插入节点操作和提取最小节点操作的运行时间，本节利用指针数组构建优先队列数据结构来实现D*算法中的开放列表，使得指针数组插入节点操作和提取最小节点操作最坏情形运行时间均为O(logN)[15].

图5 D*算法采用的数据结构

3 局部路径规划

局部自主代客泊车路径规划的任务是在全局路径规划的基础上，综合考虑汽车运动学约束和汽车机械约束，逐步规划出可安全引导汽车进入目标泊车位的路径.本节采用动态窗口法和圆弧-直线组合方式规划局部自主代客泊车路径.如图6所示，假设汽车自主代客泊车过程中所有车轮无侧偏地绕同一瞬时圆心做圆周运动，由此得到汽车运动学方程为[16]

图6 汽车运动学模型

(3)

式中，xr和yr分别为汽车后轴中点横坐标和纵坐标；φ为汽车横摆角；vr和δf分别为汽车后轴中点速度和汽车前轮转向角；L为汽车轴距.

采用前向欧拉法将式(3)离散化为

(4)

式中，h为计算步长.

若汽车允许的最大加速度和最大前轮转向角速度分别为amax和ωmax，依据k时刻汽车后轴中点速度预测设定值vr,p∈[Vmin,Vmax]和汽车前轮转向角预测设定值δf,p∈[-δmax,δmax]，可得k时刻汽车后轴中点速度和汽车前轮转向角控制输入为

(5)

(6)

结合式(4)和式(5)、(6)，可得到k时刻到k+N时刻的汽车预测轨迹.若均匀采样汽车后轴中点速度可行空间[Vmin,Vmax]和汽车前轮转向角可行空间[-δmax,δmax]，可得到一簇k时刻到k+N时刻的汽车预测轨迹[17].

本节首先融合几何边界碰撞检测方法和栅格空间覆盖枚举方法，判断汽车预测轨迹是否为无碰撞轨迹，再利用评价函数从无碰撞的汽车预测轨迹中筛选出最优汽车预测轨迹，并将最优汽车预测轨迹对应的k时刻汽车后轴中点速度和汽车前轮转向角控制输入作为局部自主代客泊车路径规划的控制指令.如图7所示，为了提高汽车碰撞检测效率，首先采用简洁高效的几何边界碰撞检测方法判断汽车中心圆包络是否与障碍物存在覆盖.若汽车中心圆包络覆盖障碍物节点，再通过栅格空间覆盖枚举方法判断汽车占据的栅格节点是否为障碍物节点，进一步提高汽车碰撞检测精度.其中，基于汽车后轴中点坐标得到的汽车中心点坐标为

图7 汽车中心圆包络

(7)

式中，xO1(k)、yO1(k)分别为汽车中心点横坐标和纵坐标；Lc和Lr分别为汽车长度和汽车后悬长度；T为旋转变换矩阵，可表示为

(8)

针对无碰撞的汽车预测轨迹，设计评价函数J(vr,p,δf,p)来筛选最优汽车预测轨迹，即

J(vr,p,δf,p)=αJ1(vr,p,δf,p)+βJ2(vr,p,δf,p)

(9)

式中，J1(vr,p,δf,p)为预测终止时刻汽车横摆角与全局路径目标点处切线方向之间的角度偏差惩罚项；J2(vr,p,δf,p)为预测终止时刻汽车预测轨迹与障碍物之间的最近距离惩罚项；α和β分别为加权系数.

在利用动态窗口法将汽车引导到目标泊车位附近后，采用圆弧-直线组合方式规划垂直泊车路径，引导汽车进入目标泊车位.如图8所示，已知垂直泊车起始点G1和目标点G3的坐标分别为(xG1,yG1)和(-Wd/2,-Lf-L)，Wd为垂直泊车位宽度，Lf为汽车前悬长度，则基于圆弧-直线组合方式规划垂向泊车路径的具体流程如下：

图8 垂向泊车路径规划

(10)

(xO2,yO2)=(xG1,yG1-R1)

(11)

③基于垂直泊车起始点G1的坐标计算点G2的坐标，计算公式为

(xG2,yG2)=(xG1-R1,yG1-R1)

(12)

4 仿真分析

本节在VC++6.0环境中实现基于D*算法和动态窗口法的自主代客泊车路径规划方法，并且以图9(a)所示的自主代客泊车场景静态环境为基础仿真验证所提方法的可行性和有效性.仿真验证参数设置为：h=0.1 s，Wd=2.400 m，Lc=4.155 m，L=2.405 m，Lr=0.950 m，W=1.645 m，δmax=0.53 rad，Vmin=1 m/s，Vmax=3 m/s，amax=2 m/s2，ωmax=0.53 rad/s，α=10和β=2.5，仿真地图横向长度为79 m，纵向长度为67 m，栅格节点间距为0.3 m，仿真结果如图9(b)～(h)所示.

(a)自主代客泊车场景静态环境

在图9(b)所示的自主代客泊车场景静态环境地图的基础上，基于D*算法规划的原始全局自主代客泊车路径如图9(c)所示，红色圆圈表示路径起始点，红色五角星表示路径目标点.当汽车沿着原始全局自主代客泊车路径行驶过程中，发现图9(d)所示的自主代客泊车场景存在动态障碍物时，则利用Dijkstra算法局部更新自主代客泊车场景静态环境地图，得到图9(e)所示的自主代客泊车场景动态环境地图，并基于D*算法重规划图9(f)所示的全局自主代客泊车路径，由于D*算法充分利用原始全局自主代客泊车路径规划过程中产生的静态路径场，极大地提高了全局路径重规划的效率.在重规划的全局自主代客泊车路径基础上，基于动态窗口法和圆弧-直线组合方式规划的局部自主代客泊车路径如图9(g)和(h)所示，该路径满足汽车运动学约束和汽车机械约束，并且可以安全无碰撞地引导汽车进入目标垂直泊车位.

5 结论

1)利用栅格扫描算法快速准确构建出自主代客泊车场景的静态环境地图，并采用Dijkstra算法实时更新动态障碍物影响的局部静态环境地图信息，降低了栅格扫描算法更新全局静态环境地图信息产生的高计算负荷.

2)利用D*算法将自主代客泊车场景静态环境地图转化为静态路径场，得到了原始全局自主代客泊车路径，同时借助静态路径场重规划动态障碍物影响的全局自主代客泊车路径，提高了动态障碍物的处理效率.

3)综合考虑汽车运动学约束和汽车机械约束，采用动态窗口法和圆弧-直线组合方式规划最优的局部自主代客泊车路径，并在规划过程中融合几何边界碰撞检测方法和栅格空间覆盖枚举方法，快速剔除不可行局部自主代客泊车路径.仿真结果表明，所提出的自主代客泊车路径规划方法可以在动态障碍物存在的自主代客泊车仿真场景中，安全无碰撞地引导汽车进入目标垂直泊车位.