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

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

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

---

---

引言

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

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

---

一、反馈控制

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

  这种 $u=-kx$ 的控制叫做反馈控制。

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

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

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

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

  这种控制就叫反馈控制，下边的红色的线就叫反馈，如果没有反馈，输入 $u$ 决定输出 $x$，有反馈的话就是先是输入 $u$ 决定 $x$，然后 $x$ 又反过来决定输入$u$，这就是反馈的意思。

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

---

二、前馈控制的引入

  先来看以下框图：

  在 $B$ 左边再加 ${\delta }_f$，${\delta }_f$ 就叫做前馈控制，不依赖于x，就好像空降一样，在反馈控制前再加前馈控制。

为什么要加前馈控制呢？

  因为要处理第四讲 ${\dot{e}}_{rr}=A{e}_{rr}+Bu+C{\dot{\theta }}_r$ 遗留下来的小尾巴 $C{\dot{\theta }}_r$

  如果没有前馈，只有反馈的话，即只用 LQR ，控制量$u=-k{e}_{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}=A{e}_{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}=A{e}_{rr}+B\left(-k{e}_{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$ 不是矩阵，是列向量，列向量是没有逆的，只有矩阵，而且是方阵才有逆，所以并不能直接令右边括号内的项为0。

---

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

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-b{k}_3-\frac{m{v}_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{m{v}_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-b{k}_3-\frac{m{v}_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_{\phi }=-\frac{{\dot{\theta }}_r}{v_x}\left(b+\frac{a}{a+b}\frac{m{v}_x^2}{C_{\alpha r}}\right)$$  可以发现 $e_{\phi }$ 不受 ${\delta }_f$ 和 $k$ 的影响，表达式里没有 ${\delta }_f$，也没有 $k$。即无论前馈和反馈取什么值，$e_{\phi }$都永远不可能为零。因为能控制的就是前馈以及反馈。

  看一下 $e_{\phi }$ 的表达式，似乎感觉能控制的只有 $v_x$，因为侧偏刚度是负的，如果 $v_x$ 取特定值，那么有可能 $e_{\phi }=0$，意味着 $v_x$ 只能取特定值，即车辆只能用特定速度，很明显不现实。

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

• 横向误差为 $0$：可以通过前馈控制。就是 ${\delta }_f$ 取那后面那一坨东西解决，横向误差可以为 $0$。

• 航向误差为0：航向误差如果按真正的定义$\phi +\beta -{\theta }_r=0$，那么 $e_{\phi }$ 就不为 $0$。

  如果真正的目的是 $\phi +\beta -{\theta }_r=0$，那么 $\phi -{\theta }_r$ 的稳态物差应该是 $-\beta$ 才对，那么问题是现在式子是不是等于 $-\beta$。

  首先对 $e_{\phi }$ 表达式进行更进一步的化简，在第四节讲到 $v$ 和它的投影 $\dot{s}$ 之间的关系：
$$\begin{array}{rl}\dot{s}& =\frac{|\vec{v}|\cos \left(\theta -{\theta }_r\right)}{1-\kappa e_d}=\frac{|\vec{v}|\cos \left(\beta +\phi -{\theta }_r\right)}{1-\kappa e_d}\\ & =\frac{|\vec{v}|\cos \beta \cos \phi -|\vec{v}|\sin \beta \sin \phi }{1-\kappa e_d}\\ & =\frac{v_x\cos e_{\phi }-v_y\sin e_{\phi }}{1-\kappa e_d}\end{array}$$   $\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_{\phi }\right|\ll 1$，$\left|v_y\right|\ll 1$，假设车辆没有漂移，所以：
$$\frac{1}{1-\kappa e_d}\approx 1v_x\cos e_{\phi }\approx v_x{v}_y\sin e_{\phi }\approx 0$$  这样直接得到 $\dot{s}\approx v_x$，那么 ：
$${\dot{\theta }}_r=\kappa \dot{s}\approx \kappa v_x$$  又因为$\kappa =1/R$ 可得：
$$\begin{array}{rl}{e}_{\phi }& =-\kappa \left(b+\frac{a}{a+b}\frac{m{v}_x^2}{C_{\alpha r}}\right)\\ & =-\left(\frac{b}{R}+\frac{a}{a+b}\frac{m{v}_x^2}{R}\frac{1}{C_{\alpha r}}\right)\end{array}$$  又因为无漂移的假设，所以把 $v_y、{\dot{v}}_y$直接忽略掉：
$$\dot{\phi }=\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{\phi }\approx v_x\dot{\phi }\approx \frac{v_x^2}{R}\end{array}$$  所以$$e_{\phi }=-\left(\frac{b}{R}+\frac{a}{a+b}m{a}_y\cdot \frac{1}{C_{\alpha r}}\right)$$  到这一步离最终结果越来越近了，因为 $m{a}_y$ 是侧向力 $F_y$， $m{a}_y$除以 $C_{\alpha r}$ 是侧边角，但是这样还是不够，因为 $m{a}_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$，如果能做到这一点，在质量分布维度上，这两个东西完全等效。

  以质心为原点建立坐标系，等效前提是
$$\{\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}\Rightarrow \{\begin{array}{l}{m}_f=\frac{b}{a+b}m\\ m_r=\frac{a}{a+b}m\end{array}$$  所以
$$e_{\phi }=-\left(\frac{b}{R}+\frac{m_r{a}_y}{C_{\alpha r}}\right)$$  这就正好可以把四轮车把按照质心分成上半和下半：

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

  也就是把两个轮子合并成一个轮子，认为两个轮子一样，$F_{yr}$ 是单个轮子的侧向力，并且自行车模型的侧偏刚度是单个轮的侧偏刚度的两倍，即把两个轮子合并成一个轮子，可直接得到 $e_{\phi }$：
$$e_{\phi }=-(\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_{\phi }=-\beta$$
  这样正好:

• $e_{\phi }$不是航向误差，$e_{\phi }=\phi -{\theta }_r$，航向误差是$\theta -{\theta }_r$， $\theta =\phi +\beta$ 。

• $e_{\phi }$ 的稳态误差为$-\beta$，这样的话会得到
  $$e_{\phi }=\phi -{\theta }_r\Rightarrow -\beta =\phi -{\theta }_r\phi +\beta ={\theta }_r$$这就是想要的结果。

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

  所以不用管 $e_{\phi }$ 不为零的事情。

  只需要用前馈控制保证横向误差 $e_d=0$ 就可以了。

5、前馈控制表达式化简

  前馈控制表达式：
$${\delta }_f=\frac{{\dot{\theta }}_r}{v_x}\left[a+b-b{k}_3-\frac{m{v}_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-b{k}_3-\frac{m{v}_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=-k{e}_{rr}+{\delta }_f$$

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

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

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

---

五、总结

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

  大家看一下公式，$u=-k{e}_{rr}+{\delta }_f$，$k$ 通过 LQR 解决，${\delta }_f$ 通过前馈控制解决。

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

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

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

---

参考资料

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

---

后记：

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

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

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

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

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