MPC (basic)
系统模型
x k + 1 = A x k + B u k + E , y k = C x k \begin{align} {x}_{k+1}={}&Ax_{k}+Bu_{k}+E,\\ y_{k}={}&Cx_{k} \end{align} xk+1=yk=Axk+Buk+E,Cxk
令(预测时域 N c N_c Nc,控制时域 N c N_c Nc)
X k = [ x k + 1 x k + 2 ⋮ x k + N p ] ∈ R n N p , Y k = [ y k + 1 y k + 2 ⋮ y k + N p ] , U k = [ u k u k + 1 ⋮ u k + N c − 1 ] ∈ R m N c \begin{align} X_k={}\left[\begin{matrix} x_{k+1}\\ x_{k+2}\\\vdots\\ x_{k+N_p} \end{matrix}\right]\in\mathbb{R}^{nN_p},Y_k={}\left[\begin{matrix} y_{k+1}\\ y_{k+2}\\\vdots\\ y_{k+N_p} \end{matrix}\right], U_k=\left[\begin{matrix} u_{k}\\ u_{k+1}\\\vdots\\ u_{k+N_c-1} \end{matrix}\right]\in\mathbb{R}^{mN_c} \end{align} Xk= xk+1xk+2⋮xk+Np ∈RnNp,Yk= yk+1yk+2⋮yk+Np ,Uk= ukuk+1⋮uk+Nc−1 ∈RmNc
则有
X k = [ A ⋮ A N p ] x k + [ B 0 ⋯ 0 A B B ⋯ 0 ⋮ ⋮ ⋱ ⋮ A N c − 1 B A N c − 2 B ⋯ B ⋮ ⋮ ⋱ ⋮ A N p − 1 B A N p − 2 B ⋯ A N p − N c B ] U k + [ I 0 ⋯ 0 A I ⋯ 0 ⋮ ⋮ ⋱ ⋮ A N p − 1 A N p − 2 ⋯ I ] ( 1 N p ⊗ E ) Y k = [ C A ⋮ C A N p ] x k + [ C B 0 ⋯ 0 C A B C B ⋯ 0 ⋮ ⋮ ⋱ ⋮ C A N c − 1 B C A N c − 2 B ⋯ C B ⋮ ⋮ ⋱ ⋮ C A N p − 1 B C A N p − 2 B ⋯ C A N p − N c B ] U k + [ C 0 ⋯ 0 C A C ⋯ 0 ⋮ ⋮ ⋱ ⋮ C A N p − 1 C A N p − 2 ⋯ C ] ( 1 N p ⊗ E ) = Φ x k + Θ U k + Ψ \begin{align} X_k={}&\left[\begin{matrix} A\\\vdots\\ A^{N_p} \end{matrix}\right]x_{k}+\left[\begin{matrix} B&0&\cdots&0\\ AB&B&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ A^{N_c-1}B&A^{N_c-2}B&\cdots&B\\ \vdots&\vdots&\ddots&\vdots\\ A^{N_p-1}B& A^{N_p-2}B&\cdots&A^{N_p-N_c}B \end{matrix}\right]U_k\notag\\&+\left[\begin{matrix} I&0&\cdots&0\\ A&I&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ A^{N_p-1}&A^{N_p-2}&\cdots&I\\ \end{matrix}\right](1_{N_p}\otimes E)\\ Y_k={}&\left[\begin{matrix} CA\\\vdots\\ CA^{N_p} \end{matrix}\right]x_{k}+\left[\begin{matrix} CB&0&\cdots&0\\ CAB&CB&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ CA^{N_c-1}B&CA^{N_c-2}B&\cdots&CB\\ \vdots&\vdots&\ddots&\vdots\\ CA^{N_p-1}B& CA^{N_p-2}B&\cdots&CA^{N_p-N_c}B \end{matrix}\right]U_k\notag\\ &+\left[\begin{matrix} C&0&\cdots&0\\ CA&C&\cdots&0\\ \vdots&\vdots&\ddots&\vdots\\ CA^{N_p-1}&CA^{N_p-2}&\cdots&C\\ \end{matrix}\right](1_{N_p}\otimes E)\notag\\ ={}&{\Phi}x_{k}+\Theta U_k+\Psi\\ \end{align} Xk=Yk== A⋮ANp xk+ BAB⋮ANc−1B⋮ANp−1B0B⋮ANc−2B⋮ANp−2B⋯⋯⋱⋯⋱⋯00⋮B⋮ANp−NcB Uk+ IA⋮ANp−10I⋮ANp−2⋯⋯⋱⋯00⋮I (1Np⊗E) CA⋮CANp xk+ CBCAB⋮CANc−1B⋮CANp−1B0CB⋮CANc−2B⋮CANp−2B⋯⋯⋱⋯⋱⋯00⋮CB⋮CANp−NcB Uk+ CCA⋮CANp−10C⋮CANp−2⋯⋯⋱⋯00⋮C (1Np⊗E)Φxk+ΘUk+Ψ
上述状态方程和输出方程在 x k + N c x_{k+N_c} xk+Nc及以后都是近似的,这是由于控制时域长度小于预测时域导致的。成本函数,如下
c o s t = ∑ i = 1 N p ∥ y k + i − y k + i r e f ∥ Q 2 + ∑ i = 0 N c − 1 ∥ u k + i ∥ R 2 = Y ~ k ⊤ ( Q ⊗ I N p ) Y ~ k + U k ( R ⊗ I N c ) U k = Y ~ k ⊤ Q ˉ Y ~ k + U k ⊤ R ˉ U k = ( Φ x k + Θ U k − Y ˉ k r e f ) ⊤ Q ˉ ( Φ x k + Θ U k − Y ˉ k r e f ) + U k ⊤ R ˉ U k = ( Φ x k + Θ U k ) ⊤ Q ˉ ( Φ x k + Θ U k ) + Y ˉ k r e f ⊤ Q ˉ Y ˉ k r e f − 2 ( Ψ x k + Θ U k ) ⊤ Q ˉ Y ˉ k r e f + U k ⊤ R ˉ U k = x k ⊤ Φ ⊤ Q ˉ Φ x k + Y ˉ k r e f ⊤ Q ˉ Y ˉ k r e f − 2 Y ˉ k r e f ⊤ Q ˉ Φ x k − 2 Y ˉ k r e f ⊤ Q ˉ Θ U k + 2 x k ⊤ Φ ⊤ Q ˉ Θ U k + U k ⊤ ( R ˉ + Θ ⊤ Q ˉ Θ ) U k = x k ⊤ Φ ⊤ Q ˉ Φ x k + Y ˉ k r e f ⊤ Q ˉ Y ˉ k r e f − 2 Y ˉ k r e f ⊤ Q ˉ M x k + 2 ( Φ x k − Y ˉ k r e f ) ⊤ Q ˉ Θ U k + U k ⊤ ( R ˉ + Θ ⊤ Q ˉ Θ ) U k = c + 2 g U k + U k ⊤ H U k \begin{align} cost={}&\sum_{i=1}^{N_p}\Vert y_{k+i}-y_{k+i}^\mathrm{ref}\Vert_{Q}^2+\sum_{i=0}^{N_c-1}\Vert u_{k+i}\Vert_{R}^2\\ ={}&\tilde{Y}_k^\top (Q\otimes{I_{N_p}})\tilde{Y}_k+U_k(R\otimes{I_{N_c}})U_k\notag\\ ={}&\tilde{Y}_k^\top\bar{Q}\tilde{Y}_k+U_k^\top\bar{R}U_k\\ ={}&({\Phi}x_{k}+{\Theta}U_k-\bar{Y}_k^\mathrm{ref})^\top\bar{Q}({\Phi}x_{k}+{\Theta}U_k-\bar{Y}_k^\mathrm{ref})+U_k^\top\bar{R}U_k\notag\\ ={}&({\Phi}x_{k}+{\Theta}U_k)^\top\bar{Q}({\Phi}x_{k}+{\Theta}U_k)+\bar{Y}_k^{\mathrm{ref}\top}\bar{Q}\bar{Y}_k^\mathrm{ref}-2({\Psi}x_{k}+{\Theta}U_k)^\top\bar{Q}\bar{Y}_k^\mathrm{ref}+U_k^\top\bar{R}U_k\notag\\ ={}&x_k^\top {\Phi}^\top\bar{Q}{\Phi}x_k+\bar{Y}_k^{\mathrm{ref}\top}\bar{Q}\bar{Y}_k^\mathrm{ref}-2\bar{Y}_k^{\mathrm{ref}\top}\bar{Q}{\Phi}x_k-2\bar{Y}_k^{\mathrm{ref}\top}\bar{Q}{\Theta}U_k+2x_k^\top {\Phi}^\top\bar{Q}{\Theta}U_k\notag\\&+U_k^\top(\bar{R}+{\Theta}^\top\bar{Q}{\Theta})U_k\notag\\ ={}&x_k^\top {\Phi}^\top\bar{Q}{\Phi}x_k+\bar{Y}_k^{\mathrm{ref}\top}\bar{Q}\bar{Y}_k^\mathrm{ref}-2\bar{Y}_k^{\mathrm{ref}\top}\bar{Q}Mx_k+2({\Phi}x_k-\bar{Y}_k^\mathrm{ref})^\top\bar{Q}{\Theta}U_k\notag\\ &+U_k^\top(\bar{R}+{\Theta}^\top\bar{Q}{\Theta})U_k\notag\\ ={}&c+2gU_k+U_k^\top HU_k \end{align} cost========i=1∑Np∥yk+i−yk+iref∥Q2+i=0∑Nc−1∥uk+i∥R2Y~k⊤(Q⊗INp)Y~k+Uk(R⊗INc)UkY~k⊤QˉY~k+Uk⊤RˉUk(Φxk+ΘUk−Yˉkref)⊤Qˉ(Φxk+ΘUk−Yˉkref)+Uk⊤RˉUk(Φxk+ΘUk)⊤Qˉ(Φxk+ΘUk)+Yˉkref⊤QˉYˉkref−2(Ψxk+ΘUk)⊤QˉYˉkref+Uk⊤RˉUkxk⊤Φ⊤QˉΦxk+Yˉkref⊤QˉYˉkref−2Yˉkref⊤QˉΦxk−2Yˉkref⊤QˉΘUk+2xk⊤Φ⊤QˉΘUk+Uk⊤(Rˉ+Θ⊤QˉΘ)Ukxk⊤Φ⊤QˉΦxk+Yˉkref⊤QˉYˉkref−2Yˉkref⊤QˉMxk+2(Φxk−Yˉkref)⊤QˉΘUk+Uk⊤(Rˉ+Θ⊤QˉΘ)Ukc+2gUk+Uk⊤HUk
其中
Q ∈ S ≥ 0 n , R ∈ S > 0 m Q ˉ = Q ⊗ I N p , R ˉ = R ⊗ I N c g = ( Φ x k − Y ˉ k r e f ) ⊤ Q ˉ Θ , H = R ˉ + Θ ⊤ Q ˉ Θ , Y ˉ k r e f = Y k r e f − Ψ \begin{align} &Q\in\mathbb{S}^{n}_{\geq 0}, R\in\mathbb{S}^m_{> 0}\\ &\bar{Q}=Q\otimes{I_{N_p}}, \bar{R}=R\otimes{I_{N_c}}\\ &g=({\Phi}x_k-\bar{Y}_k^\mathrm{ref})^\top\bar{Q}{\Theta}, H=\bar{R}+{\Theta}^\top\bar{Q}{\Theta}, \bar{Y}_k^\mathrm{ref}={Y}_k^\mathrm{ref}-\Psi \end{align} Q∈S≥0n,R∈S>0mQˉ=Q⊗INp,Rˉ=R⊗INcg=(Φxk−Yˉkref)⊤QˉΘ,H=Rˉ+Θ⊤QˉΘ,Yˉkref=Ykref−Ψ
可以对输入 u u u或者对其增量施加约束
可以加入松弛因子,增强可解性,如下
c o s t = ∑ i = 0 N p ∥ y k + i − y k + i r e f ∥ Q 2 + ∑ i = 0 N c ∥ u k + i ∥ R 2 + w λ 2 = c + 2 g ˉ U ˉ k + U ˉ k ⊤ H ˉ U ˉ k \begin{align} cost={}&\sum_{i=0}^{N_p}\Vert y_{k+i}-y_{k+i}^\mathrm{ref}\Vert_{Q}^2+\sum_{i=0}^{N_c}\Vert u_{k+i}\Vert_{R}^2+w\lambda^2\\ ={}&c+2\bar{g}\bar{U}_k+\bar{U}_k^\top\bar{H}\bar{U}_k \end{align} cost==i=0∑Np∥yk+i−yk+iref∥Q2+i=0∑Nc∥uk+i∥R2+wλ2c+2gˉUˉk+Uˉk⊤HˉUˉk
其中
g ˉ = [ g 0 ] , H ˉ = [ H 0 0 w ] , U ˉ k = [ U k λ ] \begin{align} \bar{g}=\left[\begin{matrix} g&0 \end{matrix}\right], \bar{H}=\left[\begin{matrix} H&0\\ 0&w \end{matrix}\right], \bar{U}_k=\left[\begin{matrix} U_k\\ \lambda \end{matrix}\right] \end{align} gˉ=[g0],Hˉ=[H00w],Uˉk=[Ukλ]
MPC (incremental)
增量系统模型
x k + 1 = A x k + B u k + E y k = C x k x ˉ k = [ x k u k − 1 ] x ˉ k + 1 = [ A x k + B u k + E u k ] = [ A x k + B u k − 1 + B d u k + E u k − 1 + d u k ] = [ A B O I ] x ˉ k + [ B I ] d u k + [ E O ] x ˉ k + 1 = A ˉ x ˉ k + B ˉ d u k + E ˉ \begin{align} x_{k+1}={}&Ax_k+Bu_k+E\\ y_{k}={}&Cx_k\\ \bar{x}_k={}&\left[\begin{matrix}x_k\\ u_{k-1}\end{matrix}\right]\\ \bar{x}_{k+1}={}&\left[\begin{matrix}Ax_k+Bu_k+E\\u_{k}\end{matrix}\right] =\left[\begin{matrix}Ax_k+Bu_{k-1}+B\mathrm{d}u_k+E\\u_{k-1}+\mathrm{d}u_k\end{matrix}\right] =\left[\begin{matrix}A&B\\O&I\end{matrix}\right]\bar{x}_k+\left[\begin{matrix}B\\I\end{matrix}\right]\mathrm{d}u_k+\left[\begin{matrix}E\\O\end{matrix}\right]\\ \bar{x}_{k+1}={}&\bar{A}\bar{x}_k+\bar{B}\mathrm{d}u_k+\bar{E} \end{align} xk+1=yk=xˉk=xˉk+1=xˉk+1=Axk+Buk+ECxk[xkuk−1][Axk+Buk+Euk]=[Axk+Buk−1+Bduk+Euk−1+duk]=[AOBI]xˉk+[BI]duk+[EO]Aˉxˉk+Bˉduk+Eˉ
进一步,定义增量系统的输出
y k = C x k = C [ I O ] x ˉ k = [ C O ] x ˉ k = C ˉ x ˉ k \begin{align} y_k=Cx_k=C\left[\begin{matrix} I\\O\end{matrix}\right]\bar{x}_k=\left[\begin{matrix} C\\O\end{matrix}\right]\bar{x}_k=\bar{C}\bar{x}_k \end{align} yk=Cxk=C[IO]xˉk=[CO]xˉk=Cˉxˉk
运用前面的方法即可
应用于车辆横向控制
以后轴中心为参考点
[
x
˙
y
˙
ϕ
˙
]
=
[
v
r
cos
ϕ
v
r
sin
ϕ
v
r
L
tan
δ
f
]
,
[
y
ϕ
]
=
[
0
1
0
0
0
1
]
[
x
y
ϕ
]
x
ˉ
=
[
x
y
ϕ
]
⊤
,
y
ˉ
=
[
y
ϕ
]
⊤
,
u
=
δ
f
,
y
ˉ
k
=
C
x
ˉ
k
\begin{align} \left[\begin{matrix}\dot{x}\\\dot {y}\\\dot{\phi}\end{matrix}\right]= \left[\begin{matrix}v_r\cos\phi\\v_r\sin\phi\\\frac{v_r}{L}\tan{\delta_f}\end{matrix}\right] , \left[\begin{matrix}y\\\phi \end{matrix}\right]= \left[\begin{matrix}0&1&0\\0&0&1 \end{matrix}\right]\left[\begin{matrix}{x}\\ {y}\\{\phi}\end{matrix}\right]\\ \bar{x}=\left[\begin{matrix}x&y&\phi\end{matrix}\right]^\top, \bar{y}=\left[\begin{matrix}y&\phi\end{matrix}\right]^\top, u=\delta_f, \bar{y}_k=C\bar{x}_k \end{align}
x˙y˙ϕ˙
=
vrcosϕvrsinϕLvrtanδf
,[yϕ]=[001001]
xyϕ
xˉ=[xyϕ]⊤,yˉ=[yϕ]⊤,u=δf,yˉk=Cxˉk
一阶泰勒展开,有
x ˉ ˙ = x ˉ ˙ k + [ 0 0 − v r k sin ϕ k 0 0 v r k cos ϕ k 0 0 0 ] ( x ˉ − x ˉ k ) + [ 0 0 v r k L cos 2 δ f k ] ( u − ( ϕ k + δ f k ) u k ) = [ 0 0 − v r k sin ϕ k 0 0 v r k cos ϕ k 0 0 0 ] x ˉ + [ 0 0 v r k L cos 2 δ f k ] u + [ v r k cos ϕ k + ϕ k v r k sin ϕ k v r k sin ϕ k − ϕ k v r k cos ϕ k v r k L tan δ f k − δ f k v r k L cos 2 δ f k ] = A x ˉ + B u + E \begin{align} \dot{\bar{x}}={}&\dot{\bar{x}}_k+ \left[\begin{matrix} 0&0&-v_{rk}\sin\phi_k\\ 0&0&v_{rk}\cos\phi_k\\ 0&0&0\end{matrix}\right](\bar{x}-\bar{x}_k)+\left[\begin{matrix} 0\\ 0\\ \frac{v_{rk}}{L\cos^2\delta_{fk}}\end{matrix}\right](u-(\phi_k+\delta_{fk})u_{k})\\ ={}& \left[\begin{matrix} 0&0&-v_{rk}\sin\phi_k\\ 0&0&v_{rk}\cos\phi_k\\ 0&0&0\end{matrix}\right]\bar{x}+\left[\begin{matrix} 0\\ 0\\ \frac{v_{rk}}{L\cos^2\delta_{fk}}\end{matrix}\right]u+\left[\begin{matrix} v_{rk}\cos\phi_k+\phi_kv_{rk}\sin\phi_k\\ v_{rk}\sin\phi_k-\phi_kv_{rk}\cos\phi_k\\ \frac{v_{rk}}{L}\tan{\delta_{fk}}-\frac{\delta_{fk}v_{rk}}{L\cos^2\delta_{fk}} \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align} xˉ˙===xˉ˙k+ 000000−vrksinϕkvrkcosϕk0 (xˉ−xˉk)+ 00Lcos2δfkvrk (u−(ϕk+δfk)uk) 000000−vrksinϕkvrkcosϕk0 xˉ+ 00Lcos2δfkvrk u+ vrkcosϕk+ϕkvrksinϕkvrksinϕk−ϕkvrkcosϕkLvrktanδfk−Lcos2δfkδfkvrk Axˉ+Bu+E
以前轴中心为参考点
[
x
˙
f
y
˙
f
ϕ
˙
]
=
[
v
f
cos
(
ϕ
+
δ
f
)
v
f
sin
(
ϕ
+
δ
f
)
v
f
sin
δ
f
L
]
,
[
y
ϕ
]
=
[
0
1
0
0
0
1
]
[
x
y
ϕ
]
x
ˉ
=
[
x
y
ϕ
]
⊤
,
y
ˉ
=
[
y
ϕ
]
⊤
,
u
=
δ
,
y
ˉ
k
=
C
x
ˉ
k
\left[\begin{matrix} \dot{x}_f\\\dot{y}_f\\ \dot{\phi} \end{matrix}\right] =\left[\begin{matrix}v_f\cos{(\phi+\delta_f)}\\ v_f\sin(\phi+\delta_f)\\ \frac{v_f\sin\delta_f}{L} \end{matrix}\right],\left[\begin{matrix}y\\\phi \end{matrix}\right]= \left[\begin{matrix}0&1&0\\0&0&1 \end{matrix}\right]\left[\begin{matrix}{x}\\ {y}\\{\phi}\end{matrix}\right]\\ \bar{x}=\left[\begin{matrix}x&y&\phi\end{matrix}\right]^\top, \bar{y}=\left[\begin{matrix}y&\phi\end{matrix}\right]^\top, u=\delta, \bar{y}_k=C\bar{x}_k
x˙fy˙fϕ˙
=
vfcos(ϕ+δf)vfsin(ϕ+δf)Lvfsinδf
,[yϕ]=[001001]
xyϕ
xˉ=[xyϕ]⊤,yˉ=[yϕ]⊤,u=δ,yˉk=Cxˉk
一阶泰勒展开
x
ˉ
˙
=
x
ˉ
˙
k
+
[
0
0
−
v
f
k
sin
(
ϕ
k
+
δ
f
k
)
0
0
v
f
k
cos
(
ϕ
k
+
δ
f
k
)
0
0
0
]
(
x
ˉ
−
x
ˉ
k
)
+
[
−
v
f
k
sin
(
ϕ
k
+
δ
f
k
)
v
f
k
cos
(
ϕ
k
+
δ
f
k
)
v
f
k
cos
δ
f
k
L
]
(
u
−
u
k
)
=
A
x
ˉ
+
B
u
+
[
v
f
k
cos
(
ϕ
k
+
δ
f
k
)
+
(
ϕ
k
+
δ
f
k
)
v
f
k
sin
(
ϕ
k
+
δ
f
k
)
v
f
k
sin
(
ϕ
k
+
δ
f
k
)
−
(
ϕ
k
+
δ
f
k
)
v
f
k
cos
(
ϕ
k
+
δ
f
k
)
v
k
sin
δ
f
k
−
δ
f
k
v
f
k
cos
δ
k
L
]
=
A
x
ˉ
+
B
u
+
E
\begin{align} \dot{\bar{x}}={}&\dot{\bar{x}}_k+\left[\begin{matrix} 0&0&-v_{fk}\sin(\phi_k+\delta_{fk})\\ 0&0&v_{fk}\cos(\phi_k+\delta_{fk})\\ 0&0&0\end{matrix}\right](\bar{x}-\bar{x}_k) +\left[\begin{matrix} -v_{fk}\sin(\phi_k+\delta_{fk})\\ v_{fk}\cos(\phi_k+\delta_{fk})\\ \frac{v_{fk}\cos\delta_{fk}}{L}\end{matrix}\right](u-u_{k})\\ ={}& A\bar{x}+Bu+ \left[\begin{matrix} v_{fk}\cos(\phi_k+\delta_{fk})+(\phi_k+\delta_{fk})v_{fk}\sin(\phi_k+\delta_{fk})\\ v_{fk}\sin(\phi_k+\delta_{fk})-(\phi_k+\delta_{fk})v_{fk}\cos(\phi_k+\delta_{fk})\\ \frac{v_k\sin{\delta_{fk}}-\delta_{fk}v_{fk}\cos\delta_{k}}{L} \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align}
xˉ˙===xˉ˙k+
000000−vfksin(ϕk+δfk)vfkcos(ϕk+δfk)0
(xˉ−xˉk)+
−vfksin(ϕk+δfk)vfkcos(ϕk+δfk)Lvfkcosδfk
(u−uk)Axˉ+Bu+
vfkcos(ϕk+δfk)+(ϕk+δfk)vfksin(ϕk+δfk)vfksin(ϕk+δfk)−(ϕk+δfk)vfkcos(ϕk+δfk)Lvksinδfk−δfkvfkcosδk
Axˉ+Bu+E
以质心为参考点
[
x
˙
c
y
˙
c
ϕ
˙
]
=
[
v
c
cos
(
ϕ
+
β
)
v
c
sin
(
ϕ
+
β
)
v
c
cos
β
(
tan
δ
f
−
tan
δ
r
)
L
]
,
β
=
arctan
l
f
tan
δ
r
+
l
r
tan
δ
f
L
x
ˉ
=
[
x
y
ϕ
]
⊤
,
y
ˉ
=
[
y
ϕ
]
⊤
,
u
=
[
δ
f
δ
r
]
⊤
,
y
ˉ
k
=
C
x
ˉ
k
\left[\begin{matrix} \dot{x}_c\\ \dot{y}_c\\ \dot{\phi}\\\end{matrix}\right]= \left[\begin{matrix}v_c\cos{(\phi+\beta)}\\v_c\sin(\phi+\beta)\\\frac{v_c\cos\beta(\tan\delta_f-\tan\delta_r)}{L}\\ \end{matrix}\right], \beta=\arctan\frac{l_f\tan\delta_r+l_r\tan\delta_f}{L}\\\bar{x}=\left[\begin{matrix}x&y&\phi\end{matrix}\right]^\top, \bar{y}=\left[\begin{matrix}y&\phi\end{matrix}\right]^\top, u=\left[\begin{matrix}\delta_f&\delta_r\end{matrix}\right]^\top, \bar{y}_k=C\bar{x}_k
x˙cy˙cϕ˙
=
vccos(ϕ+β)vcsin(ϕ+β)Lvccosβ(tanδf−tanδr)
,β=arctanLlftanδr+lrtanδfxˉ=[xyϕ]⊤,yˉ=[yϕ]⊤,u=[δfδr]⊤,yˉk=Cxˉk
一阶泰勒展开
x
ˉ
˙
=
x
ˉ
˙
k
+
[
0
0
−
v
c
k
sin
(
ϕ
k
+
β
k
)
0
0
v
c
k
cos
(
ϕ
k
+
β
k
)
0
0
0
]
(
x
ˉ
−
x
ˉ
k
)
+
[
−
v
c
k
sin
(
ϕ
k
+
β
k
)
β
k
′
f
−
v
c
k
sin
(
ϕ
k
+
β
k
)
β
k
′
r
v
c
k
cos
(
ϕ
k
+
β
k
)
β
k
′
f
v
c
k
cos
(
ϕ
k
+
β
k
)
β
k
′
r
−
v
c
k
(
β
k
′
f
sin
β
k
(
tan
δ
k
f
−
tan
δ
k
r
)
+
cos
β
k
sec
2
δ
k
f
)
L
−
v
c
k
(
β
k
′
r
sin
β
k
(
tan
δ
k
f
−
tan
δ
k
r
)
−
cos
β
k
sec
2
δ
k
r
)
L
]
(
u
−
u
k
)
=
A
x
ˉ
+
B
u
+
[
v
c
k
cos
(
ϕ
k
+
β
k
)
+
(
ϕ
k
+
β
k
′
f
δ
f
k
+
β
k
′
r
δ
r
k
)
v
c
k
sin
(
ϕ
k
+
β
k
)
v
c
k
sin
(
ϕ
k
+
β
k
)
−
(
ϕ
k
+
β
k
′
f
δ
f
k
+
β
k
′
r
δ
r
k
)
v
c
k
cos
(
ϕ
k
+
β
f
k
)
v
c
k
(
cos
β
k
+
(
β
k
′
f
δ
f
k
+
β
k
′
r
δ
r
k
)
sin
β
k
)
(
tan
δ
f
k
−
tan
δ
r
k
)
+
cos
β
k
(
sec
2
δ
k
f
δ
r
k
−
sec
2
δ
r
k
δ
r
k
)
L
]
=
A
x
ˉ
+
B
u
+
E
β
k
′
f
=
l
r
sec
2
δ
f
L
(
1
+
β
0
2
)
β
k
′
r
=
l
f
sec
2
δ
r
L
(
1
/
+
β
0
2
)
β
0
=
l
f
tan
δ
r
+
l
r
tan
δ
f
L
\begin{align} \dot{\bar{x}}={}&\dot{\bar{x}}_k+\left[\begin{matrix} 0&0&-v_{ck}\sin(\phi_k+\beta_{k})\\ 0&0&v_{ck}\cos(\phi_k+\beta_{k})\\ 0&0&0\end{matrix}\right](\bar{x}-\bar{x}_k) \\&+\left[\begin{matrix} -v_{ck}\sin(\phi_k+\beta_{k})\beta_{k}'^f&-v_{ck}\sin(\phi_k+\beta_{k})\beta_{k}'^r\\ v_{ck}\cos(\phi_k+\beta_{k})\beta_{k}'^f&v_{ck}\cos(\phi_k+\beta_{k})\beta_{k}'^r\\ -\frac{v_{ck}(\beta_k'^f\sin\beta_k(\tan\delta_{kf}-\tan\delta_{kr})+\cos\beta_k\sec^2\delta_{kf})}{L}& -\frac{v_{ck}(\beta_k'^r\sin\beta_k(\tan\delta_{kf}-\tan\delta_{kr})-\cos\beta_k\sec^2\delta_{kr})}{L}&\end{matrix}\right](u-u_{k})\\ ={}& A\bar{x}+Bu+ \left[\begin{matrix} v_{ck}\cos(\phi_k+\beta_{k})+(\phi_k+\beta_k'^f\delta_{fk}+\beta_k'^r\delta_{rk})v_{ck}\sin(\phi_k+\beta_{k})\\ v_{ck}\sin(\phi_k+\beta_{k})-(\phi_k+\beta_k'^f\delta_{fk}+\beta_k'^r\delta_{rk})v_{ck}\cos(\phi_k+\beta_{fk})\\ v_{ck}\frac{(\cos\beta_k+(\beta_k'^f\delta_{fk}+\beta_k'^r\delta_{rk})\sin\beta_k)(\tan\delta_{fk}-\tan\delta_{rk}) +\cos\beta_k(\sec^2\delta_{kf}\delta_{rk}-\sec^2\delta_{rk}\delta_{rk})}{L} \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \\ \beta_k'^f={}&\frac{l_r\sec^2\delta_f}{L(1+\beta_0^2)}\\ \beta_k'^r={}&\frac{l_f\sec^2\delta_r}{L(1/+\beta_0^2)}\\ \beta_0={}&\frac{l_f\tan\delta_r+l_r\tan\delta_f}{L} \end{align}
xˉ˙===βk′f=βk′r=β0=xˉ˙k+
000000−vcksin(ϕk+βk)vckcos(ϕk+βk)0
(xˉ−xˉk)+
−vcksin(ϕk+βk)βk′fvckcos(ϕk+βk)βk′f−Lvck(βk′fsinβk(tanδkf−tanδkr)+cosβksec2δkf)−vcksin(ϕk+βk)βk′rvckcos(ϕk+βk)βk′r−Lvck(βk′rsinβk(tanδkf−tanδkr)−cosβksec2δkr)
(u−uk)Axˉ+Bu+
vckcos(ϕk+βk)+(ϕk+βk′fδfk+βk′rδrk)vcksin(ϕk+βk)vcksin(ϕk+βk)−(ϕk+βk′fδfk+βk′rδrk)vckcos(ϕk+βfk)vckL(cosβk+(βk′fδfk+βk′rδrk)sinβk)(tanδfk−tanδrk)+cosβk(sec2δkfδrk−sec2δrkδrk)
Axˉ+Bu+EL(1+β02)lrsec2δfL(1/+β02)lfsec2δrLlftanδr+lrtanδf
注释:实际应用中可用
δ
k
−
1
≈
δ
k
\delta_{k-1}\approx\delta_{k}
δk−1≈δk,但由于转角一般很小,一般直接将其忽略。同时对于车体作弊坐标系有
x
ˉ
k
=
0
\bar{x}_k=0
xˉk=0。故模型可以被简化为:
以后轴中心为参考点
x ˉ ˙ = [ 0 0 0 0 0 v r k 0 0 0 ] x ˉ + [ 0 0 v r k L cos 2 δ f k ] u + [ v r k 0 v r k L tan δ f k − δ f k v r k L cos 2 δ f k ] = A x ˉ + B u + E \begin{align} \dot{\bar{x}}={}&\left[\begin{matrix} 0&0&0\\ 0&0&v_{rk}\\ 0&0&0\end{matrix}\right]\bar{x}+\left[\begin{matrix} 0\\ 0\\ \frac{v_{rk}}{L\cos^2\delta_{fk}}\end{matrix}\right]u+\left[\begin{matrix} v_{rk}\\ 0\\ \frac{v_{rk}}{L}\tan{\delta_{fk}}-\frac{\delta_{fk}v_{rk}}{L\cos^2\delta_{fk}} \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align} xˉ˙== 0000000vrk0 xˉ+ 00Lcos2δfkvrk u+ vrk0Lvrktanδfk−Lcos2δfkδfkvrk Axˉ+Bu+E
以前轴中心为参考点
x ˉ ˙ = [ 0 0 − v f k sin δ f k 0 0 v f k cos δ f k 0 0 0 ] x ˉ + [ − v f k sin δ f k v f k cos δ f k v f k cos δ f k L ] u + [ v f k cos δ f k + δ f k v f k sin δ f k v f k sin δ f k − δ f k v f k cos δ f k v k sin δ f k − δ f k v f k cos δ k L ] = A x ˉ + B u + E \begin{align} \dot{\bar{x}}={}&\left[\begin{matrix} 0&0&-v_{fk}\sin\delta_{fk}\\ 0&0&v_{fk}\cos\delta_{fk}\\ 0&0&0\end{matrix}\right]\bar{x} +\left[\begin{matrix} -v_{fk}\sin\delta_{fk}\\ v_{fk}\cos\delta_{fk}\\ \frac{v_{fk}\cos\delta_{fk}}{L}\end{matrix}\right]u+ \left[\begin{matrix} v_{fk}\cos\delta_{fk}+\delta_{fk}v_{fk}\sin\delta_{fk}\\ v_{fk}\sin\delta_{fk}-\delta_{fk}v_{fk}\cos\delta_{fk}\\ \frac{v_k\sin{\delta_{fk}}-\delta_{fk}v_{fk}\cos\delta_{k}}{L} \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align} xˉ˙== 000000−vfksinδfkvfkcosδfk0 xˉ+ −vfksinδfkvfkcosδfkLvfkcosδfk u+ vfkcosδfk+δfkvfksinδfkvfksinδfk−δfkvfkcosδfkLvksinδfk−δfkvfkcosδk Axˉ+Bu+E
以质心为参考点
x ˉ ˙ = [ 0 0 − v c k sin β k 0 0 v c k cos β k 0 0 0 ] x ˉ + [ − v c k β k ′ f sin β k − v c k β k ′ r sin β k v c k β k ′ f cos β k v c k β k ′ r cos β k − v c k ( β k ′ f sin β k ( tan δ k f − tan δ k r ) + cos β k sec 2 δ k f ) L − v c k ( β k ′ r sin β k ( tan δ k f − tan δ k r ) − cos β k sec 2 δ k r ) L ] u + [ v c k cos β k + ( β k ′ f δ f k + β k ′ r δ r k ) v c k sin β k v c k sin β k − ( β k ′ f δ f k + β k ′ r δ r k ) v c k cos β f k v c k ( cos β k + ( β k ′ f δ f k + β k ′ r δ r k ) sin β k ) ( tan δ f k − tan δ r k ) + cos β k ( sec 2 δ k f δ r k − sec 2 δ r k δ r k ) L ] = A x ˉ + B u + E \begin{align} \dot{\bar{x}}={}&\left[\begin{matrix} 0&0&-v_{ck}\sin\beta_{k}\\ 0&0&v_{ck}\cos\beta_{k}\\ 0&0&0\end{matrix}\right]\bar{x} \\&+\left[\begin{matrix} -v_{ck}\beta_{k}'^f\sin\beta_{k}&-v_{ck}\beta_{k}'^r\sin\beta_{k}\\ v_{ck}\beta_{k}'^f\cos\beta_{k}&v_{ck}\beta_{k}'^r\cos\beta_{k}\\ -\frac{v_{ck}(\beta_k'^f\sin\beta_k(\tan\delta_{kf}-\tan\delta_{kr})+\cos\beta_k\sec^2\delta_{kf})}{L}& -\frac{v_{ck}(\beta_k'^r\sin\beta_k(\tan\delta_{kf}-\tan\delta_{kr})-\cos\beta_k\sec^2\delta_{kr})}{L}&\end{matrix}\right]u\\ & + \left[\begin{matrix} v_{ck}\cos\beta_{k}+(\beta_k'^f\delta_{fk}+\beta_k'^r\delta_{rk})v_{ck}\sin\beta_{k}\\ v_{ck}\sin\beta_{k}-(\beta_k'^f\delta_{fk}+\beta_k'^r\delta_{rk})v_{ck}\cos\beta_{fk}\\ v_{ck}\frac{(\cos\beta_k+(\beta_k'^f\delta_{fk}+\beta_k'^r\delta_{rk})\sin\beta_k)(\tan\delta_{fk}-\tan\delta_{rk}) +\cos\beta_k(\sec^2\delta_{kf}\delta_{rk}-\sec^2\delta_{rk}\delta_{rk})}{L} \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align} xˉ˙== 000000−vcksinβkvckcosβk0 xˉ+ −vckβk′fsinβkvckβk′fcosβk−Lvck(βk′fsinβk(tanδkf−tanδkr)+cosβksec2δkf)−vckβk′rsinβkvckβk′rcosβk−Lvck(βk′rsinβk(tanδkf−tanδkr)−cosβksec2δkr) u+ vckcosβk+(βk′fδfk+βk′rδrk)vcksinβkvcksinβk−(βk′fδfk+βk′rδrk)vckcosβfkvckL(cosβk+(βk′fδfk+βk′rδrk)sinβk)(tanδfk−tanδrk)+cosβk(sec2δkfδrk−sec2δrkδrk) Axˉ+Bu+E
注释:笔者认为在高速场景下,前后轮转角对横向控制的影响较大,因此这里简化中没有直接将前后轮转角近似为0,但到底影响有多大需要测试
低速时,再次近似
以后轴中心为参考点
x ˉ ˙ = [ 0 0 0 0 0 v r k 0 0 0 ] x ˉ + [ 0 0 v r k L ] u + [ v r k 0 0 ] = A x ˉ + B u + E \begin{align} \dot{\bar{x}}={}&\left[\begin{matrix} 0&0&0\\ 0&0&v_{rk}\\ 0&0&0\end{matrix}\right]\bar{x}+\left[\begin{matrix} 0\\ 0\\ \frac{v_{rk}}{L}\end{matrix}\right]u+\left[\begin{matrix} v_{rk}\\ 0\\ 0 \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align} xˉ˙== 0000000vrk0 xˉ+ 00Lvrk u+ vrk00 Axˉ+Bu+E
以前轴中心为参考点
x ˉ ˙ = [ 0 0 0 0 0 v f k 0 0 0 ] x ˉ + [ 0 v f k v f k L ] u + [ v f k 0 0 ] = A x ˉ + B u + E \begin{align} \dot{\bar{x}}={}&\left[\begin{matrix} 0&0&0\\ 0&0&v_{fk}\\ 0&0&0\end{matrix}\right]\bar{x} +\left[\begin{matrix} 0\\ v_{fk}\\ \frac{v_{fk}}{L}\end{matrix}\right]u+ \left[\begin{matrix} v_{fk}\\ 0\\ 0 \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \end{align} xˉ˙== 0000000vfk0 xˉ+ 0vfkLvfk u+ vfk00 Axˉ+Bu+E
以质心为参考点
x ˉ ˙ = [ 0 0 0 0 0 v c k 0 0 0 ] x ˉ + [ 0 0 v c k l r L v c k l f L − v c k L v c k L ] u + [ v c k 0 0 ] = A x ˉ + B u + E β k ′ f = l r L , β k ′ r = l f L , β 0 = 1 \begin{align} \dot{\bar{x}}={}&\left[\begin{matrix} 0&0&0\\ 0&0&v_{ck}\\ 0&0&0\end{matrix}\right]\bar{x} +\left[\begin{matrix} 0&0\\ v_{ck}\frac{l_r}{L}&v_{ck}\frac{l_f}{L}\\ -\frac{v_{ck}}{L}& \frac{v_{ck}}{L}&\end{matrix}\right]u + \left[\begin{matrix} v_{ck}\\ 0\\ 0 \end{matrix}\right]\\ ={}&A\bar{x}+Bu+E \\ \beta_k'^f={}&\frac{l_r}{L}, \beta_k'^r=\frac{l_f}{L}, \beta_0=1\end{align} xˉ˙==βk′f= 0000000vck0 xˉ+ 0vckLlr−Lvck0vckLlfLvck u+ vck00 Axˉ+Bu+ELlr,βk′r=Llf,β0=1
差分离散
x ˉ k + 1 = ( I + d t A ) x ˉ k + d t B u k + d t E = A ˉ x ˉ k + B ˉ u k + E ˉ , \begin{align} \bar{x}_{k+1}= (I+\mathrm{d}tA)\bar{x}_k+\mathrm{d}tBu_k+\mathrm{d}tE=\bar{A}\bar{x}_k+\bar{B}u_k+\bar{E}, \end{align} xˉk+1=(I+dtA)xˉk+dtBuk+dtE=Aˉxˉk+Bˉuk+Eˉ,
中点欧拉离散
y n + 1 = y ( t + d t ) = y k + d t f ( t + 1 2 d t , y ˉ k + d t 2 f ( t , y ˉ k ) ) = y k + d t f ( t + 1 2 d t , 1 2 ( y ˉ k + y ˉ k + 1 ) ) \begin{align}y_{n+1}=y(t+\mathrm{d}t)=y_k+\mathrm{d}tf(t+\frac{1}{2}\mathrm{d}t, \bar{y}_k+\frac{\mathrm{d}t}{2}f(t, \bar{y}_{k}))=y_k+\mathrm{d}tf(t+\frac{1}{2}\mathrm{d}t, \frac{1}{2}(\bar{y}_k+\bar{y}_{k+1}))\end{align} yn+1=y(t+dt)=yk+dtf(t+21dt,yˉk+2dtf(t,yˉk))=yk+dtf(t+21dt,21(yˉk+yˉk+1))
用 t + 0.5 d t t+0.5\mathrm{d}t t+0.5dt的导数值来代替整个周期的导数(平均值)
x ˉ k + 1 = x ˉ k + d t ( 1 2 A ( x ˉ k + 1 + x ˉ k ) + B u k + E ) = ( I − d t 2 A ) − 1 ( ( I + d t 2 A ) x ˉ k + d t B u k + d t E ) \begin{align} \bar{x}_{k+1}={}& \bar{x}_k+\mathrm{d}t(\frac{1}{2}A(\bar{x}_{k+1}+\bar{x}_k)+Bu_k+E)\notag\\ ={}&(I-\frac{\mathrm{d}t}{2}A)^{-1}\left((I+\frac{\mathrm{d}t}{2}A)\bar{x}_k+\mathrm{d}tBu_k+\mathrm{d}tE\right) \end{align} xˉk+1==xˉk+dt(21A(xˉk+1+xˉk)+Buk+E)(I−2dtA)−1((I+2dtA)xˉk+dtBuk+dtE)
4阶龙格-库塔
{ y n + 1 = y n + d t 6 ( k 1 + 2 k 2 + 2 k 3 + k 4 ) k 1 = f ( t n , y n ) k 2 = f ( t n + d t 2 , d t 2 k 1 ) k 3 = f ( t n + d t 2 , d t 2 k 2 ) k 4 = f ( t n + d t , d t k 3 ) \begin{align} \left\{\begin{aligned} y_{n+1}&=y_n+\frac{\mathrm{d}t}{6}(k_1+2k_2+2k_3+k_4)\\ k_1&=f(t_n, y_n)\\ k_2&=f(t_n+\frac{\mathrm{d}t}{2}, \frac{\mathrm{d}t}{2}k_1)\\ k_3&=f(t_n+\frac{\mathrm{d}t}{2}, \frac{\mathrm{d}t}{2}k_2)\\ k_4&=f(t_n+\mathrm{d}t, \mathrm{d}tk_3) \end{aligned}\right. \end{align} ⎩ ⎨ ⎧yn+1k1k2k3k4=yn+6dt(k1+2k2+2k3+k4)=f(tn,yn)=f(tn+2dt,2dtk1)=f(tn+2dt,2dtk2)=f(tn+dt,dtk3)
4阶龙格-库塔法即近似值与真值之间的误差是四阶导数的高阶无穷小(欧拉中值法为二阶)。计算复杂,略。
【注释】无论中点欧拉还是龙格-库塔,都需要带入半采样周期,并依赖其预估采样周期内某点的导数。但是对于扰动或者控制输入,半周期处的值是不可知的( u k + 1 2 u_{k+\frac{1}{2}} uk+21),因此估计出来的值不一定是准确的,因此,非状态自治系统采用这两种方式得到离散模型的精度是否更高?高多少?都是未可知的。
应用上面的额MPC方法即可。