写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。
👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!
📚 系列专栏:【运动控制】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
- 引言
- 一、反馈控制
- 二、前馈控制的引入
- 三、前馈控制与稳态误差
- 1、稳态误差的定义与影响
- 2、稳态误差与前馈控制的关系
- 四、前馈控制的计算与优化
- 1、前馈控制表达式推导
- 2、航向角误差的近似处理
- 3、车辆质量等效处理
- 4、航向误差与侧偏角的关系推导
- 5、前馈控制表达式化简
- 6、反馈与前馈控制的结合
- 五、总结
- 参考资料
引言
本篇博客是 自动驾驶控制算法 系列的第六节。内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。
各位小伙伴们大家好,本节博客讲解前馈控制与航向误差。在上一节介绍了离散 LQR 以及连续 LQR,分别对应离散系统和连续系统。 LQR 的核心就是求黎卡提方程的 P P P,求出来之后就可以算出最优控制 u = − k x u =-k x u=−kx。
一、反馈控制
如果是连续系统,可以用连续 LQR 算出 k k k ,当然也可以将连续系统离散化,用离散 LQR 再算出 k k k 出来,这两个 k k k 可能不完全一样,但应该非常接近,因为无论是连续系统还是离散系统,都是对相同物理现象的不同描述,所以算出来的 k k k 应该大差不差。
这种 u = − k x u =-k x u=−kx 的控制叫做反馈控制。
为什么叫反馈控制呢?什么叫反馈?可以简单解释一下。
对于 X ˙ = A X + B u \dot{X}=AX+Bu X˙=AX+Bu 的系统,画出框图如下:
1 s \frac{1}{s} s1代表积分,即 x ˙ \dot x x˙ 经过此模块就变成了 x x x。
如果把 u u u 看成输入,把 x x x 换成看成输出,即输入什么样的 u u u,就会得到什么样的 x x x,那什么叫反馈呢?LQR 算出来 u = − k x u =-k x u=−kx 实际上就是在图中 x x x 这条线上,加一条红色线。比如这样:
这种控制就叫反馈控制,下边的红色的线就叫反馈,如果没有反馈,输入 u u u 决定输出 x x x,有反馈的话就是先是输入 u u u 决定 x x x,然后 x x x 又反过来决定输入 u u u,这就是反馈的意思。
但这种反馈,容易出现代数环问题,即输入直接影响输出,而输出又直接影响输入,那就变成先有鸡还是先有蛋的问题。不过现在还碰不到代数环问题,等碰到时再细说,本篇博客的任务就是讲前馈控制。
二、前馈控制的引入
先来看以下框图:
在 B B B 左边再加 δ f \delta_f δf, δ f \delta_f δf 就叫做前馈控制,不依赖于x,就好像空降一样,在反馈控制前再加前馈控制。
为什么要加前馈控制呢?
因为要处理第四讲 e ˙ r r = A e r r + B u + C θ ˙ r \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_{r} e˙rr=Aerr+Bu+Cθ˙r 遗留下来的小尾巴 C θ ˙ r C\dot \theta_r Cθ˙r
如果没有前馈,只有反馈的话,即只用 LQR ,控制量 u = − k e r r u=-ke_{rr} u=−kerr,误差微分方程为:
e ˙ r r = ( A − B K ) e r r + C θ ˙ r \dot{e}_{rr}=\left( A-BK \right) e_{rr}+C\dot{\theta}_r e˙rr=(A−BK)err+Cθ˙r 观察微分方程可以发现,无论 k k k 取何值,误差 e r r e_{rr} err 和误差的导数 e r r e_{rr} err 都不可能同时为 0 0 0,但我们希望误差可能一开始不是 0 0 0,经过 k k k 反馈控制让它慢慢变成 0 0 0,然后就一直是 0 0 0 稳定运行下去。
但是微方程能告诉我们这是不可能的事情,因为误差等于 0 0 0,同时误差的导数等于 0 0 0,根本就不是微分方程的解。
微分方程描述的是物理规律,是误差所满足的牛顿运动定律,不能被违背。既然不能被违背的话,就意味着如果只用反馈控制,根本不可能达到理想状态,即误差是 0 0 0,误差的导数也是 0 0 0,这种状态根本不是方程的解,能控制的其实就是反馈的 k k k,但是怎么调 k k k 都没有用。
所以要引入前馈控制,即针对系统
e ˙ r r = A e r r + B u + C θ ˙ r \dot{e}_{rr}=Ae_{rr}+Bu+C\dot{\theta}_{r} e˙rr=Aerr+Bu+Cθ˙r 令 u = − k x + δ f u=-kx+\delta_f u=−kx+δf,其中, − k x -kx −kx 是由 LQR 计算出来的反馈控制,就是用 X ˙ = A X + B u \dot X=AX+Bu X˙=AX+Bu 把尾巴去掉算出来的 k k k,那么 δ f \delta_f δf 就是前馈控制,前馈控制的引入是为了消除稳态误差。
三、前馈控制与稳态误差
1、稳态误差的定义与影响
稳态误差 就是稳定状态的误差,稳定就是代表误差变成常数,不会再变了。
但由于物理限制,误差常数肯定不是 0 0 0,因为 0 0 0 不是方程的解,存在稳态误差。LQR 控制系统最后一定会稳定,稳定下来最终结果就是误差不再变了,即误差的导数 e ˙ r r = 0 \dot e_{rr}=0 e˙rr=0 ,并且误差本身不是 0 0 0
为什么 LQR 最终会导致经过一段时间后误差就稳定不变了,系统就稳定下来了呢?
如果系统是不稳定的,那么 LQR 是没有解的;如果系统是 LQR 有解,就是 k 能算出来,那就意味着一定可以通过 LQR 使系统达到稳定状态。
那什么叫 LQR 有解?什么叫 LQR 无解?
如果看过第五节就会明白,解 LQR 时需要解黎卡提方程,通过迭代出来,如果有解的话,就意味着迭代收敛,黎卡提方程的解 P P P 不会再变。如果系统是不稳定的,黎卡提方程迭代就会发散,LQR 就没有解。
2、稳态误差与前馈控制的关系
回到刚才说的误差,误差最终等于多少?
代进去算一下,把误差的导数等于0,带到误差方程里:
0 = ( A − B K ) e r r + C θ ˙ r 0=\left( A-BK \right) e_{rr}+C\dot{\theta}_r 0=(A−BK)err+Cθ˙r
最终会得到稳态误差:
e r r = − ( A − B k ) − 1 C θ ˙ r e_{rr}=-(A-Bk)^{-1}C\dot{\theta}_r err=−(A−Bk)−1Cθ˙r
引入前馈控制之后误差的导数:
e ˙ r r = A e r r + B ( − k e r r + δ f ) + C θ ˙ r \dot{e}_{rr}=Ae_{rr}+B\left( -ke_{rr}+\delta _f \right) +C\dot{\theta}_r e˙rr=Aerr+B(−kerr+δf)+Cθ˙r
稳定后,误差的导数 e ˙ r r = 0 \dot e_{rr}= 0 e˙rr=0 ,代进去:
e r r = − ( A − B k ) − 1 ⋅ ( B δ f + C θ ˙ r ) e_{rr}=-(A-Bk)^{-1}\cdot(B\delta_{f}+C\dot{\theta}_{r}) err=−(A−Bk)−1⋅(Bδf+Cθ˙r) 现在目的就很简单了,选取合适的 δ f \delta_{f} δf,使得稳态误差 e r r = − ( A − B k ) − 1 ⋅ ( B δ f + C θ ˙ r ) e_{rr}=-(A-Bk)^{-1}\cdot(B\delta_{f}+C\dot{\theta}_{r}) err=−(A−Bk)−1⋅(Bδf+Cθ˙r) 尽可能为 0 0 0 。
注意: B B B 不是矩阵,是列向量,列向量是没有逆的,只有矩阵,而且是方阵才有逆,所以并不能直接令右边括号内的项为0。
四、前馈控制的计算与优化
1、前馈控制表达式推导
代入第四节中 A 、 B 、 C A、B、C A、B、C 具体的表达式:
其中, A A A 是 4 × 4 4\times 4 4×4 矩阵, B B B 是 4 × 1 4\times 1 4×1 矩阵, C C C 也是 4 × 1 4\times 1 4×1 矩阵,而且它们都有很非常复杂的符号。现在要求逆,但这根本就不是人手算就可以完成的工作。所以将借助数学软件进行运算。
下面用软件 Mathematical
进行矩阵求逆运算:
其中, k = ( k 1 , k 2 , k 3 , k 4 ) k=(k_1,k_2,k_3,k_4) k=(k1,k2,k3,k4) .
根据 Mathematic
得到的化简结果,再进行进一步化简,最后得到误差:
e r r = ( 1 k 1 { δ f − θ ˙ r v x [ a + b − b k 3 − m v x 2 a + b ( b C α f + a C α r k 3 − a C α r ) ] } 0 − θ ˙ r v x ( b + a a + b m v x 2 C α r ) 0 ) e_{rr}=\left( \begin{array}{c} \frac{1}{k_1}\left\{ \delta _f-\frac{\dot{\theta}_r}{v_x}\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right] \right\}\\ 0\\ -\frac{\dot{\theta}_r}{v_x}\left( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C_{\alpha r}} \right)\\ 0\\ \end{array} \right) err= k11{δf−vxθ˙r[a+b−bk3−a+bmvx2(Cαfb+Cαrak3−Cαra)]}0−vxθ˙r(b+a+baCαrmvx2)0 列向量的第一行和第三行都有 θ ˙ r \dot \theta_r θ˙r,这就是 θ ˙ r \dot \theta_r θ˙r 对误差的影响。写到这其实就很明显,前馈控制表达式:
δ f = θ ˙ r v x [ a + b − b k 3 − m v x 2 a + b ( b C α f + a C α r k 3 − a C α r ) ] \delta _f=\frac{\dot{\theta}_r}{v_x}\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right] δf=vxθ˙r[a+b−bk3−a+bmvx2(Cαfb+Cαrak3−Cαra)] 此时, e d = 0 e_d=0 ed=0。其中, k 3 k_3 k3是反馈行向量 k = ( k 1 , k 2 , k 3 , k 4 ) k=(k_1,k_2,k_3,k_4) k=(k1,k2,k3,k4) 中的 k 3 k_3 k3。
可见,前馈依赖于反馈,所以要先算反馈 k k k,然后再算前馈 δ f \delta _f δf。
2、航向角误差的近似处理
再来看一下误差的第三行:
e φ = − θ ˙ r v x ( b + a a + b m v x 2 C α r ) e_\varphi =-\frac{\dot{\theta}_r}{v_x}\left( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C_{\alpha r}} \right) eφ=−vxθ˙r(b+a+baCαrmvx2) 可以发现 e φ e_\varphi eφ 不受 δ f \delta_f δf 和 k k k 的影响,表达式里没有 δ f \delta_f δf,也没有 k k k。即无论前馈和反馈取什么值, e φ e_\varphi eφ都永远不可能为零。因为能控制的就是前馈以及反馈。
看一下 e φ e_\varphi eφ 的表达式,似乎感觉能控制的只有 v x v_x vx,因为侧偏刚度是负的,如果 v x v_x vx 取特定值,那么有可能 e φ = 0 e_\varphi=0 eφ=0,意味着 v x v_x vx 只能取特定值,即车辆只能用特定速度,很明显不现实。
注意: e φ e_\varphi eφ 不是航向误差, e φ e_\varphi eφ 定义的是 φ − θ r \varphi -\theta_r φ−θr,而航向误差应该是 φ + β − θ r \varphi +\beta -\theta_r φ+β−θr。我们的目的是想让航向误差和横向误差都为 0 0 0:
-
横向误差为 0 0 0:可以通过前馈控制。就是 δ f \delta_f δf 取那后面那一坨东西解决,横向误差可以为 0 0 0。
-
航向误差为0:航向误差如果按真正的定义 φ + β − θ r = 0 \varphi +\beta -\theta_r=0 φ+β−θr=0,那么 e φ e_\varphi eφ 就不为 0 0 0。
如果真正的目的是 φ + β − θ r = 0 \varphi +\beta -\theta_r =0 φ+β−θr=0,那么 φ − θ r \varphi -\theta_r φ−θr 的稳态物差应该是 − β -\beta −β 才对,那么问题是现在式子是不是等于 − β -\beta −β。
首先对 e φ e_\varphi eφ 表达式进行更进一步的化简,在第四节讲到 v v v 和它的投影 s ˙ \dot s s˙ 之间的关系:
s ˙ = ∣ v ⃗ ∣ cos ( θ − θ r ) 1 − κ e d = ∣ v ⃗ ∣ cos ( β + φ − θ r ) 1 − κ e d = ∣ v ⃗ ∣ cos β cos φ − ∣ v ⃗ ∣ sin β sin φ 1 − κ e d = v x cos e φ − v y sin e φ 1 − κ e d \begin{aligned} \dot{s}&=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d}=\frac{|\vec{v}|\cos \left( \beta +\varphi -\theta _r \right)}{1-\kappa e_d}\\ &=\frac{|\vec{v}|\cos \beta \cos \varphi -|\vec{v}|\sin \beta \sin \varphi}{1-\kappa e_d}\\ &=\frac{v_x\cos e_{\varphi}-v_y\sin e_{\varphi}}{1-\kappa e_d}\\ \end{aligned} s˙=1−κed∣v∣cos(θ−θr)=1−κed∣v∣cos(β+φ−θr)=1−κed∣v∣cosβcosφ−∣v∣sinβsinφ=1−κedvxcoseφ−vysineφ s ˙ \dot{s} s˙ 和 θ ˙ r \dot\theta_r θ˙r 有很密切的关系
在直角坐标系下曲率的计算式:
κ = y ′ ′ ( 1 + y ′ ) 3 2 \kappa =\frac{y^{\prime\prime}}{(1+y^{\prime})^{\frac{3}{2}}} κ=(1+y′)23y′′ 大家都非常熟悉,但是曲率有定义式:
κ = d θ d s = d θ / d t d s / d t = θ ˙ s ˙ \kappa =\frac{d\theta}{ds}=\frac{d\theta /dt}{ds/dt}=\frac{\dot{\theta}}{\dot{s}} κ=dsdθ=ds/dtdθ/dt=s˙θ˙ 这是曲率最原始的定义式,由此可得:
θ ˙ = κ s ˙ \dot{\theta}=\kappa \dot{s} θ˙=κs˙ 其中, s ˙ \dot s s˙ 的表达式太复杂,要进行近似。
一般规划的曲率 ∣ k ∣ ≪ 1 \left| k \right|\ll 1 ∣k∣≪1, ∣ e φ ∣ ≪ 1 \left| e_{\varphi} \right|\ll 1 ∣eφ∣≪1, ∣ v y ∣ ≪ 1 \left| v_y \right|\ll 1 ∣vy∣≪1,假设车辆没有漂移,所以:
1 1 − κ e d ≈ 1 v x cos e φ ≈ v x v y sin e φ ≈ 0 \frac{1}{1-\kappa e_d}\approx 1\quad v_x\cos e_{\varphi}\approx v_x\quad v_y\sin e_{\varphi}\approx 0 1−κed1≈1vxcoseφ≈vxvysineφ≈0 这样直接得到 s ˙ ≈ v x \dot{s}\approx v_x s˙≈vx,那么 :
θ ˙ r = κ s ˙ ≈ κ v x \dot{\theta}_r=\kappa \dot{s}\approx \kappa v_x θ˙r=κs˙≈κvx 又因为 κ = 1 / R \kappa = 1/R κ=1/R 可得:
e φ = − κ ( b + a a + b m v x 2 C α r ) = − ( b R + a a + b m v x 2 R 1 C α r ) \begin{aligned} e_{\varphi}&=-\kappa \left( b+\frac{a}{a+b}\frac{mv_{x}^{2}}{C_{\alpha r}} \right)\\ &=-\left( \frac{b}{R}+\frac{a}{a+b}\frac{mv_{x}^{2}}{R}\frac{1}{C_{\alpha r}} \right)\\ \end{aligned} eφ=−κ(b+a+baCαrmvx2)=−(Rb+a+baRmvx2Cαr1) 又因为无漂移的假设,所以把 v y 、 v ˙ y v_y、\dot v_y vy、v˙y直接忽略掉:
φ ˙ = v ⃗ R = v ⃗ x + v ⃗ y R ≈ v ⃗ x R \dot{\varphi}=\frac{\vec{v}}{R}=\frac{\vec{v}_{x}+\vec{v}_{y}}{R}\approx\frac{\vec{v}_{x}}{R} φ˙=Rv=Rvx+vy≈Rvx a y = v y + v x φ ˙ ≈ v x φ ˙ ≈ v x 2 R \begin{array}{c} a_y=v_y+v_x\dot{\varphi}\approx v_x\dot{\varphi}\approx \frac{v_{x}^{2}}{R}\\ \end{array} ay=vy+vxφ˙≈vxφ˙≈Rvx2 所以 e φ = − ( b R + a a + b m a y ⋅ 1 C α r ) e_{\varphi}=-\left( \frac{b}{R}+\frac{a}{a+b}ma_y\cdot \frac{1}{C_{\alpha r}} \right) eφ=−(Rb+a+bamay⋅Cαr1) 到这一步离最终结果越来越近了,因为 m a y ma_y may 是侧向力 F y F_y Fy, m a y ma_y may除以 C α r C_{\alpha r} Cαr 是侧边角,但是这样还是不够,因为 m a y ma_y may 是总侧向力,包括前轮和后轮,而 C α r C_{\alpha r} Cαr 只是后轮的侧偏刚度,如果是后轮的侧向力除以后轮的侧偏刚度,那就得到后轮的侧偏角。
怎样才能得到后轮的侧向力呢?
3、车辆质量等效处理
对车辆质量进行等效处理
看到 a a + b \frac{a}{a+b} a+ba是不是感觉有点熟悉? 比如有个质量块:
质量为 m,质心到前边的距离为 a,到后边距离为 b。只考虑质心的话,可以完全等效成叠加的两个质量块,质量是 m f m_f mf 和 m r m_r mr。
如果是考虑别的东西的话,那把质量块分成两个肯定是不能完全等效的。但是如果仅仅在质量分布以及质心维度上考虑的话,可以选取适量的 m f m_f mf和 m r m_r mr,使得质心和原来大质量块的质心完全一样,并且 m f + m r = m m_f+m_r=m mf+mr=m,如果能做到这一点,在质量分布维度上,这两个东西完全等效。
以质心为原点建立坐标系,等效前提是
{ m f + m r = m m f ⋅ a 2 + m r ⋅ ( − b 2 ) = 0 ⇒ { m f = b a + b m m r = a a + b m \left\{ \begin{array}{l} m_f+m_r=m\\ m_f\cdot \frac{a}{2}+m_r\cdot \left( -\frac{b}{2} \right) =0\\ \end{array} \right. \Rightarrow \left\{ \begin{array}{l} m_f=\frac{b}{a+b}m\\ m_r=\frac{a}{a+b}m\\ \end{array} \right. {mf+mr=mmf⋅2a+mr⋅(−2b)=0⇒{mf=a+bbmmr=a+bam 所以
e φ = − ( b R + m r a y C α r ) e_{\varphi}=-\left( \frac{b}{R}+\frac{m_ra_y}{C_{\alpha r}} \right) eφ=−(Rb+Cαrmray) 这就正好可以把四轮车把按照质心分成上半和下半:
上半质量为 m f m_f mf,下半质量为 m r m_r mr, m r a y m_ra_y mray 就等于后轮侧向力,严格来说应该是后轮侧向力之和。因为我们把汽车模型简化为自行车模型:
也就是把两个轮子合并成一个轮子,认为两个轮子一样, F y r F_{yr} Fyr 是单个轮子的侧向力,并且自行车模型的侧偏刚度是单个轮的侧偏刚度的两倍,即把两个轮子合并成一个轮子,可直接得到 e φ e_\varphi eφ:
e φ = − ( b R + α r ) e_{\varphi}=-(\frac{b}{R}+\alpha_r) eφ=−(Rb+αr) 化简到这一步已经化简非常多了。但还可以进一步化解。
4、航向误差与侧偏角的关系推导
自行车模型如下:
后轮侧偏角为 − α r -\alpha_r −αr,因为用的是右手系,以左为正,以右为负,在轮胎中轴线右边,所以是 − α r -\alpha_r −αr。
因为 R > > b R>>b R>>b,根据弧度的定义,上面的角 γ ≈ b R \gamma \approx\frac{b}{R} γ≈Rb
在三角形中,三角形的内角和等于 180 180 180 度,所以
γ + π 2 − β + π 2 − ( − α r ) = π ⇒ − β = − ( γ + α r ) = − ( b R + α r ) \begin{array}{c}{{\gamma+\frac{\pi}{2}-\beta+\frac{\pi}{2}-(-\alpha_r)=\pi}}\\{\Rightarrow-\beta=-(\gamma+\alpha_{r})=-(\frac{b}{R}+\alpha_{r})}\\\end{array} γ+2π−β+2π−(−αr)=π⇒−β=−(γ+αr)=−(Rb+αr) 所以
e φ = − β e_{\varphi}=-\beta eφ=−β
这样正好:
-
e φ e_{\varphi} eφ不是航向误差, e φ = φ − θ r e_{\varphi}=\varphi - \theta_r eφ=φ−θr,航向误差是 θ − θ r \theta - \theta_r θ−θr, θ = φ + β \theta=\varphi+\beta θ=φ+β 。
-
e φ e_{\varphi} eφ 的稳态误差为 − β -\beta −β,这样的话会得到
e φ = φ − θ r ⇒ − β = φ − θ r φ + β = θ r e_{\varphi}=\varphi-\theta_{r}\quad\Rightarrow-\beta=\varphi-\theta_{r}\quad\varphi+\beta=\theta_{r} eφ=φ−θr⇒−β=φ−θrφ+β=θr这就是想要的结果。
虽然 e φ e_{\varphi} eφ 不可能通过 δ f \delta_f δf 和 k k k 去调节,但是不用去理会事情,因为最终的目的是 θ − θ r = 0 \theta - \theta_r=0 θ−θr=0,就意味 e φ = − β e_{\varphi}=-\beta eφ=−β,而推导出来的 e φ e_{\varphi} eφ稳态误差正好就是 − β -\beta −β。
所以不用管 e φ e_{\varphi} eφ 不为零的事情。
只需要用前馈控制保证横向误差 e d = 0 e_d=0 ed=0 就可以了。
5、前馈控制表达式化简
前馈控制表达式:
δ f = θ ˙ r v x [ a + b − b k 3 − m v x 2 a + b ( b C α f + a C α r k 3 − a C α r ) ] \delta _f=\frac{\dot{\theta}_r}{v_x}\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right] δf=vxθ˙r[a+b−bk3−a+bmvx2(Cαfb+Cαrak3−Cαra)]若令 θ ˙ r = κ v x \dot{\theta}_r= \kappa v_x θ˙r=κvx,则可进一步化简为
δ f = κ [ a + b − b k 3 − m v x 2 a + b ( b C α f + a C α r k 3 − a C α r ) ] \delta _f=\kappa\left[ a+b-bk_3-\frac{mv_{x}^{2}}{a+b}\left( \frac{b}{C_{\alpha f}}+\frac{a}{C_{\alpha r}}k_3-\frac{a}{C_{\alpha r}} \right) \right] δf=κ[a+b−bk3−a+bmvx2(Cαfb+Cαrak3−Cαra)]
6、反馈与前馈控制的结合
所以最终的控制量 u u u:
u = − k e r r + δ f u=-ke_{rr}+\delta _f u=−kerr+δf
- k k k 通过 LQR 算出来,称为 反馈控制
- δ f \delta _f δf 根据上面公式算出来,称为 前馈控制
通过反馈和前馈控制就可以将误差变成
e r r = ( 0 0 − β 0 ) e_{rr}=\left( \begin{array}{c} 0\\ 0\\ -\beta\\ 0\\ \end{array} \right) err= 00−β0
e φ e_{\varphi} eφ不是航向误差,但最终导致航向误差为 0 0 0。
五、总结
本篇博客讲解了航向误差以及前馈控制。下一节会讲基于离散 Frenet 坐标系的规划点误差的计算。
大家看一下公式, u = − k e r r + δ f u=-ke_{rr}+\delta _f u=−kerr+δf, k k k 通过 LQR 解决, δ f \delta _f δf 通过前馈控制解决。
那么最后就是误差 e r r e_{rr} err,误差在前面第四节讲解过误差如何计算,但遗憾的是基于连续曲线的误差,而一般规划点都是离散的。
所以还要再讲离散点误差的计算,讲完之后一切准备工作就完成了,在后续博客中会讲具体的横向控制算法,代码编写以及联合仿真。
本篇博客的内容到此结束,欢迎关注后续内容!
参考资料
【基础】自动驾驶控制算法第六讲 前馈控制与航向误差
后记:
🌟 感谢您耐心阅读这篇关于 前馈控制与航向误差 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀