写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。
🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。
👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!
📚 系列专栏:【运动控制】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!
🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~
文章目录
引言
本篇博客是 自动驾驶控制算法 系列的第六节。内容整理自 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,误差在前面第四节讲解过误差如何计算,但遗憾的是基于连续曲线的误差,而一般规划点都是离散的。
所以还要再讲离散点误差的计算,讲完之后一切准备工作就完成了,在后续博客中会讲具体的横向控制算法,代码编写以及联合仿真。
本篇博客的内容到此结束,欢迎关注后续内容!
参考资料
后记:
🌟 感谢您耐心阅读这篇关于 前馈控制与航向误差 的技术博客。 📚
🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢
🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀
🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡
🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀