车辆动力学推导

车辆动力学模型

沿着ｘ轴方向：$m(\ddot{x}-\dot{y}\dot{\phi })=2{\text{F}}_{xf}+2{\text{F}}_{xr}$

沿着y轴方向 ：$m(\ddot{y}+\dot{x}\dot{\phi })=2{\text{F}}_{yf}+2{\text{F}}_{yr}$
绕Z轴方向上：${\text{I}}_z\ddot{\phi }=2a{\text{F}}_{yf}-2b{\text{F}}_{yr}$

其中，$\dot{x}$为车辆质心纵向速度，$\dot{y}$为车辆质心横向速度，$\dot{\phi }$为车辆质心横摆角速度。

轮胎沿车辆ｙ轴方向上所受的轮胎力${\text{F}}_{i,jy}$和轮胎侧偏力(沿着轮胎ｙ轴方向上的力)${\text{F}}_{i,jc}$和轮胎纵向力${\text{F}}_{i,jl}$力之间有如下关系：
$${\text{F}}_{F,jy}={\text{F}}_{F,jl}sin{\delta }_f+{\text{F}}_{F,jc}cos{\delta }_f$$
沿ｘ轴方向上：
$${\text{F}}_{F,jx}={\text{F}}_{F,jl}cos{\delta }_f-{\text{F}}_{F,jc}sin{\delta }_f$$
$i$表示前后轴，$j$表示左右轮。
对于绝大多数前轮转向的车辆而言，后轮的关系为：
$$\begin{gathered} {\text{F}}_{R,jy}={\text{F}}_{R,jc} \\ {\text{F}}_{R,jx}={\text{F}}_{R,jl} \end{gathered}$$

轮胎纵向力和轮胎侧偏力 可用路面附着系数、轮胎动载荷 、轮胎
侧偏角及滑移率的非线性复杂函数计算得到:
$$\begin{gathered} {\text{F}}_l=f_l(\alpha ,s,\mu ,{\text{F}}_z) \\ {\text{F}}_c=f_c(\alpha ,s,\mu ,{\text{F}}_z) \end{gathered}$$
轮胎侧偏角$\alpha$可以由几何关系得到：
$$\alpha =arctan\frac{v_c}{v_l}$$
$v_c$和$v_l$分别是轮胎在自身坐标系下侧向和纵向的速度。
可以用车身坐标系方向上的速度$v_x$和$v_y$表示为：
$$\begin{gathered} v_l=v_{y}sin{\delta }_f+v_{x}cos{\delta }_f \\ {v}_c=v_{y}cos{\delta }_f-v_{x}sin{\delta }_f \end{gathered}$$
实际行驶过程中,车轮沿 x 轴方向的速度 $v_x$ 和沿 y 轴方向的速度 $v_y$ 难以直接获取, 一般通过质心的纵向速度$\dot{x}$, 横向速度$\dot{y}$,横摆角速度$\dot{\phi }$和车辆的尺寸参数得到：
$$\begin{gathered} v_{F,jy}=\dot{y}+a\dot{\phi } \\ {v}_{R,jy}=\dot{y}-b\dot{\phi } \\ {v}_{i,lx}=\dot{x}-\frac{1}{2}D\dot{\phi } \\ {v}_{i,rx}=\dot{x}+\frac{1}{2}D\dot{\phi } \end{gathered}$$
$D$为车辆的轮距，如果将车辆简化为两轮的自行车模型，则
$$\begin{gathered} v_{F,y}=\dot{y}+a\dot{\phi } \\ {v}_{R,y}=\dot{y}-b\dot{\phi } \\ {v}_{F,x}=\dot{x} \\ {v}_{R,x}=\dot{x} \end{gathered}$$
则：
$$\begin{gathered} {\alpha }_f=arctan(\frac{(\dot{y}+a\dot{\phi })cos{\delta }_f-\dot{x}sin{\delta }_f}{(\dot{y}+a\dot{\phi })sin{\delta }_f+\dot{x}cos{\delta }_f}) \\ {\alpha }_r=arctan(\frac{\dot{y}-b\dot{\phi }}{\dot{x}}) \end{gathered}$$
在小角度假设下，即满足如下近似条件：
$$cos\theta \approx 1,sin\theta \approx \theta ,tan\theta \approx \theta$$
则上式可以简化为：
$$\begin{gathered} {\alpha }_f=\frac{(\dot{y}+a\dot{\phi })-\dot{x}{\delta }_f}{\dot{x}}＝\frac{(\dot{y}+a\dot{\phi })}{\dot{x}}－{\delta }_f \\ {\alpha }_r=arctan(\frac{\dot{y}-b\dot{\phi }}{\dot{x}})=\frac{\dot{y}-b\dot{\phi }}{\dot{x}} \end{gathered}$$
ｘ：$m(\ddot{x}-\dot{y}\dot{\phi })=2{\text{F}}_{xf}+2{\text{F}}_{xr}=2(C_{lf}{s}_f+C_{cf}({\delta }_f-\frac{\dot{y}+a\dot{\phi }}{\dot{x}}){\delta }_f+C_{lr}{s}_r)$

y轴 ：$m(\ddot{y}+\dot{x}\dot{\phi })=2{\text{F}}_{yf}+2{\text{F}}_{yr}=2(C_{cf}({\delta }_f-\frac{\dot{y}+a\dot{\phi }}{\dot{x}})+C_{cr}\frac{\dot{y}-b\dot{\phi }}{\dot{x}})$
--------本来前轮的侧向力应该是$C_{cf}({\delta }_f-\frac{\dot{y}+a\dot{\phi }}{\dot{x}})cos\delta$,在小偏角假设下去掉$cos\delta$
Z轴：${\text{I}}_z\ddot{\phi }=2a{\text{F}}_{yf}-2b{\text{F}}_{yr}=2(a{C}_{cf}({\delta }_f-\frac{\dot{y}+a\dot{\phi }}{\dot{x}})-b{C}_{cr}\frac{\dot{y}-b\dot{\phi }}{\dot{x}})$

世界坐标系和车辆坐标系的转换关系为：
$$\begin{gathered} \dot{X}=\dot{x}cos\phi -\dot{y}sin\phi \\ \dot{Y}=\dot{x}sin\phi +\dot{y}cos\phi \end{gathered}$$

轮胎在地面上的滑移率：

车辆的行驶动力学方程可用下式表达：
$$J_w=F_l{r}_e+T_w$$
$$\begin{gathered} \dot{X}=\dot{x}cos\phi -\dot{y}sin\phi \\ \dot{Y}=\dot{x}sin\phi +\dot{y}cos\phi \\ \dot{\phi }=\dot{\phi } \\ \ddot{\phi }=\frac{2}{I_z}(\frac{b{C}_{cr}-a{C}_{cf}}{\dot{x}}\dot{y}-\frac{a^2{C}_{cf}+b^2{C}_{cr}}{\dot{x}}\dot{\phi }+a{C}_{cf}{\delta }_f) \\ \ddot{x}＝\dot{y}\dot{\phi }+(\frac{T}{2}-F_i-F_f-F_s)/m+\frac{2C_{cf}}{m}({\delta }_f-\frac{\dot{y}+a\dot{\phi }}{\dot{x}}) \\ \ddot{y}=2(-\frac{C_{cf}+C_{cr}}{m\dot{x}}\dot{y}+(\frac{b{C}_{cr}-a{C}_{cf}}{m\dot{x}}-\dot{x})\dot{\phi }+\frac{C_{c}f}{m}{\delta }_f) \end{gathered}$$

离散化动力学模型

考虑线性时变系统－－状态量$\text{x}＝[X,Y,\phi ,\dot{\phi },\dot{x},\dot{y}{]}^T$和控制量$u=[a,{\delta }_f{]}^T$满足：
$\dot{{\text{x}}_r}=f_t({\text{x}}_r,u_r)$
在任意参考点－$({\text{x}}_r,u_r))$处对上式进行一阶泰勒展开，得到如下线性时变系统：
$\dot{\text{x}}=f({\text{x}}_r,{u\text{u}u}_r)+J_f({\text{x}}_r)+J_f({\text{u}}_r)(\text{u}-{\text{u}}_r)$
$$\begin{gathered} J_f({\text{x}}_t)=\frac{\partial f}{\partial \text{x}}{|}_{\text{x}={\text{x}}_t,u=u_t} \\ ={\text{A}}_t \\ =\begin{array}{c}\left[\begin{array}{cccccc}0& -1& -{\dot{x}}_{t}sin{\phi }_t-{\dot{y}}_{t}cos{\phi }_t& 0& cos{\phi }_t& -sin{\phi }_t\\ 0& 0& {\dot{x}}_{t}cos{\phi }_t-{\dot{y}}_{t}sin{\phi }_t& 0& sin{\phi }_t& cos{\phi }_t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& -2\frac{a^2{C}_{cf}+b^2{C}_{cr}}{{\text{I}}_z{\dot{x}}_t}& \frac{\partial f_{\dot{{\phi }_t}}}{\partial {\dot{x}}_t}& 2\frac{b{C}_{cr}-a{C}_{cf}}{{\text{I}}_z{\dot{x}}_t}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 2(\frac{b{C}_{cr}-a{C}_{cf}}{m{\dot{x}}_t}-{\dot{x}}_t)& \frac{\partial f_{{\dot{y}}_t}}{\partial {\dot{x}}_t}& -2\frac{C_{cf}+C_{cr}}{m{\dot{x}}_t}\end{array}\right]\end{array} \end{gathered}$$
其中： $\frac{\partial f_{\dot{\phi }}}{\partial \dot{x}}=\frac{2}{I_z}(-\frac{b{C}_{cr}-a{C}_{cf}}{{\dot{x}}^2}\dot{y}+\frac{a^2{C}_{cf}+b^2{C}_{cr}}{{\dot{x}}^2}\dot{\phi })$
$\frac{\partial f_{\dot{y}}}{\partial \dot{x}}=2\frac{C_{cf}+C_{cr}}{m{\dot{x}}^2}\dot{y}-2\frac{b{C}_{cr}-a{C}_{cf}}{m{\dot{x}}^2}\dot{\phi }-\dot{\phi }$
$$\begin{gathered} J_f(u_r)=\frac{\partial f}{\partial u\text{u}u}{|}_{\text{x}={\text{x}}_r,u=u_r} \\ ={\text{B}}_t={\begin{array}{c}\left[\begin{array}{cccccc}0& 0& 0& 0& 1& 0\\ 0& 0& 0& \frac{2}{{\text{I}}_z}a{C}_{cf}& 0& \frac{C_{c}f}{m}\end{array}\right]\end{array}}^T \end{gathered}$$
对于以上我们建立的车辆动力学模型

将上两式相减得到：
$$\begin{gathered} \dot{\tilde{\text{x}}}=J_f({\text{x}}_r)\tilde{\text{x}}+J_f({\text{u}}_r)(\tilde{\text{u}}) \\ \dot{\tilde{\text{x}}}={\text{A}}_t\tilde{\text{x}}+{\text{B}}_t(\tilde{\text{u}}) \end{gathered}$$
式中：$\tilde{\text{x}}=\text{x}-{\text{x}}_r,\tilde{\text{u}}=\text{u}-{\text{u}}_r,{\text{A}}_t=J_f({\text{x}}_r),{\text{B}}_t=J_f({\text{u}}_r)$。
上式为基于参考系统的联系状态方程，不能直接用于模型预测控制器的设计，需要对其进行离散化:
$$\tilde{\text{x}}(k+1)={\text{A}}_{k,t}\tilde{\text{x}}(k)+{\text{B}}_{k,t}\tilde{\text{u}}(k)$$
其中：${\text{A}}_{k,t}={\text{I}}_n+T\text{A}(t),{\text{B}}_{k,t}=T\text{B}(t)$。${\text{I}}_n$为单位矩阵，$T$为离散系统的采样时间。

为了将系统的控制量转变为设
$$\zeta \text{ζ}\zeta (k)=\begin{array}{c}\left[\begin{array}{c}\tilde{\text{x}}(k)\\ \tilde{\text{u}}(k-1)\end{array}\right]\end{array}$$
有
$$\begin{gathered} \zeta \text{ζ}\zeta (k+1)={\tilde{\text{A}}}_{k,t}\zeta \text{ζ}\zeta (k)+{\tilde{\text{B}}}_{k,t}△\text{△}△\text{u}(k) \\ \eta \text{η}\eta (k)={\tilde{\text{C}}}_{k,t}\zeta \text{ζ}\zeta (k) \end{gathered}$$
式中，${\tilde{\text{A}}}_{k,t}=\begin{array}{c}\left[\begin{array}{cc}{\text{A}}_{k,t}& {\text{B}}_{k,t}\\ {\text{0}}_{n\ast m}& {\text{I}}_n\end{array}\right]\end{array},{\tilde{\text{B}}}_{k,t}=\begin{array}{c}\left[\begin{array}{c}{\text{B}}_{k,t}\\ {\text{I}}_n\end{array}\right]\end{array},△\text{△}△\text{u}(k)=\begin{array}{c}\left[\begin{array}{c}△\text{△}△\text{a}(k)\\ △\text{△}△\delta (k)\end{array}\right]\end{array}$
m为状态量的个数，n为控制量的个数。
如果认为在预测时域内都等于当前系统输出状态处线性化的模型，则可以假设：
$${\text{A}}_{k,t}={\text{A}}_{t,t},k=1,2,...,N.t为系统的当前时刻$$
系统的输出表达式为：
$$\text{Y}(t)={\Phi \text{Φ}\Phi }_t\zeta \text{ζ}\zeta (t)+{\Theta \text{Θ}\Theta }_t△\text{△}△\text{U}(t)$$
式中：
$$\begin{gathered} \text{Y}(t)=\begin{array}{c}\left[\begin{array}{c}\eta \text{η}\eta (t+1|t)\\ ...\\ \eta \text{η}\eta (t+N_p|t)\\ ...\\ \eta \text{η}\eta (t+N_p|t)\end{array}\right]\end{array}{\Phi \text{Φ}\Phi }_t=\left[\begin{array}{c}\tilde{\text{C}}{\tilde{\text{A}}}_{t,t}\\ ...\\ \tilde{\text{C}}{\tilde{\text{A}}}_{t,t}^{N_c}\\ ...\\ \tilde{\text{C}}{\tilde{\text{A}}}_{t,t}^{N_p}\end{array}\right] \\ {\Theta \text{Θ}\Theta }_t=\left[\begin{array}{cccc}\tilde{\text{C}}{\tilde{\text{B}}}_{t,t}& \text{0}& & \text{0}\\ & & \cdots & \\ \tilde{\text{C}}{\tilde{\text{A}}}_{t,t}{\tilde{\text{B}}}_{t,t}& \tilde{\text{C}}{\tilde{\text{B}}}_{t,t}& & ⋮\\ & & \ddots & \\ ⋮& ⋮& & \\ \tilde{\text{C}}{\tilde{\text{A}}}_{t,t}^{N_p-1}{\tilde{\text{B}}}_{t,t}& \tilde{\text{C}}{\tilde{\text{A}}}_{t,t}^{N_p-2}{\tilde{\text{B}}}_{t,t}& & \tilde{\text{C}}{\tilde{\text{A}}}_{t,t}^{N_p-N_c-1}{\tilde{\text{B}}}_{t,t}\\ & & \cdots & \end{array}\right]△\text{△}△\text{U}(t)=\left[\begin{array}{c}u\text{u}u(t+1|t)\\ u\text{u}u(t+2|t)\\ ...\\ u\text{u}u(t+N_c|t)\end{array}\right] \end{gathered}$$
$Np--20Nc--5--0.02ssteps$
做了很多近似，1.在控制周期和预测周期内都使用此刻的模型
– 看下百度控制周期和预测控制步长