--关注、星标、回复“自主泊车”--
↓↓领取：自主代客泊车系统总体技术要求↓↓
原文链接：https://reurl.cc/WDW6Ze
简介
1.1.无人驾驶车辆
大到类似百度apollo无人车，无人配送车；小到扫地机器人，其实都是无人驾驶车辆，都是一个复杂的软硬件结合系统。
其软件算法模块主要包含：感知、定位、轨迹预测、行为决策、运动规划及反馈控制等，在这里介绍和补充的是反馈控制部分
欢迎关注「智驾最前沿」微信视频号
部分软件实现可参考：个人开源项目path-following-control
1.2.横向控制和纵向控制
反馈控制是无人车最底层的软件算法模块，包括：
横向控制，主要用于车辆方向盘的控制
纵向控制，主要用于车辆油门、刹车的控制
两者协同工作以使无人车按照预定的参考轨迹行驶，其中，横向控制根据上层运动规划输出的路径、曲率等信息进行跟踪控制，以减少跟踪误差，同时保证车辆行驶的稳定性和舒适性
1.3.横向控制
根据横向控制使用车辆模型的不同，可以将其分为两种类型，包括：
无模型的横向控制方法
基于模型的横向控制方法
1.3.1.无模型横向控制
无模型的横向控制即传统的PID控制算法，将车辆当前的路径跟踪偏差作为输入量，对跟踪偏差进行比例(Proportion)、积分(Integration)和微分(Differentiation)控制得到转向控制量，但该算法由于没有考虑车辆本身的特性，因此算法对外界干扰的鲁棒性较差，无法满足车辆在高速行驶过程中的有效控制。
1.3.2.基于模型横向控制
而其中基于模型的方法又可分为：
基于车辆运动学模型的横向控制方法
基于车辆动力学模型的横向控制方法
因此，将主要介绍包括：
基于车辆运动学模型的纯跟踪控制(PurePursuit)算法
后轮反馈控制(Rearwheelfeedback)
前轮反馈控制(Frontwheelfeedback)算法
基于车辆动力学模型的线性二次型调节器(LinearQuadraticRegulator，LQR)控制算法
基于车辆运动学模型的控制方法
2.1.车辆运动学模型
基于车辆运动学模型的控制方法主要考虑了车辆的运动学约束对车辆控制的影响
首先了解一下常见的车辆运动学模型：
平时见到的汽车属于阿克曼转向，通常可以简化为两轮的自行车模型
其假设车辆的前后轮均为刚体，只能沿着车轮滚动的方向前进，无侧向滑动
由上述假设可得自行车模型的运动微分方程如下式：
角速度和角速度的关系：v=ωr，即ω=v/r，因此这里ψ˙=v*tan(δ)/L，其他的比较简单，就不加以说明。
2.2.纯跟踪控制算法
纯跟踪控制算法（PurePursuit）是一种典型的横向控制方法，最早由R.Wallace在1985年提出，该方法对外界的鲁棒性较好。
该算法的思想：
基于当前车辆后轮中心位置，在参考路径上向llld（自定义）的距离匹配一个预瞄点，假设车辆后轮中心点可以按照一定的转弯半径行驶抵达该预瞄点，然后根据预瞄距离Id，转弯半径，车辆坐标系下预瞄点的朝向角之间的几何关系来确定前轮转角。
弧的弧度为2是因为这个三角形是等腰三角形，边长为，而其中一个角为π/2-，所以这个弧度为π-（π/2-）*2=2。
如图所示，弧线为经过车辆后轮中心以及预瞄点的圆弧，圆弧的半径与朝向角和预瞄距离llld有关，其实也就是与(xg，yg)有关
作一条辅助线d，垂直于，则：
根据图中的几何关系，,,与ld的关系为：
再作一条辅助线，垂直于ld，则：
根据图中的几何关系，,,与ld的关系为：
根据运动学方程(2-1)中ψ˙=v∗tan(δ)/L，
则tan(δ)=ψ˙∗L/v，而在这里ψ˙/v=，即：
再把式(2-4)带入(2-5)可得前轮转角为：
2.3.后轮反馈控制算法
后轮反馈式（Rearwheelfeedback）是利用后轮中心的路径跟踪偏差量来进行转向控制量计算的方法。
根据运动学方程(2-1)及车辆后轮与参考路径的几何关系，可推导出参考路径坐标系(,)。
Ferent坐标系下(,,ψ\psiψ)的变化率为：
Frenet坐标系使用道路的中心线作为参考线，使用参考线的切线向量和法线向量建立坐标系，那么基于参考线的位置，就可以使用纵向距离和横向距离来描述任意位置，同时纵向和横向的速度、加速度、加加速度等信息也更便于计算。
对于二次连续可导的参考线，需要设计车身横摆角速度保证在李亚普洛夫方程下局部渐进收敛：
李亚普洛夫稳定（一个分析系统的稳定性方法）用数学的语言描述为：
可控可微分的状态方程x˙=f(x,u)
在给定的参考轨Xref(t)下，存在∀ε>0，t10，满足下列条件：
对于李亚普洛夫稳定也分为渐进稳定和指数稳定，李亚普洛夫稳定性理论是从能量的观点出发的，系统的能量随着时间的推移不断变化（增加or减少），在有限时间内，系统趋于稳定，能量不再变化，此时，系统的k+1时刻的能量减去k时刻的能量是<0的，即V(k+1)-V(k)<0，V代表能量函数。
渐进稳定指对于时变系统，在条件(1)下独立于时间t1，指数稳定指收敛率是以指数下降，对于后轮反馈式算法，为了保证车辆的李亚普洛夫稳定性，车身横摆角速度可表示为式(2-10)。
其中，ψ为横摆角偏差反馈控制增益，e为横向位置偏差反馈控制增益，因此，根据运动学方程(2-1)，可得到前轮转角为:
2.4.前轮反馈控制算法
前轮反馈控制（Frontwheelfeedback）又称Stanley控制，其核心思想是利用前轮中心的路径跟踪偏差量对方向盘转向控制量进行计算。
前轮中心横向位置跟踪偏差的变化率为:
式(2-12)中可以看出前轮横向位置偏差的变化率e˙可以直接通过前轮转角进行控制，当定义e=−k后，可以保证前轮横向位置偏差满足李亚普洛夫的指数化收敛，即：
李亚普洛夫指数：在相空间中相互靠近的两条轨线随着时间的推移，按指数分离或聚合的平均变化速率。
关于arcsinarcsinarcsin函数：
那么对于式(2-13)来说，当∣−ke/vf∣>1时，函数无解。
对于有限域内的指数收敛可以放宽到局部指数收敛，从而将前轮转角定义为：
Stanley前轮反馈控制算法在变曲率路径且路径连续可导的条件下，可以满足局部指数收敛特性，但也可以看出该算法不适用于倒车的场景，导致对于自动泊车等工况不适用。
基于车辆动力学模型的横向控制
基于车辆运动学模型的方法忽略了车辆的动力学特性，因此在车速过快或者曲率变化率过大的情况下该算法无法满足车辆的稳定性控制要求，这里介绍一种典型的基于车辆动力学模型的横向控制方法，包括
车辆线性二自由度动力学模型的构建
车辆路径跟踪偏差状态方程的构建
LQR横向控制算法的求解
3.1.车辆线性二自由度动力学模型
对于基于车辆动力学模型的控制方法，首要的工作是对车辆动力学进行建模，由于车辆动力学模型具有强非线性时变的特性，因此精确建模整个车辆动力学十分困难，在横向控制中，主要关心的是车辆横向运动特性。
通常将车辆动力学模型简化为的二自由度横向动力学模型，假设车身的纵向速度Vx保持不变,其横向动力学模型的两个自由度为：
横向运动
横摆运动
由于精确的二自由度动力学模型是非线性的，为了便于进行实时的跟踪控制计算，通常还需要在精确的二自由度动力学模型基础上进行一些简化近似，得到线性二自由度动力学模型。
这里介绍基于转向小角度及轮胎动力学线性化假设下的车辆线性二自由度动力学模型，下图为车辆为车辆横向动力学坐标系示意图。
假设道路平整(不考虑道路横向的倾斜)，且转向角度较小时，则车身轴方向应用牛顿第二定律可得：
其中，是整车质量，y是车辆质心处的横向加速度，Fyf，Fyr为地面给前轮胎和后轮施加的横向力，而横向加速度y由两部分组成：
车辆沿车身轴横向运动产生的加速度y˙
车身横摆运动产生的向心加速度Vxψ˙
因此：
将(3-2)代入(3-1)可得：
然后，车辆绕轴的转矩平衡方程为：
其中，lf,lr分别是前轮轴和后轮轴到质心的距离。
接下来对横向轮胎力Fyf，Fyr进行计算，实验结果表明，轮胎侧偏角较小时，地面施加给轮胎的横向力(侧偏力)与轮胎侧偏角成线性关系(侧偏角是车轮速度方向与车轮纵轴的夹角)。
前轮侧偏角为:
θvf是前轮胎速度方向与车身纵轴的夹角，为前轮转向角。
后轮的侧偏角为:
θvr是后轮胎速度方向与车身纵轴的夹角，因此，前轮(2个)横向轮胎力为:
Cf是前轮侧偏刚度(corneringstiffness)
同样，后轮(2个)横向轮胎力为:
Cr是后轮侧偏刚度
计算θvf与θvr：
θvf与θvr较小时可近似得到:
将(3-5)，(3-6)，(3-7)，(3-8)，(3-11)，(3-12)代入(3-3)和(3-4)可得线性二自由度模型的状态方程:
3.2.车辆路径跟踪偏差状态方程的构建
有了式(3-13)的系统状态方程之后，就可以分析出在给定的前轮转角输入下，车辆的横向位移、横向速度、横摆角以及横摆角速度的响应。但是横向跟踪控制的目的是为了减小跟踪偏差，需要的状态方程是能够分析在给定的前轮转角下车辆跟踪偏差的响应，因此些偏差通常是：
横向位置偏差1及横向位置偏差变化率e˙1
横摆角偏差2及横摆角偏差变化率e˙2
下面介绍具体的转换过程
车辆期望横摆角速度:
期望的横向加速度ydes为:
则横向速度偏差e˙1为:
因此横向位置偏差变化率e˙1(横向速度偏差)为：
此外，横摆角偏差2为:
将式(3-16)，(3-17)，(3-18)代入(3-3)，(3-4)化简后可以得到新的状态方程:
即：
进一步忽略ψ˙des项之后:
上式(3-20)就是横向控制所需的路径跟踪偏差状态方程
3.3.LQR横向控制算法的求解
基于前面推导的路径跟踪偏差系统状态方程(3-20)，就可以分析在前轮转角控制输入作用下，车辆路径跟踪偏差的响应特性，期望的响应特性就是跟踪偏差能够快速、稳定地趋近于零，并保持平衡。同时前轮转角控制输入又尽可能小，这就是一个典型的多目标优化最优控制问题，且优化的目标函数可以表示为跟踪过程累计的跟踪偏差与累计的控制输入的加权和，如式(3-21)所示：
其中，为半正定的状态加权矩阵,为正定的控制加权矩阵，且,通常取为对角阵。
若只是x和δ做为代价，那么存在x的正误差被δ的负误差消除。相反，自身相乘，这样负值也会产生正平方，称这些为二次项。矩阵元素变大意味着希望跟踪偏差能够快速趋近于零，矩阵元素变大意味着希望控制输入能够尽可能小
设M是n阶方阵，如果对任何非零向量z，都有zTMz>0，其中zT表示z的转置，就称M为正定矩阵。
则式(3-21)目标函数中，第一项优化目标∫0∞(xTQx)dt表示跟踪过程路径偏差的累积大小，第二项优化目标∫0∞(δT Rδ)dt表示跟踪过程控制能量的损耗，这样就将横向控制问题转化为一个最优控制问题：
求解最优的前轮转角控制输入δ*，使得式(3-21)目标函数可以取极小值
而式(3-21)是一个关于状态变量和控制输入的二次型目标函数，对它的优化求解是一个典型的LQR最优控制问题，根据LQR最优控制理论，对式(3-21)目标函数的优化求解，解出的最优控制规律δ\deltaδ*是关于状态变量的线性函数:
其中，d和Bd代表状态方程(3-20)离散化后的，B1矩阵,是式(3-23)黎卡提方程的解:
因此，根据式(3-22)可以设计一个状态反馈调节器，通过状态反馈实现闭环最优控制，如图所示
根据求解的状态反馈调节器增益，则可以得到最优的状态反馈前轮转角δfb：
由于在LQR状态反馈控制求解过程中没有考虑路径本身的动态变化特性，因此只有式(3-25)的反馈控制输入时LQR控制在曲线行驶时会存在稳态误差。
稳态误差是系统从一个稳态过渡到新的稳态，或系统受扰动作用又重新平衡后，系统出现的偏差。
为了消除该稳态误差，还需要加上一个前馈控制输入量δff，前馈控制考虑了路径的曲率及车辆的转向不足特性
其中，KV为不足转向梯度，表征了车辆的转向特性，e2ss为稳态时的角度跟踪偏差，k3为矩阵中的角度偏差反馈控制增益部分
因此，总的LQR前轮转角输入为:
总结
本文主要介绍了基于车辆运动学和动力学模型的横向控制算法，从上文对两类方法的分析可以看出，两类方法在应用场景上都存在一定的局限性，并具有较强的互补性，因此在实际使用中，综合使用这些方法会更有效。
无模型的PID横向控制算法参数少，简单易用，但是由于没有考虑车辆系统动力学特性及路径本身的动态变化特性，对外界干扰的鲁棒性较差，在高速或曲率较大的弯道场景时，会出现较大的跟踪误差和“画龙”现象。因此，该方法比较适合应用于低速曲率较小的路径跟踪控制场景中。
基于运动学模型的横向控制算法中，Pure Pursuit和Stanley前轮反馈算法在中低速场景下，他们的路径跟踪的性能较好，Pure Pursuit在大的跟踪误差和非连续的路径场景下鲁棒性较好，其控制的关键在于对最佳前向预瞄距离的确定。其中，增大前向预瞄距离将提高车辆控制的稳定性，但随之会带来路径跟踪性能降低及稳态误差增大的后果，表现出转弯内切现象。
相比于Pure Pursuit算法，Stanley前轮反馈算法还额外考虑了横摆角偏差，因此在大多数场景下，跟踪性能更佳。然而，由于没有设置前向预瞄，Stanley算法会出现转向过度的情况，与Pure Pursuit和Stanley算法相比，后轮反馈控制算法计算更加复杂，对路径的平滑性要求更高。
在中等速度下的跟踪性能及鲁棒性与Stanley方法近似，然而在速度较大时，稳态误差也会变大，从而导致控制效果不佳，LQR算法使用二自由度动力学模型来设计横向控制器，与前述基于运动学模型的几种算法相比，LQR参数调节更加复杂。其不仅需要获取车辆自身的模型参数，还需要调节LQR目标函数的Q，R矩阵以获得较优的跟踪性能。
LQR算法的优点在于，通过与转向前馈进行有效结合，LQR能够很好的解决曲线行驶时的稳态跟踪误差，在中等速度曲线行驶时其稳态误差趋近于零，从而极大提升跟踪性能，LQR非常适用于路径平滑的高速公路及城市驾驶场景，具有较好的车辆高速控制性能。
但是，由于模型的固有缺陷，LQR与前馈控制的结合也无法解决所有跟踪控制问题，由于该方法采用基于简化后的二自由度动力学模型，因此当车辆运动不满足二自由度动力学模型转向小角度，或轮胎动力学线性化的假设条件时，LQR算法的跟踪性能会大幅降低，从而导致控制失效。