自动驾驶控制算法：前馈控制与航向误差详解
自动驾驶控制算法：前馈控制与航向误差详解
本篇博客是自动驾驶控制算法系列的第六节，主要讲解前馈控制与航向误差。内容整理自B站知名up主忠厚老实的老王的视频，作为学习笔记分享给大家共同学习。
引言
本篇博客是自动驾驶控制算法系列的第六节。内容整理自B站知名up主忠厚老实的老王的视频，作为学习笔记分享给大家共同学习。
在上一节介绍了离散LQR以及连续LQR，分别对应离散系统和连续系统。LQR的核心就是求黎卡提方程的解，求出来之后就可以算出最优控制。
一、反馈控制
如果是连续系统，可以用连续LQR算出控制量。当然也可以将连续系统离散化，用离散LQR再算出控制量，这两个控制量可能不完全一样，但应该非常接近，因为无论是连续系统还是离散系统，都是对相同物理现象的不同描述，所以算出来的控制量应该非常接近。
为什么叫反馈控制呢？什么叫反馈？可以简单解释一下。系统的框图如下：
代表积分，即经过此模块就变成了。可以把看成输入，把换成看成输出，即输入什么样的，就会得到什么样的。那什么叫反馈呢？LQR算出来实际上就是在图中这条线上，加一条红色线。比如这样：
决定输出。有反馈的话就是先是输入决定，然后又反过来决定输入，这就是反馈的意思。
但这种反馈，容易出现代数环问题，即输入直接影响输出，而输出又直接影响输入，那就变成先有鸡还是先有蛋的问题。不过现在还碰不到代数环问题，等碰到时再细说，本篇博客的任务就是讲前馈控制。
二、前馈控制的引入
先来看以下框图：
左边再加。为什么要加前馈控制呢？
遗留下来的小尾巴，误差微分方程为：
观察微分方程可以发现，无论取何值，误差和误差的导数都不可能同时为。但我们希望误差可能一开始不是，经过反馈控制让它慢慢变成，然后就一直是，同时误差的导数等于，根本就不是微分方程的解。
，误差的导数也是，这种状态根本不是方程的解，能控制的其实就是反馈的，但是怎么调
所以要引入前馈控制，即针对系统
令，其中，是由LQR计算出来的反馈控制，就是用把尾巴去掉算出来的，那么
三、前馈控制与稳态误差
1、稳态误差的定义与影响
稳态误差就是稳定状态的误差，稳定就是代表误差变成常数，不会再变了。
，因为不是方程的解，存在稳态误差。LQR控制系统最后一定会稳定，稳定下来最终结果就是误差不再变了，即误差的导数，并且误差本身不是
为什么LQR最终会导致经过一段时间后误差就稳定不变了，系统就稳定下来了呢？
如果系统是不稳定的，那么LQR是没有解的；如果系统是LQR有解，就是k能算出来，那就意味着一定可以通过LQR使系统达到稳定状态。
那什么叫LQR有解？什么叫LQR无解？
如果看过第五节就会明白，解LQR时需要解黎卡提方程，通过迭代出来，如果有解的话，就意味着迭代收敛，黎卡提方程的解不会再变。如果系统是不稳定的，黎卡提方程迭代就会发散，LQR就没有解。
2、稳态误差与前馈控制的关系
回到刚才说的误差，误差最终等于多少？
代进去算一下，把误差的导数等于0，带到误差方程里：
最终会得到稳态误差：
引入前馈控制之后误差的导数：
，代进去：
现在目的就很简单了，选取合适的，使得稳态误差尽可能为
注意：
四、前馈控制的计算与优化
1、前馈控制表达式推导
具体的表达式：
其中，是矩阵，是矩阵，也是
下面用软件Mathematical进行矩阵求逆运算：
其中，
根据Mathematic得到的化简结果，再进行进一步化简，最后得到误差：
列向量的第一行和第三行都有，这就是对误差的影响。写到这其实就很明显，前馈控制表达式：
此时，。其中，是反馈行向量中的。
，然后再算前馈。
2、航向角误差的近似处理
再来看一下误差的第三行：
可以发现不受和的影响，表达式里没有，也没有。即无论前馈和反馈取什么值，都永远不可能为零。因为能控制的就是前馈以及反馈。
的表达式，似乎感觉能控制的只有，因为侧偏刚度是负的，如果取特定值，那么有可能，意味着
注意：
不是航向误差，定义的是，而航向误差应该是。我们的目的是想让航向误差和横向误差都为：
- 横向误差为：可以通过前馈控制。就是取那后面那一坨东西解决，横向误差可以为。
- 航向误差为0：航向误差如果按真正的定义，那么就不为。的稳态物差应该是才对，那么问题是现在式子是不是等于。
表达式进行更进一步的化简，在第四节讲到和它的投影之间的关系：
和
在直角坐标系下曲率的计算式：
大家都非常熟悉，但是曲率有定义式：
这是曲率最原始的定义式，由此可得：
其中，，，，假设车辆没有漂移，所以：
这样直接得到，那么：
又因为可得：
又因为无漂移的假设，所以把直接忽略掉：
所以
到这一步离最终结果越来越近了，因为是侧向力，除以是侧边角，但是这样还是不够，因为是总侧向力，包括前轮和后轮，而
怎样才能得到后轮的侧向力呢？
3、车辆质量等效处理
对车辆质量进行等效处理
是不是感觉有点熟悉？比如有个质量块：
质量为m，质心到前边的距离为a，到后边距离为b。只考虑质心的话，可以完全等效成叠加的两个质量块，质量是和。和，使得质心和原来大质量块的质心完全一样，并且，如果能做到这一点，在质量分布维度上，这两个东西完全等效。
以质心为原点建立坐标系，等效前提是
所以
这就正好可以把四轮车把按照质心分成上半和下半：
上半质量为，下半质量为，就等于后轮侧向力，严格来说应该是后轮侧向力之和。因为我们把汽车模型简化为自行车模型：
是单个轮子的侧向力，并且自行车模型的侧偏刚度是单个轮的侧偏刚度的两倍，即把两个轮子合并成一个轮子，可直接得到：
4、航向误差与侧偏角的关系推导
自行车模型如下：
，因为用的是右手系，以左为正，以右为负，在轮胎中轴线右边，所以是。
，根据弧度的定义，上面的角度，所以
所以
这样正好：
- 不是航向误差，，航向误差是
- 的稳态误差为，这样的话会得到
这就是想要的结果。不可能通过和去调节，但是不用去理会事情，因为最终的目的是，就意味，而推导出来的稳态误差正好就是。
5、前馈控制表达式化简
前馈控制表达式：
若令，则可进一步化简为
6、反馈与前馈控制的结合
：
- 通过LQR算出来，称为反馈控制
- 根据上面公式算出来，称为前馈控制
通过反馈和前馈控制就可以将误差变成不是航向误差，但最终导致航向误差为。
五、总结
本篇博客讲解了航向误差以及前馈控制。下一节会讲基于离散Frenet坐标系的规划点误差的计算。
，通过LQR解决，，误差在前面第四节讲解过误差如何计算，但遗憾的是基于连续曲线的误差，而一般规划点都是离散的。
所以还要再讲离散点误差的计算，讲完之后一切准备工作就完成了，在后续博客中会讲具体的横向控制算法，代码编写以及联合仿真。
本篇博客的内容到此结束，欢迎关注后续内容！
参考资料
【基础】自动驾驶控制算法第六讲 前馈控制与航向误差