[algorithm] 自动驾驶规划非线性优化学习系列之1 ：车辆横向运动动力学详细解释

写在前面

最近时空联合规划很火，想学习。由于在学校主打学习新能源电力电子方向，转行后也想好好零散的知识体系。计划从车辆运动动力学习，模型预测控制（经典控制目前看主打应用，不会再去深入），非线性优化开始梳理，到最后复现时空联合规划的论文。知识梳理会进行的比较快，实际复现和代码编写会慢慢来完成。

当中如果遇到和实际问题有关的细节知识，作为自己的未来解决方案储备也会强调一下。

目前计划借助的资料有（每本书阅读相关章节）：
在这里插入图片描述

车辆动力学及控制第二版

在这里插入图片描述

数值优化
在这里插入图片描述
模型预测控制

控制之美卷2

其他资料：深蓝学院：机器人中的数值优化

本文是车辆动力学及控制第二版学习的笔记，优先学习2-6章：

主要内容是：车辆侧向简单常用模型

第二章车辆侧向动力学

车辆侧向运动的运动学模型

在这里插入图片描述
图片来源 Vehicle Dynamics and Control 第二版

这里书上的推导思路复现如下左图
在这里插入图片描述
感觉书上求解目的性不强，这里把为求beta的过程直接利用r相等替换掉未知的r求beta

此外关于求yawrate的时候我感觉一个小三角形即在知道beta后不需要联立就可以没有必要如书中相加来求，可能是为了简化，可以快速估计，tan近似为线性，而lr+lf 可以直接用轴距而不需要找质心

如果后轮不能转向则有两种思考方式，那么还是按照左边求法，直接然后忽略后轮转角即可。或者同时由于忽略了后轮转角，角boc就是纯b，同时角obc也是直角，所以通过tan（前轮转角）=ob/ab 求出ob，再用勾股求出oc即为转弯半径。同时b也可求出了。有了r的同时我们还有v，所以i也可求出来w了。

对于以上这个模型的假设为：
在这里插入图片描述

总结：
在这里插入图片描述
图片来源 Vehicle Dynamics and Control 第二版

此外我对这个模型名字有一个疑惑，如果把车视为整个刚体的话，他有三个自由度，为什么叫二自由度模型呢？网上解说是因为横向运动是由纵向和转弯导致的。但是我依然认为如果认真来说整车为刚体且忽略z向和 pitch roll，这个刚体还剩三个自由度。

答：我觉得这个自由度和力学里不太一样。按照力学定义必然是三个自由度。但是这里由于该模型研究对象为横向。车辆纵向速度恒定的时候，只关注车辆yawrate和横向速度两个变化量。故称为”二“自由度。

补充忽略后轮转向和侧偏角时可以进一步简化

见下图左图
在这里插入图片描述
见我的总结

车辆侧向运动的动力学模型

对于高速的应用最好采用动力学模型，因为车轮速度方向不再好假设为车轮朝向。

这里要求一步轮胎侧偏角，其中前轮转角知道还要多求一步theta vf
在这里插入图片描述
这里如果认为车辆没有后轮转向则有

在这里插入图片描述
图片来源 Vehicle Dynamics and Control 第二版
注意轮胎有两个

整体推导见我的总结

主要是利用四个方程：两个轮胎侧向受力模型和横向受力平衡以及转矩平衡。用已知量：车身几何，魔术系数，来表达四个被选取作为车辆运动状态的状态量回头控制量间的关系。即状态转移矩阵和控制输入矩阵。见下图。

结果为
在这里插入图片描述
对于以上这个模型的假设为：车辆自身侧偏移角不大。

如果道路有侧倾

可以在横向受力里带入侧倾公式

在这里插入图片描述
在实际工程中，该角度可以从地图（如果有）或者车道线曲率和速度求得量和总线横向加速度之差获得。但是，试验过不那么准确。或者imu有这个自由度也可以测量。同时当坡度过大的时候，轮胎模型也需要改变，在书的第13章有介绍，但是不是本次学习重点，实际有需要的时候再行学习。

定义误差模型

表述车横向状态的空间状态方程服务目的改为基于横向误差得状态描述的话，需要做如下调整：

定义状态量为从车道中心线到车辆质心的距离，相对车道的车辆方向误差。

该模型要基于三个假设，车速恒定，半径恒定，半径r不是很小，让tan可以线性化。
在这里插入图片描述

结果为

结果可以整理为上图。

当所走路线为直线的时候则认为我期待一个不存在heading的状态（忽略beta侧偏角）那么Pi des 为0.

若考虑坡度则要再横向受力维度加上 sin bank
在这里插入图片描述
书中随后还有提供侧偏角的动力学推导方式，本人实际尚未使用，但原理和之前一样，带入受力和转矩平衡中，以及加入轮胎受力分析，这里要考虑车的速度方向与轴向不一致带来的不同，截图如下（同时还有侧倾角度）在大速度和大弯道下未来可以尝试一下。在MPC建模的时候考虑。
在这里插入图片描述
书中2.7的坐标转化本人理解所讲述意思，但是实际中坐标车辆坐标系和全局坐标系转换另有方法，待未来遇到这种做法再行研究，因为这里的e1 e2 不是一个直观的量，需要量测手段来求
而2.8 我接触的常见道路模型有散点，有三次多项式，有向量。暂时没有接触，且与我复现论文的最终目标关系不大，此两章节省略。

第三章自动车道保持系统的转向控制

该章节本意在分析使用pid的稳定性和可控性。实际中车辆难以被优化为一个线性时不变系统，同时也不写传递函数，pid一版是在估算前馈后直接调参，虽然有很多工程细节，但是再次收敛于我的目的，不过多研究学习。同时控制我们在使用MPC时候，发现稳定性会比pid好很多。

动力学方程的稳态误差

对于在这里插入图片描述
控制输入为前轮转角 delta，其中把delta是写成状态加控制律的形式，则有

PI des之前提到是道路弯道有关，如果是直线则不用考虑。
书中研究使用前馈是否可确保弯道行驶的零稳态误差。文中做了拉普拉斯变换以及matlab求解，这里不深入，结果是可以确保稳态横向误差为0但是heading稳态不能保证。
在这里插入图片描述
红框下我的理解是，稳态过弯道的时候，可以通过前馈来让横向误差为0，但是方向误差会存在。

用几何法来找到稳态转向角的方法见下图
在这里插入图片描述

基于假设半径很大，但是可以有轮胎侧偏角（带入横向动力学），且车辆处于稳态
在这里插入图片描述
则有

之后较为细节的讨论不做深入。只用PD不开积分足以解决大部分问题。侧倾和死区可以略微开I但是I要比较小做好饱和处理和过点清零工程我见过的简单应用基本到此为止，未来有机会再深入。
一个闭环的pid带前馈的示意图（实际我们用mpc了）
在这里插入图片描述