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

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝


🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

---

文章目录

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

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

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

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

• 五、总结
• 参考资料

---

引言

  本篇博客是 自动驾驶控制算法 系列的第六节。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  各位小伙伴们大家好，本节博客讲解前馈控制与航向误差。在上一节介绍了离散 LQR 以及连续 LQR，分别对应离散系统和连续系统。 LQR 的核心就是求黎卡提方程的 $P$，求出来之后就可以算出最优控制 $u =-k x$。

---

一、反馈控制

  如果是连续系统，可以用连续 LQR 算出 $k$ ，当然也可以将连续系统离散化，用离散 LQR 再算出 $k$ 出来，这两个 $k$ 可能不完全一样，但应该非常接近，因为无论是连续系统还是离散系统，都是对相同物理现象的不同描述，所以算出来的 $k$

$u =-k x$

为什么叫反馈控制呢？什么叫反馈？可以简单解释一下。

$\dot{X}=AX+Bu$ 的系统，画出框图如下：

$\frac{1}{s}$代表积分，即 $\dot x$ 经过此模块就变成了 $x$。

$u$ 看成输入，把 $x$ 换成看成输出，即输入什么样的 $u$，就会得到什么样的 $x$，那什么叫反馈呢？LQR 算出来 $u =-k x$ 实际上就是在图中 $x$ 这条线上，加一条红色线。比如这样：

$u$ 决定输出 $x$，有反馈的话就是先是输入 $u$ 决定 $x$，然后 $x$ 又反过来决定输入$u$，这就是反馈的意思。

  但这种反馈，容易出现代数环问题，即输入直接影响输出，而输出又直接影响输入，那就变成先有鸡还是先有蛋的问题。不过现在还碰不到代数环问题，等碰到时再细说，本篇博客的任务就是讲前馈控制。

---

二、前馈控制的引入

  先来看以下框图：

$B$ 左边再加 $\delta_f$，$\delta_f$

为什么要加前馈控制呢？

$\dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_{r}$ 遗留下来的小尾巴 $C\dot \theta_r$

$u=-ke_{rr}$，误差微分方程为：
$\dot{e}_{rr}=\left( A-BK \right) e_{rr}+C\dot{\theta}_r$  观察微分方程可以发现，无论 $k$ 取何值，误差 $e_{rr}$ 和误差的导数 $e_{rr}$ 都不可能同时为 $0$，但我们希望误差可能一开始不是 $0$，经过 $k$ 反馈控制让它慢慢变成 $0$，然后就一直是 $0$

$0$，同时误差的导数等于 $0$，根本就不是微分方程的解。

$0$，误差的导数也是 $0$，这种状态根本不是方程的解，能控制的其实就是反馈的$k$，但是怎么调 $k$

  所以要引入前馈控制，即针对系统
$\dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_{r}$  令 $u=-kx+\delta_f$，其中，$-kx$ 是由 LQR 计算出来的反馈控制，就是用$\dot X=AX+Bu$ 把尾巴去掉算出来的 $k$，那么 $\delta_f$

---

三、前馈控制与稳态误差

1、稳态误差的定义与影响

  稳态误差 就是稳定状态的误差，稳定就是代表误差变成常数，不会再变了。

$0$，因为 $0$ 不是方程的解，存在稳态误差。LQR 控制系统最后一定会稳定，稳定下来最终结果就是误差不再变了，即误差的导数 $\dot e_{rr}=0$ ，并且误差本身不是 $0$

为什么 LQR 最终会导致经过一段时间后误差就稳定不变了，系统就稳定下来了呢？

  如果系统是不稳定的，那么 LQR 是没有解的；如果系统是 LQR 有解，就是 k 能算出来，那就意味着一定可以通过 LQR 使系统达到稳定状态。

那什么叫 LQR 有解？什么叫 LQR 无解？

  如果看过第五节就会明白，解 LQR 时需要解黎卡提方程，通过迭代出来，如果有解的话，就意味着迭代收敛，黎卡提方程的解 $P$ 不会再变。如果系统是不稳定的，黎卡提方程迭代就会发散，LQR 就没有解。

2、稳态误差与前馈控制的关系

回到刚才说的误差，误差最终等于多少？

  代进去算一下，把误差的导数等于0，带到误差方程里：
$0=\left( A-BK \right) e_{rr}+C\dot{\theta}_r$
  最终会得到稳态误差：
$e_{rr}=-(A-Bk)^{-1}C\dot{\theta}_r$

  引入前馈控制之后误差的导数：
$\dot{e}_{rr}=Ae_{rr}+B\left( -ke_{rr}+\delta _f \right) +C\dot{\theta}_r$

$\dot e_{rr}= 0$ ，代进去：
$e_{rr}=-(A-Bk)^{-1}\cdot(B\delta_{f}+C\dot{\theta}_{r})$  现在目的就很简单了，选取合适的 $\delta_{f}$，使得稳态误差 $e_{rr}=-(A-Bk)^{-1}\cdot(B\delta_{f}+C\dot{\theta}_{r})$ 尽可能为 $0$

注意：$B$

---

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

1、前馈控制表达式推导

$A、B、C$ 具体的表达式：

  其中，$A$ 是 $4\times 4$ 矩阵， $B$ 是 $4\times 1$ 矩阵， $C$ 也是 $4\times 1$  下面用软件 Mathematical 进行矩阵求逆运算：

  其中，$k=(k_1,k_2,k_3,k_4)$ .

  根据 Mathematic 得到的化简结果，再进行进一步化简，最后得到误差：

$e_{rr}=\left( \begin{array}{c} \frac{1}{k_1}\left\{ \delta _f-\frac{\dot{\theta}_r}{v_x}\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right] \right\}\\ 0\\ -\frac{\dot{\theta}_r}{v_x}\left( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C_{\alpha r}} \right)\\ 0\\ \end{array} \right)$  列向量的第一行和第三行都有 $\dot \theta_r$，这就是 $\dot \theta_r$ 对误差的影响。写到这其实就很明显，前馈控制表达式：

$\delta _f=\frac{\dot{\theta}_r}{v_x}\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right]$  此时，$e_d=0$。其中，$k_3$是反馈行向量$k=(k_1,k_2,k_3,k_4)$ 中的 $k_3$。

$k$，然后再算前馈 $\delta _f$。

2、航向角误差的近似处理

  再来看一下误差的第三行：
$e_\varphi =-\frac{\dot{\theta}_r}{v_x}\left( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C_{\alpha r}} \right)$  可以发现 $e_\varphi$ 不受 $\delta_f$ 和 $k$ 的影响，表达式里没有 $\delta_f$，也没有 $k$。即无论前馈和反馈取什么值，$e_\varphi$都永远不可能为零。因为能控制的就是前馈以及反馈。

$e_\varphi$ 的表达式，似乎感觉能控制的只有 $v_x$，因为侧偏刚度是负的，如果 $v_x$ 取特定值，那么有可能 $e_\varphi=0$，意味着 $v_x$

注意：$e_\varphi$ 不是航向误差，$e_\varphi$ 定义的是$\varphi -\theta_r$，而航向误差应该是 $\varphi +\beta -\theta_r$。我们的目的是想让航向误差和横向误差都为 $0$：

• 横向误差为 $0$：可以通过前馈控制。就是 $\delta_f$ 取那后面那一坨东西解决，横向误差可以为 $0$。
• 航向误差为0：航向误差如果按真正的定义$\varphi +\beta -\theta_r=0$，那么 $e_\varphi$ 就不为 $0$。

$\varphi +\beta -\theta_r =0$，那么 $\varphi -\theta_r$ 的稳态物差应该是 $-\beta$ 才对，那么问题是现在式子是不是等于 $-\beta$。

$e_\varphi$ 表达式进行更进一步的化简，在第四节讲到 $v$ 和它的投影 $\dot s$ 之间的关系：
$\begin{aligned} \dot{s}&=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d}=\frac{|\vec{v}|\cos \left( \beta +\varphi -\theta _r \right)}{1-\kappa e_d}\\ &=\frac{|\vec{v}|\cos \beta \cos \varphi -|\vec{v}|\sin \beta \sin \varphi}{1-\kappa e_d}\\ &=\frac{v_x\cos e_{\varphi}-v_y\sin e_{\varphi}}{1-\kappa e_d}\\ \end{aligned}$   $\dot{s}$ 和 $\dot\theta_r$

  在直角坐标系下曲率的计算式：
$\kappa =\frac{y^{\prime\prime}}{(1+y^{\prime})^{\frac{3}{2}}}$  大家都非常熟悉，但是曲率有定义式：
$\kappa =\frac{d\theta}{ds}=\frac{d\theta /dt}{ds/dt}=\frac{\dot{\theta}}{\dot{s}}$  这是曲率最原始的定义式，由此可得：
$\dot{\theta}=\kappa \dot{s}$  其中，$\dot s$

$\left| k \right|\ll 1$，$\left| e_{\varphi} \right|\ll 1$，$\left| v_y \right|\ll 1$，假设车辆没有漂移，所以：
$\frac{1}{1-\kappa e_d}\approx 1\quad v_x\cos e_{\varphi}\approx v_x\quad v_y\sin e_{\varphi}\approx 0$  这样直接得到 $\dot{s}\approx v_x$，那么 ：
$\dot{\theta}_r=\kappa \dot{s}\approx \kappa v_x$  又因为$\kappa = 1/R$ 可得：
$\begin{aligned} e_{\varphi}&=-\kappa \left( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C_{\alpha r}} \right)\\ &=-\left( \frac{b}{R}+\frac{a}{a+b}\frac{mv_{x}^{2}}{R}\frac{1}{C_{\alpha r}} \right)\\ \end{aligned}$  又因为无漂移的假设，所以把 $v_y、\dot v_y$直接忽略掉：
$\dot{\varphi}=\frac{\vec{v}}{R}=\frac{\vec{v}_{x}+\vec{v}_{y}}{R}\approx\frac{\vec{v}_{x}}{R}$$\begin{array}{c} a_y=v_y+v_x\dot{\varphi}\approx v_x\dot{\varphi}\approx \frac{v_{x}^{2}}{R}\\ \end{array}$  所以$e_{\varphi}=-\left( \frac{b}{R}+\frac{a}{a+b}ma_y\cdot \frac{1}{C_{\alpha r}} \right)$  到这一步离最终结果越来越近了，因为 $ma_y$ 是侧向力 $F_y$， $ma_y$除以 $C_{\alpha r}$ 是侧边角，但是这样还是不够，因为 $ma_y$ 是总侧向力，包括前轮和后轮，而 $C_{\alpha r}$

怎样才能得到后轮的侧向力呢？

3、车辆质量等效处理

  对车辆质量进行等效处理

$\frac{a}{a+b}$是不是感觉有点熟悉？ 比如有个质量块：

  质量为 m，质心到前边的距离为 a，到后边距离为 b。只考虑质心的话，可以完全等效成叠加的两个质量块，质量是 $m_f$ 和 $m_r$。

$m_f$和$m_r$，使得质心和原来大质量块的质心完全一样，并且 $m_f+m_r=m$，如果能做到这一点，在质量分布维度上，这两个东西完全等效。

  以质心为原点建立坐标系，等效前提是

$\left\{ \begin{array}{l} m_f+m_r=m\\ m_f\cdot \frac{a}{2}+m_r\cdot \left( -\frac{b}{2} \right) =0\\ \end{array} \right. \Rightarrow \left\{ \begin{array}{l} m_f=\frac{b}{a+b}m\\ m_r=\frac{a}{a+b}m\\ \end{array} \right.$  所以

$e_{\varphi}=-\left( \frac{b}{R}+\frac{m_ra_y}{C_{\alpha r}} \right)$  这就正好可以把四轮车把按照质心分成上半和下半：

  上半质量为 $m_f$，下半质量为 $m_r$，$m_ra_y$ 就等于后轮侧向力，严格来说应该是后轮侧向力之和。因为我们把汽车模型简化为自行车模型：

$F_{yr}$ 是单个轮子的侧向力，并且自行车模型的侧偏刚度是单个轮的侧偏刚度的两倍，即把两个轮子合并成一个轮子，可直接得到 $e_\varphi$：
$e_{\varphi}=-(\frac{b}{R}+\alpha_r)$

4、航向误差与侧偏角的关系推导

  自行车模型如下：

$-\alpha_r$，因为用的是右手系，以左为正，以右为负，在轮胎中轴线右边，所以是 $-\alpha_r$。

$R>>b$，根据弧度的定义，上面的角 $\gamma \approx\frac{b}{R}$

$180$ 度，所以
$\begin{array}{c}{{\gamma+\frac{\pi}{2}-\beta+\frac{\pi}{2}-(-\alpha_r)=\pi}}\\{\Rightarrow-\beta=-(\gamma+\alpha_{r})=-(\frac{b}{R}+\alpha_{r})}\\\end{array}$  所以
$e_{\varphi}=-\beta$
  这样正好:

• $e_{\varphi}$不是航向误差，$e_{\varphi}=\varphi - \theta_r$，航向误差是$\theta - \theta_r$， $\theta=\varphi+\beta$
• $e_{\varphi}$ 的稳态误差为$-\beta$，这样的话会得到
  $e_{\varphi}=\varphi-\theta_{r}\quad\Rightarrow-\beta=\varphi-\theta_{r}\quad\varphi+\beta=\theta_{r}$这就是想要的结果。

$e_{\varphi}$ 不可能通过$\delta_f$ 和 $k$ 去调节，但是不用去理会事情，因为最终的目的是$\theta - \theta_r=0$，就意味$e_{\varphi}=-\beta$，而推导出来的$e_{\varphi}$稳态误差正好就是$-\beta$。

$e_{\varphi}$

$e_d=0$

5、前馈控制表达式化简

  前馈控制表达式：
$\delta _f=\frac{\dot{\theta}_r}{v_x}\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right]$若令 $\dot{\theta}_r= \kappa v_x$，则可进一步化简为
$\delta _f=\kappa\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right]$

6、反馈与前馈控制的结合

$u$：
$u=-ke_{rr}+\delta _f$

• $k$ 通过 LQR 算出来，称为 反馈控制
• $\delta _f$ 根据上面公式算出来，称为 前馈控制

  通过反馈和前馈控制就可以将误差变成
$e_{rr}=\left( \begin{array}{c} 0\\ 0\\ -\beta\\ 0\\ \end{array} \right)$

$e_{\varphi}$不是航向误差，但最终导致航向误差为 $0$。

---

五、总结

  本篇博客讲解了航向误差以及前馈控制。下一节会讲基于离散 Frenet 坐标系的规划点误差的计算。

$u=-ke_{rr}+\delta _f$，$k$ 通过 LQR 解决，$\delta _f$

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

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

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

---

参考资料

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

---

后记：

🌟 感谢您耐心阅读这篇关于 前馈控制与航向误差 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀