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