【自动驾驶】控制算法（六）前馈控制与航向误差

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

文章目录

• 引言
• 一、反馈控制
• 二、前馈控制的引入
• 三、前馈控制与稳态误差

• 1、稳态误差的定义与影响
• 2、稳态误差与前馈控制的关系

• 四、前馈控制的计算与优化

• 1、前馈控制表达式推导
• 2、航向角误差的近似处理
• 3、车辆质量等效处理
• 4、航向误差与侧偏角的关系推导
• 5、前馈控制表达式化简
• 6、反馈与前馈控制的结合

• 五、总结
• 参考资料

---

引言

  本篇博客是 自动驾驶控制算法 系列的第六节。内容整理自 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 解决，

，误差在前面第四节讲解过误差如何计算，但遗憾的是基于连续曲线的误差，而一般规划点都是离散的。

  所以还要再讲离散点误差的计算，讲完之后一切准备工作就完成了，在后续博客中会讲具体的横向控制算法，代码编写以及联合仿真。

  本篇博客的内容到此结束，欢迎关注后续内容！

---

参考资料

   【基础】自动驾驶控制算法第六讲 前馈控制与航向误差

---

后记：

🌟 感谢您耐心阅读这篇关于 前馈控制与航向误差 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡
🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀