写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、反馈控制
- 二、前馈控制的引入
- 三、前馈控制与稳态误差
- 1、稳态误差的定义与影响
- 2、稳态误差与前馈控制的关系
- 四、前馈控制的计算与优化
- 1、前馈控制表达式推导
- 2、航向角误差的近似处理
- 3、车辆质量等效处理
- 4、航向误差与侧偏角的关系推导
- 5、前馈控制表达式化简
- 6、反馈与前馈控制的结合
- 五、总结
- 参考资料
引言
本篇博客是 自动驾驶控制算法 系列的第六节。内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。
各位小伙伴们大家好,本节博客讲解前馈控制与航向误差。在上一节介绍了离散 LQR 以及连续 LQR,分别对应离散系统和连续系统。 LQR 的核心就是求黎卡提方程的 ,求出来之后就可以算出最优控制 。
一、反馈控制
如果是连续系统,可以用连续 LQR 算出 ,当然也可以将连续系统离散化,用离散 LQR 再算出 出来,这两个 可能不完全一样,但应该非常接近,因为无论是连续系统还是离散系统,都是对相同物理现象的不同描述,所以算出来的
为什么叫反馈控制呢?什么叫反馈?可以简单解释一下。
的系统,画出框图如下:
代表积分,即 经过此模块就变成了 。
看成输入,把 换成看成输出,即输入什么样的 ,就会得到什么样的 ,那什么叫反馈呢?LQR 算出来 实际上就是在图中 这条线上,加一条红色线。比如这样:
决定输出 ,有反馈的话就是先是输入 决定 ,然后 又反过来决定输入,这就是反馈的意思。
但这种反馈,容易出现代数环问题,即输入直接影响输出,而输出又直接影响输入,那就变成先有鸡还是先有蛋的问题。不过现在还碰不到代数环问题,等碰到时再细说,本篇博客的任务就是讲前馈控制。
二、前馈控制的引入
先来看以下框图:
左边再加 ,
为什么要加前馈控制呢?
遗留下来的小尾巴
,误差微分方程为:
观察微分方程可以发现,无论 取何值,误差 和误差的导数 都不可能同时为 ,但我们希望误差可能一开始不是 ,经过 反馈控制让它慢慢变成 ,然后就一直是
,同时误差的导数等于 ,根本就不是微分方程的解。
,误差的导数也是 ,这种状态根本不是方程的解,能控制的其实就是反馈的,但是怎么调
所以要引入前馈控制,即针对系统
令 ,其中, 是由 LQR 计算出来的反馈控制,就是用 把尾巴去掉算出来的 ,那么
三、前馈控制与稳态误差
1、稳态误差的定义与影响
稳态误差 就是稳定状态的误差,稳定就是代表误差变成常数,不会再变了。
,因为 不是方程的解,存在稳态误差。LQR 控制系统最后一定会稳定,稳定下来最终结果就是误差不再变了,即误差的导数 ,并且误差本身不是
为什么 LQR 最终会导致经过一段时间后误差就稳定不变了,系统就稳定下来了呢?
如果系统是不稳定的,那么 LQR 是没有解的;如果系统是 LQR 有解,就是 k 能算出来,那就意味着一定可以通过 LQR 使系统达到稳定状态。
那什么叫 LQR 有解?什么叫 LQR 无解?
如果看过第五节就会明白,解 LQR 时需要解黎卡提方程,通过迭代出来,如果有解的话,就意味着迭代收敛,黎卡提方程的解 不会再变。如果系统是不稳定的,黎卡提方程迭代就会发散,LQR 就没有解。
2、稳态误差与前馈控制的关系
回到刚才说的误差,误差最终等于多少?
代进去算一下,把误差的导数等于0,带到误差方程里:
最终会得到稳态误差:
引入前馈控制之后误差的导数:
,代进去:
现在目的就很简单了,选取合适的 ,使得稳态误差 尽可能为
注意:
四、前馈控制的计算与优化
1、前馈控制表达式推导
具体的表达式:
其中, 是 矩阵, 是 矩阵, 也是 下面用软件 Mathematical
进行矩阵求逆运算:
其中, .
根据 Mathematic
得到的化简结果,再进行进一步化简,最后得到误差:
列向量的第一行和第三行都有 ,这就是 对误差的影响。写到这其实就很明显,前馈控制表达式:
此时,。其中,是反馈行向量 中的 。
,然后再算前馈 。
2、航向角误差的近似处理
再来看一下误差的第三行:
可以发现 不受 和 的影响,表达式里没有 ,也没有 。即无论前馈和反馈取什么值,都永远不可能为零。因为能控制的就是前馈以及反馈。
的表达式,似乎感觉能控制的只有 ,因为侧偏刚度是负的,如果 取特定值,那么有可能 ,意味着
注意: 不是航向误差, 定义的是,而航向误差应该是 。我们的目的是想让航向误差和横向误差都为 :
- 横向误差为 :可以通过前馈控制。就是 取那后面那一坨东西解决,横向误差可以为 。
- 航向误差为0:航向误差如果按真正的定义,那么 就不为 。
,那么 的稳态物差应该是 才对,那么问题是现在式子是不是等于 。
表达式进行更进一步的化简,在第四节讲到 和它的投影 之间的关系:
和
在直角坐标系下曲率的计算式:
大家都非常熟悉,但是曲率有定义式:
这是曲率最原始的定义式,由此可得:
其中,
,,,假设车辆没有漂移,所以:
这样直接得到 ,那么 :
又因为 可得:
又因为无漂移的假设,所以把 直接忽略掉:
所以 到这一步离最终结果越来越近了,因为 是侧向力 , 除以 是侧边角,但是这样还是不够,因为 是总侧向力,包括前轮和后轮,而
怎样才能得到后轮的侧向力呢?
3、车辆质量等效处理
对车辆质量进行等效处理
是不是感觉有点熟悉? 比如有个质量块:
质量为 m,质心到前边的距离为 a,到后边距离为 b。只考虑质心的话,可以完全等效成叠加的两个质量块,质量是 和 。
和,使得质心和原来大质量块的质心完全一样,并且 ,如果能做到这一点,在质量分布维度上,这两个东西完全等效。
以质心为原点建立坐标系,等效前提是
所以
这就正好可以把四轮车把按照质心分成上半和下半:
上半质量为 ,下半质量为 , 就等于后轮侧向力,严格来说应该是后轮侧向力之和。因为我们把汽车模型简化为自行车模型:
是单个轮子的侧向力,并且自行车模型的侧偏刚度是单个轮的侧偏刚度的两倍,即把两个轮子合并成一个轮子,可直接得到 :
4、航向误差与侧偏角的关系推导
自行车模型如下:
,因为用的是右手系,以左为正,以右为负,在轮胎中轴线右边,所以是 。
,根据弧度的定义,上面的角
度,所以
所以
这样正好:
- 不是航向误差,,航向误差是,
- 的稳态误差为,这样的话会得到
这就是想要的结果。
不可能通过 和 去调节,但是不用去理会事情,因为最终的目的是,就意味,而推导出来的稳态误差正好就是。
5、前馈控制表达式化简
前馈控制表达式:
若令 ,则可进一步化简为
6、反馈与前馈控制的结合
:
- 通过 LQR 算出来,称为 反馈控制
- 根据上面公式算出来,称为 前馈控制
通过反馈和前馈控制就可以将误差变成
不是航向误差,但最终导致航向误差为 。
五、总结
本篇博客讲解了航向误差以及前馈控制。下一节会讲基于离散 Frenet 坐标系的规划点误差的计算。
, 通过 LQR 解决,
,误差在前面第四节讲解过误差如何计算,但遗憾的是基于连续曲线的误差,而一般规划点都是离散的。
所以还要再讲离散点误差的计算,讲完之后一切准备工作就完成了,在后续博客中会讲具体的横向控制算法,代码编写以及联合仿真。
本篇博客的内容到此结束,欢迎关注后续内容!
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 前馈控制与航向误差 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀