车辆模型——运动学模型

在机器人的研究领域中，移动机器人的系统建模与分析是极为关键的基础环节，本文以非完整约束的轮式移动机器人为研究对象进行建模。

约束及系统

在力学的世界里，约束是对系统位姿和运动的限制。约束的类型丰富多样

• 几何约束：主要限制系统的位姿，就像给系统的位置设定了边界，其表达式为$$f(p,t)=0^k$$ 其中，$p\in {\mathbb{R}}^n$代表系统位姿坐标向量，$t$ 表示时间，$k\in \mathbb{R}$ 表示约束个数。
• 运动约束：不仅仅对系统的位姿进行约束，同时对于各质点的速度也进行了约束，其表达式$$f(p,\dot{p},t)=0^k$$ 其中，$p\in {\mathbb{R}}^n$代表系统位姿坐标向量，$\dot{p}\in {\mathbb{R}}^n$代表系统位姿坐标向量，$t$ 表示时间，$k\in \mathbb{R}$表示约束个数。

“非完整”和“完整”约束源于近代分析力学，由德国物理学家赫兹提出，如果利用积分运算等数学方法，可以将运动约束转化为几何约束的形式，则该约束即为完整约束，反之则为非完整约束。

• 完整约束系统：仅含有完整约束的系统。会降低系统的位置空间自由度。
• 非完整约束系统：含有至少一个非完整约束的系统。不会降低系统的位置空间自由度，但会降低速度空间自由度。

来看典型移动机器人的简化示意图，它在X - Y二维平面上运动。

当机器人进行无滑动滚动运动时，其速度方向有着特定的限制，即速度始终沿后轮中心指向前轮中心的方向，并且不会出现垂直于车轴的移动趋势。由此可以得到约束
$$v_c=\dot{x}\sin (\phi )+\dot{y}\cos (\phi )=0$$

设定$q=(x,y,\phi {)}^T$，整理成运动约束矩阵形式为
$$A(q)\dot{q}=[\sin (\phi )-\cos (\phi )0]\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\phi }\end{array}\right]=0$$
其中，$x$表示后轮中心横坐标、$y$表示后轮中心纵坐标和$\phi$表示速度方向角。

经过分析发现，利用积分运算无法将这个运动约束转化为几何约束，所以这类移动机器人属于非完整约束系统。

移动机器人运动学模型（Kinematic Model）

为了确保机器人的安全并实现轨迹跟踪，我们需要明确障碍物和机器人的位姿、运动信息，还要建立全局坐标系来统一这些信息。

自行车模型

在轨迹跟踪控制研究中，常用基于前轮转向，后轮驱动的简易自行车模型来构建移动机器人模型。

$X$-$Y$空间为全局坐标，$(X_r,Y_r)$ 为后轮中心参考位置，$(X_f,Y_f)$ 为前轮中心参考位置，$l$ 为前后轮中心距离（即车长），$v_r$ 为机器人移动速度，$\phi$ 为机器人中心轴方向（即航向角），${\delta }_f$ 前轮转向角表示前轮与移动方向的偏转角度（逆时针为正），$(X_o,Y_o)$ 为移动机器人移动所围绕的圆心，$R$ 为移动机器人移动旋转半径。

对这个模型做出一些合理假设

• 左右侧车轮速度转角一致，这样可将左右侧车轮合并为一个车轮；
• 车辆行驶速度变化缓慢，忽略前后轴载荷；
• 车身及悬架系统为刚性系统；
• 车辆后轮负责驱动，前轮负责转向。

以移动机器人简化模型的后轮中心为基准点，通过一系列推导，可以得到其速度、前后轴运动学约束等关系式，进而得出移动机器人简单的运动学模型
$$\left[\begin{array}{c}{\dot{X}}_r\\ {\dot{Y}}_r\\ \dot{\phi }\end{array}\right]=\left[\begin{array}{c}{v}_r\cos (\phi )\\ v_r\sin (\phi )\\ \frac{v_r\tan \left({\delta }_f\right)}{l}\end{array}\right]$$

一般形式为 $\dot{p}=f(p,u)$ ，其中 $p=[X_r,Y_r,\varphi {]}^T$ 表示系统状态，$u=[v_r,{\delta }_f{]}^T$ 表示系统输入。

含有加速度 $a$ 的自行车模型

上述简单运动学模型将速度 $v_r$ 和前轮转向角 ${\delta }_f$ 作为输入，但在实际生活中，以无人车为例，驾驶员是通过油门和刹车改变车辆加速度来控制车辆的，速度不能突变。

因此，对模型进行了改进，把速度 $v_r$ 当作系统状态，引入加速度 $a_r$ 作为系统输入。设系统状态 $p=[X_r,Y_r,\phi ,v_r{]}^T$，系统输入 $u=[a_r,{\delta }_f{]}^T$，新系统模型 $\dot{p}=f(p,u)$ 设计如下：
$$\left[\begin{array}{c}{\dot{X}}_r\\ {\dot{Y}}_r\\ \dot{\phi }\\ {\dot{v}}_r\end{array}\right]=\left[\begin{array}{c}{v}_r\cos (\phi )\\ v_r\sin (\phi )\\ \frac{v_r\tan \left({\delta }_f\right)}{l}\\ a_r\end{array}\right]$$
将其表示为仿射非线性系统形式为
$$\dot{p}=f(p)+g(p)u=\left[\begin{array}{c}{v}_r\cos (\phi )\\ v_r\sin (\phi )\\ 0\\ 0\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& \frac{1}{l{\cos }^2({\delta }_f)}\\ 1& 0\end{array}\right]\left[\begin{array}{c}{a}_r\\ {\delta }_f\end{array}\right]$$

为了简化系统便于后续计算和仿真，在误差允许范围内，对模型进行线性化处理，比如假设${\delta }_f$数值较小，进行$\tan ({\delta }_f)\approx {\delta }_f$的化简。

系统偏差模型

在实现轨迹跟踪目标时，选择参考轨迹上的一点作为目标轨迹点，为了让机器人跟随目标轨迹点，需要将模型线性化并通过坐标变换将目标轨迹点移动至原点，得到系统偏差模型$\dot{\tilde{p}}=A\tilde{p}+B\tilde{u}$ ，通过控制状态偏差收敛到0，就能实现轨迹跟踪的控制目标。

在实现轨迹跟踪目标时，选择参考轨迹上的一点作为目标轨迹点，为了让机器人跟随目标轨迹点，需要将模型线性化并通过坐标变换将目标轨迹点移动至原点，得到系统偏差模型$\dot{\tilde{p}}=A\tilde{p}+B\tilde{u}$ ，通过控制状态偏差收敛到0，就能实现轨迹跟踪的控制目标。

取运动学模型
$$\dot{p}=\tilde{f}(p,u)=f(p)+g(p)u=\left[\begin{array}{c}{v}_r\cos (\phi )\\ v_r\sin (\phi )\\ \frac{v_r\tan \left({\delta }_f\right)}{l}\\ a_r\end{array}\right]$$

在实现轨迹跟踪的目标时，在参数已知的参考轨迹上选择一点 $(x_{ref},y_{ref})$ 作为目标轨迹点，满足
$${\dot{p}}_{ref}=\tilde{f}(p_{ref},u_{ref})$$

为了使机器人能够跟随目标轨迹点，使系统状态逐渐收敛至目标轨迹点状态，将模型进行线性化并通过坐标变换将目标轨迹点移动至原点。在系统本身状态处进行泰勒展开，省略高阶项只保留一阶展开，得
$$\dot{p}=\tilde{f}(p,u)=\tilde{f}(p_{ref},u_{ref})+\frac{\partial \tilde{f}(p,u)}{\partial p}{|}_{p=p_{ref}}(p-p_{ref})+\frac{\partial \tilde{f}(p,u)}{\partial u}{|}_{u=u_{ref}}(u-u_{ref})$$
其中，$\frac{\partial \tilde{f}}{\partial p}$，$\frac{\partial \tilde{f}}{\partial u}$ 为雅克比矩阵，具体内容为

$$\frac{\partial \tilde{f}(p,u)}{\partial p}=\left[\begin{array}{cccc}\frac{\partial {\tilde{f}}_1(p,u)}{\partial x}& \frac{\partial {\tilde{f}}_1(p,u)}{\partial y}& \frac{\partial {\tilde{f}}_1(p,u)}{\partial \phi }& \frac{\partial {\tilde{f}}_1(p,u)}{\partial v}\\ \frac{\partial {\tilde{f}}_2(p,u)}{\partial x}& \frac{\partial {\tilde{f}}_2(p,u)}{\partial y}& \frac{\partial {\tilde{f}}_2(p,u)}{\partial \phi }& \frac{\partial {\tilde{f}}_2(p,u)}{\partial v}\\ \frac{\partial {\tilde{f}}_3(p,u)}{\partial x}& \frac{\partial {\tilde{f}}_3(p,u)}{\partial y}& \frac{\partial {\tilde{f}}_3(p,u)}{\partial \phi }& \frac{\partial {\tilde{f}}_3(p,u)}{\partial v}\\ \frac{\partial {\tilde{f}}_4(p,u)}{\partial x}& \frac{\partial {\tilde{f}}_4(p,u)}{\partial y}& \frac{\partial {\tilde{f}}_4(p,u)}{\partial \phi }& \frac{\partial {\tilde{f}}_4(p,u)}{\partial v}\end{array}\right]=\left[\begin{array}{cccc}0& 0& -v\sin (\phi )& \cos (\phi )\\ 0& 0& v\cos (\phi )& \sin (\phi )\\ 0& 0& 0& \frac{\tan (\delta )}{l}\\ 0& 0& 0& 0\end{array}\right]$$

$$\frac{\partial f(p,u)}{\partial u}=\left[\begin{array}{cc}\frac{\partial {\tilde{f}}_1(p,u)}{\partial a}& \frac{\partial {\tilde{f}}_1(p,u)}{\partial \delta }\\ \frac{\partial {\tilde{f}}_2(p,u)}{\partial a}& \frac{\partial {\tilde{f}}_2(p,u)}{\partial \delta }\\ \frac{\partial {\tilde{f}}_3(p,u)}{\partial a}& \frac{\partial {\tilde{f}}_3(p,u)}{\partial \delta }\\ \frac{\partial {\tilde{f}}_4(p,u)}{\partial a}& \frac{\partial {\tilde{f}}_4(p,u)}{\partial \delta }\end{array}\right]=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& \frac{v}{l\cos (\delta {)}^2}\\ 1& 0\end{array}\right]$$

引入偏差变量
$$\begin{gathered} \tilde{p}=p-p_{ref} \\ \tilde{u}=u-u_{ref} \end{gathered}$$
可得系统偏差模型为
$$\dot{\tilde{p}}=\frac{\partial \tilde{f}(p,u)}{\partial p}\tilde{p}+\frac{\partial \tilde{f}(pu,)}{\partial u}\tilde{u}$$
写成状态方程形式为
$$\dot{\tilde{p}}=A\tilde{p}+B\tilde{u}$$
其中，矩阵$A=\frac{\partial \tilde{f}(p,u)}{\partial p}$，$B=\frac{\partial \tilde{f}(p,u)}{\partial u}$。该偏差模型的主要作用是进行系统的轨迹跟踪，控制状态偏差 $\tilde{p}$ 逐步收敛到 0，即系统状态 $p=p_{ref}$，则说明此时系统状态跟踪上目标轨迹点状态，实现轨迹跟踪的控制目标。