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

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


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

---

文章目录

• 引言
• 一、微分方程与规划轨迹

• 1、误差定义
• 2、代价函数定义

• 二、误差微分方程的推导

• 1、误差微分方程的建立
• 2、横向误差与速度误差的关系

• 简单证明

• 3、投影点速度的计算
• 4、航向角误差与速度误差的联系
• 5、误差微分方程的线性化

• 三、线性误差微分方程组

• 1、航向误差问题
• 2、投影点航向角速度计算问题
• 3、线性误差微分方程组的形式问题

• 参考资料

---

引言

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

  在上一节得到了这样的微分方程：

  反映了前轮转角对侧向速度和横摆角的影响，可以写成如下形式：

---

一、微分方程与规划轨迹

  比如，在绝对坐标系上规划一条轨迹：

  在这条轨迹上取一些离散点作为参考点，运动控制的目标是通过控制方向盘，让真实车辆的位置、速度以及航向角尽可能接近规划的位置、速度、航向角。

时，规划点包含了一系列信息 ，意味着按照规划点，在第五秒时，车辆位置应该是，速度应该是 ，航向角应该是 ，加速度应该是 ，这些

1、误差定义

  但是如果车辆真实运动状态和规划点之间存在误差：

  图中绿色线为误差，在真实轨迹和规划轨迹之间，有纵向误差、横向误差、航向角误差、速度误差、加速度误差。一般定义误差为真实值减去规划值，即
其中，规划值 已知，真实值

用已知的规划值 以及误差 代替，代入微分方程，得到误差微分方程：
误差微分方程是我们真正要的东西，它反映了误差 随控制量

2、代价函数定义

，让 尽可能接近

的平方加控制量 的平方最小。
  除了保证误差最小，还希望花费最小。误差 的平方和 的平方可以加权，不一定是 关系，可能是 和 的关系：
由于误差 和控制量 一般为列向量，所以加权平方可改写成这样的形式：
其中，、

在约束条件 下取最小值的数学问题。其实就是控制里的LQR，L 就是 ，即线性约束，、

---

二、误差微分方程的推导

关于误差的线性微分方程非常重要，因为只要把它解出来，后面就是在约束条件下求小值的问题。

注意：坐标系选择自然坐标系而不是直角坐标系，要求自然坐标系下

1、误差微分方程的建立

  假设在绝对坐标系下，以已有的规划轨迹为坐标轴建立自然坐标系：

，在自然坐标系下投影点的切线方向为 ，投影点的法线方向为 ，投影点速度为 ，车辆到投影点的距离为 ， 和 与 轴夹角分为 和 。

定义误差如下：

• 横向误差为
• 航向误差为
• 速度误差

其中，速度误差属于纵向公式，本篇博客介绍横向控制，所以速度误差暂时先不管。

注意： 和

2、横向误差与速度误差的关系

为速度方向，

注意： 和 不是车身坐标系的 轴和 轴方向，因为 是质心速度，质心和绝对坐标系 轴的夹角为航向角 ，而车身坐标系是以横摆角 为方向，即 轴沿着横摆角方向， 轴垂直于横摆角方向，所以 和 并不是车身坐标系的 轴和

做两个向量，一头指向投影点 ，一头指向真实的车辆位置 。 、 和 构成三角形，就有
其中， 是数，不是向量，所以要作为向量的计算法，用线段大小 乘以的单位方向向量，即这样得到

，是以 为基本的位置量。为了和 联系起来，需要对

注意： 是向量不是常矢量，会随曲线投影点的变化而变化，大小是单位数量，但是方向会不断变化，它的方向变化和曲线的曲率有关，所以

求导得到：

• 车辆质心的真实位矢 ，其导数是 。
• 是投影点的位矢，其导数为 。

简单证明

  有人可能对向量问题不熟悉，下面简单证明。

  比如在直角坐标系下：

，经过 的时间，运动到下一点，它的位矢变成 ，根据向量的三角形法则，蓝色线就是 ，根据复合求导
其中，

时， 和 大小趋于相等，的方向趋向于 的切线方向。所以其中， 是 的切线方向，所以 。

---

的导数：
  同样 可以用复合求导：
其中，，但 等于什么呢？这里要用到数学上的向量微积分中的 Frenet 公式。

  二维曲线的 Frenet 公式：
其中，

  把 Frenet 公式带进去，得到，根据向量的加减法，，再带到 里：

  又因为 和 垂直，所以后面一项为 ，即

   和 间的几何关系如下：

和 之间的角度等于，所以：
  因为 和

和 、 、

3、投影点速度的计算

的导数：
  把之前求得的结果带进去：
  等式两边同时点成 ：

则：
  得到两个非常重要的公式：

4、航向角误差与速度误差的联系

、

代进去：

  一般认为 是小量，所以又可进一步化简为：
  同样可以将 写成 和 的形式，不过到纵向控制才用得到，所以关于

，在自然坐标系里，令，注意 并不是航向误差。航向误差应该是航向角减去参考点的夹角 ，即 。

，因为质心侧偏角 和横摆角 相比较小，可以近似认为是航向误差，但要清楚它并不是航向误差，不能直接把它忽略掉，认为 。

就是航向误差，但仅限本节，所以要有潜意识，

和 ，可将 的方程改写为：

  假设 是常数，那么 就被忽略掉了。

  忽略掉 二阶以上的导数。因为道路一般曲率变化比较平缓，所以 考虑到一阶导数为止。

  引入 和 ，可得到：

  代入二自由度动力学微分方程：

得到：

5、误差微分方程的线性化

  上面两个方程太长了，看起来不方便，把它简化一下：
  这有点线性微分方程 意思，但很明显现在还不是线性微分方程组，将它改造：

---

三、线性误差微分方程组

  将上式变成线性微分方程组，写成矩阵形式：

  得到基于自然坐标系下的线性误差微分方程组：

注意：这里的A B和第三节的A B不是同一个符号

  第四节大部分任务已经完成，讲到了关于误差的线性微分方程该怎么推导。

不过本节遗留了三个问题：

1、航向误差问题

，但实际上航向误差应该是 ，将 近似认为

2、投影点航向角速度计算问题

，该怎么计算呢？

3、线性误差微分方程组的形式问题

的形式，可以用 LQR 计算。但现在形式是 ，后面还有个小尾巴 ，和 LQR 问题比较像，但不完全一致。

，而 是代表道路的几何信息，是没有办法控制的，能控制的只有方向盘转角，对于

  在下一节博客会具体解释，欢迎关注！

---

参考资料

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

---

后记：

🌟 感谢您耐心阅读这篇关于 坐标变换与横向误差微分方程 的技术博客。 📚

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

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

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

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

原创作者: u_16954161 转载于: https://blog.51cto.com/u_16954161/11911321