运动学建模、线性化、离散化过程，是使用控制算法进行控制前常用的操作，本文主要以车辆/单车模型为例，详细介绍运动学建模、线性化、离散化等过程，为后续的使用LQR、MPC等控制算法进行轨迹跟踪或其他控制任务做准备。

   一、车辆/单车 运动学建模

   设机器人当前的位姿为$（x_c,y_c,{\phi }_c）$, 其中$（x_c,y_c,）$ 为其当前位置坐标，${\phi }_c$为其当前姿态角，当前前轮的转向角为${\delta }_c$,当前的后轮车速为$v_c$，前轮车速为$v_f$，前后轮距为$l$，默认采用后轮驱动模式，即可控的是后轮车速$v_c$，如下图所示：

   容易得到$v_c$在x轴和y轴的的分量为别为$v_{c}cos({\phi }_c)$、$v_{c}sin({\phi }_c)$，即：

   $$\begin{gathered} \dot{x_c}=v_{c}cos({\phi }_c) \\ \dot{y_c}=v_{c}sin({\phi }_c) \end{gathered}$$

   刚体在同一方向上的速度是相同的，即前轮车速在车辆当前移动方向$v_c$上的分量应该与$v_c$相同，即$v_{fx}=v_c$，这个分量与车辆运行方向相同，对车辆的转向无作用，导致车辆运行方向改变的是分量$v_{fy}=v_{c}tan({\delta }_c)$,将其视为车辆绕后轮旋转的线速度，除以半径$l$，即为旋转角速度，如下所示：

   $$\dot{{\phi }_c}=\frac{v_{c}tan({\delta }_c)}{l}$$

   所以，我们可以得到完整的单车运动学模型的状态空间表达式如下：

   $$\{\begin{array}{l}\dot{x_c}=v_{c}cos({\phi }_c)\\ \dot{y_c}=v_{c}sin({\phi }_c)\\ \dot{{\phi }_c}=\frac{v_{c}tan({\delta }_c)}{l}\end{array}$$

   二、将状态空间表达式线性化

   第一部分中得到了运动学建模的状态空间表达式，然而它是非线性的，想要用来做控制，需要将其化为线性的，先将其写成向量的形式 ，取状态量$X_c=[x_c{y}_c{\phi }_c{]}^T$,控制输入为$u=[v_c{\delta }_c{]}^T$, 即前轮转向，后轮驱动。则向量形式的状态空间可写为(因为是非线性的，不能写成矩阵形式)：

   $$\dot{X}=f(X,u)$$

   最常用的近似线性化方法就是泰勒展开，在$x_0$点处展开的泰勒展开式如下所示：

   $$f\left(x\right)=\frac{f\left(x_0\right)}{0!}+\frac{f^{\prime }\left(x_0\right)}{1!}\left(x-x_0\right)+\frac{f^{\prime \prime }\left(x_0\right)}{2!}{\left(x-x_0\right)}^2+...+\frac{f^{\left(n\right)}\left(x_0\right)}{n!}{\left(x-x_0\right)}^n+R_n(x)$$

   使用泰勒展开进行线性化需要选择一个展开点，前面我们已经设定了当前位姿（状态）为$（x_c,y_c,{\phi }_c）$,现在补充设定目标位姿（状态）为$（x_r,y_r,{\phi }_r）$，将目标位姿作为展开点，对$f(X,u)$进行泰勒展开，展开到一阶线性项，如下所示：

   $$\dot{X}\approx f(X_r,u_r)+\frac{\partial f(X_r,u_r)}{\partial X_r}(X-X_r)+\frac{\partial f(X_r,u_r)}{\partial u_r}(u-u_r)$$

   即：

   $$\dot{X}-\dot{X_r}\approx \frac{\partial f(X_r,u_r)}{\partial X_r}(X-X_r)+\frac{\partial f(X_r,u_r)}{\partial u_r}(u-u_r)$$

   这里的线性化，并不是把非线性关系，转换为线性关系，而是通过展开，使其误差变为一个线性关系，如上式所示，展开后的误差是线性的， 所以我们可以取当前量与目标量（参考量）之间的差值为新的状态量和输入项，如下所示：

   $$\dot{\tilde{X}}=\left[\begin{array}{c}\dot{x_c}-{\dot{x}}_r\\ \dot{y_c}-{\dot{y}}_r\\ \dot{{\phi }_c}-{\dot{\phi }}_r\end{array}\right],\tilde{X}=\left[\begin{array}{c}{x}_c-x_r\\ y_c-y_r\\ {\phi }_c-{\phi }_r\end{array}\right],\tilde{\mathbf{u}}=\left[\begin{array}{c}{v}_c-v_r\\ {\delta }_c-{\delta }_r\end{array}\right]$$

   则上面得到的展开式可以写为：

   $$\dot{\tilde{X}}\approx \frac{\partial f(X_r,u_r)}{\partial X_r}\tilde{X}+\frac{\partial f(X_r,u_r)}{\partial u_r}\tilde{u}$$

   也就是我们常见的$\dot{X}=AX+Bu$的状态空间表达式的形式，成功完成了线性化，求矩阵A和B，也就是$\frac{\partial f(X_r,u_r)}{\partial X_r}$和$\frac{\partial f(X_r,u_r)}{\partial u_r}$实际上，也就是在求雅克比矩阵，如下所示：

   注：下式中的$f$是$f(X_r,u_r)$的简称

   $$\{\begin{array}{l}{f}_1=v_{r}cos({\phi }_r)\\ f_2=v_{r}sin({\phi }_r)\\ f_3=\frac{v_{r}tan({\delta }_r)}{l}\end{array}$$

   $$A=\frac{\partial f\left(X_r,u_r\right)}{\partial X_r}=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_r}& \frac{\partial f_1}{\partial y_r}& \frac{\partial f_1}{\partial {\phi }_r}\\ \frac{\partial f_2}{\partial x_r}& \frac{\partial f_2}{\partial y_r}& \frac{\partial f_2}{\partial {\phi }_r}\\ \frac{\partial f_3}{\partial x_r}& \frac{\partial f_3}{\partial y_r}& \frac{\partial f_3}{\partial {\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]$$

   $$B=\frac{\partial f\left(X_r,u_r\right)}{\partial u_r}=\left[\begin{array}{cc}\frac{\partial f_1}{\partial v_r}& \frac{\partial f_1}{\partial {\delta }_r}\\ \frac{\partial f_2}{\partial v_r}& \frac{\partial f_2}{\partial {\delta }_r}\\ \frac{\partial f_3}{\partial v_r}& \frac{\partial f_3}{\partial {\delta }_r}\end{array}\right]=\left[\begin{array}{cc}\cos {\phi }_r& 0\\ \sin {\phi }_r& 0\\ \frac{\tan {\delta }_r}{l}& \frac{v_r}{l{\cos }^2{\delta }_r}\end{array}\right]$$

   三、离散化

   为了方便使用递推式进行迭代，我们还需要对线性化后的结果进行离散化，用差商代替微分，如下所示：

   $$\dot{\tilde{X}}=\frac{\tilde{X}(k+1)-\tilde{X}(k)}{T}=A\tilde{X}(k)+B\tilde{u}(k)$$

   所以：

   $$\tilde{X}(k+1)=(I+TA)\tilde{X}(k)+TB\tilde{u}(k)$$

   我们定义$\tilde{A}=(I+TA)$, $\tilde{B}=TB$，则上式可写为：

   $$\tilde{X}(k+1)=\tilde{A}\tilde{X}(k)+\tilde{B}\tilde{u}(k)$$

   由前面推出的A和B的结果，容易得知：

   $$\tilde{A}=\left[\begin{array}{ccc}1& 0& -T{v}_r\sin {\phi }_r\\ 0& 1& T{v}_r\cos {\phi }_r\\ 0& 0& 1\end{array}\right]$$

   $$\tilde{B}=\left[\begin{array}{cc}T\cos {\phi }_r& 0\\ T\sin {\phi }_r& 0\\ \frac{T\tan {\delta }_r}{l}& \frac{T{v}_r}{l{\cos }^2{\delta }_r}\end{array}\right]$$

   至此，我们已经得到完整的离散化的状态空间表达式，上面式子中带有较多的差值符号 ~，这是因为在第二部分中将状态量和输入量分别取为$X=[xy\phi {]}^T$,控制输入为$u=[v\delta {]}^T$导致的， 所以，现在我们直接将状态量和输入量取为当前量与期望值的差值，即

   $$X=\left[\begin{array}{c}{x}_c-x_r\\ y_c-y_r\\ {\phi }_c-{\phi }_r\end{array}\right],\mathbf{u}=\left[\begin{array}{c}{v}_c-v_r\\ {\delta }_c-{\delta }_r\end{array}\right]$$

   此时：

   $$\begin{array}{rl}\mathbf{X}(\mathrm{k}+1)& =[\begin{array}{ccc}1& 0& -{\mathrm{Tv}}_{\mathrm{r}}\sin {\psi }_{\mathrm{r}}\\ 0& 1& {\mathrm{Tv}}_{\mathrm{r}}\cos {\psi }_{\mathrm{r}}\\ 0& 0& 1\end{array}]\mathbf{X}(\mathrm{k})+\left[\begin{array}{cc}\mathrm{T}\cos {\psi }_{\mathrm{r}}& 0\\ \mathrm{T}\sin {\psi }_{\mathrm{r}}& 0\\ \frac{\mathrm{T}\tan {\delta }_{\mathrm{r}}}{l}& \frac{T{\mathrm{v}}_{\mathrm{r}}}{l{\cos }^2{\delta }_{\mathrm{r}}}\end{array}\right]\mathbf{u}(\mathrm{k})\\ & =\mathrm{AX}(\mathrm{k})+\mathrm{Bu}(\mathrm{k})\end{array}$$

   有了上面的离散化线性运动学模型的状态空间方程，就可以基于此来使用LQR、MPC等控制算法进行轨迹跟踪控制或完成其他控制任务了

   参考资料：

   1、【控制】模型预测控制 MPC 【合集】Model Predictive Control