全自动泊车系统路径跟随控制策略

2022-08-04张书恺刘美岑

汽车工程 2022年7期

张书恺，陈慧，刘美岑

（同济大学汽车学院，上海 201804）

前言

自动泊车技术作为汽车智能化的一项代表性技术，已经受到了高校与企业的高度关注。具有高精度性能的自动泊车指令控制算法，不仅可以使得车辆泊车无需过大的安全裕度，有助于改善库位规划增大土地利用效率，也可与未来需要车辆精准泊车的自动充电技术等进行有机结合。

现行的自动泊车指令控制算法大致可分为两种：（1）端到端直接根据环境信息输出控制指令；（2）先规划路径再输出控制指令跟踪路径。前者多为启发式算法如神经网络等，十分依赖传感器与车辆相关零件如转向器等的性能，若传感器信息存在噪声或车辆零件惯性影响过大，控制效果将受到影响；后者可依靠启发式算法如模糊逻辑，或闭环控制器如PID、LQR、滑模控制器、MPC等，其效果的好坏取决于反馈控制状态变量的选取，如相对路径的横向偏距+角度误差、相对参考点的向误差+向误差+角度误差等，控制理论相对成熟，效果相对可靠，因此考虑使用该方式进行控制。

在先规划后跟踪的整体框架下，通常以误差为状态变量列出状态方程：

式中：为输入变量，如前轮转角、速度等；与为与车辆轴距等参数相关的矩阵。通过设计输入变量使得误差向0衰减，实现精确跟随。而所需状态变量的获取方式与路径规划的输出结果直接相关：如路径规划结果为可以直接用数学函数表示的显式路径，则可用曲线相交方式求取参考点，而后求取相对路径的横向偏距与角度误差作为状态变量；如路径规划结果为无法直接用数学函数表示的、仅能用形状与长度等信息定义的隐式路径，则利用几何形状推算参考点，而后利用相对参考点的向误差+向误差+角度误差作为状态变量会较为方便；如路径规划结果为散点序列，则需要借助曲线拟合、插值或按照一定逻辑直接使用散点信息提取状态变量。可见，规划结果的输出形式决定了状态变量的选择，进而导致状态方程的不同。

同时，状态变量反映的误差信息不同，同样导致控制器的不同。文献［7］中使用的横向偏距+角度误差能够完全反映与路径的横向偏差程度，因此仅需设计横向控制器进行跟踪，输入变量仅为转向盘转角；而文献［8］中使用的相对参考点的向误差+向误差+角度误差同时包含了相对路径的横向与纵向误差，在未进行横纵向误差解耦的情况下，只能根据唯一的同时包含横纵向误差的状态方程设计反馈控制，输入变量转向盘转角与速度两项。可见，控制器的设计需确保与状态变量相对应。而若如文献［8］中，状态变量同时包含了相对路径的横向与纵向误差，理想的控制器应能将两部分误差解耦，并分别交给对应的执行机构进行控制，如此在确保跟随性能的情况下，便于横向与纵向控制的独立设计，降低设计难度。

本文基于以上考虑，在路径为隐式路径（直线段与曲率连续曲线，即CC 曲线），采取跟随参考点进而跟随路径的方案的情况下，基于微分平坦理论，通过将车辆后轴中点与参考点之间的误差进行横纵向解耦并进行横纵向分别控制：纵向控制上，提出有效距离积分方法，并设定纵向误差修正方法；横向控制上，采用线性二次调节器（LQR），并将路径曲率变化率与可以加速横向误差衰减的因子建立联系。如此使得车辆全过程能够以更高精度跟随沿路径移动的参考点，进而完成对整条路径更高精度的跟随。

1 问题描述与分析

1.1 泊车路径说明

如图1和图2所示，平行泊车与垂直泊车的规划路径均可视为一系列至之间的直线段与CC 曲线的拼接。而规划结果的表达形式为分段的隐式形式，如式（2）所示。

图2 垂直泊车基本规划路径

式中：、、为沿弧长的分段边界，可理解为图1 中、等之间的弧长距离；、、为本段内的曲率值；、、为本段路径的车辆行驶方向（前进为1，倒车为-1）；、、为本段路径的转向盘转向方向（左转为1，右转为-1）；为沿路径的长度。

图1 平行泊车基本规划路径

1.2 参考点的获取

由于路径为隐式路径，且难以直接离散为散点序列，因此参考点坐标及航向角的计算是根据几何形状递推获得。将总参考距离，也即沿路径的长度，与分段边界值、、等相比较，从而获得此时参考点的曲率、行驶方向和转向方向。若此时参考点坐标及航向角分别为、、，沿路径的长度增加∆，则可递推下一时刻参考点坐标及航向角、、为

在泊车低速运动下，上述递推准确度能够满足要求。

1.3 系统控制框图与控制目标

车辆控制的整体框图如图3 所示。整个控制器整体分为纵向控制（上半部分）与横向控制（下半部分）：

图3 系统控制整体框图

（1）纵向控制中，车辆速度规划器内部存储目标车速与沿路径长度、路径总长度相关的函数=(，)，通过参考行驶距离计算器实时获取总参考行驶距离，结合路径总长度计算目标车速；目标车速与实际车速之间的差值被传输给车辆速度控制器，由车辆速度控制器控制驱动电机给出驱动力矩，最终实现轮速控制。

（2）横向控制中，将规划路径总长度与总参考行驶距离相比对，获取沿路径长度处应有的曲率等信息，通过递推计算参考点位置信息；参考点位置信息与车辆后轴中点的差值被传输给车辆转角控制器，由车辆转角控制器控制转向电机给出转向力矩，最终实现转向盘转角控制。

如此，整个系统通过使用总参考行驶距离与规划路径长度形成反馈信息作用于横向与纵向控制，实现了控制上的横纵向解耦，如此便于车速、转角的单独设计。

然而，文献［11］和文献［12］中直接以车辆全部行驶距离作为总参考行驶距离，如此则未能进行误差上的解耦，导致信息反馈有误。图4 为实际车辆进行路径跟随时的一种常见场景。

图4 车辆以全部行驶距离作为总参考行驶距离sd时参考点Or与车辆后轴中点O之间的位置关系示意

（1）纵向控制中，函数=(，)应确保车辆在全程行驶完毕后停车，因此设计时，当总参考行驶距离达到规划路径的总长度时，被设计为0。此时，若计算不当，则存在如图5 左图所示的沿路径的纵向误差时，参考点到达终点导致车速指令值为0，使得车辆将提前停车，车辆后轴中点无法到达终点。

图5 车辆纵向误差（左）与横向误差（右）

（2）横向控制中，车辆后轴中点与参考点之间的误差将被反馈给横向控制器进行横向控制。若车辆后轴中点与参考点之间的误差同时包含图5 所示的纵向误差与横向误差两部分，则横向控制将一同接受不能被其控制的纵向误差信息，而纵向误差反而将对横向控制造成影响，这一点将在后续第3章说明。

基于上述对横纵向误差的解耦分析，本文在文献［11］和文献［12］的基础上，对横纵向控制器进行设计，从而实现对上述横纵向误差的分别控制：纵向上，改进图3（1）处的参考行驶距离计算器，增加修正策略计算，使图5左图所示的车辆后轴中点与参考点之间的纵向误差保持在较小值；横向上，改进图3（2）处的车辆转角控制器，基于微分平坦理论改进反馈控制器进行转角控制，使图5 右图所示的车辆后轴中点与参考点之间的横向误差在跟随全过程均保持在较小值。

2 纵向控制设计

如前所述，总参考行驶距离的计算将同时决定车速指令与参考点的位置，进而决定控制效果的好坏，因此需要首先进行改进。基于先前分析结果，需要重新设计参考行驶距离计算器，使得车辆尽可能避免出现图4 所示的表示的纵向误差；若确实出现，则能够通过对的计算进行调整，达到消除纵向误差的效果。

2.1 有效距离积分方法

车辆尽管始终在前进，但车辆走过的全部距离并非都是沿路径行驶的。仍以图4 为例，车辆虽然由点出发并运行至点，但在沿路径的方向上，车辆实际上仅行驶到了点。若直接以车辆行驶过的全部距离，即弧长的长度作为总参考行驶距离，则会导致参考点位于如所在的领先于点的位置，如此则导致了纵向误差。

为应对上述问题，引入有效距离积分方法。有效距离被定义为车辆行驶时行驶路径长度向规划路径投影后的长度，示意图如图6 所示。由于车辆行驶时，车辆会在直线部分、定曲率圆弧部分、变曲率曲线部分处行驶，在3 种情况下，对应的有效距离按如下规则定义：

图6 车辆在3种路径下有效距离定义

（1）直线部分：有效距离为实际行驶路径在直线上的直接投影长度；

（2）定曲率圆弧部分：车辆行驶时，通过使用车辆后轴中点起点与终点的坐标，可计算出车辆相对圆弧行驶过的有效角度，有效距离为这部分角度对应的圆弧弧长；

（3）变曲率曲线部分：由于这部分有效距离难以精确算出，因此在计算时，利用起点与终点对应的曲率按（2）中计算方式分别计算前后曲率下对应的长度，而后取两者平均值近似为有效距离。

通过这种方式，每当车辆运行一个时间间隔，则将车辆在这个时间间隔内的前进距离按照上述规则进行修正获取本段时间间隔内运行的有效距离∆，并用这样的∆进行积分，获取总长度。

2.2 纵向积分修正方法

仅仅对有效距离进行积分，仍不足以达到最优性能，这是由于有效距离积分的计算会受到偶然误差的影响导致计算存在偏差。此外，如图7 所示，由于车辆进行多段泊车时，在跟随完第1 段路径时，处于换段交界处点时存在溜车的情况，这导致车辆会在跟随下一段时存在箭头标识出的初始纵向误差，而这部分不能被上述积分机制所弥补。因此，还需要设计对纵向积分的修正方法来消除这部分误差。

图7 车辆在分段交界处溜车示意

存在初始的纵向误差时，一个最直观的想法是，当车辆落后于参考点时，希望参考点移动较少使得车辆追上参考点；反之，希望参考点移动较多使得车辆与参考点之间的误差减少。按此思路，可设置纵向积分修正方法。在计算出有效距离∆后，还需进行以下修正才能被计入总长度：

（1）车辆落后于参考点时：希望参考点不要移动过大，本次计入的有效距离按的倍数缩小；

（2）车辆领先于参考点时：希望计入更多距离，使得参考点追上车辆，本次计入的有效距离按的倍数放大。

、为可调节的纵向补偿系数。在补偿时，车辆的超前与落后需要计算沿路径的纵向误差，并结合车辆行驶方向来判断。同时，为避免系统过于敏感，这种修正应在纵向误差超过某一阈值后才能进行，该阈值可设为一个码盘的刻度值。

由于仅有一个参考点的信息，因此纵向补偿时需要的纵向误差实际上无法计算出精确值，仅能通过使用参考点的信息对纵向误差进行近似计算。

图8展示了一种跟随过程常见的场景，车辆后轴中点与参考点之间同时存在横向与纵向误差，显然最理想的横向参考点为点，而与之间的距离为纵向误差。然而目前仅有关于的信息，无法求出两者之间的路径长度。但是由于的坐标、航向角、所在处曲率、转向方向均已知，因此所在的、与其航向角方向相切的圆是唯一确定的。直线与之间对应的圆弧长，即认为是近似的纵向误差，其正负结合车辆的前进方向确定。实际运行时，在数据每一次更新时均计算一次纵向误差，若超过阈值，则对该时刻的有效距离∆进行修正并计入总长度。由于纵向误差无法获取精确值，其对纵向补偿实际上起到的是“警告”作用。

图8 纵向误差近似计算示意

同时，如图3 所示，由于横纵向控制均基于同一个有效距离，若在实际跟随时存在纵向误差，则在纵向误差未被补偿完毕之前，横向控制器仍然受到纵向误差的影响而无法良好跟随路径，引入额外的横向误差。然而在泊车的低速工况下，近似计算能够引入的纵向误差很小，同时，即使出现溜车，引入的纵向误差也仅为10 cm 左右。因此，纵向误差实时数据更新3～4 次，在很短时间内即可补偿完毕，而补偿完毕后横向控制器即可根据相应设计，消除在纵向误差未补偿完毕时引入的横向误差。

综上，设计参考行驶距离计算器如图9所示。

图9 参考行驶距离计算器整体框图

3 横向控制设计

3.1 车辆运动学模型

由于在泊车过程中，车辆运动速度很低，因此忽略车辆侧偏角的影响，使用车辆运动学模型：

式中：()、()为车辆后轴中点坐标；()为车辆航向角；()为后轴中点速度；()为前轮转角，为轴距。

进一步，车辆的转向机构也可简化为惯性环节：

式中：()为执行机构的输入；为惯性环节的时间常数。

由式（4）和式（5）可见，运动学方程中包含了时变量速度()，后续控制指令设计时，不利于速度与转角的独立设计。因此，首先引入弧长作为新自变量，以实现横纵向解耦。同时，为便于后续反馈线性化处理，引入时间尺度函数()：

式中：即为沿路径的总行进距离；()实际上可理解为微小时间d内，车辆实际行驶距离d与沿路径行驶距离d之比，而其用于反馈线性化的方式将在后文说明。显然，若车辆能够完全跟随路径，意味着车辆的全部行驶距离等同于沿路径的行驶距离，此时() = 1。如此，通过链式法则利用式（6）对式（4）和式（5）进行处理，可将()、()、()、()均转化为以弧长为自变量的关系式：

同时

这里需说明，式（7）中包含的速度=()。

3.2 反馈线性化

式（7）为车辆关于弧长的运动学模型，仍然为非线性模型，不便于后续状态方程的处理和反馈控制的设计，因此还需进行反馈线性化的过程。

微分平坦理论形成于20世纪90年代，其可被用来进行将非线性系统进行反馈线性化的方法。通过将系统所有状态和输入均转化为平坦输出元素及其有限次导数的函数，反馈控制率即可对平坦输出元素进行设计，最终达到要求。应用于自动泊车的微分平坦算法由Mueller 等于2007 年提出，后来，程昆朋等在此基础上增加了纵向控制并成功在实车平台上实现了功能。

在微分平坦空间中仍然以弧长为自变量，并将微分平坦输出设置为车辆后轴中点横纵坐标()、()。文献［10］和文献［11］中均对系统的微分平坦性进行了证明，因此这里仅给出最终结果。

通过整理，可将系统涉及到的变量整理为微分平坦输出元素本身及其有限阶导数形式：

状态方程的整理同样需要理想点处的相应值。由于前文所述的参考点递推关系，参考点坐标、、参考点处理想航向角、参考点所在处曲率可直接获取。则

式中及其各阶导数等计算与等相同。

根据式（7）有

两次求导即

将式（15）展开，令=，=，有

基于以上说明，整理车辆前轮转角控制器设计框图如图10所示。

图10 车辆转角控制器设计框图

这里对于先前提及的纵向误差对横向的影响进行简要说明。如图11所示，车辆在定曲率、半径为的圆弧上行驶，根据阿克曼转向模型，若车辆前轮转角与轴距、路径半径的关系保持为

图11 车辆存在纵向误差时在定曲率路径上行驶

则车辆可完全跟随半径为的圆弧路径。

3.3 反馈控制说明

上一部分提到，通过使用设计好的、替换、进行计算。实际上，、是真实存在的变量，其与设计好的、存在误差，可表示为

利用LQR 计算时，使用矩阵+代替矩阵来进行计算，这就使得系统的主导极点更加远离虚轴，从而使得误差以更快的速度衰减。这里为单位矩阵。

3.4 误差一致性证明

图12 车辆横向误差示意

对于横向偏距显然有

对于角度偏差-，结合ˉ≈1 与小角度假设-<5°，有

因此

3.5 反馈控制设计

由于车辆的初始误差为0，而在跟随过程中出现了误差，这说明是由于路径的形状变化引入了误差。而又由于路径的形状是连续变化的，因此误差的引入也是一个平滑的过程。因此，若能对误差的引入规律进行研究，则可以结合其引入规律将误差在泊车全程均控制在较小值。

若将误差视为一个函数()，则根据矩形积分公式，按∆步长离散化，配合阶跃函数，可改写为

式中(0)、(∆)为阶跃函数。上式说明，误差可视为一系列阶跃函数值的依次叠加，其在数值上的正确性可通过代入= ∆、= 2∆等进行验证。因此，若能获得(0)、(∆)等的规律，则可在误差刚刚开始叠加时即设计控制率进行消除，使得误差值不会累积至较大值。

由于车辆在保持前轮转角固定时，车辆的运行轨迹即为固定的圆弧。若规划路径为定曲率圆弧，则车辆在前轮转角稳定后，不会继续出现横向误差。因此，误差的累积是在曲率发生变化时产生的。

图13 为车辆在变曲率处的运行示意图。当车辆处于点时，车辆与参考点重合，由于没有误差反馈，车辆保持圆弧路径行驶至点，然而此时由于曲率发生了变化，车辆前进∆后参考点实际应为。如此，系统产生了横向误差，而该误差的累积最终将表现为车辆偏离了路径。

图13 中的之间的距离即为前进∆后引入的横向偏距，与之间的航向角偏差即为引入的角度误差。记圆与半径为与，曲率为与，则

图13 车辆在变曲率处运行示意

利用余弦定理有长度为

则

这里为曲率变化率。式（33）表明，经过固定的∆长度，由曲率变化率引入系统的横向偏距与本身近似成正比例函数关系。

同理，由引入的角度误差为

可见，引入的角度误差也与曲率变化率近似成正比例函数关系。

如文献［12］中所述，若系统的主导极点实部为-，则误差以e的速度衰减。若假设系统有一初始误差(0)，在经过∆后，误差变为

以横向偏距为例，若曲率变化率为，则结合介入的横向偏距误差∆后，误差变为

若此时加入与曲率变化率相关的衰减因子=，则

观察式（37）第2 部分可见，如果参数能够适度调整，则在误差积累到某一值之后，由衰减因子增加的衰减速度可以将后续引入的误差完全抵消掉，也就是说，误差将维持在某一上限值(0)不会继续增加。同时，为保持车辆在定曲率路径行驶时有足够的误差衰减速度，可设定=+，其中、为可调参数，由车辆实际跟踪效果确定。

这里说明，实际运行时，误差上限值(0)不能根据式（37）由精确确定，这是由于因子的加入不严格使得主导极点更远离虚轴的距离；同时，纵向误差虽然可保持在较小值，但无法完全消除至0，残余的纵向误差仍会对横向控制造成影响；在推导中的近似也会造成些微影响。

4 仿真实验验证

仿真实验的目的是证明横纵向的改进策略有效。

为实现上述目的，本文使用3 种控制方法进行对比：

（1）纵向改进横向不改进：纵向控制采取有效距离积分与纵向误差修正；横向控制仅按文献［12］加入固定不变的因子，即= 0、≠0；

（2）横纵向均改进：纵向控制采取有效距离积分与纵向误差修正；横向控制在文献［12］的基础上，增加与曲率变化率的相关量，即≠0、≠0，且与（1）中相同；

（3）横向改进纵向不改进：纵向控制仅使用车辆全部已行走距离作为总参考距离；横向控制在文献［11］的基础上，增加与曲率变化率的相关量，即≠0、≠0，取值与（2）相同。

图14 3种控制方法下仿真跟随结果

图15 3种控制方法下x方向误差

由图14～图16可见，横纵向均改进时，由于增加了项缓解了曲率变化时误差的累积，而纵向补偿又可对溜车误差进行补偿，因此车辆能将在溜车时（图15 和图16 时间12 和26 s 处）引发的异常误差值快速消除，之后将误差控制在较小值。这里需同时说明，由于参考点是根据路径信息递推出来的，因此参考点与路径本身也存在部分误差，但在泊车的低速运动下，这部分误差是可以接受的。

图16 3种控制方法下y方向误差

5 实车实验验证

实车实验的目的是验证算法在实际车辆上的可执行性，以及是否能在不同工况下均可保持高性能以便实际应用。实车平台为如图17 所示的基于荣威E50改制的智能驾驶测试平台。车辆搭载了超声波雷达、单线激光雷达、多线激光雷达、单目摄像头、环视摄像头、GPS/INS 组合导航仪RT3000 和dSPACE 的MicroAutoBox 作为控制器，以便实现自动驾驶决策、规划、控制算法。这里需说明，本文使用的路径规划算法仅以充分利用空间为目标，并未考虑安全距离，因此车辆轮廓会与库位轮廓有轻微碰撞。不过，由于本文的目的仅是证明路径跟随算法的有效性，因此本文的规划算法能够满足要求，而实际使用时若需确保安全性，只需在规划算法中设定安全距离即可。