单车模型及其线性化

1 单车模型

  这里讨论的是以后轴为中心的单车运动学模型，由下式表达：

S ˙ = [ x ˙ y ˙ ψ ˙ ] = [ v c o s ( ψ ) v s i n ( ψ ) v t a n ( ψ ) L ] \dot S = \begin{bmatrix} \dot x\\ \dot y\\ \dot \psi \\ \end{bmatrix} = \begin{bmatrix} vcos(\psi)\\ vsin(\psi)\\ v\frac{tan(\psi)}{L} \\ \end{bmatrix} S˙= ​x˙y˙​ψ˙​​ ​= ​vcos(ψ)vsin(ψ)vLtan(ψ)​​ ​

其中$S$代表系统状态，$\delta$为单车模型的前轮转角，$x,y$是车辆在世界坐标系下的位置，$L$为轴距，$v$为车辆速度，$\psi$代表航向角，单车模型中应该是假设了质心侧偏角为0，所以$\psi$应该也等于横摆角，即车头朝向。

  如果把加速度和前轮转角的变化加上，则可以写成：
S ˙ = [ x ˙ y ˙ ψ ˙ v ˙ δ ˙ ] = [ v c o s ( ψ ) v s i n ( ψ ) v t a n ( ψ ) L a γ ] = [ f 1 f 2 f 3 f 4 f 5 ] = f ( S ) \dot S = \begin{bmatrix} \dot x\\ \dot y\\ \dot \psi \\ \dot v \\ \dot \delta \end{bmatrix} = \begin{bmatrix} vcos(\psi)\\ vsin(\psi)\\ v\frac{tan(\psi)}{L} \\ a \\ \gamma \end{bmatrix} = \begin{bmatrix} f_1\\ f_2\\ f_3 \\ f_4 \\ f_5 \end{bmatrix} =f(S) S˙= ​x˙y˙​ψ˙​v˙δ˙​ ​= ​vcos(ψ)vsin(ψ)vLtan(ψ)​aγ​ ​= ​f1​f2​f3​f4​f5​​ ​=f(S)
这里$\gamma$是我自己取的名字，看别的博客把速度和前轮转角用额外的矩阵表示了，这里我放在一起推一推。

2 线性化

  将上述系统$\dot{S}=f(S)$在 S = S r = [ x r y r ψ r v r δ r ] S = S_r = \begin{bmatrix}x_r\\y_r\\\psi_r\\v_r\\\delta_r\end{bmatrix} S=Sr​= ​xr​yr​ψr​vr​δr​​ ​处泰勒展开，忽略高次项：

$$\dot{S}\approx f(S_r)+\frac{df}{dS}(S-S_r)$$

进而有：
S ˙ − S ˙ r = ( d f 1 d x d f 1 d y d f 1 d ψ d f 1 d v d f 1 d δ d f 2 d x d f 2 d y d f 2 d ψ d f 2 d v d f 2 d δ d f 3 d x d f 3 d y d f 3 d ψ d f 3 d v d f 3 d δ d f 4 d x d f 4 d y d f 4 d ψ d f 4 d v d f 4 d δ d f 5 d x d f 5 d y d f 5 d ψ d f 5 d v d f 5 d δ ) [ x − x r y − y r ψ − ψ r v − v r δ − δ r ] \dot S - \dot S_r = \begin{pmatrix} \frac{df_1}{dx} & \frac{df_1}{dy} & \frac{df_1}{d\psi} & \frac{df_1}{dv} & \frac{df_1}{d\delta} \\ \frac{df_2}{dx} & \frac{df_2}{dy} & \frac{df_2}{d\psi} & \frac{df_2}{dv} & \frac{df_2}{d\delta} \\ \frac{df_3}{dx} & \frac{df_3}{dy} & \frac{df_3}{d\psi} & \frac{df_3}{dv} & \frac{df_3}{d\delta} \\ \frac{df_4}{dx} & \frac{df_4}{dy} & \frac{df_4}{d\psi} & \frac{df_4}{dv} & \frac{df_4}{d\delta} \\ \frac{df_5}{dx} & \frac{df_5}{dy} & \frac{df_5}{d\psi} & \frac{df_5}{dv} & \frac{df_5}{d\delta} \\ \end{pmatrix} \begin{bmatrix}x - x_r\\y - y_r\\\psi - \psi_r\\v - v_r\\\delta - \delta_r\end{bmatrix} S˙−S˙r​= ​dxdf1​​dxdf2​​dxdf3​​dxdf4​​dxdf5​​​dydf1​​dydf2​​dydf3​​dydf4​​dydf5​​​dψdf1​​dψdf2​​dψdf3​​dψdf4​​dψdf5​​​dvdf1​​dvdf2​​dvdf3​​dvdf4​​dvdf5​​​dδdf1​​dδdf2​​dδdf3​​dδdf4​​dδdf5​​​ ​ ​x−xr​y−yr​ψ−ψr​v−vr​δ−δr​​ ​

求导可得：
S ˙ − S ˙ r = ( 0 0 − v s i n ( ψ ) c o s ( ψ ) 0 0 0 v c o s ( ψ ) s i n ( ψ ) 0 0 0 0 t a n ( δ ) L v L c o s 2 ( δ ) 0 0 0 0 0 0 0 0 0 0 ) [ x − x r y − y r ψ − ψ r v − v r δ − δ r ] = J Δ S \dot S - \dot S_r = \begin{pmatrix} 0 & 0 & -vsin(\psi) & cos(\psi) & 0 \\ 0 & 0 & vcos(\psi) & sin(\psi) & 0 \\ 0 & 0 & 0 & \frac{tan(\delta)}{L} & \frac{v}{Lcos^2(\delta)} \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ \end{pmatrix}\begin{bmatrix}x - x_r\\y - y_r\\\psi - \psi_r\\v - v_r\\\delta - \delta_r\end{bmatrix} = J\Delta S S˙−S˙r​= ​00000​00000​−vsin(ψ)vcos(ψ)000​cos(ψ)sin(ψ)Ltan(δ)​00​00Lcos2(δ)v​00​ ​ ​x−xr​y−yr​ψ−ψr​v−vr​δ−δr​​ ​=JΔS

最后可得线性化表示

$$\Delta \dot{S}=\dot{S}-{\dot{S}}_r=J\Delta S$$

在实际应用中需要把它写成差分的形式，假设采样时间间隔为$\Delta t$，则有：

$$\frac{\Delta S_{k+1}-\Delta S_k}{\Delta t}=J\Delta S_k$$
最后得到车辆状态变化的差分方程形式
$$\Delta S_{k+1}=(J\Delta t+I)\Delta S_k$$

3 实现效果

  现在假设采样时间为0.1秒，车辆初始状态在原点，车头朝向是45度，运行1.5秒，分别用单车模型和线性化方法实现两个单车模型，看看跑出来的误差是怎样的。这里给的加速度是$4m/s^2$，前轮转角不变。效果如下红色是单车模型，绿色是线性化后的单车模型，这里的动画运行时间间隔是0.2s，方便看效果。可以看到在前1秒内这个线性化算出来的车辆状态和单车模型本身的误差还是可以的。

4 参考资料

• 无人车系统（一）：运动学模型及其线性化
• 汽车运动学模型的线性化推导过程
• 上面的视频制作软件是manim，3B1B的视频制作软件哦，用来自己做做简单的动画还是ok的