【技术解析】无人车横向控制解读
技术解析是由美团点评无人配送部技术团队主笔，每期发布一篇无人配送领域相关技术解析或应用实例，本期为您带来的是无人车横向控制解读
本文主笔：快乐的外卖小哥
本文共7778，预计阅读时间20分钟
1简介
无人车是一个复杂的软硬件结合系统，其软件算法模块主要包含感知、定位、轨迹预测、行为决策、运动规划及反馈控制等。
反馈控制是无人车最底层的软件算法模块，包括横向控制和纵向控制两个部分，其中横向控制主要用于车辆方向盘的控制，纵向控制则主要负责车辆油门、刹车的控制，两者协同工作以使无人车按照预定的参考轨迹行驶。其中，横向控制根据上层运动规划输出的路径、曲率等信息进行跟踪控制，以减少跟踪误差，同时保证车辆行驶的稳定性和舒适性。
横向控制研究最初是由美国通用汽车公司和美国无线电公司在20世纪 50 年代末合作开展，目的是解决自动化公路系统(AHS)车辆自动驾驶问题[1]。
2004 年起美国国防部高级研究计划局(DARPA)举办的三届 DARPA 无人驾驶挑战赛是现代无人驾驶发展的重要里程碑事件，许多无人驾驶横向控制算法在 DARPA 系列挑战赛中发挥重要作用。
图 1-1 介绍了几种典型的横向控制方法在 DARPA 系列挑战赛中的应用实例，其中图 1-1(a)是卡耐基梅隆大学开发的无人车 Sandstorm，其采用了基于车辆运动学模型的横向控制方法，顺利完成了2005年度的DARPA Grand Challenge 比赛，并取得了第二名的优异成绩；图 1- 1(b)是斯坦福大学开发的无人车 Stanley，也采用车辆运动学模型设计横向控制器，并获得了2005 年度 DARPA Grand Challenge 的第一名;图 1-1(c)是卡耐基梅隆大学开发的无人车 Boss，其采用模型预测控制算法获得了 2007 年度 DARPA Urban Challenge 的冠军。
DARPA 挑战赛之后，整个无人驾驶技术获得了更多的关注和更快的发展，也有更多的横向控制算法被开发并应用到无人车上。本文将通过对现有典型横向控制方法进行充分调研， 并对比不同横向控制方法的优缺点，从而在实际应用中为研究者在横向控制方法选择上提供参考。
图 1-1 横向方法的实际应用
根据横向控制使用车辆模型的不同，可以将其分为两种类型，包括无模型的横向控制 方法和基于模型的横向控制方法。其中基于模型的横向控制方法又可分为:基于车辆运动 学模型的横向控制方法以及基于车辆动力学模型的横向控制方法。
无模型的横向控制即传统的 PID 控制算法，将车辆当前的路径跟踪偏差作为输入量，对跟踪偏差进行比例(Proportion)、积分(Integration)和微分(Differentiation)控制得到转向控制量。但该算法由于没有考虑车辆本身的特性，因此算法对外界干扰的鲁棒性较差，无法满足车辆在高速行驶过程中的有效控制。
因此，本文将主要介绍基于车辆模型横向控制方法，包括基于车辆运动学模型的纯跟踪控制(Pure Pursuit)算法、后轮反馈控制(Rear wheel feedback)及前轮反馈控制(Front wheel feedback)算法，以及基于车辆动力学模型的线性二次型调节器(Linear Quadratic Regulator，LQR)控制算法。
2基于车辆运动学模型的控制方法
2.1车辆运动学模型
图 2-1 运动学模型
基于车辆运动学模型的控制方法主要考虑了车辆的运动学约束对车辆控制的影响。车辆的运动学模型通常可以简化为两轮的自行车模型，其假设车辆的前后轮均为刚体，只能沿着车轮滚动的方向前进，无侧向滑动。由上述假设可得自行车模型的运动微分方程如下式：
其中，x, y 为车辆在全局坐标下的位置，𝜓为车身纵轴与全局坐标系x轴的夹角，
2.2 纯跟踪控制算法(Pure Pursuit)
纯跟踪控制算法是一种典型的横向控制方法，最早由R. Wallace在1985年提出[4]。由于该方法对外界的鲁棒性较好，包括 DARPA Grand Challenge 和 DARPA Urban Challenge 在内的三辆无人车均采用该控制算法对车辆进行横向控制，并取得了很好的控制效果。
该算法的思想是基于当前车辆后轮中心位置，在参考路径上向前𝑙𝑑 的距离匹配一个预瞄点，假设车辆后轮中心点可以按照一定的转弯半径𝑅行驶抵达该预瞄点，然后根据预瞄距离𝑙𝑑， 转弯半径𝑅，车辆坐标系下预瞄点的朝向角𝛼之间的几何关系来确定前轮转角。
如图 2-2 所 示，弧线为经过车辆后轮中心以及预瞄点的圆弧，圆弧的半径与车辆坐标系下预瞄点的朝向角𝛼和预瞄距离𝑙𝑑有关。
图2-2 纯跟踪控制几何结构
其中，(𝑥𝑟, 𝑦𝑟)为车辆后轮中心坐标，(𝑥𝑔, 𝑦𝑔)为预瞄点坐标，𝜓为车辆在全局坐标系下的横摆角。
根据图 2-2 的几何关系，𝑅, 𝛼, 与𝑙𝑑的关系为
根据运动学方程(2-1)和
L为车身轴距。式(2-4)带入(2-5)可得前轮转角为：
2.3 后轮反馈控制算法（Rear wheel feedback）
图 2-3. 后轮反馈控制示意
后轮反馈式是利用后轮中心的跟踪偏差来进行转向控制量计算的方法，根据运动学方程(2-1)及车辆后轮与参考路径的几何关系，可推导出参考路径坐标系(𝑠, 𝑙)，Ferent 坐标系，下(𝑠, 𝑒, 𝜓𝑒 )的变化率为[5]:
̇其中，𝑒表示参考路径上离本车最近点和本车的横向位置误差，𝜓𝑒为参考路径上离本车最近点的切线角和本车的横摆角的误差值，𝑘(𝑠)为参考点𝑠处的曲率，𝑣𝑟为车辆后轮线速度，𝜔为车身横摆角速度。
对于二次连续可导的参考线，需要设计车身横摆角速度𝜔保证在李亚普洛夫方程𝑉( 𝑒, 𝜓𝑒 ) = 𝑒2 + 𝜓𝑒 2⁄𝑘2 下局部渐进收敛。李亚普洛夫稳定用数学的语言描述为:可控可微分的状态方程𝑥̇ = 𝑓(𝑥,𝑢)，在给定的参考轨迹𝑥𝑟𝑒𝑓(𝑡)下，满足下列条件:∀𝜖>0,𝑡1<𝑡2，存在𝛽>0:对于李亚普洛夫稳定也分为渐进稳定和指数稳定。渐进稳定指对于时变系统，𝛽在条件(1)下独立于时间𝑡1;指数稳定指收敛率是以指数下降。
对于后轮反馈式算法，为了保证车辆的李亚普洛夫稳定性，𝜔可表示为式(2-10)
其中，𝑘𝜓为横摆角偏差反馈控制增益，𝑘𝑒为横向位置偏差反馈控制增益。因此，根据运动学方程(2-1)，可得到前轮转角𝛿为:
2.4 前轮反馈控制（Front wheel feedback）
图2-4. 前轮反馈控制示意图
前轮反馈控制又称 Stanley 控制，其核心思想是利用前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算。前轮中心横向位置跟踪偏差𝑒的变化率为:
式(2-12)中可以看出前轮横向位置偏差的变化率可以直接通过前轮转角𝛿进行控制，𝜓𝑒 为横摆角偏差。当定义𝑒̇ = −𝑘𝑒后，可以保证前轮横向位置偏差𝑒满足李亚普洛夫的指数化收敛，即：
对于式(2-13)来说，当|−𝑘𝑒⁄𝑣𝑓|>1 时，函数无解，对于有限域内的指数收敛可以放宽到局部指数收敛，从而将𝛿定义为：
Stanley 前轮反馈控制算法在变曲率路径且路径连续可导的的条件下，可以满足局部指数收敛特性。但也可以看出该算法不适用于倒车的场景，导致对于自动泊车等工况不适用。
3基于车辆运动学模型的横向控制
基于车辆运动学模型的方法忽略了车辆的动力学特性，因此在车速过快或者曲率变化率过大的情况下该算法无法满足车辆的稳定性控制要求。本节将介绍一种典型的基于车辆动力学模型的横向控制方法，包括介绍车辆线性二自由度动力学模型的构建，车辆路径跟踪偏差状态方程的构建和 LQR 横向控制算法的求解。
3.1 车辆线性二自由度动力学模型
对于基于车辆动力学模型的控制方法，首要的工作是对车辆动力学进行建模。由于车辆动力学模型具有强非线性时变的特性，因此精确建模整个车辆动力学十分困难。
在横向控制中，主要关心的是车辆横向运动特性，通常将车辆动力学模型简化为的二自由度横向动力学模型。假设车身的纵向速度𝑉 𝑥保持不变, 其横向动力学模型的两个自由度为:横向运动和横摆运动。
由于精确的二自由度动力学模型是非线性的，为了便于进行实时的跟踪控制计算，通常还需要在精确的二自由度动力学模型基础上进行一些简化近似，得到线性二自由度动力学模型。
本节将会介绍基于转向小角度及轮胎动力学线性化假设下的车辆线性二自由度动力学模型。
图 3-1 为车辆为车辆横向动力学坐标系示意图，其中𝑥𝑦为车身坐标系，𝑋𝑌为全局坐标系，𝜓是车身𝑥坐标轴与全局坐标系𝑋𝑌中𝑋轴的夹角。𝑝𝑎𝑡h为跟踪的参考路径，𝜓𝑑𝑒𝑠 当前车辆期望的横摆角。
图 3-1. 车辆横向动力学坐标系示意图
假设道路平整(不考虑道路横向的倾斜)，且转向角度较小时，则车身𝑦轴方向应用牛顿第二定律可得：
其中，𝑚是整车质量，𝑎 是车辆质心处的横向加速度，𝐹 ，𝐹 为地面给前轮胎和后轮施加的横向力，横向加速度𝑎𝑦由两部分组成:车辆沿车身𝑦轴横向运动产生的加速度𝑦̇，以及车身横摆运动产生的向心加速度𝑉𝑥
将(3-2)代入(3-1)可得:
然后，车辆绕𝑧轴的转矩平衡方程为:
其中，𝑙𝑓,𝑙𝑟分别是前轮轴和后轮轴到质心的距离。
接下来对横向轮胎力𝐹 ,𝐹 进行计算。实验结果表明，轮胎侧偏角较小时，地面施加给轮胎的横向力(侧偏力)与轮胎侧偏角(slip-angles)成线性关系(侧偏角是车轮速度方向与车轮纵轴的夹角)。
图 3-2. 轮胎侧偏角
前轮侧偏角为:
𝜃𝑉𝑓是前轮胎速度方向与车身纵轴的夹角，𝛿为前轮转向角，后轮的侧偏角为:
𝜃𝑉𝑟是后轮胎速度方向与车身纵轴的夹角
因此，前轮(2 个)横向轮胎力为:
𝐶𝛼𝑓是前轮侧偏刚度(cornering stiffness)
同样，后轮(2 个)横向轮胎力为:
𝐶𝛼𝑟 是后轮侧偏刚度
计算𝜃𝑉𝑓与𝜃𝑉𝑟：
𝜃𝑉𝑓与𝜃𝑉𝑟较小时可近似得到:
将(3-5)，(3-6)，(3-7)，(3-8)，(3-11)，(3-12)代入(3-3)和(3-4)可得线性二自由度模型的状态方程[6]:
3.2 车辆路径跟踪偏差状态方程
有了式(3-13)的系统状态方程之后，就可以分析出在给定的前轮转角输入下，车辆的横向位移、横向速度、横摆角以及横摆角速度的响应。但是横向跟踪控制的目的是为了减小跟踪偏差，需要的状态方程是能够分析在给定的前轮转角下车辆跟踪偏差的响应，因此些偏差通常是横向位置偏差𝑒1 及横向位置偏差变化率𝑒̇1，横摆角偏差𝑒2 及横摆角偏差变化率𝑒̇2，下面介绍具体的转换过程。
图 3-3. 路径跟踪偏差量
车辆期望横摆角速度:
期望的横向加速度𝑎𝑦𝑑𝑒𝑠为:
则横向加速度偏差𝑒̇1 为:
因此横向位置偏差变化率𝑒̇1 (横向速度偏差)为：
此外，横摆角偏差𝑒2为:
将式(3-16)，(3-17)，(3-18)代入(3-3)，(3-4)化简后可以得到新的状态方程:
即：
进一步忽略
上式(3-20)就是横向控制所需的路径跟踪偏差状态方程。
3.3 LQR横向控制算法
基于前面推导的路径跟踪偏差系统状态方程(3-20)，就可以分析在前轮转角控制输入𝛿作用下，车辆路径跟踪偏差𝑥的响应特性。
期望的响应特性就是跟踪偏差能够快速、稳定地趋近于零，并保持平衡，同时前轮转角控制输入又尽可能小，这就是一个典型的多目标优化最优控制问题。且优化的目标函数可以表示为跟踪过程累计的跟踪偏差与累计的控制输入的加权和，如式(3-21)所示：
其中， 𝑄为半正定的状态加权矩阵, 𝑅为正定的控制加权矩阵, 且 𝑄, 𝑅通常取为对角阵，𝑄矩阵元素变大意味着希望跟踪偏差能够快速趋近于零，𝑅矩阵元素变大意味着希望控制输入能够尽可能小。
则式(3-21)目标函数中第一项优化目标∫0∞(𝑥𝑇 𝑄𝑥)𝑑𝑡就表示跟踪过程路径偏差的累积大小，第二项优化目标∫0∞(𝛿𝑇𝑅𝛿)𝑑𝑡表示跟踪过程控制能量的损耗。
这样就将横向控制问题转化为一个最优控制问题:求解最优的前轮转角控制输入𝛿∗，使得式(3-21)目标函数𝐽可以取极小值。而式(3-21)是一个关于状态变量𝑥和控制输入𝛿的二次型目标函数，对它的优化求解是一个典型的 LQR 最优控制问题。
根据 LQR 最优控制理论，对式(3-21)目标函数的优化求解，解出的最优控制规律𝛿∗是关于状态变量𝑥的线性函数:
其中，𝐴𝑑 和𝐵𝑑 代表状态方程(3-20)离散化后的𝐴，𝐵1矩阵, 𝑃是式(3-23)黎卡提方程的解[7]:
因此，根据式(3-22)可以设计一个状态反馈调节器，通过状态反馈实现闭环最优控制，如图 3-4 所示。
图 3-4. LQR 反馈控制原理
根据求解的状态反馈调节器增益𝐾,则可以得到最优的状态反馈前轮转角𝛿𝑓𝑏：
由于在 LQR 状态反馈控制求解过程中没有考虑路径本身的动态变化特性，因此只有式(3-25)的反馈控制输入时 LQR 控制在曲线行驶时会存在稳态误差，为了消除该稳态误差， 还需要加上一个前馈控制输入量𝛿𝑓𝑓，前馈控制考虑了路径的曲率及车辆的转向不足特性。
其中，𝐾𝑉为不足转向梯度，表征了车辆的转向特性;𝑒2𝑠𝑠为稳态时的角度跟踪偏差,𝑘3为𝐾矩阵中的角度偏差反馈控制增益部分。
因此，总的 LQR 前轮转角输入为:
4总结与展望
4.1 总结
本文主要介绍了基于车辆运动学和动力学模型的横向控制算法，从上文对两类方法的 分析可以看出，两类方法在应用场景上都存在一定的局限性，并具有较强的互补性，因此在实际使用中，综合使用这些方法会更有效。下面比较了这几种典型横向控制算法在鲁棒性，路径要求，转弯内切，超调，稳态误差，适用场景等几方面的性能，如图 4-1 所示。
图 4-1. 横向控制算法比较
无模型的 PID 横向控制算法参数少，简单易用。但是由于没有考虑车辆系统动力学特性及路径本身的动态变化特性，对外界干扰的鲁棒性较差，在高速或曲率较大的弯道场景时，会出现较大的跟踪误差和“画龙”现象。因此，该方法比较适合应用于低速，曲率较小的路径跟踪控制场景中。
基于运动学模型的横向控制算法中， Pure Pursuit 和 Stanley 前轮反馈算法在中低速场 景下，他们的路径跟踪的性能较好。Pure Pursuit 在大的跟踪误差和非连续的路径场景下鲁棒性较好，其控制的关键在于对最佳前向预瞄距离的确定。
其中，增大前向预瞄距离将提高车辆控制的稳定性，但随之会带来路径跟踪性能降低及稳态误差增大的后果，表现出转弯内切现象。相比于 Pure Pursuit 算法，Stanley 前轮反馈算法还额外考虑了横摆角偏差， 因此在大多数场景下，跟踪性能更佳。然而，由于没有设置前向预瞄，Stanley 算法会出现转向过度的情况。与 Pure Pursuit 和 Stanley 算法相比，后轮反馈控制算法计算更加复杂， 对路径的平滑性要求更高，在中等速度下的跟踪性能及鲁棒性与 Stanley 方法近似，然而在速度较大时，稳态误差也会变大，从而导致控制效果不佳。
LQR 算法使用二自由度动力学模型来设计横向控制器，与前述基于运动学模型的几种算法相比，LQR 参数调节更加复杂，其不仅需要获取车辆自身的模型参数，还需要调节LQR 目标函数的 Q，R 矩阵以获得较优的跟踪性能。
LQR 算法的优点在于，通过与转向前馈进行有效结合，LQR 能够很好的解决曲线行驶时的稳态跟踪误差，在中等速度曲线行驶 时其稳态误差趋近于零，从而极大提升跟踪性能。LQR 非常适用于路径平滑的高速公路及城市驾驶场景，具有较好的车辆高速控制性能。
但是，由于模型的固有缺陷，LQR 与前馈控制的结合也无法解决所有跟踪控制问题。由于该方法采用基于简化后的二自由度动力学模型，因此当车辆运动不满足二自由度动力学模型转向小角度，或轮胎动力学线性化的假设条件时，LQR 算法的跟踪性能会大幅降低，从而导致控制失效。
4.2 展望
综上所述，本文介绍了几种典型横向控制方法，并对各方法的优缺点进行了对比分析，以期在实际应用中为研究者在横向控制方法选择上，提供一定的指导意义。从上述横 向控制方法的对比分析中，可以看出这些方法都存在一定的局限性，其主要原因在于:
1、对车辆运动模型的简化处理。如只考虑车辆的运动学模型或简化为二自由度动力 学模型。
2、未考虑横向和纵向控制的耦合特性。仅仅将横向和纵向独立控制，而并无法将纵 横向控制协同处理、分析，从而限制了横向控制算法的跟踪性能。
根据现有方法的不足，本文作者认为车辆横向控制方法具有以下几个热点:
1、整车动力学的精确建模。虽然线性二自由度车辆模型在轮胎侧偏角较小时能较好 地模拟车辆侧向和横摆运动特性，但当轮胎侧偏角较大时，侧偏角与侧偏力不再呈线性关 系，因此需要对更高自由度的车辆动力学模型进行精确建模。
2、控制模型参数的有效估计。车辆控制模型中参数的不确定性、时变特性及控制算 法调节参数的时变性将极大的影响车辆的控制效果，因此如何快速、准确、有效的对这些 参数进行估计和调节也是横向控制未来研究的重点。
3、基于学习方法的车辆控制策略。随着大量车辆行驶数据的快速有效获取，未来基 于深度学习或者深度强化学习的端到端控制策略将可能成为无人车控制领域新的突破口。
参考文献
[1] 陈慧岩, 陈舒平, 龚建伟. 智能汽车横向控制方法研究综述[J]. 兵工学报, 2017, 38(6):1203-1214.[2] J. P. Hespanha et al., “Trajectory-tracking and path-following of under-actuated autonomous vehicleswith parametric modeling uncertainty,”Transactions on Automatic Control, vol. 52, pp. 1362–1379, 2007. [3] 佐思产研, 谈谈无人车横向控制, https://mp.weixin.qq.com/s/pqs8UccxqfzaEnLauI7WBQ.
[4] R. Wallace, A. Stentz, C. E. Thorpe, H. Maravec, W. Whittaker, and T. Kanade, “First results in robot road-following.,” in IJCAI, pp. 1089–1095, 1985.
[5] Paden B, Čáp M, Yong S Z, et al. A Survey of Motion Planning and Control Techniques for Self-Driving Urban Vehicles[J]. IEEE Transactions on Intelligent Vehicles, 2016, 1(1):33-55.
[6] Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006.
[7] Snider J M. Automatic Steering Methods for Autonomous Automobile Path Tracking[J]. Robotics Institute, 2009.
美团无人配送原创内容均由美团点评无人配送部成员呕心沥血写成，写作不易，希望大家珍惜我们的劳动成果，如需转载，请后台留言申请授权。同时本文欢迎大家转发朋友圈，并评论留言，一起学习讨论。
▼扫一扫获取更多解读文章
近期美团无人配送有一场关于视觉技术的对外分享，有兴趣的同学可以点击【阅读全文】报名收听