坐标变换与横向误差微分方程

最新推荐文章于 2024-09-14 10:11:50 发布

诗和远方ya

最新推荐文章于 2024-09-14 10:11:50 发布

阅读量930

点赞数 26

分类专栏：自动驾驶决策规划算法文章标签：汽车

本文链接：https://blog.csdn.net/sc_1313/article/details/141218701

版权

自动驾驶决策规划算法专栏收录该内容

6 篇文章 0 订阅

订阅专栏

1、动力学模型的状态空间表达式

在之前的文章车辆动力学模型中，我们建立了车辆侧向速度和横摆角速度关于前轮转角的表达式，我们也可以将表达式用状态空间表达式的形式写出来。
在这里插入图片描述

2、想一想

通过车辆动力学模型，我们可以在给定前轮转角的情况下，通过t时刻的横向速度和横摆角速度，估计得到车辆在t+1时刻的状态，即dot_vy和dot_wr，我们把它当作车辆的实际状态。
在这里插入图片描述
要想让车辆按照预期轨迹行驶，很显然，我们需要让车辆的实际状态尽可能地接近预期状态，预期状态是通过路径规划得到的。也就是说，要求实际状态和预期状态之间的误差值尽可能接近于0。
我们用X代表实际状态，Xr代表预期状态，下标rr代表误差项。
在这里插入图片描述
我们的控制目标是让误差err接近于0，可以选取一个代价函数J，J越小，控制效果越好。

3、横向误差微分方程

在这里插入图片描述
在全局坐标系中有一条轨迹，该轨迹为车辆预期的行驶轨迹，车辆的实际位置为O，车辆的实际位置到轨迹的投影点为Or。图中反应的信息包括：车辆分别在实际位置和投影点的速度、航向角、切向向量和法向向量、横向位置误差d。
需要注意的是，dot_s和v只代表速度的大小，不代表速度的方向，速度的方向由航向角体现。
在这里插入图片描述
很显然，控制的目的是让误差小，要想实现对误差的控制，就需要知道误差的表达式。
下面，分别求解d和dot_s。

过全局坐标系原点作两个向量，分别指向O和Or。向量OOr+向量Xr=向量X。

3.1 横向误差d

在这里插入图片描述
链接：Frenet公式

3.2 预期速度dot_s

这一小节是顺便写出来的，实际上，本篇文章讲的是横向误差微分方程，而速度是纵向控制里面的内容。
可是，速度不是车辆坐标系x轴和y轴方向速度的合成吗？为什么说速度是纵向控制呢？
这里就要转变一下思维了，我们之前的动力学模型是在车辆坐标系下建立的，而这一小节，我们用的是Frenet坐标系即自然坐标系。在Frenet坐标系中，车辆的速度方向是与切向向量重合的，因此我们说速度控制为纵向控制。
从这里，我们就可以看出Frenet坐标系相对于车辆坐标系的优势了，Frenet坐标系能将车辆的纵向控制和横向控制解耦合，这样控制过程就更加简单了。
在这里插入图片描述
纵向控制：油门、刹车–>力–>纵向加速度–>纵向速度–>纵向位移
横向控制：方向盘–>前轮转角–>横向位移/航向角
好了，下面进入正题，求dot_s。

3.3 与动力学模型结合

在这里插入图片描述
通过将动力学模型和横向误差的式子进行结合，原本动力学模型中关于横向速度和横摆角速度的式子，转化成了误差的表达式。
将线性微分方程写成状态空间表达式形式。

4、总结

可能没有意识到的是，本篇文章的工作其实就是在做坐标系的转换，将车辆坐标系转换为Frenet坐标系。
我们不妨从动力学模型看起，在动力学模型中，我们在车辆坐标系下建立力学方程，求得横摆角加速度dot_dot_fai和横向速度dot_y的表达式。但是，由于在车辆坐标系下，我们无法做到横向控制和纵向控制的解耦合，控制过程复杂，因此我们选择使用Frenet坐标系。那么，要想建立Frenet坐标系下的动力学模型，我们只需要在车辆坐标系的动力学模型的基础上，将坐标系转换掉即可。因此，在本文中，我们写了很多很多，最终得到这两个式子，完成了坐标系的转换工作。
在这里插入图片描述
简言之，我们的思路是：1.在车辆坐标系下建立动力学模型。2.进行坐标系转换，得到Frenet坐标系下的动力学模型。