【自动驾驶】控制算法（八）横向控制Ⅰ | 算法与流程

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

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

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

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

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

---

---

前言

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

  本节内容是整个横向控制的核心，要把前七节所有内容都用在第八节上。第八节分四小节，本小节讲解具体算法，下一小节讲解模型搭建和代码编程，再下一小节讲解代码优化、注意事项以及仿真结果。

  算法这一讲可以说是整个第八节的核心所在。写代码、做仿真的来源是算法，有了具体算法后，无论用什么平台仿真，用什么语言编写程序，都是水到渠成的事情。只要照着算法编程逻辑是十分清晰的，因为算法已经把逻辑梳理好了，只要照着算法编就可以了。

  相反如果没有写过算法，然后就直接就去编程也可以，但只针对较简单的项目、简单的算法。对于复杂算法，建议各位先把算法写好，把逻辑理清楚，然后再编程。

  本篇博客讲解横向控制算法，前面讲到的控制律：
$$u=-k{e}_{rr}+{\delta }_f$$其中，$k=\mathrm{lqr}(A,B,Q,R)$ 或 $\mathrm{dlqr}(\bar{A},\bar{B},Q,R)$的解。

关于这些量的计算：

• $A、B$ 的计算在第三节和第四节
• $k$ 的计算在第五节
• $e_{rr}$ 的计算在第四节和第七节
• ${\delta }_f$ 的计算在第六节

  第四节讲的是连续规划轨迹的误差计算，第七节是讲的是离散规划轨迹的误差计算。

  将前七节所有的知识把综合起来，就变成了第八节，讲解横向控制的完整算法。横向控制很复杂，模块非常多，所以必须要把算法梳理一遍，不然很容易出错。

---

一、算法输入与输出

1、算法输入

根据前七讲，算法输入分为三类：

• 整车参数
  轴距L、质心到前后轮的距离 $a、b$ 、侧偏刚度 $C_{\alpha }$、质量 $m$、转动惯量 $I$。

• 车辆位置与状态
  包括车辆的 $(x,y,\phi )$ 以及它的速度 $v_x、v_y$ ，横摆角速度 $\dot{\phi }$。

• 规划轨迹点
  轨迹点为四维数组，包含坐标 $x_r,y_r$ 以及轨迹点的切线方向与 x 轴夹角 ${\theta }_r$ 以及轨迹点的曲率 ${\kappa }_r$。
  $$(\begin{array}{c}{x}_r\\ y_r\\ {\theta }_r\\ {\kappa }_r\end{array})=\left(\begin{array}{ccc}{x}_1& x_2& \cdots \\ y_1& y_2& \cdots \\ {\theta }_1& {\theta }_2& \cdots \\ {\kappa }_1& {\kappa }_2& \cdots \end{array}\right)$$

2、算法输出

  算法输出为
$$u=-k{e}_{rr}+{\delta }_f$$

---

二、算法流程

1、算法过程概述

  第一步：通过整车参数加上 $v_x$，可以算出 $A、B$，再设置权重矩阵 $Q、R$，通过 LQR 可得到反馈矩阵 $k$。

  第二步：车辆的位置状态，加上规划轨迹点，可以算出误差 $e_{rr}$ 以及投影曲率 $\kappa$ ，即真实位置在轨迹点的投影曲率。

  第三步：第一步得到反馈矩阵 $k$，以及第二步所得到的投影的曲率 $\kappa$，加上整车参数，再加 $v_x$，可得到前馈控制 ${\delta }_f$。

  第四步：第一步得到了 $k$ 加上第二步得到的误差 $e_{rr}$，再加上第三步得到 ${\delta }_f$，可以得到 $u=-k{e}_{rr}+{\delta }_f$

2、算法流程图

  流程图可以直观地反映算法的结构，基本上流程画出来，算法逻辑顺序就理清楚了，流程图如下：

• $A、B$ 计算模块
  首先是整车参数和 $v_x$ 作为输入模块，输入进 $A、B$ 计算模块，可算出 $A、B$

• LQR模块
  把 $A、B$ 算出来后，再输入 $Q$ 和 $R$ 进入 LQR 模块，就可算出最终的反馈矩阵 $k$。

• 误差曲率计算模块
  接下来是车辆状态及速度，加上规划轨迹点以及 $v_x$ 作为输入，这里把 $v_x$ 单独拿出来，因为 $v_x$ 在其他模块上也有用。作为输入之后输入进误差和 $k$ 的计算模块，得到误差以及曲率 $\kappa$。

• 前馈控制计算模块
  整车参数再加上 $v_x$ 再加上 $k$ 及 $\kappa$ 输入前馈控制计算模块，可算出前馈控制 ${\delta }_f$。

• 最终控制计算模块
  最后将反馈矩阵 $k$ 、前馈 ${\delta }_f$ 以及误差代入最终控制计算模块，可得到最后的控制量 $u$，

  其中，$A、B$ 计算模块、误差 $e_{rr}$、反馈矩阵 $k$ 计算模块、前馈控制 ${\delta }_f$ 计算模块都用到$v_x$ ，将 $v_x$ 连在一起，这样得到完整算法的流程图。

  流程图包含了从第三节到第七节的所有内容，这就是后续编程的理论依据，以后写代码就是按照流程图来写。

  可以看一下整个算法分为五个模块，写算法具体就是写这五个模块，模块写出来了，算法也就编写出来了。

---

三、具体算法

  下面看一下这五个模块的算法到底该如何编写。

1、AB 计算模块

  本模块最简单，代公式即可，因为 $A、B$ 的具体表达式的在前面推导过：
$$A=\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{C_{\alpha f}+C_{\alpha r}}{m{v}_x}& -\frac{C_{\alpha f}+C_{\alpha r}}{m}& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{m{v}_x}\\ 0& 0& 0& 1\\ 0& \frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I{v}_x}& -\frac{a{C}_{\alpha f}-b{C}_{\alpha r}}{I}& \frac{a^2{C}_{\alpha f}+b^2{C}_{\alpha r}}{I{v}_x}\end{array}\right)$$$$B=\left(\begin{array}{c}0\\ -\frac{C_{\alpha f}}{m}\\ 0\\ -\frac{a{C}_{\alpha f}}{I}\end{array}\right)$$  上式和车辆参数、 $v_x$ 有关的矩阵，把车辆参数及 $v_x$ 代入计算即可。

  注意：要考虑当 $v_x=0$ 时的奇异性，$A$ 中有无穷大出现，所以在编程时要考虑奇异性。

2、LQR模块

  LQR 模块是最费时间的模块，需要解黎卡提方程，需要矩阵迭代。但是有小技巧可以加快 LQR 模块的计算。因为 LQR 只与 $A、B、Q、R$ 有关，而 $A、B$ 只与整车参数和 $v_x$ 有关，整车参数可近似认为不变。

  注意：整车参数只是近似认为不变，并不是一成不变。在某些情况下就是整车参数不能认为近似不变，必须要考虑整车参数变化所带来对 $k$ 的影响。

在什么情况下不可近似处理呢？

(1)急加速

  如果车辆在疯狂加速，即加速度非常大、非常猛时：

  根据达朗贝尔原理，车辆有很大的惯性力，导致前后轮的垂向力发生变化。虽然前后轮的垂向力加起来等于车重，但分配不一样，不是轮子分别近似分配 $1/4$ ，可能前轮垂向力减小的非常多，而后轮垂向力会增大的非常多，垂向力的变化直接导致轮胎侧偏刚度发生变化，所以在加速特别猛时，不能把侧偏刚度看成常数，要考虑侧偏刚度的变化。

(2)急速过弯

  非常急速的过弯时，整个车都倾斜起来：

  此时车左右轮明显不对称，因为右轮几乎承受了车的全部重量，而左轮几乎没有承受任何力，导致自行车模型不再适用。

  所有的理论都基于自行车模型，即把左右两个轮认为是一个轮，等于是把车压扁了，意味着自行车模型只适用于左右对称或是近似对称的情况。而在极速过弯情况下，不能认为左右轮对称。

  在前急加速模型仍然可用自行车模型，因为左右两个轮对称，仅需考虑侧偏刚度变化带来对 $k$ 的影响，而后面急速过弯就完全不可以用自行车模型，所以在路径规划时，要极力避免急速过弯的情况出现。因为急加速还可以调整，但如果是急速过弯的话，就完全没办法调整。

  在汽车界一般认为侧向加速度就是超过 $0.4$ 倍的重力加速度，即$a_y>0.4g$，就属于急速过弯。

  回到模块上来，如果认为整车参数近似不变，那么 $A、B$ 只与 $v_x$ 有关。

$k=\mathrm{lqr}(A,B,Q,R)$ 或 $\mathrm{dlqr}(\bar{A},\bar{B},Q,R)$，这样每个 $v_x$ 都有唯一的 $k$ 与之对应。

  因此，可离线算出 $v_x$ 与 $k$ 的对应表，实际应用中不需要求解黎卡提方程，直接查表即可。

  比如有如下形式的对应表：

$v_x$	$k$
0.01	$k_1$
0.02	$k_2$
0.03	$k_3$
…	…
50	$k_n$

  反映了 $v_x$ 与 $k$ 的对应关系。这样测出 $v_x$ 后，直接查表查出 $k$，这就是用离线查表法计算 LQR。

查表法计算 LQR 的优缺点如下：

• 优点
  大大加快计算速度
• 缺点
  耗费存储空间

  这是典型为了加快程序运算速度而采用空间换时间的算法，通过查表法算， LQR 可以把速度提升好几个数量级，特别是在控制中，对实时性的要求是最高的，这就意味着必须要尽一切可能加快程序运算速度。

  一般在实际应用中的 LQR 不是通过调包计算的，而是查表查出来的。而且查表法算LQR 对高速运动的控制也有效果，因为高速和低速的区别就在于侧偏刚度变化，其他变化不大。

  在这种情况下，$k$ 就和 $v_x$ 以及侧偏刚度 $C_{\alpha }$ 有关，就是从一维表格变成三维表格。可以把速度以及侧偏刚度输进去，查 $k$ 出来，一样可以达到控制效果，而且高速对控制性的实时性要求更高，因为低速控制实时性不高的话，危险性不大。但高速情况下的控制，若实时性不够会很容易发生事故。

3、误差与曲率计算模块

  该模块是整个算法的核心。

(1)规划点

  首先要有规划轨迹，就是一系列的四维数组点：
$$(\begin{array}{c}{x}_r\\ y_r\\ {\theta }_r\\ {\kappa }_r\end{array})=\left(\begin{array}{ccc}{x}_1& x_2& \cdots \\ y_1& y_2& \cdots \\ {\theta }_1& {\theta }_2& \cdots \\ {\kappa }_1& {\kappa }_2& \cdots \end{array}\right)$$

(2)匹配点

  遍历$(x_R,y_r)$ 找到与 $(x,y)$ 最近的规划点的序列号，记为 $d_{min}$。

【序列号】是距离最短的点的下标序列，遍历所有规划点，比如找到第四个规划点距离车辆当前位置 $(x,y)$ 最近，那么$d_{min}=4$。最短距离的点叫做匹配点，$d_{min}$ 就是匹配点的序列号。

(3)方向向量

  写出匹配点的切线方向量 $\vec{\tau }$ 与法线方向量 $\vec{n}$：
$$\vec{\tau }=\left(\begin{array}{c}\cos \left({\theta }_{d_{\min }}\right)\\ \sin \left({\theta }_{d_{\min }}\right)\end{array}\right)\vec{n}=\left(\begin{array}{c}-\sin \left({\theta }_{d_{\min }}\right)\\ \cos \left({\theta }_{d_{\min }}\right)\end{array}\right)$$

(4)误差的距离的向量

  误差的距离的向量 $d_{-}{\vec{e}}_{rr}$：
$$d_{-}{\vec{e}}_{rr}=\left(\begin{array}{c}x-x_{d_{min}}\\ y-y_{d_{min}}\end{array}\right)$$  实际上就是两个向量的减法，第一个向量是车辆真实位矢向量，第二个向量是匹配点的位矢向量，这两个相减就是误差的距离向量，对应的就是第七节的 $\vec{x}-{\vec{x}}_m$。

(5)横向误差

$$e_d={\vec{n}}^T\cdot d_{-}{\vec{e}}_{\mathrm{rr}}$$

(6)纵向误差

$$e_s={\vec{\tau }}^T\cdot d_{-}{\vec{e}}_{rr}$$

(7)投影点切线方向与x轴夹角

  计算 ${\theta }_r$，即投影点切线方向与 $x$ 轴夹角，因为匹配点不等于投影点，所以还要进行相应的运算：
$${\theta }_r={\theta }_{d_{\min }}+{\kappa }_{d_{\min }}\cdot e_s$$

(8)横向误差导数

$${\dot{e}}_d=|\vec{v}|\sin \left(\theta -{\theta }_r\right)=v_y\cos \left(\phi -{\theta }_r\right)+v_x\sin \left(\phi -{\theta }_r\right)$$

(9)航向角误差

$$e_{\phi }=\phi -{\theta }_r$$

(10)弧速度

$$\dot{s}=\frac{v\cos \left(\theta -{\theta }_r\right)}{1-{\kappa }_{d_{min}}\cdot e_d}=\frac{v_x\cos \left(\phi -{\theta }_r\right)-v_y\sin \left(\phi -{\theta }_r\right)}{1-{\kappa }_{d_{\min }}{e}_d}$$

(11)横摆角速度误差

$${\dot{e}}_{\phi }=\dot{\phi }-{\kappa }_{{\text{d}}_{\min }}\cdot \dot{s}$$

(12)投影点曲率

$${\kappa }_r={\kappa }_{{\text{d}}_{\min }}$$

(13)输出

  第三个模块的算法是整个控制算法中最复杂的，这一块涉及到的公式很多，而且很容易出问题。

  为了得到 LQR 的 $A$ 和 $B$，做了一系列假设，其中假设 $e_{\phi }$ 比较小，所以$\sin e_{\phi }=e_{\phi }$，以及$\cos e_{\phi }$近似认为是1，然后才能得到 $A$ 和 $B$ 的具体的表达式。

  但第九步 $e_{\phi }=\phi -{\theta }_r$ 有问题，角度具有多值性，即 $\phi +2\pi$，还是原来角度，但从 $e_{\phi }$ 的角度上来说，$\phi +2\pi$ 就不是小角度了，包括 ${\theta }_r$ 也一样，${\theta }_r=0$ 和 ${\theta }_r=2\pi$ 实际上是同一个角度，但如果用$e_{\phi }=\phi -{\theta }_r$，可能导致 $e_{\phi }$ 很大。

  最终控制 $u=-k{e}_{rr}+{\delta }_f$ ，误差 $e_{rr}={\left(e_d,{\dot{e}}_d,e_{\phi },{\dot{e}}_{\phi }\right)}^T$ ，最终控制中的 $u$ 和$e_{\phi }$ 有关系，这样可能导致$e_{\phi }$ 在短时间内突变成多 $2\pi$，导致控制量 $u$ 在短时间内突变，进而导致控制失效。

  所以第九步最容易出错，在具体写代码时注意避免。

4、前馈控制计算模块

$$k_3=k(3)$$$${\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]$$

5、最终控制计算模块

$$\delta =-k{e}_{rr}+{\delta }_f$$  这样关于横线控制五个模块的算法都已经讲完了。

  只要写好这五个模块，通过流程图把这五个模块拼起来，就可以得到最终控制量 $u$。

---

四、预测模块

  但这样还不够，因为这五个模块只是前七节讲到的内容，为了更好地体现算法性能，还要再加预测模块。

1、预测模块的重要性

为什么要加预测模块？

  首先因为车本身具有惯性，导致控制天生有滞后性。为了解决这个问题，要进行提前控制，加入预测模块。

  这里具体解释一下。比如有一段规划轨迹：

  如果是人开车，看到这样一条路径规划，会不会去打方向盘？应该不会，因为人具有预见性，能看到未来路径到底长什么样。虽然现在的状态和规划路径有误差，但是如果保持当前状态行驶，在未来一段时间内，车和规划路径就没有误差了。所以人不会打方向盘，因为人能知道未来的路径规划是什么。

  但算法不行，算法开车只会机械地寻找与真实位置最接近的规划轨迹点。如果算出误差不等于0，那么算法就会打方向盘。

  下面的例子也说明了同样的道理：

  人开车的话，知道未来的路径规划不是直线，所以即使现在的运动状态和规划的轨迹完全贴合，没有误差，人也会提前动方向盘。

  但算法开车只看此时误差为 $0$，算法不会动方向盘，只在匀速直线运行到下一时刻时，发现此时误差不等于 $0$，才会动方向盘。所以算法控制具有一定的滞后性，看不到未来的路径规划，只关心哪个轨迹点与真实位置最近。

  所以要加上预测模块，为了更好地控制，算法需要具有一定的预见性。

  预测就是在车辆当前状态下，假设车辆在一段时间内做匀速直线运动，运动后的点就是预测点。比如说下面这样：

  红色点为预测点，如果没有预测，算法很明显和轨迹有误差 $d$，所以 $d$ 不等于 $0$， $u$ 就不等于 $0$，算法就会打方向盘。

  但是如果做了预测，其实就是求预测点和轨迹之间的误差。预测点和轨迹贴在一起，没有误差，就意味着 $x_{pre}$ 和轨迹重合，$u=0$，所以就不打方向盘，这时算法就具有预见性。

  对于上面提到的第二种情况也一样，比如预测点在这本来用原本真实点计算误差，那就是 $0$，现在用预测点和规划点计算误差，此时误差不等于 $0$，所以打方向盘。

2、预测模块算法

  下面编写具体的预测算法，比如下图的直角坐标系：

  车辆速度为 $v$，与 $x$ 轴夹角为 $\theta$，预测时间记为 $t_s$，假设在预测时间内，车辆做匀速直线运动，那么前视距离为 $v{t}_s$。若有加速度信号，则前视距离为 $v{t}_s+\frac{1}{2}a{t}_s^2$。

  预测点的信息记为 $(x_{pre},y_{pre},{\phi }_{pre},v_{xpre},v_{ypre},{\dot{\phi }}_{pre})$

  根据几何关系有：
$$\begin{array}{rl}{x}_{pre}& =x+v{t}_s\cos \theta =x+v{t}_s\cos \left(\beta +\phi \right)=x+v_x{t}_s\cos \phi +v_y{t}_s\sin \phi \\ y_{pre}& =y+v{t}_s\sin \theta =y+v_y{t}_s\cos \phi +v_x{t}_s\sin \phi \\ {\phi }_{pre}& =\phi +\dot{\phi }{t}_s\\ v_{xpre}& =v_x\\ v_{y\text{pre}}& =v_y\\ {\dot{\phi }}_{\text{pre}}& =\dot{\phi }\end{array}$$  这就是预测模块的完整算法。

---

五、完整算法流程图

  加入预测模块后的算法流程图：

---

六、总结

  本来是真实的 $(x,y,\phi ,v_x,v_y,\dot{\phi })$ 和规划轨迹 $\left(x_r,y_r,{\theta }_r,{\kappa }_r\right)$ 做误差计算，现在加入预测模块，就用预测点的 $\left(x_{pre},y_{pre},{\phi }_{pre},v_{xpre},v_{ypre},{\dot{\phi }}_{pre}\right)$ 和规划轨迹做预测，这样算法具有一定的预见性。

  这才是想要的完整算法，后续编程就按照算法流程图来编写，一共六大模块，已经全部讲完。

  本篇博客就写到这里，但第八节还没有完，下一小节讲解具体的代码编写以及模型搭建。欢迎关注！

---

参考资料

  【基础】自动驾驶控制算法第八节(一）横向控制算法与流程图

---

后记：

🌟 感谢您耐心阅读这篇关于 横向控制Ⅰ | 算法与流程 的技术博客。 📚

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

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

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

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