单车模型的线性化与离散化推导详解

写在前面

整个单车模型的线性化过程可以分为两个部分，一个部分是在参考轨迹点附近利用Taylor展开将非线性模型转为线性模型，另一个部分就是在参考时间点附近做Taylor展开将连续模型离散化

线性化过程

首先，我们可以写出单车模型的非线性模型为

\begin{cases}\dot x=v\cos(\varphi)=f_1\\\dot y=v\sin(\varphi)=f_2\\\dot\varphi=\frac vL\tan\delta_f=f_3\end{cases}

其中:

\dot{x} 表示车辆在世界坐标系中的x方向上的速度分量
\dot{y} 表示车辆在世界坐标系中的y方向上的速度分量
\dot\varphi 表示了车辆在世界坐标系中的角速度分量，\varphi代表的就是车辆在世界坐标系中的车头朝向
v 代表的是车辆在车头朝向方向上的线速度
\delta_f代表的是车辆前轮的方向

这里的单车模型对应的是阿克曼模型的小车，如果说是双阿克曼，只需要将\dot\varphi=\frac {v}{L}\tan\delta_f=f_3改为\dot\varphi=\frac {2v}{L}\tan\delta_f=f_3即可，推导参照阿克曼与双阿克曼模型正逆运动学以及状态空间方程即可

依旧是拿单车模型举例，我们可以选取状态量\boldsymbol{\chi}=[x,y,\varphi]^{T}，控制量为\mathbf{u}=[v,\delta_f]^T，则我们选取参考轨迹上一点r，我们可以得到在该点的状态量与控制量为:

\boldsymbol{\chi_r}=[x_r,y_r,\varphi_r]^{T} , \mathbf{u}=[v_r,\delta_{fr}]^T

因此我们可以得到参考点的状态空间方程为:

\dot{\boldsymbol{\chi}}=f(\boldsymbol{\chi},\mathbf{u})\Rightarrow\dot{\boldsymbol{\chi}}_r=f\left(\boldsymbol{\chi}_r,\mathbf{u}_r\right)

所以当我们在参考点进行进行Taylor展开后可以得到

\dot{\boldsymbol{\chi}}=f\left(\boldsymbol{\chi}_{r},\mathbf{u}_{r}\right)+\frac{\partial f(\boldsymbol{\chi},\mathbf{u})}{\partial\boldsymbol{\chi}}(\boldsymbol{\chi}-\boldsymbol{\chi}_{r})+\frac{\partial f(\boldsymbol{\chi},\mathbf{u})}{\partial\mathbf{u}}(\mathbf{u}-\mathbf{u}_{r})+\text{高阶项}

在这个地方，我们只保留Taylor展开的第一项和第二项，那么非线性的模型\dot{\boldsymbol{\chi}}=f(\boldsymbol{\chi},\mathbf{u})在参考点r附近就变成了线性的了

然后再计算雅可比矩阵\frac{\partial f(\boldsymbol{\chi},\mathbf{u})}{\partial\boldsymbol{\chi}}和\frac{\partial f(\boldsymbol{\chi},\mathbf{u})}{\partial\mathbf{u}}，可以得到：

\begin{gathered}\frac{\partial f(\boldsymbol{\chi},\mathbf{u})}{\partial\boldsymbol{\chi}}=\begin{bmatrix}\frac{\partial f_1}{\partial x} & \frac{\partial f_1}{\partial y} & \frac{\partial f_1}{\partial\varphi}\\ \frac{\partial f_2}{\partial x} & \frac{\partial f_2}{\partial y} & \frac{\partial f_2}{\partial\varphi}\\ \frac{\partial f_3}{\partial x} & \frac{\partial f_3}{\partial y} & \frac{\partial f_3}{\partial\varphi}\end{bmatrix}=\begin{bmatrix}0 & 0 & -v_{r}\sin\varphi_{r}\\ 0 & 0 & v_{r}\cos\varphi_{r}\\ 0 & 0 & 0\end{bmatrix}\\ \frac{\partial f(\boldsymbol{\chi},\mathbf{u})}{\partial\mathbf{u}}=\begin{bmatrix}\frac{\partial f_1}{\partial v} & \frac{\partial f_1}{\partial\delta}\\ \frac{\partial f_2}{\partial v} & \frac{\partial f_2}{\partial\delta}\\ \frac{\partial f_3}{\partial v} & \frac{\partial f_3}{\partial\delta}\end{bmatrix}=\begin{bmatrix}\cos\varphi_{r} & 0\\ \sin\varphi_{r} & 0\\ \frac{\tan\delta_r}{L} & \frac{v_r}{L\cos^2\delta_r}\end{bmatrix}\end{gathered}

则状态量误差可以表示为\dot{\tilde{\chi}}，有：

\begin{aligned}\dot{\tilde{\chi}} & =\begin{bmatrix}\dot{x}-\dot{x}_{r}\\ \dot{y}-\dot{y}_{r}\\ \dot{\varphi}-\dot{\varphi}_{r}\end{bmatrix}\\ & =\begin{bmatrix}0 & 0 & -v_{r}\sin\varphi_{r}\\ 0 & 0 & v_{r}\cos\varphi_{r}\\ 0 & 0 & 0\end{bmatrix}\begin{bmatrix}x-x_{r}\\ y-y_{r}\\ \varphi-\varphi_{r}\end{bmatrix}+\begin{bmatrix}\cos\varphi_{r} & 0\\ \sin\varphi_{r} & 0\\ \frac{\tan\delta_r}{L} & \frac{v_{r}}{L\cos^2\delta_{r}}\end{bmatrix}\begin{bmatrix}v-v_{r}\\ \delta-\delta_{r}\end{bmatrix}\\ & =A\tilde{\chi}+B\tilde{\mathbf{u}}\end{aligned}

这里的状态量误差指的其实就是当前状态与参考点状态之间的差值

也可以理解为需要达到某一个点时，直接将当前点与目标点的差值作为状态量，输入的差值作为控制量，这样可以达到一个根据通过增量控制的效果，更符合真实的控制情况，并且在之后规划问题的目标函数中，也会更加方便。

离散化过程

离散化过程就是对上述的状态空间方程\dot{\tilde{\chi}}=A\tilde{\chi}+B\tilde{\mathbf{u}}做前向欧拉离散过程

前向欧拉离散化

令\dot{x}=f(x(t),u(t))在t=kT处做Taylor展开为：

x(t+T) = x(t)+T\dot{x}(t)+\text{高阶项}

与上面同理，为了线性化，我们依然选择阶段高阶项，从而获得x(t+T) = x(t)+T\dot{x}(t)

离散化推导

设时间步长(采样步长)为T，有：

{\tilde{\chi}}(k+1) = {\tilde{\chi}}(k)+T\dot{\tilde{\chi}}(k) \\ {\tilde{\chi}}(k+1) = {\tilde{\chi}}(k)+T({A\tilde{\chi}}(k)+B\tilde{\mathbf{u}}(k)) \\ {\tilde{\chi}}(k+1) = (I+TA){\tilde{\chi}}(k)+B\tilde{\mathbf{u}}(k) \\

其中，I为单位矩阵，维度与矩阵A一致

最终求解可得：

\begin{aligned}\tilde{\chi}(k+1) & =(TA+I)\tilde{\chi}(k)+TB\tilde{\mathbf{u}}(k)\\ & =\begin{bmatrix}1 & 0 & -Tv_{r}\sin\varphi_{r}\\ 0 & 1 & Tv_{r}\cos\varphi_{r}\\ 0 & 0 & 1\end{bmatrix}\tilde{\chi}(k)+\begin{bmatrix}T\cos\varphi_{r} & 0\\ T\sin\varphi_{r} & 0\\ \frac{T\tan\delta_r}{L} & \frac{v_rT}{L\cos^2\delta_r}\end{bmatrix}\tilde{\mathbf{u}}(k)\\ & =\tilde{A}\tilde{\chi}(k)+\tilde{B}\tilde{\mathbf{u}}(k)\end{aligned}

tips

原先我一直以为线性化之后的模型的输出就是保持一条直线的，最紧把代码写出来之后发现并不是这样的，我们可以把上面的最终公式写为

\begin{bmatrix}x_{k+1}-x_{r,k+1}\\y_{k+1}-y_{r,k+1}\\\varphi_{k+1}-\varphi_{r,k+1}\end{bmatrix}=\begin{bmatrix}x_k-x_{r,k}\\y_k-y_{r,k}\\\varphi_k-\varphi_{r,k}\end{bmatrix}+T\cdot\begin{bmatrix}-v_r\sin\varphi_{r,k}(\varphi_k-\varphi_{r,k})+\cos\varphi_{r,k}(v_k-v_{r,k})\\v_r\cos\varphi_{r,k}(\varphi_k-\varphi_{r,k})+\sin\varphi_{r,k}(v_k-v_{r,k})\\\frac{\tan\delta_{r,k}}{L}(v_k-v_{r,k})+\frac{v_{r,k}}{L\cos^2\delta_{r,k}}(\delta_k-\delta_{r,k})\end{bmatrix}

这样就可以发现，如果(\varphi_{k}-\varphi_{r,k})、(v_k-v_{r,k})和(\delta_k-\delta_{r,k})越接近于0，也就是状态越接近参考点，模型越接近线性，反之，越远离参考点则会表现出非线性

参考