基于库位跟踪的自动泊车决策规划系统*

熊璐 严森炜 余卓平 张培志

（同济大学，上海 201804）

主题词：自动泊车 库位跟踪 多次规划 动态调整 卡尔曼滤波

1 前言

自动泊车技术已经问世多年，但目前仍不能较好地解决如下问题：对不同库位环境和初始位姿的普适性、算法良好的实时性、算法的可靠性和可控性、最后停车的理想位姿。自动泊车算法主要分为4种：一是基于路径规划的控制算法[1-6]，即根据环境约束规划几何曲线（包括搜索采样算法），然后通过路径跟踪和底盘控制决定车辆的动作，优点是规划运算耗时较短、规划路径基本可控，缺点是路径跟踪和底盘控制存在误差，导致最后泊车位姿不理想；二是基于经验的控制算法[7-11]，即利用模糊逻辑或神经网络，根据实时的环境输入得到航向角和车速的控制量，优点是闭环控制，缺点是控制参数难以确定、泊车路线不可控、实车应用效果不佳；三是基于数值优化的算法[12-13]，通过建立系统运动学方程和结合参数约束进行路径最优化求解，优点是对不同环境适用性广，缺点是计算量大、实时性差；四是基于学习的算法[14-16]，利用行为克隆或强化学习进行从环境到车辆纵、侧向控制的映射，优点是可以将各模块的误差直接融合在算法中消除，缺点是学习算法训练难以收敛，容易陷入局部最优值，泛化能力一般。

本文基于几何规划，结合Reeds-Shepp曲线路径最短[2]和螺旋线适用性广的优点设计了对自车初始位姿无要求的自动泊车决策规划算法。通过融合环视库位坐标信息和车辆底盘运动学信息对目标库位进行跟踪，同时对航位推算结果进行修正。对于路径跟踪和底盘执行器控制带来的误差，通过库位跟踪和多次规划的方法予以减小。此外，设计了多次规划和动态调整算法解决泊车过程中由于空间狭窄难以入库的问题。本文设计的算法对各类库位的适应性强、实时性好、可靠性强，并且大幅改善了最后泊车姿态的理想程度。上述算法通过仿真和实车试验进行了验证。

2 方案设计

2.1 自动泊车系统架构

自动泊车系统架构如图1所示。环境感知模块利用环视相机给出库位坐标，结合超声波雷达检测障碍物，将探测到的环境信息送到决策规划模块用以决策和规划泊车路径。运动控制模块中，底盘执行机构包括电动助力转向系统（EPS）以及驱动、制动执行器。

图1 自动泊车系统架构

2.2 决策规划系统流程

自动泊车系统的目标功能为：驾驶员将车辆驶入停车场或沿停车位行驶时，启动自动泊车功能后，自车接管车辆控制，前进过程中自动判别库位类型，实现自动泊车。在泊车过程中，自车通过多次更新库位坐标减小库位坐标检测误差和航位推算累积误差，使泊车姿态更理想。对于平行泊车，设计了多次规划模块，即库位长度无法满足一次倒车入库时，自动增加调整次数，直至能够泊入车位，理论上只需车位长度大于车长即可入库；对于垂直泊车，停车通道宽度不足时，进行动态调整，使泊车规划不受障碍物的限制。简要流程如图2所示。

图2 决策规划系统流程

2.3 车辆模型

本文使用阿克曼转向性质的车辆模型。将车辆视为平面刚体，具有3个自由度，选取后轴中心作为参考点，车辆在世界坐标系下的坐标为(x,y)，航向角为φ，则车辆位置唯一确定，如图3所示。图3中，γ为转向轮转角，r、κ分别为车辆转弯半径和曲率，L为轴距。车辆的状态方程为：

式中，v为车速。

图3 阿克曼转向车辆模型

3 基础路径规划

3.1 平行泊车

自车起始时（首次获得环视系统得到的库位角点P0、P1时，库位宽度通过超声波雷达测算或按国标推算），后轴中点位于S点，将S点设为规划的原点，建立的坐标系和基础路径规划结果如图4所示。车辆沿X轴方向前进，到达某点A时，以最小转弯半径Rmin倒车，再以为半径反向倒车，后轴中心点轨迹分别为和BC，此时车身与库位平行，然后前进至库位中心位置。

图4 平行泊车基础路径规划示意

C点的坐标为：

初始位置自车纵轴线与库位外侧连线的夹角为：

第2段、第1段圆弧的圆心O2、O1的坐标分别为：

规划辅助计算角β1和β2分别为：

中间过渡点B的X坐标为：

式中，Lr为车辆后轴与后端的距离；Lw为轮距；Lsafe1、Lsafe2分别为车辆纵向、横向安全距离；Lpark为库位宽度（P0与P1的距离）。

为了保证车辆不与库位发生碰撞，车辆在2段圆弧上行驶时均不能发生碰撞，如图5所示。通过几何分析，第1段、第2段圆弧不碰撞的判断条件分别为：

其中，O1与K、P1及O2与I、P2的距离分别为：

式中，Lf为车辆后轴与前端的距离。

图5 2段圆弧不发生碰撞的判断条件

实际泊车过程中，库位长度可能不满足一次规划的要求，即式（8）、式（9）不成立。参考人类驾驶员的泊车经验，本文提出多次规划算法，即降低一次规划的目标，使车身部分停入库位，再通过不断规划调整，最终停入库位中心。如图6，一次规划计算发现会发生碰撞，因此逐渐增加目标终点从库位中心向外侧的偏移值，直至能够满足无碰撞条件。当行驶完一次规划路径后，根据与库位的相对位置再次规划路径S'A'B'C'D'，直至自车停在库位中心。库位长度越短，单次调整纵向距离受限，因此横向距离变化也越小，所需调整次数越多。

图6 平行泊车多次规划示意

3.2 垂直泊车

自车起始时（首次获得环视系统得到的库位角点P0、P1坐标时），后轴中点位于S点，将S点设为规划原点，如图7所示。车辆沿X轴方向前进，到达某点A时，以Rmin为半径倒车，车辆后轴中心点轨迹为AB。

图7 垂直泊车A点计算示意

θ的计算见式（3），O1位于直线：

几何推导易证，车辆外轮廓中，右后轮K处的轨迹与P1点距离最近，因而将安全距离Lsafe3定义为K与P1间的距离。因此O1同时满足：

由式（12）和式（13）联立解得O1的X坐标（即A的X坐标）为：

特殊地，当θ=0°时：

若需完成一次倒车入库（见图8），则由A到B的行驶过程中，某时刻自车纵轴线与车库纵轴线达到平行且过程中一直满足：

式中，Lsafe4=0.1 m为安全距离，可设与泊车车速正相关。

图8 垂直泊车一次规划入库示意

反之，则需多次规划入库。当式（16）取等号时停车，车辆后轴中心位于点B，根据此时自车相对库位的位姿规划后续路径。为便于表述，将坐标系原点重设为泊车目标点E，如图9所示。E点的X坐标为：

图9 垂直泊车基础路径规划示意

从B点开始以Rmin为半径车辆前进，车辆后轴中心点轨迹为，可推得其圆心O2的X'坐标为：

此后车辆以Rmin为半径沿倒车，最终使车辆纵轴线与车库纵轴线重合，可推得的圆心的X'坐标为：

则有车辆从点B以Rmin前进至点C及从点C以Rmin倒车至点D的停止判别条件为航向角分别达到：

式中，ψB、ψC、ψD分别为B、C、D点处自车航向角。

在垂直泊车工况中，常遇到泊车通道宽度不足的问题，如较窄的停车通道或泊车规划路径上存在墙或其他车辆，导致自车无法按照原规划路径行驶，此时需要进行如图10所示的动态调整。自车按原规划路径行驶过程中，车前方超声波雷达探测到障碍物距离小于某一阈值，则进行动态调整，将转向盘转至右极限位置，路径为，到达B2点后按的规划方法，规划和，按规划路径泊车入库。若仍检测到障碍物距离小于安全阈值，则重复动态调整，直至顺利入库。

图10 垂直泊车动态调整示意

4 垂直泊车规划算法优化

在实车试验中发现，由于垂直泊车的侧向空间小，对路径跟踪、底盘执行器控制和航位推算误差敏感，因此考虑在C点增加一次库位跟踪规划，即在C点根据自车和库位的相对位姿进行重新规划。

为了继承基础路径规划的思路，将CD段（此时基础路径规划的BC段和原CD段圆弧半径稍大于Rmin）路径调整为3段曲线，如图11所示，中间段仍采用圆弧曲线，前后两段采用二次螺旋线便于位姿调整。二次螺旋线的表达式为：

该曲线对应的车辆航向角为：

图11 改进后的垂直泊车路径

设第1段螺旋线的长度为s1，终点处的车辆位姿为X1=(X1,Y1,θ1)，中间段圆弧的圆心坐标是(XR,YR)，圆弧结束点处车辆位姿为X2=(X2,Y2,θ2)，第2段螺旋线的长度为s2。设两组螺旋线方程的未知参数分别为(a1,b1,c1)和(a2,b2,c2)。初始点位姿为X0=(X0,Y0,θ0)=(Xi,Yi,θi)，终点位姿为X3=(X3,Y3,θ3)=(Xg,Yg,θg)，κ0、κ3分别为起点和终点的曲率。根据边界条件有：

式（24）有(a1,b1,c1,s1)、(a2,b2,c2,s2)、(X1,Y1,θ1)、(X2,Y2,θ2)共14个未知量，设中间圆弧段的半径为Rmin，用圆心坐标(XR,YR)和(θ1,θ2)表示(X1,Y1)和(X2,Y2)：

结合式（24）和式（25），借助MIDACO优化工具进行求解，设置优化条件为曲线终点与目标点欧式距离最短和整个路径的长度最短，同时满足环境约束。另外，需要满足曲率约束条件：

式中，κmax为最大曲率。

式（24）中第4、5、9、10个方程为非线性方程，为了简化计算，可借助辛普森公式计算三角函数值：

式中，sg为积分区间的右边界。

考虑到初值的选择对于MIDACO求解有影响，因此考虑用成功生成的算例来训练神经网络，作为MIDACO求解的初值。流程如下：首先已知(Xi,Yi,θi,ki)和(Xg,Yg,θg,kg)，通过MIDACO求解待求参数a1、s1、a2、s2（在现有设备上的平均计算周期为1.2 s），用求解参数和初始条件生成曲线，二次验证筛选掉可能发生碰撞的结果，使用筛选后的数据集训练BP神经网络，含5层隐藏层，神经元数分别为200、100、50、20、10，激活函数选择Leaky ReLU，输入、输出均归一化参与训练，选择Mini-Batch和AdaGrad作为优化方法。最终训练结果中4个参数的平均误差约10%。

使用训练好的神经网络输出的4个参数作为MIDACO求解的初值，平均计算周期降低为0.8 s。图12所示为选取的一定区域内，初始航向角30°～90°（为了显示清晰每5°采样一次）的路径库。

图12 改进的规划路径库（选取部分离散初始条件）

在实车试验中若出现MIDACO求解的结果不符合要求的情况，可微调目标点(Xg,Yg,θg,kg)再次运行MIDACO求解。若多次微调目标点的结果均不符合要求，则进行自车位姿微调后根据新的相对位姿进行求解。

5 库位跟踪

因传统自动泊车规划算法采用航位推算来估计自车的位姿，其使用的惯性导航系统或车辆底盘信息存在噪声，同时，泊车工况的前进、倒车切换频繁，导致实际泊车中的航位推算累积误差达到了分米级以上，这对于泊车工况来说是难以接受的。环视系统的标定、拼接、光照、阴影、遮挡等具有随机性，此外，在检测库位时车辆非静止而影像系统帧数有限，导致测量误差的存在。

将航位推算信息和环视信息融合能够有效地解决上述问题。因方程非线性，采用拓展卡尔曼滤波算法。世界坐标系XwOwYw、车辆坐标系XvOvYv和状态量的定义如图13所示。

图13 卡尔曼滤波的坐标系和状态量定义

系统运动模型为：

式中，(x,y,φ)为自车相对于世界坐标系的横、纵坐标和航向角；(x1,y1)为相对于世界坐标系的库位角点坐标；V为自车车速；w(k)=(wx(k),wy(k),wφ(k),0,0)T为过程噪声，假设为高斯白噪声。

若非第一次检测到此角点，则将该库位点与已有库位点匹配，匹配方法为：若与系统状态中已有库位点的欧式距离大于某一阈值，则认为是新库位点，此时需对系统的状态进行增广，否则即利用EKF算法进行更新。

系统观测模型：

式中，v(k)=(vr(k),vθ(k))T为测量噪声，假设为高斯白噪声。

状态转移函数的雅可比矩阵为：

测量函数的雅可比矩阵为：

首先预测k时刻的系统状态向量初步估计值和误差协方差矩阵的初步估计值P(k)-：

式中，Q、R分别为状态转移协方差矩阵和测量噪声协方差矩阵。

然后计算k时刻的卡尔曼滤波增益矩阵K(k)，更新变量估计值和误差协方差矩阵的估计值P(k)：

增加了库位跟踪后，大幅度降低了传感器误差的影响，联合前文所述的决策规划算法，能够显著改善泊车效果，如图14所示。

图14 有、无库位跟踪的情况对比

6 仿真与实车试验验证

6.1 仿真车辆主要参数

基于Simulink和PreScan联合仿真，参数见表1。

表1 仿真车辆主要参数 m

6.2 平行泊车仿真验证

针对不同的横向距离，自车均能准确地停入库位。此规划算法适用于可能的任何横向距离，如图15所示。

此规划算法适用于任何初始角度，无论初始角度正、负，最终均能准确停入库位，如图16所示。

图15 不同初始横向距离的平行泊车仿真路径

图16 不同初始角度的平行泊车仿真路径

多次规划算法通过首次规划将自车调整到与库位平行。之后更新库位与自车的相对位置，并再次规划趋于库位中心的路径，如图17所示。

图17 多次规划的平行泊车仿真路径

6.3 垂直泊车仿真验证

随着横向距离递增，第1段前进直线段加长，而第2段圆弧调整段缩短。但不同的横向距离对于最终的泊车姿态没有影响，如图18所示。

无论初始角度正、负，最终均能准确停入库位。此规划算法适用于现实工况中可能的初始角度，如图19所示。

图18 不同初始横向距离的垂直泊车仿真路径

图19 不同初始角度的垂直泊车仿真路径

动态调整是无目标终点的规划，无法预知调整次数，可行区域越小，调整次数越多，一旦满足无碰撞入库条件即终止动态调整开始沿规划路径入库，如图20所示。

图20 动态调整的垂直泊车仿真路径

6.4 实车试验验证

实车试验车辆主要参数见表2，车辆如图21所示。

表2 试验车辆主要参数 m

环视相机通过USB总线与工控机通讯。工控机、超声波雷达、路径跟踪所需的传感器和执行器通过CAN总线与嵌入式控制器通讯。本文的决策规划程序在工控机中运行，不同模块间通过轻量级通信与数据封送库（Lightweight Communications and Marshalling，LCM）通讯。主要传感器和控制器的型号如表3所示。

表3 试验车辆主要传感器和控制器

为了对比库位跟踪的效果，对于平行泊车，先不开启库位跟踪模块，即在首次收到环视系统获得的库位坐标后仅用航位推算更新自车相对库位的位姿，以此为基础进行规划，最终泊车位姿并不理想，自车与真实库位不平行。开启库位跟踪模块后，重新更新库位的坐标，进行规划调整，调整后的泊车位姿较调整前有明显改善，如图22所示。

图22 平行泊车实车试验路径

对于垂直泊车，在同一初始位姿进行2次试验。首先通过测量得到库位相对自车初始位置的真值。第1次不使用库位跟踪，在首次收到库位坐标后仅用航位推算更新自车相对库位的位姿，以此为基础进行规划。第2次使用库位跟踪，用卡尔曼滤波实时更新自车的相对位姿，以此为基础进行规划，同时记录载波相位差分（Real-Time Kinematic，RTK）定位结果。结果如图23所示，基于库位闭环的决策规划算法能够明显改善泊车位姿。

图23 垂直泊车实车试验路径

7 结束语

本文设计的自动泊车决策规划系统能够较大限度地发挥几何规划路径可靠、算法实时性好的优点，同时对自车初始位姿无要求。通过库位跟踪，能够有效降低航位推算和环视系统库位检测的误差，提高车辆与库位的相对定位精度。将库位跟踪和本文所设计的自动泊车决策规划算法结合起来，能够较大幅度提升自动泊车最终泊车位姿的理想程度。此外，对于自动泊车场景中常见的平行泊车长度不足或垂直泊车可行通道较窄的问题设计了多次规划和动态调整算法，能够有效地解决上述问题。