概述：本文主要学习车辆横向控制的相关应用——几何横向控制。
文章目录
前言
本文将继续深入研究自动驾驶车辆基于几何模型的横向控制。
目前主流的横向控制方法主要有两种：
- 基于几何追踪的方法
- 基于模型的方法
一、纯追踪法（Pure Pursuit）
纯追踪法是一种实际广泛使用的纯几何的追踪方法，在低速行驶的状态下有比较好的追踪效果。
回顾：
- 自行车模型：四轮简化为两轮模型进行设计，假设在平面上行驶；
- 阿克曼几何模型：简化了前轮转向角和后轮遵循的曲率关系。
1.1 模型建立
假设前轮转向角为δ，后轮滑过的半径是R（遵循圆），如图所示。
可以得到：
上述公式，可以在车辆低速行驶下，对于车辆的运动进行简单的估计。
1.2 算法思想
纯追踪法主要基于自行车模型和阿克曼几何模型进行算法设计。
基本思想：参考人类驾驶行为，以后轮为基点，通过前轮进行转向，使车辆沿着经过预描点的圆弧进行行驶。
如图所示。
如上图，其基本思想就是车辆以后轮为切点，车辆纵轴（车身）为切线，通过控制前轮的转角，使得车辆沿着过目标点的圆弧进行行驶。
其中：
- 目标点（gx,gy），目标点到后轮的距离ld就是前视距离；
- 路径，如图path曲线所示；
- α，目标点到后轮的连线与车辆航向角的夹角；
1.3 算法解析
基于上述几何关系，解析转向角与目标点关系。
如图所示。
（1）
基于三角函数正弦定理，可知：
我们找到了半径R和前视转向角α之间的关系：
（2）
结合自行车、阿克曼模型建立的表达式，可以得到：
（3）
可以推出：
（4）
构建三角形AGB，如图所示
其中，eld为横向偏离误差。则根据三角函数可以列出表达式：
将其带入（3）式中可得：
综上上述，纯追踪法实际上就是一个比例控制器。这样，我们建立了转向角与当前横向偏差及L之间的关系。
1.4 算法步骤
- 确定车辆目前的位置（可以拿到车辆的实时位置）；
- 确定车辆最近的路径点（在多等距目标点情况下进行分析）；
- 确定目标点（以车轮后轮质心为中心，ld为半径，画圆，与path（规划路径）相交的点）；
- 将目标点转换到车辆坐标系；
- 运用纯追踪法表达式计算出车辆的转向角，并控制车辆进行转向运动；
- 根据单位时间内运动更新，更新车辆实时状态。
1.5 算法影响因素及调节
-
前视距离ld
从算法表达式可以看出，影响pure pursuit最重要的一个量就是前视距离ld，它会直接影响到车辆转向角的大小，从而影响车辆对轨迹追踪的一个能力。
ld较小，则追踪更加准确，由于它更加靠近车辆，所以比较好追踪。但是需要不断更换目标点，会导致系统的稳定性会随之降低。ld较大，它会追踪更长时间，表现更稳定，但是其准确性会降低。 -
前视距离和速度相关联：
如图所示，ld=kv(t)，随着速度增加，ld会越来越大，可以提高纯追踪法适应不同速度的能力，这是一种非常常用的方法。
系数k太小，会造成不稳定，k太大时准确性会降低。
所以，可以设置最大最小值范围来保证其稳定性和准确度。
由于纯追踪法与路径没有直接关系，所以对于不连续的路径由很好的鲁棒性，稳定性和准确性高。
1.6 难点和局限性
- 如何选择一个最佳的前视距离（look-ahead distance）
车辆追踪轨迹完全依靠ld（goal point），与路径的曲率没有关联，这会导致车辆在没有到达预描点时，已经更换了预描点，最终导致车辆对于曲线路径的追踪效果误差增大。
所以，在选择ld时，可以把路径的曲率或者横向追踪误差考虑进来，进一步提高纯追踪算法的鲁棒性。 - 在特定路径上不要过度追求准确性/稳定性
因为车辆追踪的是前方轨迹，在曲线上总会有误差，过度在某一路径上调节算法的准确性可能导致其它路径下的稳定性降低。反之，如果过度追求稳定性可能导致追踪的准确性降低。 - 在高速情况下稳态误差会变大。
与速度关联，速度增加，ld增加，其误差也会变大，即使不与速度关联，速度增加，ld更换频率增加，预描点更新频率加快，车辆吗没有达到预描点时，已经对新的预描点进行轨迹追踪，这样也会导致其误差变大。
二、Stanley Method
Stanley方法也是一种基于几何路径追踪的控制方法，广泛适用于自动驾驶汽车的追踪控制。
它考虑了基于路径的全局收敛性和上述pure pursuit的不足点即误差不随速度的变化而变化。
2.1 特点
- 以前轮中心为参考点，不是以质心或者后轮中心为参考点，性能更优；
- 没有考虑前视距离，而是基于前轮中心直接考虑横向误差和导航角的误差。（导航角：前轮到路径最近点的切线方向平移到前轮中心与横摆角夹角，横向误差：两条切线的距离（平移量））。
- 直观的控制器：矫正heading error 、position error、限制转向角的范围。
2.2 方法解析
由上述三个特点进行算法解析：
（1）为了消除路径的导航误差（heading error ，θe)，假设转向角等于转向误差。
假设没有横向误差e，前轮只需转过导航角θe就可以完全追踪参考轨迹。
（2）为了消除横向追踪误差e，可以构建三角形，∠ABC为δe：
由图可知，AC为横向误差，假设BC的距离为d，则:
可知，d是一个与速度v有关的量，再添加一个与速度相反比例增益K（可以根据实际场景进行调整），则：
所以，
则：
横向误差就体现在δe上的误差。
（3）转向角保持在最大值和最小值之间：
反正切函数控制在对称的角度范围。
综上可以得出：
即最终的转向角就是对导航角误差的补偿加上对横向误差的补偿，最后把转向角控制在最大最小值的范围之内。
2.3 不同误差信号的转向角分析
（1）导航角误差很大
此时，要想通过车轮转角纠正与轨迹的错位就会有很大导航误差。
当导航角大于前轮最大转向角时，控制器就会使用最大的转向命令，直到导航角回到前轮能够纠正的平衡范围内。
（2）横向误差很大
由于横向误差很大，反映到δe上，可知该角度趋于Π/2，
假设，导航角θe很小趋近于0，车辆方向趋于轨迹方向（平行），此时主要误差是横向误差，要使车辆靠近轨迹，所以转向角设置一个最大范围：
当车辆向轨迹方向偏转时，此时导航角误差会负向增大，当增大到-Π/2时，航向角误差和横向误差相互抵消：
此时，转向角为0，车辆向轨迹方向直线行驶，当行驶过程中横向误差减小：
此时，再纠正航向角的误差，使其与轨迹方向对齐。
2.4 稳定性分析
分析Stanley控制器稳定性时，首先要建立横向误差的变化率（不在最大转向角时）：
其中， 假设d(t)为1，e为ke/vf，则斜边v：
所以：
把上述项带入变化率表达式，可得:
假设横向误差比较小时，分母可以近似等于1，则：
可知，当k越大时，可以加快误差的纠正。
从相轨迹图可以看出，它的误差范围是在一个稳定的边界变化：
2.5 方法调整
实际应用中，stanley依然是一个基于几何模型的追踪控制器，没有考虑过车辆噪声的测试、执行器动力学、轮胎力效应等方面，这些方面会在stanley控制行驶过程中产生不良特性，所以需要对其性能进行调整从而避免这些问题。
- 低速情况下，速度误差会对转向执行命令造成很大的影响，使车辆剧烈摆动 ，影响驾驶舒适性。
解决方法：在横向误差项加入一个大于0的常数Ks（可以调整），确保该项始终有一个最小值，保证低速行驶的稳定性。
- 高速情况下，即使有速度关联，但是其响应还是过于激进，所以需要更缓慢的变化，以至于侧向力不会过大。
解决方法：在导航角误差项加入一个PD控制器（阻尼项），保证高速行驶缓慢平稳。 - 没有考虑路径的曲率，导致车辆追踪性能下降。
解决方法：引入前馈项（与路径曲率转向角一致），提高车辆轨迹追踪的性能。
综上所述：其修正Stanley表达式：
其中，δff为前馈项，和实际路径曲率kappa相关联。
这样就可以使Stanley控制器成为一个更加舒适准确的控制器。
三、方法对比
pure pursuit 和stanley方法对比：
相同点：
- 基于同样的运动学模型，执行同样的任务；
- 具有同样的通过调节前进速度来调整增益的特性；
- 都有比例控制信号的反正正切部分；
- 都不能针对某个场景做过度调节；
- 速度增加，都会影响追踪准确性。
不同点：
- Stanley比较直观具有两个独立的部分，导航角误差补偿和横向误差补偿；
- Stanley没有前视距离；
- 在转弯时，Stanley转大弯，pure pursuit转小弯（cut corners）；
总结
本文主要是对于自动驾驶规划控制学习中的横向控制理论知识进行学习，主要介绍了两种几何横向控制算法：Pure pursuit和Stanley，详细介绍了其算法解析以及算法调整，最后对两者进行对比。这篇文章希望可以对想要学习自动驾驶规划控制方向的同学们有一定的帮助。
喜欢的朋友们动动小手点个关注，我会定期分享我的一些知识总结和心得体会，感谢大家!