四轮移动机器人(小车)数学建模

写在前面：
  好快啊转眼间就研二了，最近自己的第二篇SCI理论工作也快完成了，在整理笔记的时候突然想起自己自从本科做智能车之后好久都没更新自己的CSDN博客了，因此新开一个专栏用于记录自己做项目和发文章的过程中学习的理论知识，博主目前的研究方向为机器人的决策规划与强化学习，欢迎大家交流分享。由于当时研一学习的时候是从机器人的建模开始的，因此这里也从机器人建模开始吧。顺便附带一个专栏目录传送门：

理论笔记专栏目录

  好了介绍结束下面开始进入正题：

1 非线性模型推导

​   在各种基于模型的控制方法中，例如MPC，均需要对控制对象建模以便于后续的分析与编程。所谓建模就是将现实中的机器人模型抽象为可用于编程参考的离散状态空间表达式：${\mathbf{x}}_{(k+1)}=A{\mathbf{x}}_{(k)}+B{\mathbf{u}}_{(k)}$，其中$\mathbf{x}$为机器人的状态向量，$\mathbf{u}$为输入控制向量，$A$为状态矩阵，$B$为控制矩阵，下标$k$表示时刻。

​   由于研究项目以四轮机器人为主，因此本节建模分析对象选取为四轮机器人，四轮机器人在世界坐标系$（XOY）$下的建模简图如下所示：

​
  首先确定机器人的状态量和输入量：仅考虑机器人的平面运动，一般选取机器人的后轴中心坐标以及车辆航向角为状态变量，即：$\mathbf{x}=[x,y,\phi {]}^T$，一般我们是通过控制机器人的速度以及转角来控制机器人的运动，因此选取前进速度$v$和转角$\delta$作为输入控制向量，即：$\mathbf{u}=[v,\delta {]}^T$

​   再来确定$\dot{\mathbf{x}}$与$\mathbf{x}$和$\mathbf{u}$的关系，在图中由简单的几何关系可推出：
$$\{\begin{array}{l}v=v_{f}cos\delta \\ v_{\phi }=v_{f}sin\delta \end{array}\Rightarrow v_{\phi }=vtan\delta$$
​ 因此有：
$$\{\begin{array}{l}\dot{x}=vcos\phi \\ \dot{y}=vsin\phi \\ \dot{\phi }=v_{\phi }/L=vtan\delta /L\end{array}\Rightarrow \dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$$
​ 至此就得到了机器人的非线性模型$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$

2 模型线性化

​   下面来试着将非线性模型$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$化为(近似为)线性模型$\dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}$，先将$f$在参考(目标)状态${\mathbf{x}}_r$与参考输入${\mathbf{u}}_r$处进行一阶泰勒展开：
$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})\approx f({\mathbf{x}}_r,{\mathbf{u}}_r)+\frac{\partial f}{\partial \mathbf{x}}{|}_{(x_r,u_r)}(\mathbf{x}-{\mathbf{x}}_r)+\frac{\partial f}{\partial \mathbf{u}}{|}_{(x_r,u_r)}(\mathbf{u}-{\mathbf{u}}_r)$$
​   可见非线性模型$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$无法化为(近似为)线性模型$\dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u}$，但我们可以令其误差通过泰勒展开近似为线性模型，即$\dot{{\mathbf{x}}_e}=A{\mathbf{x}}_e+B{\mathbf{u}}_e$，其中${\mathbf{x}}_e=\mathbf{x}-{\mathbf{x}}_r$表示状态误差向量， ${\mathbf{u}}_e=\mathbf{u}-{\mathbf{u}}_r$表示输入误差向量，由此可得：
$$\begin{gathered} \dot{{\mathbf{x}}_e}=\dot{\mathbf{x}}-\dot{{\mathbf{x}}_r}=f(\mathbf{x},\mathbf{u})-f({\mathbf{x}}_r,{\mathbf{u}}_r)=\frac{\partial f}{\partial \mathbf{x}}{|}_{(x_r,u_r)}(\mathbf{x}-{\mathbf{x}}_r)+\frac{\partial f}{\partial \mathbf{u}}{|}_{(x_r,u_r)}(\mathbf{u}-{\mathbf{u}}_r) \\ =\frac{\partial f}{\partial \mathbf{x}}{|}_{(x_r,u_r)}{\mathbf{x}}_e+\frac{\partial f}{\partial \mathbf{u}}{|}_{(x_r,u_r)}{\mathbf{u}}_e=A{\mathbf{x}}_e+B{\mathbf{u}}_e \end{gathered}$$
​ 其中：
$$\begin{gathered} A=\frac{\partial f}{\partial \mathbf{x}}{|}_{(x_r,u_r)}=\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}{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}{\partial \mathbf{u}}{|}_{(x_r,u_r)}=\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}{cc}cos{\phi }_r& 0\\ sin{\phi }_r& 0\\ tan{\delta }_r/L& v_{r}se{c}^2{\delta }_r/L\end{array}\right] \end{gathered}$$
​ 至此就得到了机器人的误差线性模型$\dot{{\mathbf{x}}_e}=A{\mathbf{x}}_e+B{\mathbf{u}}_e$

​

3 模型离散化

​   最后再将线性模型$\dot{{\mathbf{x}}_e}=A{\mathbf{x}}_e+B{\mathbf{u}}_e$离散化为${\mathbf{x}}_{e(k+1)}=A^{\prime }{\mathbf{x}}_{e(k)}+B^{\prime }{\mathbf{u}}_{e(k)}$，即可用于后续的算法分析与编程。假设采样间隔为$T$，利用前项欧拉法进行离散化，有：
$$\dot{{\mathbf{x}}_e}=\frac{x_{e(k+1)}-x_{e(k)}}{T}=A{x}_{e(k)}+B{u}_{e(k)}$$

​ 上式整理即得：
$$x_{e(k+1)}=(I+TA)x_{e(k)}+TB{u}_{e(k)}=A^{\prime }{x}_{e(k)}+B^{\prime }{u}_{e(k)}$$
​ 其中:
$$\begin{gathered} A^{\prime }=I+TA=\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] \\ {B}^{\prime }=TB=\left[\begin{array}{cc}Tcos{\phi }_r& 0\\ Tsin{\phi }_r& 0\\ Ttan{\delta }_r/L& T{v}_{r}se{c}^2{\delta }_r/L\end{array}\right] \end{gathered}$$
​ 至此就得到了机器人的误差线性离散模型${\mathbf{x}}_{e(k+1)}=A^{\prime }{\mathbf{x}}_{e(k)}+B^{\prime }{\mathbf{u}}_{e(k)}$