写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、微分方程与规划轨迹
- 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 问题比较像,但不完全一致。
,而
是代表道路的几何信息,是没有办法控制的,能控制的只有方向盘转角,对于
在下一节博客会具体解释,欢迎关注!
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 坐标变换与横向误差微分方程 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀