Hi All

新年挖新坑,今日開啟船新連載.內容是無人車的橫向控制,整體涵蓋從0-1為車輛橫向控制設計MPC控制器設計與MPC+MRAC耦合控制.大家有問題,有興趣可以在評論區多多交流.

車輛橫向動力學模型

引言

首先我們要問:針對車輛橫向控制的問題,我們為什么需要建立動力學模型?
簡單來說,當車輛在較高速度下行駛時,運動學模型(自行車模型)中提出"汽車輪胎速度方向與車輛朝向相同"的假設不再成立.車輛受到的橫向力將不可忽視,如向心力將隨著速度的增大而平方倍地增大.因此引入動力學模型,旨在建立更高階量之間的聯系,以更好地描述車輛轉彎的非線性特性.

那么,讓我們開始吧.為了在不失一般性的前提下盡可能簡化模型,動力學模型將建立在以下幾個假設上
1.輪胎速度方向與車輛縱向方向( x )的夾角 θ v (后統稱輪胎速度方向角)較小且滿足小角度假設: a ) θ v ≈ t a n θ v 2.輪胎轉角( δ )與輪胎速度方向角( θ v )的夾角 α (后統稱輪胎側滑角)較小 3.車輛縱向速度維持不變: a ) V x : = C o n s t a n t 4.忽略路堤角( ? )對橫向控制的影響 \begin{aligned} &\text{1.輪胎速度方向與車輛縱向方向($x$)的夾角$\theta_v$(后統稱輪胎速度方向角)較小且滿足小角度假設:}\\ &\qquad a) \ \theta_v \approx tan\theta_v \\ &\text{2.輪胎轉角($\delta$)與輪胎速度方向角($\theta_v$)的夾角$\alpha$(后統稱輪胎側滑角)較小} \\ &\text{3.車輛縱向速度維持不變:} \\ &\qquad a) \ V_x : = Constant \\ &\text{4.忽略路堤角($\phi$)對橫向控制的影響} \end{aligned} ?1.輪胎速度方向與車輛縱向方向(x)的夾角θv?(后統稱輪胎速度方向角)較小且滿足小角度假設:a) θv?≈tanθv?2.輪胎轉角(δ)與輪胎速度方向角(θv?)的夾角α(后統稱輪胎側滑角)較小3.車輛縱向速度維持不變:a) Vx?:=Constant4.忽略路堤角(?)對橫向控制的影響?

坐標系

本模型在 F L U (Front-Left-Universe) FLU \text{(Front-Left-Universe)} FLU(Front-Left-Universe) 慣性坐標系下建立.坐標系原點固定在車輛質心位置, x x x軸方向為車輛縱向方向,指向車頭前方. y y y軸方向與x軸垂直且指向車輛左側, z z z軸方向垂直于 x x x, y y y軸且指向天空.值得注意的是,全域(地圖)坐標系為ENU(East-North-Universe),其xyz指向規則于FLU相似,分別指向東北天.還有一個區域坐標系為Frenet坐標系,其固定在理想軌跡上,這里就不展開講了,詳見下圖.

受力分析

車輛受力分析圖如下

根據牛頓第二定律,對車輛y方向(橫向)進行受力分析
F y f + F y r = m a y F_{yf} + F_{yr} = ma_y Fyf?+Fyr?=may?
其中 F y f F_{yf} Fyf? 與 F y r F_{yr} Fyr? 分別是車輛前輪和后輪在 y y y方向受到的力, m m m為車輛質量, a y a_y ay?為車輛在 y y y方向上的加速度.

車輛在 y y y方向上的加速度由兩部分構成:
1.因車輛在 y y y方向上運動產生的加速度,定義為 y ¨ \ddot{y} y¨?.
2.車輛的向心加速度,記為 a y c a_{yc} ayc?.
a y = y ¨ + a y c = y ¨ + ω 2 R = y ¨ + ψ ˙ 2 R = y ¨ + V x ψ ˙ a_y = \ddot{y}+a_{yc} = \ddot{y}+\omega^2R = \ddot{y}+\dot{\psi}^2R = \ddot{y}+V_x\dot{\psi} ay?=y¨?+ayc?=y¨?+ω2R=y¨?+ψ˙?2R=y¨?+Vx?ψ˙?

因此
F y f + F y r = m ( y ¨ + V x ψ ˙ ) F_{yf} + F{yr} = m(\ddot{y}+V_x\dot{\psi}) Fyf?+Fyr=m(y¨?+Vx?ψ˙?)

對z軸進行偏航動力學分析,由力矩平衡可得
I z ψ ¨ = l f F y f ? l r F y r I_z\ddot{\psi}=l_fF_{yf} - l_rF_{yr} Iz?ψ¨?=lf?Fyf??lr?Fyr?
其中 l f l_f lf? 與 l r l_r lr? 分別是車輛前輪和后輪距離車輛重心的距離.

下一步,我們要對橫向力 F y f F_{yf} Fyf? 與 F y r F_{yr} Fyr? 進行分析.實驗表明,當輪胎側滑角 α \alpha α 較小時,輪胎受到的橫向力的大小與輪胎側滑角成正比.其中輪胎側滑角被定義為輪胎轉角與輪胎速度方向角的夾角.

因此,前輪(方向輪)側滑角 α f \alpha_f αf? 為
α f = δ ? θ v f \alpha_f = \delta - \theta_{vf} αf?=δ?θvf?
其中 δ \delta δ為前輪轉角, θ v f \theta_{vf} θvf? 為前輪速度方向角.

同理,后輪(假定后輪無法轉向)側滑角為
α r = 0 ? θ v r = ? θ v r \alpha_r = 0 - \theta_{vr} = - \theta_{vr} αr?=0?θvr?=?θvr?
其中 θ v r \theta_{vr} θvr? 為后輪速度方向角.

基于上述兩點推斷, 輪胎橫向力可被改寫為以下形式
F y f = 2 C α f ( δ ? θ v f ) F y r = ? 2 C α r θ v r \begin{aligned} &F_{yf} = 2C_{\alpha_f}(\delta - \theta_{vf}) \\ &F_{yr} = -2C_{\alpha_r}\theta_{vr} \end{aligned} ?Fyf?=2Cαf??(δ?θvf?)Fyr?=?2Cαr??θvr??
其中 C α f C_{\alpha_f} Cαf?? 與 C α r C_{\alpha_r} Cαr?? 分別為前輪與后輪的側滑剛度系數.

由小角度假設與牽連運動公式可得
θ v f ≈ t a n ( θ v f ) = y ˙ + l f ψ ˙ V x θ v r ≈ t a n ( θ v r ) = y ˙ ? l r ψ ˙ V x \begin{aligned} &\theta_{vf} \approx tan(\theta_{vf}) = \frac{\dot{y} + l_f\dot{\psi}}{V_x} \\ &\theta_{vr} \approx tan(\theta_{vr}) = \frac{\dot{y} - l_r\dot{\psi}}{V_x} \end{aligned} ?θvf?≈tan(θvf?)=Vx?y˙?+lf?ψ˙??θvr?≈tan(θvr?)=Vx?y˙??lr?ψ˙???

綜上可得

y ¨ = 2 m [ C α f δ ? C α f + C α r V x y ˙ + ? C α f l f + C α r l r V x ψ ˙ ] ? V x ψ ˙ ψ ¨ = 2 I z ( C α f l f δ ? C α f l f ? C α r l r V x y ˙ ? C α f l f 2 + C α r l r 2 V x ψ ˙ ) \begin{aligned} &\ddot{y} = \frac{2}{m}[C_{\alpha_f}\delta - \frac{C_{\alpha_f}+C_{\alpha_r}}{V_x}\dot{y}+ \frac{-C_{\alpha_f}l_f+C_{\alpha_r}l_r}{V_x}\dot{\psi}] -V_x\dot{\psi} \\ &\ddot{\psi}=\frac{2}{I_z}(C_{\alpha_f}l_f\delta-\frac{C_{\alpha_f}l_f- C_{\alpha_r}l_r}{V_x}\dot{y}-\frac{C_{\alpha_f}l_f^2+C_{\alpha_r}l_r^2}{V_x}\dot{\psi}) \end{aligned} ?y¨?=m2?[Cαf??δ?Vx?Cαf??+Cαr???y˙?+Vx??Cαf??lf?+Cαr??lr??ψ˙?]?Vx?ψ˙?ψ¨?=Iz?2?(Cαf??lf?δ?Vx?Cαf??lf??Cαr??lr??y˙??Vx?Cαf??lf2?+Cαr??lr2??ψ˙?)?

將上式改寫為矩陣形式

[ y ˙ y ¨ ψ ˙ ψ ¨ ] = [ 0 1 0 0 0 ? 2 ( C α f + C α r ) m V x 0 2 ( ? C α f l f + C α r l r ) m V x ? V x 0 0 0 1 0 ? 2 ( C α f l f ? C α r l r ) I z V x 0 ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x ] [ y y ˙ ψ ψ ˙ ] + [ 0 2 C α f m 0 2 C α f l f I z ] δ \begin{bmatrix} \dot{y} \\ \ddot{y} \\ \dot{\psi} \\ \ddot{\psi} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2(C_{\alpha_f}+C_{\alpha_r})}{mV_x} & 0 & \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2(C_{\alpha_f}l_f- C_{\alpha_r}l_r)}{I_zV_x} & 0 & -\frac{2(C_{\alpha_f}l_f^2+C_{\alpha_r}l_r^2)}{I_zV_x} \\ \end{bmatrix} \begin{bmatrix} y \\ \dot{y} \\ \psi \\ \dot{\psi} \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha_f}}{m} \\ 0 \\ \frac{2C_{\alpha_f}l_f}{I_z} \\ \end{bmatrix} \delta ?????y˙?y¨?ψ˙?ψ¨???????=??????0000?1?mVx?2(Cαf??+Cαr??)?0?Iz?Vx?2(Cαf??lf??Cαr??lr?)??0000?0mVx?2(?Cαf??lf?+Cαr??lr?)??Vx?1?Iz?Vx?2(Cαf??lf2?+Cαr??lr2?)?????????????yy˙?ψψ˙???????+??????0m2Cαf???0Iz?2Cαf??lf?????????δ

動力學誤差模型

將動力學模型中的狀態變數設定為誤差量,將有利于簡化后續控制器的設計.因此,定義誤差狀態變數
e 1 : = y ? y d e s 橫向誤差 e 2 : = ψ ? ψ d e s 前輪航向角誤差 \begin{aligned} &e_1:= y-y_{des} \quad\text{橫向誤差} \\ &e_2:= \psi-\psi_{des} \quad\text{前輪航向角誤差} \\ \end{aligned} ?e1?:=y?ydes?橫向誤差e2?:=ψ?ψdes?前輪航向角誤差?

誤差動力學分析

理想航向角速率
ψ ˙ d e s = V x R \dot{\psi}_{des} = \frac{V_x}{R} ψ˙?des?=RVx??
理想橫向加速度為
a y d e s = y ¨ d e s + a y c d e s = 0 + V x 2 R = V x ψ ˙ d e s a_{y_{des}} = \ddot{y}_{des}+a_{yc_{des}} = 0+\frac{V^2_x}{R} = V_x\dot{\psi}_{des} aydes??=y¨?des?+aycdes??=0+RVx2??=Vx?ψ˙?des?
因此,橫向誤差 e 1 e_1 e1?可改寫為
e ¨ 1 = a y ? a y d e s = y ¨ + V x ψ ˙ ? V x ψ ˙ d e s = y ¨ + V x ( ψ ˙ ? ψ ˙ d e s ) = y ¨ + V x e ˙ 2 \ddot{e}_1 = a_y - a_{y_{des}} = \ddot{y}+V_x\dot{\psi}-V_x\dot{\psi}_{des} = \ddot{y}+V_x(\dot{\psi}-\dot{\psi}_{des}) = \ddot{y}+V_x\dot{e}_2 e¨1?=ay??aydes??=y¨?+Vx?ψ˙??Vx?ψ˙?des?=y¨?+Vx?(ψ˙??ψ˙?des?)=y¨?+Vx?e˙2?
前面提到 V x V_x Vx?為常數,因此對上式積分可得
e ˙ 1 = y ˙ + V x ( ψ ? ψ d e s ) = y ˙ + V x e 2 \dot{e}_1 = \dot{y}+V_x(\psi-\psi_{des}) = \dot{y}+V_xe_2 e˙1?=y˙?+Vx?(ψ?ψdes?)=y˙?+Vx?e2?

由此可得
y ˙ = e ˙ 1 ? V x e 2 y ¨ = e ¨ 1 ? V x e ˙ 2 ψ ˙ = ψ ˙ d e s + e ˙ 2 \begin{aligned} & \dot{y} = \dot{e}_1 - V_xe_2\\ & \ddot{y} = \ddot{e}_1 - V_x\dot{e}_2 \\ & \dot{\psi}= \dot{\psi}_{des}+\dot{e}_2 \\ \end{aligned} ?y˙?=e˙1??Vx?e2?y¨?=e¨1??Vx?e˙2?ψ˙?=ψ˙?des?+e˙2??

將上述式子代入到動力學模型中,替換掉原先的變數可得

( e ¨ 1 ? V x e ˙ 2 ) = 2 m [ C α f δ ? C α f + C α r V x ( e ˙ 1 ? V x e 2 ) + ? C α f l f + C α r l r V x ( ψ ˙ d e s + e ˙ 2 ) ] ? V x ( ψ ˙ d e s + e ˙ 2 ) ψ ¨ = 2 I z ( C α f l f δ ? C α f l f ? C α r l r V x ( e ˙ 1 ? V x e 2 ) ? C α f l f 2 + C α r l r 2 V x ( ψ ˙ d e s + e ˙ 2 ) ) \begin{aligned} &(\ddot{e}_1 - V_x\dot{e}_2) = \frac{2}{m}[C_{\alpha_f}\delta - \frac{C_{\alpha_f}+C_{\alpha_r}}{V_x}(\dot{e}_1 - V_xe_2)+ \frac{-C_{\alpha_f}l_f+C_{\alpha_r}l_r}{V_x}(\dot{\psi}_{des}+\dot{e}_2)] -V_x(\dot{\psi}_{des}+\dot{e}_2) \\ &\ddot{\psi}=\frac{2}{I_z}(C_{\alpha_f}l_f\delta-\frac{C_{\alpha_f}l_f- C_{\alpha_r}l_r}{V_x}(\dot{e}_1 - V_xe_2)-\frac{C_{\alpha_f}l_f^2+C_{\alpha_r}l_r^2}{V_x}(\dot{\psi}_{des}+\dot{e}_2)) \end{aligned} ?(e¨1??Vx?e˙2?)=m2?[Cαf??δ?Vx?Cαf??+Cαr???(e˙1??Vx?e2?)+Vx??Cαf??lf?+Cαr??lr??(ψ˙?des?+e˙2?)]?Vx?(ψ˙?des?+e˙2?)ψ¨?=Iz?2?(Cαf??lf?δ?Vx?Cαf??lf??Cαr??lr??(e˙1??Vx?e2?)?Vx?Cαf??lf2?+Cαr??lr2??(ψ˙?des?+e˙2?))?

整理可得

e ¨ 1 = 2 C α f m δ + ? 2 ( C α f + C α r ) m V x e ˙ 1 + 2 ( C α f + C α r ) m e 2 + 2 ( ? C α f l f + C α r l r ) m V x e ˙ 2 + ( 2 ( ? C α f l f + C α r l r ) m V x ? V x ) ψ ˙ d e s ψ ¨ = 2 C α f l f I z δ + ? 2 ( C α f l f ? C α r l r ) I z V x e ˙ 1 + 2 ( C α f l f ? C α r l r ) I z e 2 + ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x e ˙ 2 + ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x ψ ˙ d e s \begin{aligned} &\ddot{e}_1 = \frac{2C_{\alpha_f}}{m}\delta+\frac{-2(C_{\alpha_f}+C_{\alpha_r})}{mV_x}\dot{e}_1 +\frac{2(C_{\alpha_f}+C_{\alpha_r})}{m}e_2 + \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}\dot{e}_2 +(\frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x)\dot{\psi}_{des} \\ &\ddot{\psi} = \frac{2C_{\alpha_f}l_f}{I_z}\delta+\frac{-2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_zV_x}\dot{e}_1 +\frac{2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_z}e_2 +\frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x}\dot{e}_2 +\frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x}\dot{\psi}_{des} \end{aligned} ?e¨1?=m2Cαf???δ+mVx??2(Cαf??+Cαr??)?e˙1?+m2(Cαf??+Cαr??)?e2?+mVx?2(?Cαf??lf?+Cαr??lr?)?e˙2?+(mVx?2(?Cαf??lf?+Cαr??lr?)??Vx?)ψ˙?des?ψ¨?=Iz?2Cαf??lf??δ+Iz?Vx??2(Cαf??lf??Cαr??lr?)?e˙1?+Iz?2(Cαf??lf??Cαr??lr?)?e2?+Iz?Vx??2(Cαf??lf2?+Cαr??lr2?)?e˙2?+Iz?Vx??2(Cαf??lf2?+Cαr??lr2?)?ψ˙?des??

改寫為矩陣形式

[ e ˙ 1 e ¨ 1 e ˙ 2 e ¨ 2 ] = [ 0 1 0 0 0 ? 2 ( C α f + C α r ) m V x 2 ( C α f + C α r ) m 2 ( ? C α f l f + C α r l r ) m V x 0 0 0 1 0 ? 2 ( C α f l f ? C α r l r ) I z V x 2 ( C α f l f ? C α r l r ) I z ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x ] [ e 1 e ˙ 1 e 2 e ˙ 2 ] + [ 0 2 C α f m 0 2 C α f l f I z ] δ + [ 0 2 ( ? C α f l f + C α r l r ) m V x ? V x 0 ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x ] ψ ˙ d e s \begin{bmatrix} \dot{e}_1 \\ \ddot{e}_1 \\ \dot{e}_2 \\ \ddot{e}_2 \\ \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & \frac{-2(C_{\alpha_f}+C_{\alpha_r})}{mV_x} & \frac{2(C_{\alpha_f}+C_{\alpha_r})}{m} & \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}\\ 0 & 0 & 0 & 1 \\ 0 & \frac{-2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_zV_x} & \frac{2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_z} & \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha_f}}{m} \\ 0 \\ \frac{2C_{\alpha_f}l_f}{I_z} \\ \end{bmatrix} \delta + \begin{bmatrix} 0 \\ \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x \\ 0 \\ \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \dot{\psi}_{des} ?????e˙1?e¨1?e˙2?e¨2???????=??????0000?1mVx??2(Cαf??+Cαr??)?0Iz?Vx??2(Cαf??lf??Cαr??lr?)??0m2(Cαf??+Cαr??)?0Iz?2(Cαf??lf??Cαr??lr?)??0mVx?2(?Cαf??lf?+Cαr??lr?)?1Iz?Vx??2(Cαf??lf2?+Cαr??lr2?)?????????????e1?e˙1?e2?e˙2???????+??????0m2Cαf???0Iz?2Cαf??lf?????????δ+??????0mVx?2(?Cαf??lf?+Cαr??lr?)??Vx?0Iz?Vx??2(Cαf??lf2?+Cαr??lr2?)????????ψ˙?des?

定義

x = [ e 1 e ˙ 1 e 2 e ˙ 2 ] A = [ 0 1 0 0 0 ? 2 ( C α f + C α r ) m V x 2 ( C α f + C α r ) m 2 ( ? C α f l f + C α r l r ) m V x 0 0 0 1 0 ? 2 ( C α f l f ? C α r l r ) I z V x 2 ( C α f l f ? C α r l r ) I z ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x ] B = [ 0 2 C α f m 0 2 C α f l f I z ] B c = [ 0 2 ( ? C α f l f + C α r l r ) m V x ? V x 0 ? 2 ( C α f l f 2 + C α r l r 2 ) I z V x ] \begin{aligned} &x = \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \\ \end{bmatrix} \quad A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & \frac{-2(C_{\alpha_f}+C_{\alpha_r})}{mV_x} & \frac{2(C_{\alpha_f}+C_{\alpha_r})}{m} & \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}\\ 0 & 0 & 0 & 1 \\ 0 & \frac{-2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_zV_x} & \frac{2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_z} & \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \\ &B = \begin{bmatrix} 0 \\ \frac{2C_{\alpha_f}}{m} \\ 0 \\ \frac{2C_{\alpha_f}l_f}{I_z} \\ \end{bmatrix} \quad B_c = \begin{bmatrix} 0 \\ \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x \\ 0 \\ \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \end{aligned} ?x=?????e1?e˙1?e2?e˙2???????A=??????0000?1mVx??2(Cαf??+Cαr??)?0Iz?Vx??2(Cαf??lf??Cαr??lr?)??0m2(Cαf??+Cαr??)?0Iz?2(Cαf??lf??Cαr??lr?)??0mVx?2(?Cαf??lf?+Cαr??lr?)?1Iz?Vx??2(Cαf??lf2?+Cαr??lr2?)????????B=??????0m2Cαf???0Iz?2Cαf??lf?????????Bc?=??????0mVx?2(?Cαf??lf?+Cαr??lr?)??Vx?0Iz?Vx??2(Cαf??lf2?+Cαr??lr2?)?????????

則誤差動力學方程為

x ˙ = A x + B δ + B c ψ ˙ d e s \dot{x} = Ax + B\delta + B_c\dot{\psi}_{des} x˙=Ax+Bδ+Bc?ψ˙?des?

離散化

離散化的方法有很多,例如雙線性離散化,前向歐拉差分,河鏌夥拉差分,AB-2差分法等等.通常的,對于系統矩陣 A A A,為了保障其離散化后的精度與穩定性,我們采用雙線性離散化的方法.對于控制矩陣 B B B與矩陣 B c B_c Bc?,為了簡化運算,我們采用前向歐拉差分法進行離散.離散后的結果如下:
A d = ( I ? T 2 A ) ? 1 ( I + T 2 A ) B d = B T B c d = B c T \begin{aligned} &A_d = (I-\frac{T}{2}A)^{-1}(I+\frac{T}{2}A)\\ &B_d = BT\\ &B_{cd} = B_cT \end{aligned} ?Ad?=(I?2T?A)?1(I+2T?A)Bd?=BTBcd?=Bc?T?
其中,下標 d d d表示離散域矩陣, T T T為離散時間

至此,我們完成了車輛橫向動力學模型的搭建與離散化.下一步,就是將其封裝為一個QP問題,并以此設計MPC控制器.