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