5.1、车辆横向动力学模型
车辆动力学模型一般包括用于分析车辆平顺性的质量-弹簧-阻尼模型和分析车辆操纵稳定性的车辆-轮胎模型。两者研究的侧重点不同,平顺性分析的重点是车辆的悬架特性,而车辆的操纵稳定性分析的重点是车辆纵向和侧向力学特性。车辆横向控制的目标是使车辆快速而稳定地跟踪给定的期望路径,属于车辆操纵稳定性的范畴,因此对悬架特性不做深入研究。
由于建立的动力学模型主要是作为控制器的预测模型使用,需要在较为准确地描述车辆动力学过程的基础上尽可能地简化,以减少控制算法的计算量。因此,在车辆动力学建模时,要进行以下理想化假设:
(1)假设无人驾驶车辆在平坦路面上行驶,忽略车辆垂向运动;
(2)假设悬架系统及车辆是刚性的,忽略悬架运动及其对耦合关系的影响;
(3)只考虑纯侧偏轮胎特性,忽略横向、纵向轮胎力的耦合关系;
(4)不考虑轮胎的横向载荷转移;
(5)认为轮距相对于转弯半径可忽略不计,使用自行车模型来描述车辆的运动;
(6)忽略横向和纵向空气动力学对车辆横摆运动的影响;
简化后的车辆运动学模型如上图所示。我们选择车辆的质心为参考点,在车身坐标系
y
y
y轴上,根据其受力平衡和力矩平衡可以得到
{
F
y
f
,
+
F
y
r
,
=
m
a
y
=
m
(
v
˙
y
+
v
x
ψ
˙
)
l
f
F
y
f
,
−
l
r
F
y
r
,
=
I
z
ψ
¨
\begin{cases} F_{yf}^,+F_{yr}^,=ma_y=m(\dot{v}_y+v_x\dot{\psi})\\ l_fF_{yf}^,-l_rF_{yr}^,=I_z\ddot{\psi} \end{cases}
{Fyf,+Fyr,=may=m(v˙y+vxψ˙)lfFyf,−lrFyr,=Izψ¨
其中
F y f , F_{yf}^, Fyf,是车辆前轴上的侧向力的合力;
F y r , F_{yr}^, Fyr,是车辆后轴上的侧向力的合力;
m m m为车辆质量;
a y a_y ay为车身坐标系下的侧向加速度;
v x v_x vx和 v y v_y vy为车身坐标系下质心的纵向速度和侧向速度;
I z I_z Iz为车辆绕 z z z轴的转动惯量;
l f l_f lf和 l r l_r lr分别为车辆质心到前、后轴的距离;
ψ \psi ψ为车辆横摆角;
车辆运动学模型与车辆动力学模型最大的区别就是车辆动力学认为轮胎的朝向与轮胎的速度方向不相同相同。在车辆动力学模型中,考虑到汽车在道路上行驶时,轮胎不仅受到纵向力,还会受到侧向力,这就导致轮胎的朝向与轮胎的速度方向不一致,即产生一个侧偏角
α
f
\alpha_f
αf。
α
f
=
δ
−
θ
v
f
\alpha_f=\delta-\theta_{vf}
αf=δ−θvf
其中
α f \alpha_f αf为轮胎侧偏角;
δ \delta δ为前轮转角;
θ v f \theta_{vf} θvf为车辆纵轴与车辆速度方向的夹角;
轮胎侧向力与轮胎侧偏角的关系如图所示。由曲线可以看出,当轮胎侧偏角较小时,轮胎侧向力可以近似表示为轮胎侧偏角的线性函数,即
F
y
f
=
2
c
f
α
f
=
2
c
f
(
δ
−
θ
v
f
)
F
y
r
=
2
c
r
α
r
=
2
c
r
(
−
θ
v
r
)
F_{yf}=2c_f\alpha_f=2c_f(\delta-\theta_{vf})\\ F_{yr}=2c_r\alpha_r=2c_r(-\theta_{vr})
Fyf=2cfαf=2cf(δ−θvf)Fyr=2crαr=2cr(−θvr)
其中
c f c_f cf、 c r c_r cr分别为前轮与后轮的侧偏刚度;
分别将前后轮速度分解到车身坐标系上,有
v
f
x
=
v
f
×
cos
(
θ
v
f
)
=
v
x
v
f
y
=
v
f
×
sin
(
θ
v
f
)
=
v
y
+
l
f
⋅
ψ
˙
v
r
x
=
v
r
×
cos
(
θ
v
r
)
=
v
x
v
r
y
=
v
r
×
sin
(
θ
v
r
)
=
v
y
−
l
r
⋅
ψ
˙
v_{fx}=v_f\times \cos(\theta_{vf})=v_x\\ v_{fy}=v_f\times \sin(\theta_{vf})=v_y+l_f\cdot\dot{\psi}\\ v_{rx}=v_r\times \cos(\theta_{vr})=v_x\\ v_{ry}=v_r\times \sin(\theta_{vr})=v_y-l_r\cdot \dot{\psi}
vfx=vf×cos(θvf)=vxvfy=vf×sin(θvf)=vy+lf⋅ψ˙vrx=vr×cos(θvr)=vxvry=vr×sin(θvr)=vy−lr⋅ψ˙
由此可以得到前后轮速度偏角的表达式
θ
v
f
=
tan
−
1
(
v
f
y
v
f
x
)
=
tan
−
1
(
v
y
+
l
f
ψ
˙
v
x
)
θ
v
r
=
tan
−
1
(
v
r
y
v
r
x
)
=
tan
−
1
(
v
y
−
l
r
ψ
˙
v
x
)
\theta_{vf}=\tan^{-1}(\frac{v_{fy}}{v_{fx}})=\tan^{-1}(\frac{v_y+l_f\dot{\psi}}{v_x})\\ \theta_{vr}=\tan^{-1}(\frac{v_{ry}}{v_{rx}})=\tan^{-1}(\frac{v_y-l_r\dot{\psi}}{v_x})
θvf=tan−1(vfxvfy)=tan−1(vxvy+lfψ˙)θvr=tan−1(vrxvry)=tan−1(vxvy−lrψ˙)
将轮胎侧向力分解到车身坐标系
y
y
y轴上,可得
(
F
y
f
cos
(
δ
)
−
F
x
f
sin
(
δ
)
)
+
F
y
r
=
m
(
v
˙
y
+
v
x
r
)
l
f
(
F
y
f
cos
(
δ
)
−
F
x
f
sin
(
δ
)
)
−
l
r
F
y
r
=
I
z
ψ
¨
(F_{yf}\cos(\delta)-F_{xf}\sin(\delta))+F_{yr}=m(\dot{v}_y+v_xr)\\ l_f(F_{yf}\cos(\delta)-F_{xf}\sin(\delta))-l_rF_{yr}=I_z\ddot{\psi}
(Fyfcos(δ)−Fxfsin(δ))+Fyr=m(v˙y+vxr)lf(Fyfcos(δ)−Fxfsin(δ))−lrFyr=Izψ¨
解得
v
˙
y
=
c
f
[
δ
−
tan
−
1
(
v
y
+
l
f
ψ
˙
v
x
)
]
cos
(
δ
)
−
c
r
tan
−
1
(
v
y
−
l
r
ψ
˙
v
x
)
−
F
x
f
sin
(
δ
)
m
−
v
x
ψ
˙
ψ
¨
=
l
f
c
f
[
δ
−
tan
−
1
(
v
y
+
l
f
ψ
˙
v
x
)
]
cos
(
δ
)
+
l
r
c
r
tan
−
1
(
v
y
−
l
r
ψ
˙
v
x
)
+
l
f
F
x
f
sin
(
δ
)
I
z
\dot{v}_y=\frac{c_f[\delta-\tan^{-1}(\frac{v_y+l_f\dot{\psi}}{v_x})]\cos(\delta)-c_r\tan^{-1}(\frac{v_y-l_r\dot{\psi}}{v_x})-F_{xf}\sin(\delta)}{m}-v_x\dot{\psi}\\ \ddot{\psi}=\frac{l_fc_f[\delta-\tan^{-1}(\frac{v_y+l_f\dot{\psi}}{v_x})]\cos(\delta)+l_rc_r\tan^{-1}(\frac{v_y-l_r\dot{\psi}}{v_x})+l_fF_{xf}\sin(\delta)}{I_z}
v˙y=mcf[δ−tan−1(vxvy+lfψ˙)]cos(δ)−crtan−1(vxvy−lrψ˙)−Fxfsin(δ)−vxψ˙ψ¨=Izlfcf[δ−tan−1(vxvy+lfψ˙)]cos(δ)+lrcrtan−1(vxvy−lrψ˙)+lfFxfsin(δ)
对转向角做小角度假设,即
cos
(
δ
)
≈
1
\cos(\delta)\approx1
cos(δ)≈1,
sin
(
δ
)
≈
0
\sin(\delta)\approx0
sin(δ)≈0,
tan
−
1
(
θ
)
≈
θ
\tan^{-1}(\theta)\approx \theta
tan−1(θ)≈θ,则有
v
˙
y
=
−
c
f
v
y
−
c
f
l
f
ψ
˙
m
v
x
+
c
f
δ
m
+
−
c
r
v
y
+
c
r
l
r
ψ
˙
m
v
x
−
v
x
r
ψ
¨
=
−
l
f
c
f
v
y
−
l
f
2
c
f
ψ
˙
I
z
v
x
+
l
f
c
f
δ
I
z
+
l
r
c
r
v
y
−
l
r
2
c
r
ψ
˙
I
z
v
x
\dot{v}_y=\frac{-c_fv_y-c_fl_f\dot{\psi}}{mv_x}+\frac{c_f\delta}{m}+\frac{-c_rv_y+c_rl_r\dot{\psi}}{mv_x}-v_xr\\ \ddot{\psi}=\frac{-l_fc_fv_y-l_f^2c_f\dot{\psi}}{I_zv_x}+\frac{l_fc_f\delta}{I_z}+\frac{l_rc_rv_y-l_r^2c_r\dot{\psi}}{I_zv_x}
v˙y=mvx−cfvy−cflfψ˙+mcfδ+mvx−crvy+crlrψ˙−vxrψ¨=Izvx−lfcfvy−lf2cfψ˙+Izlfcfδ+Izvxlrcrvy−lr2crψ˙
将系统状态量侧向速度
v
y
v_y
vy和横摆角速度
ψ
˙
\dot{\psi}
ψ˙以及系统控制量前轮转角
δ
\delta
δ提取,则有
v
˙
y
=
−
(
c
f
+
c
r
)
m
v
x
v
y
+
[
(
l
r
c
r
−
l
f
c
f
)
m
v
x
−
v
x
]
ψ
˙
+
c
f
m
δ
ψ
¨
=
l
r
c
r
−
l
f
c
f
I
z
v
x
v
y
+
−
(
l
f
2
c
f
+
l
r
2
c
r
)
I
z
v
x
ψ
˙
+
l
f
c
f
I
z
δ
\dot{v}_y=\frac{-(c_f+c_r)}{mv_x}v_y+[\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x]\dot{\psi}+\frac{c_f}{m}\delta\\ \ddot{\psi}=\frac{l_rc_r-l_fc_f}{I_zv_x}v_y+\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\dot{\psi}+\frac{l_fc_f}{I_z}\delta
v˙y=mvx−(cf+cr)vy+[mvx(lrcr−lfcf)−vx]ψ˙+mcfδψ¨=Izvxlrcr−lfcfvy+Izvx−(lf2cf+lr2cr)ψ˙+Izlfcfδ
结合上式,可以得到基于前轮偏角小角度假设和线性化轮胎模型的车辆横摆动力学模型。
令
ξ
=
[
v
y
ψ
˙
]
T
\xi=[v_y\quad\dot{\psi}]^T
ξ=[vyψ˙]T为状态矢量,
u
1
=
δ
u_1=\delta
u1=δ为输入矢量,线性化的车辆横摆动力学模型可以写成状态空间方程的形式:
ξ
˙
=
A
ξ
+
B
1
u
1
\dot{\xi}=A\xi+B_1u_1
ξ˙=Aξ+B1u1
其中
A
=
[
−
(
c
f
+
c
r
)
m
v
x
(
l
r
c
r
−
l
f
c
f
)
m
v
x
−
v
x
l
r
c
r
−
l
f
c
f
I
z
v
x
−
(
l
f
2
c
f
+
l
r
2
c
r
)
I
z
v
x
]
A=\left[\begin{array}{ccc|c}\frac{-(c_f+c_r)}{mv_x}&\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x\\\frac{l_rc_r-l_fc_f}{I_zv_x}&\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\end{array}\right]
A=[mvx−(cf+cr)Izvxlrcr−lfcfmvx(lrcr−lfcf)−vxIzvx−(lf2cf+lr2cr)],
B
1
=
[
c
f
m
l
f
c
f
I
z
]
B_1=\left[\begin{array}{ccc|c}\frac{c_f}{m}\\\frac{l_fc_f}{I_z}\end{array}\right]
B1=[mcfIzlfcf];
令
X
=
[
y
y
˙
ψ
ψ
˙
]
T
X=[y\quad\dot{y}\quad\psi\quad\dot{\psi}]^T
X=[yy˙ψψ˙]T为状态矢量,
u
2
=
δ
u_2=\delta
u2=δ为输入矢量,线性化的车辆动力学模型可以写成状态空间方程式的形式:
X
˙
=
A
X
+
B
1
u
2
\dot{X}=AX+B_1u_2
X˙=AX+B1u2
其中,
A
=
[
0
1
0
0
0
−
(
c
f
+
c
r
)
m
v
x
0
(
l
r
c
r
−
l
f
c
f
)
m
v
x
−
v
x
0
0
0
1
0
l
r
c
r
−
l
f
c
f
I
z
v
x
0
−
(
l
f
2
c
f
+
l
r
2
c
r
)
I
z
v
x
]
A=\left[\begin{array}{cccc}0&1&0&0\\0&\frac{-(c_f+c_r)}{mv_x}&0&\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x\\0&0&0&1\\0&\frac{l_rc_r-l_fc_f}{I_zv_x}&0&\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\end{array}\right]
A=
00001mvx−(cf+cr)0Izvxlrcr−lfcf00000mvx(lrcr−lfcf)−vx1Izvx−(lf2cf+lr2cr)
,
B
1
=
[
0
c
f
m
0
l
f
c
f
I
z
]
B_1=\left[\begin{array}{c}0\\\frac{c_f}{m}\\0\\\frac{l_fc_f}{I_z}\end{array}\right]
B1=
0mcf0Izlfcf
;
5.2、线性二次调节器(LQR)
LQR(Linaer Quadratic Regulator),即线性二次型调节器,是一种现代控制理论中设计状态反馈控制器(State Variable Feedback,SVFB)的方法。
假设有一个线性系统能用状态向量的形式表示成:
x
˙
=
A
x
+
B
u
y
=
C
x
+
D
u
\dot{x}=Ax+Bu\\ y=Cx+Du
x˙=Ax+Buy=Cx+Du
其中
x
(
t
)
∈
R
n
x(t)\in R^n
x(t)∈Rn,
u
(
t
)
∈
R
n
u(t)\in R^n
u(t)∈Rn,初始条件是
x
(
0
)
x(0)
x(0)。并且假设这个系统的所有状态变量都是可以测量到的。
我们设计一个线性反馈控制器
u
=
−
K
x
u=-Kx
u=−Kx,使闭环系统能够满足我们期望的性能。则此时的状态方程可以写为
x
˙
=
A
x
−
B
K
X
=
(
A
−
B
K
)
x
=
A
c
l
x
\dot{x}=Ax-BKX=(A-BK)x=A_{cl}x
x˙=Ax−BKX=(A−BK)x=Aclx
由于让系统稳定的条件是矩阵
A
c
l
A_{cl}
Acl的特征值得实部均为负数,因此可以手动选择几个满足上述条件的特征值,然后反解出
K
K
K,从而得出控制器。
现在定义一种代价函数
J
J
J:
J
=
∫
0
∞
x
T
Q
x
+
u
T
R
u
d
t
J=\int^{\infty}_0x^TQx+u^TRu\, dt
J=∫0∞xTQx+uTRudt
其中,
Q
Q
Q和
R
R
R是两个对角参数矩阵,分别决定了状态向量
x
x
x和输入向量
u
u
u的重要性。显然,代价函数
J
J
J是一个二次型函数。
我们希望的是在满足系统稳定的前提下,通过设计合适的 K K K,让代价函数 J J J最小。
下面分析一下代价函数的意义。考虑一个双变量系统,即 x = [ x 1 x 2 ] x=\left[ \begin{array}{c} x_1\\x_2\end{array}\right] x=[x1x2],我们希望设计的控制器可以表示为 u = − [ k 1 k 2 ] [ x 1 x 2 ] = − k 1 x 1 − k 2 x 2 u=-\left[\begin{array}{cc} k_1&k_2\end{array}\right]\left[ \begin{array}{c} x_1\\x_2\end{array}\right]=-k_1x_1-k_2x_2 u=−[k1k2][x1x2]=−k1x1−k2x2.
此时代价函数可以写为:
J
=
∫
0
∞
[
x
1
x
2
]
Q
[
x
1
x
2
]
+
(
−
[
k
1
k
2
]
[
x
1
x
2
]
)
T
R
(
−
[
k
1
k
2
]
[
x
1
x
2
]
)
d
t
J=\int^\infty_0\left[\begin{array}{cc} x_1&x_2\end{array}\right]Q\left[ \begin{array}{c} x_1\\x_2\end{array}\right]+(-\left[\begin{array}{cc} k_1&k_2\end{array}\right]\left[ \begin{array}{c} x_1\\x_2\end{array}\right])^TR(-\left[\begin{array}{cc} k_1&k_2\end{array}\right]\left[ \begin{array}{c} x_1\\x_2\end{array}\right])dt
J=∫0∞[x1x2]Q[x1x2]+(−[k1k2][x1x2])TR(−[k1k2][x1x2])dt
令
Q
Q
Q和
R
R
R分别为
Q
=
[
q
1
q
2
]
Q=\left[ \begin{array}{cc} q_1&\\&q_2\end{array}\right]
Q=[q1q2]
R = r ( u 是一维向量 ) R=r\quad(u是一维向量) R=r(u是一维向量)
则代价函数可以写为:
J
=
∫
0
∞
q
1
x
1
2
+
q
2
x
2
2
+
r
u
2
d
t
J=\int^{\infty}_{0}q_1x_1^2+q_2x_2^2+ru^2dt
J=∫0∞q1x12+q2x22+ru2dt
显然,如果令
q
1
>
q
2
>
r
q_1>q_2>r
q1>q2>r,则状态变量
x
1
x_1
x1在代价函数中的占比就更大,这意味着如希望代价函数最小,
x
1
x_1
x1必须更小。又因为
Q
Q
Q越大意味着闭环系统矩阵
A
c
l
A_{cl}
Acl的极点在
s
s
s平面中跟偏左,因此
x
1
x_1
x1收敛得更快。若
r
>
q
1
>
q
2
r>q_1>q_2
r>q1>q2,则希望输入量收敛得更快,也就是以更小的代价实现系统稳定,通常意味着更节省能量。
因为对象是线性的,并且代价函数是二次型,因此这种选择 K K K设计状态反馈控制器以最小化代价函数 J J J的方法被称为“线性二次型调节器( L Q R LQR LQR)”。
那么,如何求解 K K K,从而让代价函数 J J J最小呢?
为此,我们定义一个辅助常量矩阵
P
P
P,使得
d
d
t
x
T
P
x
=
−
(
x
T
Q
x
+
u
T
R
u
)
\frac{d}{dt}x^TPx=-(x^TQx+u^TRu)
dtdxTPx=−(xTQx+uTRu)
则代价函数
J
J
J可化为
J
=
−
∫
0
∞
d
d
t
x
T
P
x
d
t
=
−
(
x
T
P
x
∣
∞
−
x
T
P
x
∣
0
)
=
−
(
0
−
x
T
P
x
∣
0
)
=
x
T
(
0
)
P
x
(
0
)
\begin{aligned} J&=-\int^{\infty}_0\frac{d}{dt}x^TPx\,dt\\ &=-(x^TPx|_{\infty}-x^TPx|_0)\\ &=-(0-x^TPx|_0)\\ &=x^T(0)Px(0) \end{aligned}
J=−∫0∞dtdxTPxdt=−(xTPx∣∞−xTPx∣0)=−(0−xTPx∣0)=xT(0)Px(0)
注:由于我们假设系统稳定,当
t
→
∞
t\to\infty
t→∞,
x
(
t
)
→
0
x(t)\to0
x(t)→0。
显然,代价函数 J J J只和参数矩阵 P P P以及系统的初始状态有关,让 P P P最小也就是让代价函数最小,所以我们需要找到满足条件的参数矩阵 P P P。
将定义
P
P
P式左边的微分项展开可得:
x
˙
T
P
x
+
x
T
P
x
˙
+
x
T
Q
x
+
x
T
K
T
R
K
x
=
0
x
T
A
c
l
T
P
x
+
x
T
P
A
c
l
x
+
x
T
Q
x
+
x
T
K
T
R
K
x
=
0
x
T
(
A
c
l
T
P
+
P
A
c
l
+
Q
+
K
T
R
K
)
x
=
0
\dot{x}^TPx+x^TP\dot{x}+x^TQx+x^TK^TRKx=0\\ x^TA_{cl}^TPx+x^TPA_{cl}x+x^TQx+x^TK^TRKx=0\\ x^T(A_{cl}^TP+PA_{cl}+Q+K^TRK)x=0
x˙TPx+xTPx˙+xTQx+xTKTRKx=0xTAclTPx+xTPAclx+xTQx+xTKTRKx=0xT(AclTP+PAcl+Q+KTRK)x=0
由于上式对于所以的
x
(
t
)
x(t)
x(t)来说都满足,因此括号中的项要恒等于零。代入
A
c
l
=
A
−
B
K
A_{cl}=A-BK
Acl=A−BK可以得到
A
c
l
T
P
+
P
A
c
l
+
Q
+
K
T
R
K
=
0
(
A
−
B
K
)
T
P
+
P
(
A
−
B
K
)
+
Q
+
K
T
R
K
=
0
A
T
P
+
P
A
+
Q
+
K
T
R
K
−
K
T
B
T
P
−
P
B
K
=
0
A_{cl}^TP+PA_{cl}+Q+K^TRK=0\\ (A-BK)^TP+P(A-BK)+Q+K^TRK=0\\ A^TP+PA+Q+K^TRK-K^TB^TP-PBK=0
AclTP+PAcl+Q+KTRK=0(A−BK)TP+P(A−BK)+Q+KTRK=0ATP+PA+Q+KTRK−KTBTP−PBK=0
假设
K
=
R
−
1
B
T
P
K=R^{-1}B^TP
K=R−1BTP,则上式可改写为
A
T
P
+
P
A
+
Q
+
(
R
−
1
B
T
P
)
T
R
(
R
−
1
B
T
P
)
−
(
R
−
1
B
T
P
)
T
B
T
P
−
P
B
(
R
−
1
B
T
P
)
=
0
A
T
P
+
P
A
+
Q
−
P
B
R
−
1
B
T
P
=
0
A^TP+PA+Q+(R^{-1}B^TP)^TR(R^{-1}B^TP)-(R^{-1}B^TP)^TB^TP-PB(R^{-1}B^TP)=0\\ A^TP+PA+Q-PBR^{-1}B^TP=0
ATP+PA+Q+(R−1BTP)TR(R−1BTP)−(R−1BTP)TBTP−PB(R−1BTP)=0ATP+PA+Q−PBR−1BTP=0
上式在现代控制理论中非常重要,也被称为黎卡提方程
A
l
g
e
b
r
a
i
c
R
i
c
c
a
t
i
E
q
u
a
t
i
o
n
(
A
R
E
)
Algebraic Riccati Equation(ARE)
AlgebraicRiccatiEquation(ARE)。
A
R
E
ARE
ARE是一个矩阵二次方程,对于给定的
(
A
,
B
,
Q
,
R
)
(A,B,Q,R)
(A,B,Q,R)可以求出辅助矩阵
P
P
P。之后,优化反馈控制器的
K
K
K以及代价函数的最小值
J
m
i
n
J_{min}
Jmin就可以通过辅助矩阵
P
P
P求出。
综上,求解 L Q R LQR LQR反馈控制器参数 K K K的过程为:
- 设计参数矩阵 Q Q Q、 R R R;
- 求解 A R E ARE ARE方程以得到辅助矩阵 P P P;
- 求解反馈控制器参数 K = R − 1 B T P K=R^{-1}B^TP K=R−1BTP;
目前已有求解 A R E ARE ARE很完善的数值程序,例如 M A T L A B MATLAB MATLAB把它封装进了 l q r ( A , B , Q , R ) lqr(A,B,Q,R) lqr(A,B,Q,R)函数中。
由于计算机处理的是离散的信号, 所以在实际运用中往往需要将 L Q R LQR LQR方法离散化,这里直接给出结论。
离散系统状态空间表达式为:
x
(
k
+
1
)
=
A
d
x
(
k
)
+
B
d
u
(
k
)
y
(
k
)
=
C
d
x
(
k
)
x(k+1)=A_dx(k)+B_du(k)\\ y(k)=C_dx(k)
x(k+1)=Adx(k)+Bdu(k)y(k)=Cdx(k)
其中,
A
d
=
e
A
Δ
t
A_d=e^{A\Delta t}
Ad=eAΔt,
B
d
=
(
∫
0
Δ
t
e
A
τ
d
τ
)
B
B_d=(\int^{\Delta t}_0e^{A\tau}d\tau)B
Bd=(∫0ΔteAτdτ)B,
e
A
t
=
L
−
1
(
s
I
−
A
)
−
1
e^{At}=L^{-1}(sI-A)^{-1}
eAt=L−1(sI−A)−1.
假设状态反馈控制器 u ∗ ( k ) = − K x ( k ) u^*(k)=-Kx(k) u∗(k)=−Kx(k);
其中 K = ( R + B d T P B d ) − 1 B d T P A d K=(R+B_d^TPB_d)^{-1}B_d^TPA_d K=(R+BdTPBd)−1BdTPAd;
代价函数 J = ∑ k = 0 ∞ x T ( k ) Q x ( k ) Q x ( k ) + u T ( k ) R u ( k ) J=\sum^{\infty}_{k=0}x^T(k)Qx(k)Qx(k)+u^T(k)Ru(k) J=∑k=0∞xT(k)Qx(k)Qx(k)+uT(k)Ru(k);
辅助常量矩阵 P = A d T P A d − A d T P B d ( R + B d T P B d ) − 1 B d T P A d + Q P=A_d^TPA_d-A_d^TPB_d(R+B_d^TPB_d)^{-1}B_d^TPA_d+Q P=AdTPAd−AdTPBd(R+BdTPBd)−1BdTPAd+Q;
5.3、基于LQR的轨迹追踪
为将 L Q R LQR LQR应用于车辆轨迹追踪中,需要基于车辆运动学模型,构建一个以误差作为状态量,转角作为控制量的系统状态方程。
我们选择系统控制量 x = ( e c g e ˙ c g e θ e ˙ θ ) T x=(e_{cg}\quad\dot{e}_{cg}\quad e_{\theta}\quad \dot{e}_{\theta})^T x=(ecge˙cgeθe˙θ)T,系统控制量 u = δ u=\delta u=δ;
其中, e c g e_{cg} ecg为车辆横向误差,即重心 C . G C.G C.G到路径点 ( c x , c y ) (c_x,c_y) (cx,cy)的垂直距离;
e ˙ c g \dot{e}_{cg} e˙cg为车辆横向误差的变化率;
e θ e_{\theta} eθ为车辆角度误差,即车辆纵轴与路径上点 ( c x , c y ) (c_x,c_y) (cx,cy)的切线间的夹角,也即车辆纵轴与绝对坐标系下 x x x轴的夹角 θ \theta θ与路径上点 ( c x , c y ) (c_x,c_y) (cx,cy)的切线与绝对坐标系下 x x x轴的夹角 θ ( s ) \theta(s) θ(s)之差;
e ˙ θ \dot{e}_{\theta} e˙θ为车辆角度误差的变化率, e ˙ θ = θ ˙ ( s ) − θ ˙ p ( s ) \dot{e}_{\theta}=\dot{\theta}(s)-\dot{\theta}_p(s) e˙θ=θ˙(s)−θ˙p(s);
在建立模型之前,假设车辆纵向速度 v x v_x vx为常量。
根据上图,可以推导出
e
˙
c
g
=
v
y
+
v
x
sin
(
θ
(
s
)
−
θ
P
(
s
)
)
=
v
y
+
v
x
sin
(
e
θ
)
\begin{aligned}\dot{e}_{cg}&=v_y+v_x\sin(\theta(s)-\theta_P(s))\\&=v_y+v_x\sin(e_{\theta})\end{aligned}
e˙cg=vy+vxsin(θ(s)−θP(s))=vy+vxsin(eθ)
车辆横向误差的加速度
e
¨
c
g
\ddot{e}_{cg}
e¨cg也可以得到
e
˙
c
g
=
(
v
˙
y
+
v
x
θ
˙
(
s
)
)
−
v
˙
y
(
s
)
=
v
˙
y
+
v
x
(
θ
˙
(
s
)
−
θ
˙
p
(
s
)
)
=
v
˙
y
+
v
x
e
˙
θ
\begin{aligned} \dot{e}_{cg}&=(\dot{v}_y+v_x\dot{\theta}(s))-\dot{v}_y(s)\\ &=\dot{v}_y+v_x(\dot{\theta}(s)-\dot{\theta}_p(s))\\ &=\dot{v}_y+v_x\dot{e}_{\theta} \end{aligned}
e˙cg=(v˙y+vxθ˙(s))−v˙y(s)=v˙y+vx(θ˙(s)−θ˙p(s))=v˙y+vxe˙θ
其中,向心加速度部分推导如下
a
c
e
n
=
v
x
2
⋅
k
(
s
)
=
v
x
⋅
θ
˙
(
s
)
\begin{aligned} a_{cen}&=v_x^2\cdot k(s)\\&=v_x\cdot\dot{\theta}(s)\end{aligned}
acen=vx2⋅k(s)=vx⋅θ˙(s)
k
(
s
)
k(s)
k(s)为当前路径点的曲率,
θ
˙
(
s
)
=
v
x
⋅
k
(
s
)
\dot{\theta}(s)=v_x\cdot k(s)
θ˙(s)=vx⋅k(s)。
则车辆侧向动力学模型和误差模型可以表示成如下形式
v
y
=
e
˙
c
g
−
v
x
sin
(
e
θ
)
v
˙
y
=
e
¨
c
g
−
v
x
e
˙
θ
θ
(
s
)
=
e
θ
+
θ
p
(
s
)
θ
˙
(
s
)
=
e
˙
θ
+
θ
˙
p
(
s
)
θ
¨
=
e
¨
θ
+
θ
¨
p
(
s
)
v_y=\dot{e}_{cg}-v_x\sin(e_{\theta})\\ \dot{v}_y=\ddot{e}_{cg}-v_x\dot{e}_{\theta}\\ \theta(s)=e_{\theta}+\theta_p(s)\\ \dot{\theta}(s)=\dot{e}_{\theta}+\dot{\theta}_p(s)\\ \ddot{\theta}=\ddot{e}_{\theta}+\ddot{\theta}_p(s)
vy=e˙cg−vxsin(eθ)v˙y=e¨cg−vxe˙θθ(s)=eθ+θp(s)θ˙(s)=e˙θ+θ˙p(s)θ¨=e¨θ+θ¨p(s)
将以上模型代入第一小节建立的车辆横向动力学模型中
v
˙
y
=
−
(
c
f
+
c
r
)
m
v
x
v
y
+
[
(
l
r
c
r
−
l
f
c
f
)
m
v
x
−
v
x
]
ψ
˙
+
c
f
m
δ
ψ
¨
=
l
r
c
r
−
l
f
c
f
I
z
v
x
v
y
+
−
(
l
f
2
c
f
+
l
r
2
c
r
)
I
z
v
x
ψ
˙
+
l
f
c
f
I
z
δ
\dot{v}_y=\frac{-(c_f+c_r)}{mv_x}v_y+[\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x]\dot{\psi}+\frac{c_f}{m}\delta\\ \ddot{\psi}=\frac{l_rc_r-l_fc_f}{I_zv_x}v_y+\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}\dot{\psi}+\frac{l_fc_f}{I_z}\delta
v˙y=mvx−(cf+cr)vy+[mvx(lrcr−lfcf)−vx]ψ˙+mcfδψ¨=Izvxlrcr−lfcfvy+Izvx−(lf2cf+lr2cr)ψ˙+Izlfcfδ
就可以得到车辆行驶误差模型
e
¨
c
g
=
−
(
c
f
+
c
r
)
m
v
x
e
˙
c
g
+
c
f
+
c
r
m
e
θ
+
(
l
r
c
r
−
l
f
c
f
)
m
v
x
e
˙
θ
+
[
(
l
r
c
r
−
l
f
c
f
)
m
v
x
−
v
x
]
θ
˙
p
(
s
)
+
c
f
m
δ
e
¨
θ
=
l
r
c
r
−
l
f
c
f
I
z
v
x
e
˙
c
g
+
l
r
c
r
−
l
f
c
f
I
z
e
θ
+
−
(
l
f
2
c
f
+
l
r
2
c
r
)
I
z
v
x
(
e
˙
θ
+
θ
˙
p
(
s
)
)
+
l
f
c
f
I
z
δ
−
θ
¨
p
(
s
)
\ddot{e}_{cg}=\frac{-(c_f+c_r)}{mv_x}\dot{e}_{cg}+\frac{c_f+c_r}{m}e_{\theta}+\frac{(l_rc_r-l_fc_f)}{mv_x}\dot{e}_{\theta}+[\frac{(l_rc_r-l_fc_f)}{mv_x}-v_x]\dot{\theta}_p(s)+\frac{c_f}{m}\delta\\ \ddot{e}_{\theta}=\frac{l_rc_r-l_fc_f}{I_zv_x}\dot{e}_{cg}+\frac{l_rc_r-l_fc_f}{I_z}e_{\theta}+\frac{-(l_f^2c_f+l_r^2c_r)}{I_zv_x}(\dot{e}_{\theta}+\dot{\theta}_p(s))+\frac{l_fc_f}{I_z}\delta-\ddot{\theta}_p(s)
e¨cg=mvx−(cf+cr)e˙cg+mcf+creθ+mvx(lrcr−lfcf)e˙θ+[mvx(lrcr−lfcf)−vx]θ˙p(s)+mcfδe¨θ=Izvxlrcr−lfcfe˙cg+Izlrcr−lfcfeθ+Izvx−(lf2cf+lr2cr)(e˙θ+θ˙p(s))+Izlfcfδ−θ¨p(s)
由于车辆行驶路径一般来说比较平滑,
θ
¨
p
(
s
)
≈
0
\ddot{\theta}_p(s)\approx0
θ¨p(s)≈0,这样就得到了以
x
=
(
e
c
g
e
˙
c
g
e
θ
e
˙
θ
)
T
x=(e_{cg}\quad\dot{e}_{cg}\quad e_{\theta}\quad \dot{e}_{\theta})^T
x=(ecge˙cgeθe˙θ)T状态量,
u
=
δ
u=\delta
u=δ为控制量,以
θ
p
(
s
)
\theta_p(s)
θp(s)为前馈量的线性状态空间方程。
x
˙
=
A
x
+
B
1
δ
+
B
2
r
d
e
s
\dot{x}=Ax+B_1\delta+B_2r_{des}
x˙=Ax+B1δ+B2rdes
A
=
[
0
1
0
0
0
−
c
f
+
c
r
m
v
c
f
+
c
r
m
l
r
c
r
−
l
f
c
f
m
v
0
0
0
1
0
l
r
c
r
−
l
f
c
f
I
z
v
l
r
c
r
−
l
f
c
f
I
z
−
l
f
2
c
f
+
l
r
2
c
r
I
z
v
]
A=\left [\begin{array} {cccc} 0&1&0&0\\0& -\frac{c_f+c_r}{mv}&\frac{c_f+c_r}{m}&\frac{l_rc_r-l_fc_f}{mv}\\0&0&0&1\\0&\frac{l_rc_r-l_fc_f}{I_zv}&\frac{l_rc_r-l_fc_f}{I_z}&-\frac{l_f^2c_f+l_r^2c_r}{I_zv} \end{array}\right]
A=
00001−mvcf+cr0Izvlrcr−lfcf0mcf+cr0Izlrcr−lfcf0mvlrcr−lfcf1−Izvlf2cf+lr2cr
;
B
1
=
[
0
c
f
m
0
l
f
c
f
I
z
]
B_1=\left [\begin{array} {c} 0\\\frac{c_f}{m}\\0\\\frac{l_fc_f}{I_z} \end{array}\right]
B1=
0mcf0Izlfcf
;
B
2
=
[
0
l
r
c
r
−
l
f
c
f
m
v
−
v
0
−
l
f
2
c
f
+
l
r
2
c
r
I
z
v
]
B_2=\left [\begin{array} {c} 0\\\frac{l_rc_r-l_fc_f}{mv}-v\\0\\-\frac{l_f^2c_f+l_r^2c_r}{I_zv} \end{array}\right]
B2=
0mvlrcr−lfcf−v0−Izvlf2cf+lr2cr
;
首先需要检查这个系统是否是可控的,即能控性矩阵 [ B 1 , A B 1 , A 2 B 1 , A 3 B 1 ] [B_1,AB_1,A^2B_1,A^3B_1] [B1,AB1,A2B1,A3B1]是否是满秩的。
其次,需要用第二小节提到的离散化方法,将连续系统变为离散化的系统。
最后,利用套用第二小节中方法,求得反馈控制器参数 K K K,则车辆转角控制信号 δ ∗ ( k ) = − K x ( k ) \delta^*(k)=-Kx(k) δ∗(k)=−Kx(k)。