车辆运动学模型的线性化及其离散化

一、数学基础：泰勒展开

从一元函数的泰勒展开说起

一元函数在$x_0$处的泰勒展开表达式为：
$$f(x)=f(x_0)+{f(x_0)}^{\prime }(x-x_0)+\frac{1}{2!}{f(x_0)}^{\prime \prime }(x-x_0)+o^n$$
那么同理，二元函数的泰勒展开为：
$$f(x,y)=f(x_0,y_0)+f_x^{{}^{\prime }}(x_0,y_0)(x-x_0)+f_y^{{}^{\prime }}(x_0,y_0)(y-y_0)+\frac{1}{2!}{f}_{xx}^{{}^{\prime \prime }}(x_0,y_0)(x-x_0{)}^2+\frac{1}{2!}{f}_{yy}^{{}^{\prime \prime }}(x_0,y_0)(y-y_0{)}^2+o^n$$
多元函数呢？
$$f(x^1,x^2,...,x^n)=f(x_0^1,x_0^2,...x_0^n)+\sum _{i=1}^n{f}_{x^i}^{{}^{\prime }}(x_0^1,x_0^2,...x_0^n)(x^i-x_0^i)+\frac{1}{2!}\sum _{i,j=1}^n{f}_{x^i{x}^j}^{{}^{\prime \prime }}(x_0^1,x_0^2,...x_0^n)(x^i-x_0^i)(x^j-x_0^j)+o^n$$
把泰勒展开式写成矩阵的形式：
$$f(X)=f(X_0)+[\nabla f(X_0){]}^T[X-X_0]+\frac{1}{2!}[X-X_0{]}^{T}H[X-X_0]+o^n$$

为方便后续讲解，上式中$X$是一个列向量为 $$X=[x_1,x_2,...x_n{]}^T$$

是不是感觉突然变成矩阵形式就开始有点紧张了，大可不必，只需要牢牢的记住$dY/dX$矩阵求导的本质就是Y中的每个元素对X中的每个元素进行求导。这里只讲$YX$拉伸，需要记住以下两个要点：
要点1：标量不变，向量拉伸
要点2：前面横向拉，后面纵向拉。
注意：这两点在后面的会用到。

如果还是感觉有点模棱两可，可以参考这个视频，讲得很详细。

二、车辆运动学模型线性化

有了上面的铺垫，这里看一个车辆运动学模型线性化的例子。这里直接给出车辆的运动学模型：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}cos\phi \\ sin\phi \\ \frac{tan\delta }{l}\end{array}\right]v$$

对上式进行简化为$\dot{X}=f(X,u)$，其中 $X=[x,y,\phi {]}^T$，$u=[v,\delta {]}^T$。
$$\dot{X}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}cos\phi \\ sin\phi \\ \frac{tan\delta }{l}\end{array}\right]v=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=f(X,u)$$

对上式在参考点$X_r=[x_r,y_r,{\phi }_r{]}^T$进行一阶泰勒展开，可以得到：
$$\dot{X}=\dot{X_r}+[f_X^{{}^{\prime }}(X_r,u_r){]}^T[X-X_r]+[f_u^{{}^{\prime }}(X_r,u_r){]}^T[u-u_r]$$

关键的步骤来了

根据要点2，$Y$横向拉伸，即把$f(X,u)$横向拉成一个行向量，可以得到：
$f_X^{{}^{\prime }}(X,u)=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial X}& \frac{\partial f_2}{\partial X}& \frac{\partial f_3}{\partial X}\end{array}\right]$

$X$纵向拉伸，即把$X$拉伸为一个列向量，$X$已经是一个列向量。根据要点1，标量不变，向量拉伸，$f_1，f_2，f_3$不变，把$X$进行拉伸，可以得到：
$f_X^{{}^{\prime }}(X,u)=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x}& \frac{\partial f_2}{\partial x}& \frac{\partial f_3}{\partial x}\\ \frac{\partial f_1}{\partial y}& \frac{\partial f_2}{\partial y}& \frac{\partial f_3}{\partial y}\\ \frac{\partial f_1}{\partial \phi }& \frac{\partial f_2}{\partial \phi }& \frac{\partial f_3}{\partial \phi }\end{array}\right]$

结合泰勒展开式写成矩阵的形式，注意转置：
$$f(X)=f(X_0)+[\nabla f(X_0){]}^T[X-X_0]$$
可以得到：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}\dot{x_r}\\ \dot{y_r}\\ \dot{{\phi }_r}\end{array}\right]+\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x}& \frac{\partial f_1}{\partial y}& \frac{\partial f_1}{\partial \phi }\\ \frac{\partial f_2}{\partial x}& \frac{\partial f_2}{\partial y}& \frac{\partial f_2}{\partial \phi }\\ \frac{\partial f_3}{\partial x}& \frac{\partial f_3}{\partial y}& \frac{\partial f_3}{\partial \phi }\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}\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{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]$$
带入计算可得：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}\dot{x_r}\\ \dot{y_r}\\ \dot{{\phi }_r}\end{array}\right]+\left[\begin{array}{ccc}0& 0& -v_{r}sin{\phi }_r\\ 0& 0& v_{r}cos{\phi }_r\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}cos{\phi }_r& 0\\ sin{\phi }_r& 0\\ \frac{tan{\delta }_r}{l}& \frac{v_r}{lco{s}^2{\delta }_r}\end{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]$$
至此完成了车辆运动学模型的线性化，至于如何将该连续系统状态空间方程离散化，可以参看这篇拙作。