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

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

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

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

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

---

---

引言

  各位小伙伴们大家好，欢迎来到自动驾驶控制算法第十二节，内容整理自 B 站知名 up 主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

  本节是超级大综合，将用到前十一节所有的知识，根据本篇博客的内容实现横纵向控制。

简单回顾一下本系列所讲的内容：

• 第一到第二节：开篇以及运动学方程
• 第三到第八节：横向控制以及 LQR
• 第九到第十一节：纵向控制，也就是双 PID 算法
• 第十二节：横纵向综合控制、规划接口，因为控制的上游是规划，必须要有规划的输入才能做控制

  所以不仅要把横纵向控制所有的模型都做搭好，还要写接口，接口就是在规划里面规划一条轨迹，然后输入到控制里，让控制根据规划的轨迹去跑，所以还要写接口。

  回顾整个系列，会发现运动学方程几乎就没怎么讲，可能只用到了 $\tan \delta =\frac{L}{R}$ 公式。但运动学方程实际上是非常有用的，只是本系列教程将淡化了。

  运动学方程适用于低速，但是有很好的特点，就是大小转角均可，而 LQR 只适用于小转角，但不限于速度，高速、低速都可以控制。所以如果想做停车、掉头之类的控制，一般用运动学方程，可以适用于大转角，即方向盘转角可以打得很大，但 LQR 不行， LQR 转角必须就是比较小，大家可以翻一翻第四节。

  【自动驾驶】控制算法（四）坐标变换与横向误差微分方程

  第四节推导二自由度动力学方程时，就假设前轮转角较小，前轮转角较大的话 LQR 也可以，但控制效果没有运动学方程那么好。

  在这里就简单提一下怎么用运动学方程做控制。

  首先是非线性方程要线性化，做一阶的泰勒展开线性化后，就变成如下形式：
$$\dot{e}=A\dot{e}+Bu$$  这样就和 LQR 的处理方式基本一样了。

---

一、规划接口

1.1 轨迹规划的定义与重要性

  首先要讲的就是规划接口，因为控制模块的功能就是接收规划轨迹，让车按照规划轨迹运动。控制模块功能实际上就暗含两个事情：

• 接受一条规划轨迹
  那首先得有一条规划的轨迹才行，所以那就是要有规划。
• 让车辆按照规划轨迹运动
  就是横纵向控制

  所以必须要讲一下怎么规划一条轨迹。

  注意：轨迹规划不是路径规划。

  在自动驾驶领域里，路径规划和轨迹规划不一样，这两个是分开的。路径规划比如导航，想要去哪，手机搜一下，就给一条路径，此路径是大概的方向性的东西，只会告诉该走哪条路，该往什么方向走，但是不会告诉速度是多少，加速度是多少，因此

• 路径规划：时间无关，只告诉该往什么地方走
• 轨迹规划：包含时间，所以轨迹规划不仅包含位置，还包含速度、加速度以及道路曲率等。

  所以轨迹规划和路径规划不一样，要给出 $s(t)$ 以及 $d(t)$。但在这里就不讲 $s(t)$ 以及 $d(t)$了，讲 $x(t),y(t)$，即不涉及曲线坐标系、自然坐标系，只在直角坐标系下去做规划，这也是由易到难的过程，先做简单的，再做难的。

1.2 不同场景的轨迹规划

  至于在自然坐标系下的轨迹规划，在进阶的课程再讲。本篇博客就讲在直角坐标系下的规划。比如这里有一辆车：

  在坐标圆点上，想移动到右上角的红色区域，坐标为 $(100,10)$，边界条件为：

坐标	$x$	$\dot{x}$	$\ddot{x}$	$y$	$\dot{y}$	$\ddot{y}$
初始	$0$	$0$	$0$	$0$	$0$	$0$
终点	$100$	$0$	$0$	$10$	$0$	$0$

  用时 $t=20s$，此场景实际上对应的就是停车。

  当然，终点也可以改，比如 $x=100$,$\dot{x}=20$，即车辆到达终点时有速度，其他都不变。此场景实际上对应的是驶入，比如在辅路上，旁边有花坛，想行驶到主路上融入交通流的场景。

  或者可以把起点也改了，比如起点 $x=0$，但速度 $\dot{x}=10$，加速度$\ddot{x}=0$，其他不变；终点处 $x=100$， $\dot{x}=20$， $\ddot{x}=0$，这其实对应就是换道和超车场景。

  给定起点和终点的位置、速度、加速度信息的规划，可以做很多场景：停车、驶入、变道。所以本篇博客就研究给定起点和终点的位置、速度、加速度信息的规划，这样的规划最简单也最常用，而且能覆盖大部分直线行驶的场景，但不能做转弯，也不能做调头，因为转化和调头必须要用到自然坐标系才可以。

  关于在自然坐标系下的规划，到后面进阶的课程再讲，本篇博客主要讲简单的规划，主要是给控制服务的，就是给控制提供规划接口。

1.3 轨迹规划的数学模型

  规划问题会转化成数学问题，就是设计一条合适的 $x(t),y(t)$，满足始末的边界条件，也就是给出初始端的位置、速度、加速度，终点的位置、速度、加速度以及耗费的时间 $T$：

起点	$x(0)$	$\dot{x}(0)$	$\ddot{x}(0)$	$y(0)$	$\dot{y}(0)$	$\ddot{y}(0)$
终点	$x(T)$	$\dot{x}(T)$	$\ddot{x}(T)$	$y(T)$	$\dot{y}(T)$	$\ddot{y}(T)$

  根据上述条件，设计出一条合理的轨迹。

  这是经典的规划问题，很常见，而且不仅仅在无人驾驶上用到，在机器人规划上也有类似的算法，给定始末点的位置、速度、加速度，然后算轨迹。

1.4 考虑车辆运动特性的轨迹规划

  但无人驾驶车辆在算法应用上不能直接照搬机器人领域的算法，这是因为它们在运动特性上存在显著差异。具体来说，无人驾驶车辆无法像机器人那样独立进行横向运动。在车辆行驶中，横向运动通常是由纵向运动引起的，即车辆不能像螃蟹那样仅依靠横向移动。而机器人则具备这种横向移动的能力，它们既可以横向移动，也可以纵向移动。因此，无人驾驶车辆的运动规划需要考虑额外的限制。

  车辆规划轨迹与机器人轨迹的不同之处在于，车辆轨迹必须受到切线曲率、加速度和速度的限制。例如，在确定起点和终点后，不能简单地绘制一条直线作为行驶路径。这是因为车辆的实际行驶路径需要遵循物理学和动力学的基本原则，以及道路和交通规则的限制。因此，无人驾驶车辆的路径规划必须更加复杂和精确，以确保行驶的安全性和效率。

  举个例子，比如给定起点和终点，能直接从起点到终点拉一条直线吗？显然不可以。

  合适规划轨迹应该长这样才符合汽车运动的规律：

  因为汽车不能平白无故的就有横向速度，横向运动必须要由纵向运动诱发。如果在起点建立直角坐标系，就是对曲线切线斜率 $\frac{dy}{dx}$ 也有要求。

  所以汽车的轨迹规划边界条件要做相应的修改：

起点	$x(0)$	$\dot{x}(0)$	$\ddot{x}(0)$	$y(0)$	$y^{\prime }(0)$	$y^{\prime \prime }(0)$
终点	$x(T)$	$\dot{x}(T)$	$\ddot{x}(T)$	$y(x_{end})$	$y^{\prime }(x_{end})$	$y^{\prime \prime }(x_{end})$

  变量符号上的点代表对时间求导 $\frac{d}{dt}$，撇代表对坐标求导 $\frac{d}{dx}$。如果是自然坐标系，撇就等于 $\frac{d}{ds}$，这里是直角坐标系，所以是 $\frac{d}{dx}$，车辆规划和机器人规划还是有很大不同的。

  机器人就是 $x$ 和 $y$，都是对时间的导数，因为机器人可以做纵向运动，也可以做横向运动，但仅用 $y^{\prime }$ 做规划还是不够，因为真正输入到控制模块里能用的轨迹必须要是 $(x(t),y(t))$，都是和时间相关的，控制就按照目标点去跑。

  所以关于车辆轨迹规划，车辆纵向的 $x$ 方向可以是关于时间的导数，因为 $x$ 直接和时间相关，但车辆横向的 $y$ 方向不行， 横向 $y$ 和纵向 $x$ 相关。所以还要写 $\dot{y}$ 和 $y^{\prime }$ 间的转化，即 $\frac{d}{dx}$ 和 $\frac{d}{dt}$ 间的转化。

  这两者之间的转换也非常简单，因为 $y(x)$ 中的 $x$ 和 $t$ 有关 $x=x(t)$，所以 $y(x)$ 和 $y(t)$ 之间的关系就是
$$y(t)=y(x(t))$$  那么 $y^{\prime }$ 和 $\dot{y}$ 之间的关系也很好计算，只要复合求导即可。
$$\dot{y}\left(t\right)=\frac{dy}{dx}\cdot \frac{dx}{dt}=y^{\prime }\cdot \dot{x}$$  $\ddot{y}$ 和 $y^{\prime \prime }$ 之间的关系还是比较复杂的，要使用稍微复杂点的复合求导，最终结果是
$$\begin{array}{rl}\ddot{y}\left(t\right)& =\frac{d}{dt}\left(\frac{dy}{dt}\right)=\frac{d}{dt}\left(\frac{dy}{dx}\cdot \frac{dx}{dt}\right)\\ & =\frac{d\left(\frac{dy}{dx}\right)}{dt}\frac{dx}{dt}+\frac{dy}{dx}\cdot \frac{d^{2}x}{d{t}^2}\\ & =\frac{d}{dx}\left(\frac{dy}{dx}\right)\cdot \frac{dx}{dt}\cdot \frac{dx}{dt}+y^{\prime }\cdot \ddot{x}\\ & =y^{\prime \prime }\cdot {\dot{x}}^2+y^{\prime }\cdot \ddot{x}\end{array}$$  这样问题就非常清晰了，给这么多边界条件：

$x\left(0\right)$	$\dot{x}\left(0\right)$	$\ddot{x}\left(0\right)$	$x\left(T\right)$	$\dot{x}\left(T\right)$	$\ddot{x}\left(T\right)$
$y\left(0\right)$	$y^{\prime }\left(0\right)$	$y^{\prime \prime }\left(0\right)$	$y\left(x_{end}\right)$	$y^{\prime }\left(x_{end}\right)$	$y^{\prime \prime }\left(x_{end}\right)$

  $x$ 共有 $6$ 个， $y$ 也有 $6$ 个，再加上耗费的时间 $T$。求轨迹满足边界条件，生成包含 $x(t),y(x)$ 的轨迹，先算出 $y$ 和 $x$ 的关系，再通过转化成 $y$ 和 $t$ 之间的关系，这样就生成了一条规划轨迹。

---

二、轨迹生成

2.1 五次多项式在轨迹规划中的应用

  对于 $x(t)$，很容易想到的就是用五次多项式：
$$x(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$$  因为五次多项式有 $6$ 个系数，正好对应这 $6$ 个边界条件。

  对于 $y(x)$ 也一样， $y$ 也用五次多项式：
$$y(x)=b_0+b_{1}x+b_2{x}^2+b_3{x}^3+b_4{x}^4+b_5{x}^5$$  因为 $y$ 也对应着 $6$ 个边界条件，五次多项式正好有 $6$ 个未知数，正好就可以对应这 $6$ 个边界条件。

2.2 解多项式系数

  通过这 $12$ 个边界条件（ $x$ 方向 $6$ 个， $y$ 方向也是 $6$ 个）解出 $a_0$ 到 $a_5$ 以及 $b_0$ 到 $b_5$ 出来。 $y$ 还要转化一下，通过以下关系式：
$$\begin{array}{rl}y\left(t\right)& =y\left(x\left(t\right)\right)\\ \dot{y}\left(t\right)& =y^{\prime }\left[x\left(t\right)\right]\cdot \dot{x}\left(t\right)\\ \ddot{y}\left(t\right)& =y^{\prime \prime }\left[x\left(t\right)\right]\cdot \dot{x}{\left(t\right)}^2+y^{\prime }\left[x\left(t\right)\right]\cdot \ddot{x}\left(t\right)\end{array}$$  解出 $y(t),\dot{y}(t),\ddot{y}(t)$，可以得到一条 $x$ 和 $t$ 的关系，以及 $y$ 和 $t$ 的关系，以及他们的导数和 $t$ 的关系，这样就是一条完整的轨迹规划。

---

三、横向控制的规划接口

得到这条轨迹规划后，该怎样才能运用于横纵向控制？

参考以第八节博客：

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

  在第八节中用到了规划中的 $(x_r,y_r,{\theta }_r,{\kappa }_r)$ 四个量。其中，${\theta }_r$ 是轨迹切线方向与 $x$ 轴的夹角， ${\kappa }_r$ 代表轨迹曲率。

3.1 匹配点的更新

  匹配点由时间给出，比如时间是 $1$ 秒，规划器给出匹配点坐标 $(x_r,y_r)$，即匹配点$(x_r,y_r,{\theta }_r,{\kappa }_r)$ 和时间相关，每过一段时间规划器就会发出匹配点坐标，控制器就按照匹配点控制。

3.2 规划点与时间的关系

  $(x_r,y_r)$ 其实就是规划器 $(x(t),y(t))$，和时间有直接关系，$({\theta }_r,{\kappa }_r)$ 和时间 $t$ 的关系如下：
$${\theta }_r\left(t\right)=\arctan \text{{}{y}^{\prime }\left[x\left(t\right)\right]\}$$$${\kappa }_r\left(t\right)=\frac{y^{\prime \prime }\left[x\left(t\right)\right]}{{\left(1+y^{\prime }{\left[x\left(t\right)\right]}^2\right)}^{\frac{3}{2}}}$$  这样横向控制和规划接口就讲完了，接下来讲纵向控制和规划接口

---

四、纵向控制的规划接口

4.1 横向误差的计算与理解

  在横向公式里面讲过了横向误差 $e_s$，如果各位如果不记得的话，要翻一下第七节和第八节：

  【自动驾驶】控制算法（七）离散规划轨迹的误差计算

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

  $e_s$ 在横向控制里有，所以横向控制可以直接输出 $e_s$，就是车辆当前位置与匹配点的距离，而且此距离是基于自然坐标系下的距离，不是基于直角坐标系下的距离。

  这里可能会有很多人会有疑惑，明明规划都是直角坐标系，怎么突然就来了自然坐标系？

  这是因为直角坐标系只用于规划，当生成了规划轨迹 $(x(t),y(t))$ 后，直角坐标系使命就完成了，控制就是基于生成的曲线作为自然坐标系的坐标轴的，所以 $e_s$ 才是位置误差。

  注意：横向控制里面 $e_s$ 还不算是真正的位置误差。

  下面看一下 $e_s$ 到底是怎么算出来的，比如这里有一条轨迹：

  目标点被视为匹配点，在此基础上，首先从车辆位置到目标点绘制一个向量，接着在目标点的切线方向上再绘制一个向量。将这两个向量进行点乘运算，其结果即为 $e_s$ ，即红色向量在蓝色向量上的投影就是 $e_s$ 。如果各位对此概念不太熟悉，可以多参考第七节的内容，那里有更详细的解释。

  【自动驾驶】控制算法（七）离散规划轨迹的误差计算

  关于 $e_s$ 的正负问题，可以这样思考：如果场景如上图所示， $e_s$ 是否为负值？由于两个向量之间的夹角为钝角，因此 $e_s$ 应为负值。但误差的定义是目标点与当前位置之间的距离。在所描述的场景中，目标点位于车辆当前位置的前方，因此 $e_s$ 按理来说应该是正值。因此，在实际的控制作用中，为了使 $e_s$ 与位置误差的实际意义相匹配，需要对 $e_s$ 加上负号。

4.2 速度误差与期望加速度

  速度误差为 $v_p-\dot{s}$， $p$ ( planning ) 是规划速度， $\dot{s}$ 在横向控制里面有。则规划速度 $v_p$ 为
$$v_p=\sqrt{{\dot{x}}_r(t{)}^2+{\dot{y}}_r(t{)}^2}$$  期望加速度 $a_p$ 为
$$a_p=\sqrt{{\ddot{x}}_r(t{)}^2+{\ddot{y}}_r(t{)}^2}$$  这里混用了 $(x(t),y(t))$ 和 $(x_r(t),y_r(t))$，他们其实是一回事，在规划模块里规划轨迹是 $(x(t),y(t))$，到控制模块时轨迹就变成了 $(x_r(t),y_r(t))$，其实是一回事儿。这样从规划到控制的理论部分就讲完了。

  把整个规划和控制逻辑梳理一下，首先通过规划得到 $x(t),y(x)$，然后通过这两个可以得到 $x(t),y(t),y(x)$，得到这三个的同时也可以得到各阶导数 $\dot{x}(t),\dot{y}(t),y^{\prime }(x)$，$\ddot{x}(t),\ddot{y}(t),y^{\prime \prime }(x)$。

4.3 横向控制的实现与调整

  由此可得横向控制公式：
$$\begin{array}{rl}{x}_r\left(t\right)& =x\left(t\right)\\ y_r\left(t\right)& =y\left(t\right)\\ {\theta }_r\left(t\right)& =\arctan \left[y^{\prime }\left[x\left(t\right)\right]\right]\\ {\kappa }_r\left(t\right)& =\frac{y^{\prime \prime }\left[x\left(t\right)\right]}{{\left(1+y^{\prime }{\left[x\left(t\right)\right]}^2\right)}^{\frac{3}{2}}}\end{array}$$

4.4 纵向控制与规划接口的整合

  以及纵向控制公式：
$$\begin{array}{rl}{e}_s& =\vec{d}{e}_{rr}\cdot \vec{\tau }\\ \dot{s}& =\frac{1}{1-{\kappa }_r{e}_d}\left(v_x\cos e_{\phi }-v_y\sin e_{\phi }\right)\\ v_p& =\sqrt{\dot{x}{\left(t\right)}^2+\dot{y}{\left(t\right)}^2}\\ e_v& =v_p-\dot{s}\\ a_p& =\sqrt{\ddot{x}{\left(t\right)}^2+\ddot{y}{\left(t\right)}^2}\end{array}$$  其中，纵向误差 $e_s$ 输入至纵向控制要加负号， $e_v$ 为速度误差，$a_p$ 为期望加速度。

  纵向控制中的 $e_s$ 、$\dot{s}$ 是通过横向控制计算出来的，所以纵向控制需要输入的只有三个。

---

五、代码实现与模型调整

5.1 代码下载与准备工作

  这样理论部分就讲完了，接下来就是重头戏代码环节。

  首先把横向控制和纵向控制的代码以及模型给下载下来，链接如下：

  自动驾驶横纵向控制代码和模型

5.2 Matlab 环境配置

  把代码和模型全部都复制到 Casim 工作目录上，然后打开 Matlab ， Matlab 共有四个 . $m$ 文件，前三个是标定用的，. $m$ 文件是第十一节里的，最后是第八节的 LQR 。可以注意到无论是 $Q$ 还是 $C_f,C_r$ 都变了，不是第八讲 LQR 了，这里要改一下，因为横纵向控制的 LQR 容易出现很大偏差，所以需要更精确的 LQR ，不能就是像第八节那样随便混一下。

  侧偏刚度需要精确解，必须要算出每个轮子精确的垂向力，通过垂向力查表，根据曲线大概估算出侧偏刚度。

  重新设置新的模型 planning_control，就不要用以前的模型了，在新的模型里，输入就是油门、刹车以及四轮的转角，输出就是 $x,y,\phi ,v_x,v_y,\dot{\phi }$ 以及发动机转速。

5.3 Carsim 标定过程

  首先做标定，把那三个标定代码跑起来，得到标定表。先不急着把横向控制模型复制进来，如果拿到代码，应该没有油门刹车标定表，所以要先标定一下，而且标定时要把 Carsim 当时的输出 $x,y,\phi ,v_x,v_y,\dot{\phi }$ 改为标定时的输出，只有 $v_x,a_x$ 以及发动机转速，并且要先从初始条件 $0$ 开始标定油门，然后把初速度改成 $180km/h$，再标定刹车。标完后再把 Carsim 的输出改成 $x,y,\phi ,v_x,v_y,\dot{\phi }$ 以及发动机转速。

5.4 Simlink 模型建立

  再建立新的 Simlink 空模型，用 Carsim 把空模型关联上去，最后把仿真的时间给改一下，改成 $40$ 秒，因为希望仿真时间长一点。然后把纵向控制的模型（就是第十一节的模型）打开，把里面所有的东西都复制到新的空模型里。

5.5 纵向控制模型整合与参数优化

  先不急着把横向控制模型复制进来，应该先做标定，先把那三份标定代码给跑起来，可以得到标定表。标定时要把 Carsim 的输入和输出，改为标定时的输入和输出：

• 输入为油门、刹车
• 输出为 $v_x$、$a_x$ 以及发动机转速

  并且要先从初始速度为 $0$ 开始标定油门，然后再改为初速度 $180km/h$，再标定刹车。标完之后再把 Carsim 的输出改为 $x,y,\phi ,v_x,v_y,\dot{\phi }$ 以及发动机转速。

  得到标定表后，把注释取消掉，把纵向双 PID 弄好。

  可以把位置 PID 的输入删掉，因为输入 $e_s$ 在横向控制里直接给，所以不需要用加减计算出来。

  把纵向控制所需要的输入先断开，用红色线起个名字再打包，这样看起来不那么混乱，因为模型很复杂，所以能打包的尽量就打包。

  纵向控制需要车的当前速度作为电机模型的输入，所以把速度写一下，写个函数：

function v = fcn(vx,vy)
    v=sqrt(vx^2+vy^2);

写好后连起来，规划函数如下：

MATLAB Function：planning

function [vp,ap,xr,yr,thetar,kr] = fcn(t)
    dx=100;%30 秒，要向前移动 100 米，然后向左移动 10 米。
    dy=10;
    T=30;
    xstart=[0,0,0];%起点到终点的位置、速度、加速度
    xend=[dx,0,0];
    ystart=[0,0,0];
    yend=[dy,0,0];
    a=zeros(1,6);%无论是 x 和y，都是五次多项式，有 6 个系数
    b=zeros(1,6);
    
    a(1)=xstart(1);
    a(2)=xstart(2);
    a(3)=xstart(3)/2;
    A1=[T^3,T^4,T^5;%解三元一次方程组
        3*T^2,4*T^3,5*T^4;
        6*T,12*T^2,20*T^3];%注意在推导时，系数是从 A0 到A5，但是从代码的编写写的是 A1 到A6
        %因为 Matlab 数组的下标是从一开始的，没有 A0 
    B1=[xend(1)-a(1)-a(2)*T-a(3)*T^2;
        xend(2)-a(2)-2*a(3)*T;
        xend(3)-2*a(3)];
    xs=inv(A1)*B1;
    a(4)=xs(1);
    a(5)=xs(2);
    a(6)=xs(3);
    b(1)=ystart(1);
    b(2)=ystart(2);
    b(3)=ystart(3)/2;
    A2=[dx^3,dx^4,dx^5;%方程组的t变成dx 
        3*dx^2,4*dx^3,5*dx^4;
        6*dx,12*dx^2,20*dx^3];
    B2=[yend(1)-b(1)-b(2)*dx-b(3)*dx^2;
        yend(2)-b(2)-2*b(3)*dx;
        yend(3)-2*b(3)];
    ys=inv(A2)*B2;
    b(4)=ys(1);
    b(5)=ys(2);
    b(6)=ys(3);
    xr=a(1)+a(2)*t+a(3)*t^2+a(4)*t^3+a(5)*t^4+a(6)*t^5;
    yr=b(1)+b(2)*xr+b(3)*xr^2+b(4)*xr^3+b(5)*xr^4+b(6)*xr^5;
    xr_dot=a(2)+2*a(3)*t+3*a(4)*t^2+4*a(5)*t^3+5*a(6)*t^4;
    yr_dx=b(2)+2*b(3)*xr+3*b(4)*xr^2+4*b(5)*xr^3+5*b(6)*xr^4;
    yr_dot=yr_dx*xr_dot;
    thetar=atan(yr_dx);
    xr_dot2=2*a(3)+6*a(4)*t+12*a(5)*t^2+20*a(6)*t^3;
    yr_dx2=2*b(3)+6*b(4)*xr+12*b(5)*xr^2+20*b(6)*xr^3;
    yr_dot2=yr_dx2*xr_dot^2+yr_dx*xr_dot2;
    kr=yr_dx2/((1+yr_dx^2)^1.5);%曲率
    vp=sqrt(xr_dot^2+yr_dot^2);
    if xr_dot2>=0
        ap=sqrt(xr_dot2^2+yr_dot2^2);
    else
        ap=-sqrt(xr_dot2^2+yr_dot2^2);
end

5.6 横向控制的集成与误差计算模块的修改

  下面加入横向控制，把横向控制的核心代码拷进来。把单位换算去掉，因为在单位换算在外面已经做过了，所以就不需要再做了。把 $(x_r,y_r,{\theta }_r,{\kappa }_r)$ 全都删掉，然后加接口进去，让 $(x_r,y_r,{\theta }_r,{\kappa }_r)$ 从外面输入。

  修改误差计算模块的代码，首先把 $e_s$ 和 $\dot{s}$ 输出出去，把 $d_{min}$ 去掉，因为$(x_r,y_r,{\theta }_r,{\kappa }_r)$ 由外面的规划模块给，不是直接给定轨迹，找最距离最短的匹配点。误差计算模块修改如下：

function [kr,err,es,s_dot] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    tor=[cos(thetar);sin(thetar)];
    nor=[-sin(thetar);cos(thetar)];
    d_err=[x-xr;y-yr];
    ed=nor'*d_err;
    es=tor'*d_err;
    %projection_point_thetar=thetar(dmin);%apollo
    projection_point_thetar=thetar+kappar*es;
    ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);
    %%%%%%%%%
    ephi=sin(phi-projection_point_thetar);
    %%%%%%%%%
    ss_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);%两步算出来s_dot
    s_dot=ss_dot/(1-kappar*ed);
    ephi_dot=phi_dot-kappar*s_dot;
    kr=kappar;
    err=[ed;ed_dot;ephi;ephi_dot];
end

  这样横向控制就做好了，连起来就可以了。转角算出来之后要做单元换算，把弧度转换为角度，后轮角度给写成 $0$ 就行了，因为 LQR 是小角度，所以要加 $-1$ 到 $1$ 的限制，最后别忘了 $e_s$ 要乘 $-1$，因为 $e_s$ 输入到纵向控制里要加负号。在纵向控制里再给加个 $10$ 到 $10$ 的限制，因为 $e_s$ 也不希望过大。

  一般实车调试的误差要小于 $0.1$ 米，而仿真至少要达到厘米级，就是零点零几米

  如果控制效果不好，一般就是加速度超了，所以首先要检查的就是加速度。

   LQR 对低高速没有限制，但是必须要小转角，第一是小转角，第二就是加速度不能超了。一般加速度 $2$ 到 $3$ ，特别是高速情况下，想再有 $4$ 到 $5$ 的加速几乎不可能，因为在高速情况下，电机的加速能力就很差了。

---

六、横向误差大的原因

6.1 横向误差的原因分析

  纵向误差一般不会特别大，如果大的话调 PID 参数就可以了，有问题的一般是横向误差，有以下几点原因：

• 侧偏刚度估算太准
  而且在跑时，前轮和后轮的垂向力不一样，因为加速度会导致轴转移，垂向力不一样就会导致侧偏刚度变化。

• LQR 模型的简化
  是基于二自由度自行车模型，本身就有一定简化，所以并不能完全的模拟车辆的横向运动，本身就有一定误差，所以导致横向跟踪也会有一定的误差。

• 汽车转向不足的固有特性
  转向不足导致横向控制存在一定误差。一般来说，在仿真误差达到厘米级就认为可以接受了。如果是实车，大概是 $0.1$ 米左右也可以接受，因为仿真没有噪声，是理想情况，所以要对仿真的要求更严格一点。

6.2 仿真与实车的差别

但是实车情况不太可能像仿真那样，实际的汽车有以下几个特点：

• 汽车不是自行车模型
• 汽车运行时的侧偏刚度会变化
• 汽车都会有存在转向不足的问题

实车肯定要考虑到转向不足，所以如果自己搭建模型，也有可能会遇到横向误差太大的问题。

---

七、解决横向误差的办法

横向误差太大，该如何避免？

有以下三种方法：

• 调节LQR参数
  把侧偏刚度估计准一点。
• 调整LQR的Q矩阵
  如果横向误差太大，就需要调 $Q$ 矩阵，给横向误差 $e_d$ 更大的惩罚值，如果 $e_d$过大，就会给很大的惩罚值，这样就尽可能的让 $e_d$收敛到 $0$ 。 $e_{\phi }$ 是不太可能收敛到 $0$ 的，因为 $e_{\phi }$ 稳态误差就是 $-\beta$。所以只要把 $e_d$ 的权重改大，但不能太大，否则导致超调。
• 处理转向不足
  因为转向不足而导致的横向误差太大

---

八、转向不足及过度转向

下面讲一下转向不足以及过度转向，比如这里有辆车，直接用自行车模型表示：

  如果给这样的前轮转角，理论上应该按照红色弧线行驶，但可能实际上并不是按红色轨迹跑，可能出现过度转向或转向不足，这是汽车本身的特性。

8.1 转向不足与过度转向的原因

为什么会发生转向不足或者过度转向呢？

这里简单解释一下，比如这里有一辆车：

  要往左转，那么自然就会受到侧向力的作用，假设为 $F_{y1}$ 和 $F_{y2}$。如果 $F_{y1}$ 和 $F_{y2}$ 之间不相匹配，就会导致在车的质心处有力矩存在。

8.2 力矩平衡与转向特性

什么叫相匹配？什么叫不相匹配？

简单举个例子，比如这样的车：

前轮和后轮都受到力的作用，到质心的距离分别是 $a$ 和 $b$ 。

• 若 $F_1\cdot a=F_2\cdot b$，则力矩平衡，导致中心转向。
• 若 $F_1\cdot a>F_2\cdot b$，则质心有正力矩，导致过度转向。
• 若 $F_1\cdot a<F_2\cdot b$，则质心有负力矩，导致转向不足。

8.3 车辆设计与转向特性的关系

  一般市面上买到的车基本上都是转向不足的，这是为了安全考虑。但是如果是赛车，一般会调教成中心转向，因为赛车需要更灵敏的转向，所以要调成中心转向。

为什么不调成过度转向呢？

  因为车天生就有过度转向的趋势，即在高速情况下， $F_1\cdot a>F_2\cdot b$，这是轮胎的特性。

所以一般来说：

• 如果是转向不足，在高速下可能会变成中心转向
• 如果是中心转向，在高速下可能会变成过度转向

  所以就没有必要调整成过度转向的，即使是赛车，也是中心转向为主。实车调试都有转向不足的问题，就会导致横向误差比较大。

8.4 使用 PID 控制器处理转向不足

那怎么去处理这件事情？

  一般在模型里用 PID ，但只用 PID 的积分模块，其他项都是 $0$ ，放到横向误差处做积分，因为有转向不足就会有误差，就对误差做积分，然后把误差积分和得到的转角相减，再输入到前轮转角，即将误差先做积分，再补偿到前轮转角中。

  很容易理解，因为 $e_d$ 向左误差为正，那么角度也是向左为正，所以一旦 $e_d$ 为正，就意味着方向盘往左打多了，所以要减掉多打的角度。

  注意弧度到角度的单位换算，应该是先做减法，再做单位换算。增益模块放到后面，对于实车来说，如果加了的话，提升会比较大。

  因为转向不足做了只用积分的 PID ，积分参数可以自己调节。

---

九、总结

  这样本系列就基本结束了，全部都讲完了。如果各位一直做到现在，可以发现，如果纯用 Matlab 确实不行，因为太慢了，特别是第 $12$ 节的模型，实际上跑起来比较费时间，特别是要调的话，每调一次 PID 参数都要跑一次，都要费很多长时间。再加上只是纯控制模型，还没有把规划加进去。规划只是接口，给定起点、终点，然后算法自动规划一条曲线，车就按照规划路径跑，但这只是接口而已，还没有做真正的轨迹规划。

  一旦把规划集成进去，运行会更慢，以后的进阶课程可能就不再以纯的 Matlab 为主了，可能只是用 Matlab 做简单的算法学习，真正要写能用的代码的话，肯定是要上 C++ 的，以及用 Linux 的，这也是没办法的事情，因为快。

  本系列博客正式结束了，感谢大家的阅读，谢谢大家，下个系列再见。

---

参考资料

  【基础】自动驾驶控制算法第十二讲 横纵向综合控制(完结)

---

后记：

🌟 感谢您耐心阅读这篇关于 自动驾驶控制算法横纵向综合控制 的技术博客。 📚

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

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

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

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