【自动驾驶】控制算法（二）三大坐标系与车辆运动学模型

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

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

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

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

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

---

引言

  本篇博客开始正式讲解自动驾驶控制算法。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

一、控制算法基础

1、控制算法分类

控制算法相对比较成熟，一般就三个算法：

• PID 算法
• LQR 算法
• MPC 算法

  其中百度 Apollo 用的是 PID 和 LQR 算法，学术界用 MPC 算法。

2、控制算法优劣分析

  从控制效果来看， MPC控制效果一般要优于 PID、LQR。但是 MPC 有个缺点是计算量太大。

  目前量产的实车上面应用算法一般是 PID + LQR。

控制算法的前提：路径规划

  在本系列博客中，默认已经做好了路径规划，有一条参考路径：

二、车辆控制基本原理

1、纵向控制原理

  控制原理其实很简单，就是通过油门和刹车来控制力，通过力来控制加速度，通过加速度控制速度，然后再控制位置，一般叫纵向控制，原理如下：

2、横向控制原理

  通过方向盘控制前轮转角，然后通过前轮转角来影响横向位移，这就是横向控制的原理。当然方向盘也可以控制车辆的航向角，横向控制原理如下：

三 、坐标系的引入

1、横向控制中的耦合现象

  在上面的控制基本原理中，可能会产生两个疑问：

• 航向角是什么？
  没有学过车辆的知识，可能不知道航向角是什么。

• 仅凭方向盘能做横向控制吗？
  也就是打方向盘，就能让车能横向运动吗？感觉横向控制应该是和纵向控制耦合在一起的，因为必须要有一定的速度，再打方向盘才能，车才能横向移动。

  在前面横向控制原理的讨论中，仅关注方向盘与纵向控制车速之间的耦合现象，而未深入探讨这一耦合的本质所在。为了清晰地阐明这一问题，需要必要引入三个关键坐标系，这三个坐标系构成了理解横向控制复杂性的基础。

2、关键坐标系的定义与作用

  在自动驾驶技术的相关文献中，尽管存在一些对原理的阐述，但并非所有作者都对其中的三个坐标系进行了详尽的解释。部分文章由于对这三个坐标系的定义和取法存在差异，导致了内容上的矛盾和混淆。这种不一致性成了理解上的混乱之源。因此，为了准确无误地解释横向控制原理，我们必须对这三个坐标系进行深入而清晰的阐述。

  比如，用长方形代表车辆，那么三个坐标系可以表示如下：

• 绝对坐标系 $(X,Y)$
  也称为大地坐标系。它是相对于地球表面的固定坐标系。在这个坐标系中，车辆的位置是相对于地球表面上的一个固定点来测量的。绝对坐标系通常用于车辆导航系统，它能够提供车辆相对于全球定位系统(GPS)或其他地面参考点的精确位置。

• 车身坐标系 $(x,y)$
  是车辆内部的一个相对坐标系，以车辆中心轴线为参考。在这个坐标系中，车辆的位置是相对于车辆本身的中心点来测量的。车身坐标系常用于车辆的内部控制系统中，如自动驾驶系统，它能够提供车辆相对于自身中心轴线的运动状态。

• 自然坐标系 $(\tau ,n)$
  无人驾驶领域特有的坐标系，也被称为 Frenet 坐标系。是一种在道路曲线上定义的坐标系，用于描述车辆在弯曲道路上的位置和方向，能够清晰地表达车辆相对于道路曲线的姿态和运动。

  之所以采用自然坐标系，是因为它可以简化控制算法的复杂性，并且与轨迹规划所采用的坐标系相匹配。此外，自然坐标系的一个重要优势在于它可以 实现纵向控制与横向控制的解耦，从而解释了为什么在传统的控制原理中，横向控制只需通过方向盘即可实现。

  当坐标系选择为大地坐标系或车身坐标系时，纵向控制和横向控制是无法解耦的，必须混合进行。当采用自然坐标系后，这两个控制可以分开进行，从而降低了计算量。因此，Frenet 坐标系在自动驾驶技术中具有重要的地位。这三个坐标系之间的坐标变换也是一个难点。

  大地坐标系和车身坐标系之间的坐标变换相对简单，而车身坐标系和自然坐标系之间的变换则较为复杂。市面上的一些关于坐标变换的博客内容可能不够清晰，因此，在实际应用中，理解坐标系及其变换至关重要。

这三个坐标系的混淆主要有三个原因：

• 首先，Frenet 坐标系和车身坐标系之间的坐标变换相对较为复杂。
• 其次，无人驾驶领域早期由不同学科背景的人员参与，有的学科使用右手系，有的使用左手系，这可能导致坐标方向上的差异，并在某些文章中表现为一个负号。
• 此外，Frenet 坐标系之间的变换也较为复杂，

  这些因素共同导致了坐标系选择上的混乱。因此，在阅读相关文献时，必须首先了解作者所采用的坐标系，因为不同的坐标系会导致同一变量在不同坐标系中的符号发生变化。

  本系列博客一律采用右手系的坐标系。什么是右手系呢？下面画一下右手的三个手指：

用这三个手指摆一个标架，让三个手指之间互相垂直，拇指向上，食指向左，中指指向自己。

• 右手系
  当右手拇指指向 x 轴，右手食指指向 y 轴时，若 z 轴和中指方向一致，则为右手系。

• 左手系
  当左手的拇指是 x 轴，左手食指指向 y 轴时，若 z 轴和左手中指向一致 ，则为左手系。

举几个例子：

  在数学和物理领域，专业人员通常采用右手坐标系来描述和分析物理现象。例如，在描述力、速度和加速度等物理量时，右手坐标系提供了一种直观且一致的框架。
  而在计算机科学领域，特别是在计算机图形学和计算机视觉领域，左手坐标系更为常见，因为这些技术在屏幕上渲染图像时更方便使用。

  自动驾驶技术作为一个涉及多个学科的领域，这种跨学科特性使得在解决相关问题时，不同背景的专业人员可能会使用他们各自的术语和知识。如果某个问题需要不同领域的专业知识来解决，那么使用不同的坐标系可能会导致理论上的矛盾和错误。

  因此，为了准确地传达理论，需要清楚地说明所使用的坐标系，以及物理量在特定坐标系中的正负号。

四、自行车模型

  在对车建模做力学分析时，一般把四个轮子变成两个轮子，简化为自行车模型：

1、自行车模型的基本假设

  假设车辆是完全左右对称的，并且认为轮距（即左轮与右轮之间的距离）相对于车辆的运动轨迹可以忽略不计。这种假设将车辆简化为一条线，从而简化问题。

  这种简化的假设也回答了为什么在横向控制中只需要考虑方向盘转扭的问题。在自然坐标系中，纵向控制和横向控制可以实现解耦，使得方向盘转扭成为唯一需要考虑的横向控制输入。关于如何具体实现解耦控制，将在后续的内容中详细讲解，目前只需记住这个概念即可。

2、自行车模型的构建

  构建自行车模型如下图所示：

  在绝对坐标系下，后轮速度为 $v$，前轮速度为 $v^{\prime }$。假设红点处为质心，质心速度可以用理论力学的速度瞬心法来求。过质心这一点，做瞬心与质心连线的垂线方向就是质心速度的方向。

  从图中可以看到，质心的速度方向和车的轴线方向一般不重合，而车的轴线方向和 $X$ 方向一般也不重合。定义两个角度，第一个角度是横摆角 $\phi$，第二个角度是质心侧偏角 $\beta$，$\phi$ 加 $\beta$ 定义为航向角。

• 横摆角 是指车的轴线方向和绝对坐标系 $X$ 方向的夹角。（绝对坐标系下）
• 质心侧偏角 是指质心速度 $v$ 与车身坐标系 $x$ 方向的夹角。（车身坐标系下）
• 航向角 是指质心速度与 $X$ 方向的夹角。（绝对坐标系下）
• 前轮转角 ${\delta }_f$，后轮转角 ${\delta }_r$。 （车身坐标系下）

自行车模型有三个假设：

• 车辆左右对称
• 车辆轮距忽略不计，把它压扁成一根线
• 车辆轮轮胎为刚性轮胎，不存在变形

  在实际情况下，车辆并不完全符合简化模型中的假设。车辆轮胎的材料是橡胶，虽然相对普通材料较硬，但与钢铁相比仍然是一种相对柔软的材料。因此，当轮胎受到力的作用时，会发生变形，这种变形是不能被忽略的。

  由于轮胎的这种变形特性，自行车模型的前轮转角与实际车辆的前轮转角并不完全相同。由于前轮转角与方向盘转角之间存在一一对应的关系，例如，如果传动比是
10，那么当方向盘转角为 30 度时，理论上前轮转角应为 3 度。但是由于轮胎的变形，实际的前轮转角可能并非完全按照这一比例变化。

  因此，在考虑车辆的实际操作时，需要考虑轮胎变形对前轮转角的影响，这可能会导致方向盘转角与前轮转角之间的实际比例与理论比例存在差异。

【侧偏】因为轮胎有变形而导致前轮转角和方向盘转角不能一一对应的现象。

  后面会讲到侧偏，现在大家只要心里有个印象就行了，就是实际上前端转角和方向盘转角不是一一对应的。但是在这里先忽略这件事情，认为他们是一一对应的。就是如果传动比里是 10 的话，方向盘转角是 30 度，那么前段转角就是 3 度。

自行车模型有两种：

• 一种是不考虑轮胎变形，就认为轮子是一个钢铁的轮子
• 一种是考虑轮胎变形，需要注意轮胎侧偏的特性

  在当前的讨论中，我们不考虑轮胎变形的情况。在低速条件下，轮胎受到的阻力较小，因此其变形也相对较小。在这种情况下，轮胎可以近似地被视为刚性轮子。因此，不考虑轮胎变形的自行车模型在某些情况下仍然是有用的。

  用 $v$ 代表直行车速，对 $v$ 沿着车身坐标系正交分解：

• 沿着 $x$ 轴方向定义为纵向车速 $v_x$，纵向加速度 $a_x$
• 垂直于车的轴线，定义为侧向速度 $v_y$，侧向加速度 $a_y$

  物理量定义好后，就可以建立微分方程了。微分方程用来描述前端转角 ${\delta }_f$ 是如何影响侧向位移与航向角的，微分方程是控制的理论基础。

五、车辆运动学建模

1、车辆运动学模型的分类

车辆模型有两种：

• 根据几何关系建立运动学模型
• 根据牛顿力学建立动力学模型

  这两个模型的区别在于运动型模型认为轮胎是刚性轮胎，不会变形；而牛顿力学会充分考虑轮胎变形。

  所以一般动力学模型要比运动学模型更加精确，但动力学模型考虑轮胎变形就很复杂，因为轮胎它是橡胶和空气混在一起的，这样的东西很难去精确地去描述轮胎的变形到底是怎样的，所以动力学模型的参数很难给做到非常准确，但是运动性模型就没有这样的缺点，它就是纯粹的几何关系，不考虑轮胎变形。

  因此，运动型模型相对较为简单，其代价在于 不考虑轮胎的变形。这种简化的模型适用于轮胎变形较小的特定情况，例如在低速行驶、大转弯半径和频繁规划轨迹的情况下。然而，对于一些更激烈的驾驶情况，运动型模型可能不再适用，因为在这种情况下，轮胎的变形可能变得显著，从而导致较大的误差。

2、车辆运动学模型的建立与推导

  首先构建自行车模型，建立坐标系：

  可以得到如下方程：
$$\begin{array}{rl}& \dot{X}=v\cos (\beta +\phi )\\ & \dot{Y}=v\sin (\beta +\phi )\\ & \dot{\phi }=\frac{v}{R}\end{array}$$  第三个式子是理论力学的知识，刚体的角速度等于它的线速度除以线速度到速度瞬心的距离。

  上述微分方程只体现质心速度 $v$ 对 $x、y、\phi$ 的影响，没体现前轮转角 ${\delta }_f$，所以还需要更进一步的推导。

  设质心到前轮的距离为 $a$，质心到后轮的距离为 $b$。

  把一个大三角形分为两个小三角形，使用正弦定理：
$$\frac{a}{\sin ({\delta }_f-\beta )}=\frac{R}{\sin (\frac{\pi }{2}-{\delta }_f)}$$$$\frac{b}{\sin ({\delta }_r+\beta )}=\frac{R}{\sin (\frac{\pi }{2}-{\delta }_r)}$$  对这两个式子化简：
$$\frac{a}{R}=\frac{\sin ({\delta }_f-\beta )}{\sin (\frac{\pi }{2}-{\delta }_f)}=\frac{\sin {\delta }_f\cos \beta -\sin \beta \cos {\delta }_f}{\cos {\delta }_f}=\tan {\delta }_f\cos \beta -\sin \beta$$$$\frac{b}{R}=\frac{\sin ({\delta }_r+\beta )}{\sin (\frac{\pi }{2}-{\delta }_r)}=\frac{\sin {\delta }_r\cos \beta +\sin \beta \cos {\delta }_r}{\cos {\delta }_r}=\tan {\delta }_r\cos \beta +\sin \beta$$   由此得到 $R$ 和 ${\delta }_f$、${\delta }_r$ 之间的关系，但是这样还不够，因为质心会变，比如多坐几个人，多装几件行李后，质心的位置就会变， $a$ 和 $b$ 的数值也会相应变化。

  所以要用不因质心改变而变化的车辆轴距 $L$ 来表示 $R$ 和 ${\delta }_f$ 、${\delta }_r$ 之间的关系：$$\frac{a+b}{R}=(\tan {\delta }_f+\tan \delta r)$$即$$\frac{1}{R}=\frac{\tan \delta f+\tan \delta r}{L}$$其中，车辆轴距 $L=a+b$.

  得到运动学微分方程：
$$\begin{array}{rl}& \dot{X}=v\cos (\phi +\beta )\\ & \dot{Y}=v\sin (\phi +\beta )\\ & \dot{\phi }=\frac{v}{R}=\frac{v(\tan {\delta }_f+\tan {\delta }_r)}{L}\end{array}$$  运动学方程的前提是低速条件，在低速条件下认为车不会发生侧向滑动，即车不会漂移，所以 $v_y\approx 0$。

  根据定义，质心侧偏角 $\beta$：
$$\beta =\arctan \frac{v_y}{v_x}=0$$  一般车都是前轮转向，后轮驱动，所以在低速产条件下，近似认为 ${\delta }_r\approx 0$ 。

  最终运动学方程如下:$$\{\begin{array}{l}\dot{x}=v\cos \phi \\ \dot{y}=v\sin \phi \\ \dot{\psi }=\frac{v\tan {\delta }_f}{L}\end{array}$$  因为 $\beta \approx 0$，所以 航向角 $\approx$ 横摆角 $\phi$ ，质心速度 $v=v_x$。

  由此建立起了车辆运动学模型，通过 $v$ 和${\delta }_f$ 控制 车辆位姿$(X,Y,\phi )$

六、总结

  本篇博客讲解了车辆控制中的三大坐标系，定义了一些物理量，并且求出了低速条件下的车辆运动学模型。对于高速情况下的动力学模型，将在后续的博客中介绍，欢迎关注！

参考资料

  【基础】自动驾驶控制算法第二讲 三个坐标系与运动学方程

---

后记：

🌟 感谢您耐心阅读这篇关于 自动驾驶三大坐标系与车辆运动学模型 的技术博客。 📚

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

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

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

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