2D车辆机器人不同模型运动方程及仿真
摘要
勿在浮沙筑高台
运动规划的终极目标是规划出机器人实时的期望位置、速度、加速度（等其它高阶信息），这些最终都要输出到机器人实际的底盘上被执行，底盘期望的是真正输出到车轮电机上的速度（扭矩）等。底盘的不同运动模型对于运动规划算法的选择、调试以及编写有一定影响。比如TEB算法对于麦轮的适配就不是很好（实际体验），即使是调大weight_kinematics等，车还是不倾向于直接平移，高速的时候性能很受影响。
本文将会讨论
- Forward-Kinematics：在给出了控制指令（速度、角速度等）后，车的位置如何变化，虽然后面做上层算法多数是数学，但这些知识作为基础有必要了解。此外，如果要做里程计，会用到这部分的知识（测量轮速，算位姿）。对于二维平面机器人，有如下形式
$$ \begin{aligned} &x(t)=\int_{0}^{t} V(t) \cos (\theta(t)) d t \\ &y(t)=\int_{0}^{t} V(t) \sin (\theta(t)) d t \\ &\theta(t)=\int_{0}^{t} \omega(t) d t \end{aligned} $$
- Inverse-Kinematics：在给出了整车期望的位置后，如何向车发送控制指令，运动规划系统中这部分的代码实际中一般会在上位机-下位机通讯之前，完成【上位机运动规划】—【下位机控制器】之间的语言翻译。
差速模型
共轴两轮，靠两侧轮子的速度差距实现转弯。
正运动学
解决的是给出两轮分别的速度$V_l, V_r$，如何解机器人的位姿$X∈R^3$，$X=[x, y, \theta]^T$
第一种推法：比较自然
后面的麦轮也是用了这种方法，至于第二种方法可以只当做扩展阅读。
首先解决如何由车体系下的速度到世界系下的速度。
从车体系转到世界系，所以要求车体系在世界系中的表达，如果不明白，参见麦克纳姆轮推导 （因为我是先写的麦轮部分），如下（注意到由于差速轮的特性，y方向一定没有速度）。 $$ {\left[\begin{array}{l} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{array}\right]=R_{z}(\theta)\left[\begin{array}{l} V \\ 0 \\ \omega \end{array}\right]=\left[\begin{array}{ccc} \cos (\theta) & -\sin (\theta) & 0 \\ \sin (\theta) & \cos (\theta) & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} V \\ 0 \\ \omega \end{array}\right]} $$ 然后是求如何由左右轮速推出车体系下的速度，由角运动公式（R为车当前的转弯半径，$v_r和v_l$为车轮线速度，$\dot\varphi_R, \dot\varphi_L$为车轮旋转的角速度） $$ \omega(R+b)=v_{r}\\ \omega(R-b)=v_{l} $$ 解得 $$ R=b \frac{v_{l}+v_{r}}{v_{r}-v_{l}};\space \omega=\frac{v_{r}-v_{l}}{2b};\space而V=\omega R=\frac{v_{l}+v_{r}}{2} $$ 又因为 $$ v_{r}=r_R{\dot\varphi_R}\\ v_{l}=r_l{\dot\varphi_L} $$ 所以 $$ \omega=\frac{r_R{\dot\varphi_R}-r_L{\dot\varphi_L}}{2b};\space V=\frac{r_R{\dot\varphi_R}+r_L{\dot\varphi_L}}{2} $$ 写成矩阵形式 $$ {\left[\begin{array}{l} V \\ \omega \end{array}\right]=\left[\begin{array}{cc} \frac{r_{R}}{2} & \frac{r_{L}}{2} \\ \frac{r_{R}}{2 b} & -\frac{r_{L}}{2 b} \end{array}\right]\left[\begin{array}{l} \dot{\varphi_R} \\ \dot{\varphi}_{L} \end{array}\right]} $$
结合上面二式，得 $$ \left[\begin{array}{c} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{array}\right]= \left[\begin{array}{c} \frac{r_{R}}{2} \cos (\theta) & \frac{r_{L}}{2} \cos (\theta) \\ \frac{r_{R}}{2} \sin (\theta) & \frac{r_{L}}{2} \sin (\theta) \\ \frac{r_{R}}{2 b} & -\frac{r_{L}}{2 b} \end{array}\right] \left[\begin{array}{c} \dot{\varphi_R} \\ \dot{\varphi}_{L} \end{array}\right] $$
这就是所谓的差速车正运动学（微分形式），如果写成积分形式（里程计用），就是
PS：如果你观察仔细，会发现上面的示意图里车前面还有一个轮子，一般称为Caster wheel，支撑轮，不提供驱动力，所以推导的时候无需考虑。
PSS：打滑（slipping）
另一种推法：利用瞬时旋转中心(ICC)
同样地，由角运动公式
$$
\omega(R+l / 2)=V_{r}\\
\omega(R-l / 2)=V_{l}\\
$$
解得
$$
R=\frac{l}{2} \frac{V_{l}+V_{r}}{V_{r}-V_{l}};\space \omega=\frac{V_{r}-V_{l}}{l}
\tag3
$$
几种特殊情况：如果$V_{r}=V_{l}$，则$R$为无穷大，$\omega$为0，于是小车做直线运动（绕着无穷远点旋转）；如果$V_{r}=-V_{l}$，则$R$为0，小车绕着两轮连线的中点做原地旋转。但是，我们没办法让机器人沿着两轮连线平移，这时候其实我们称差速车是non-holonomic constrained（非完整约束）的模型。不严谨地说，即不能任意操控机器人的位姿（书上6.2.1节会详细讲）。
一般地，知道了 $V_{l}, V_{r}$ 后，由公式（3） 我们可以得到机器人的瞬时运动中心（ICC）的位姿
$$
I C C=[x-R \sin (\theta), y+R \cos (\theta)]\tag4
$$
那么在 $t+\delta t$ 时刻的位姿（即差速车运动的微分方程）就可以由下式给出 $$ \left[\begin{array}{l} x^{\prime} \\ y^{\prime} \\ \theta^{\prime} \end{array}\right]=\left[\begin{array}{ccc} \cos (\omega \delta t) & -\sin (\omega \delta t) & 0 \\ \sin (\omega \delta t) & \cos (\omega \delta t) & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{c} x-I C C_{x} \\ y-I C C_{y} \\ \theta \end{array}\right]+\left[\begin{array}{c} I C C_{x} \\ I C C_{y} \\ \omega \delta t \end{array}\right] $$
对于这个式子有一个直观的理解方式，即：认为是先将瞬心平移到原点，再旋转，再平移回去（这也是直观理解旋转中心不在原点的向量旋转如何操作的办法）
也就是说，给定了机器人当前的位置，左右轮的速度，当前时刻$t$以及步长$\delta t$，通过公式（5）就可以得出下一时刻的位置。
逆运动学
不幸的是，non-holonomic约束的机器人很难解，我们只讨论一种特殊情况。假设左轮和右轮都以恒定速度运动，即$V_l(t)=V_l,\space V_r(t)=V_r$，且不相等，那么由之前提到的正运动学公式，我们可以反解出下式
仿真
利用第一种方法仿真， $$ \left[\begin{array}{c} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{array}\right]= \left[\begin{array}{c} \frac{r_{R}}{2} \cos (\theta) & \frac{r_{L}}{2} \cos (\theta) \\ \frac{r_{R}}{2} \sin (\theta) & \frac{r_{L}}{2} \sin (\theta) \\ \frac{r_{R}}{2 b} & -\frac{r_{L}}{2 b} \end{array}\right] \left[\begin{array}{c} \dot{\varphi_R} \\ \dot{\varphi}_{L} \end{array}\right] $$ // TODO
Ackermann模型
对于四轮车，最简单的想法是当转弯时两个前轮始终平行。但是这样会导致左前轮和右前轮中轴线的延长线不会交于同一点，即这两个轮旋转中心不是同一点。但是转弯时车作为一个刚体，整体是绕着同一个圆心旋转的，这样就会导致左右前轮转弯时打滑、变形，影响性能。
阿克曼转向（Ackermann steering），又称kingpin steering，就是一种为了解决转弯时，左右转向轮中轴线不相交的设计方法。
如下图，当转弯时两前轮角度不同，就可以使得二者中轴线延长后交于同一点，达成整车绕着同一个圆心旋转的目的。显然，这种情况下内轮要比外轮转的角度大，这种称为正阿克曼，对于很多高速赛车，为了更佳性能会采用反阿克曼，参考此篇 。
直观理解：机械设计
与轮轴连杆固定点的位置有关
下面推导转弯时两轮偏角的关系。
推导可得 $$ \alpha_i=tan^{-1}(\frac{L}{R-\frac{T}{2}}); \space \alpha_o=tan^{-1}(\frac{L}{R+\frac{T}{2}}) $$ 即，对于转弯半径为R时，最优的两轮配置。这个与路径规划上层关系不大，网上大多数都是研究汽车、赛车的人在研究这个，比如为什么赛车不同赛道要换悬挂，就是为了更好地适配不同转弯。
运动学推导
和差速车类似
麦克纳姆轮模型
可以实现holonomic约束，即机器人可以在2D平面任意改变位姿。其全向移动的关键是轮子的辊子和轮子中轴线不平行，有45°的夹角，这样一来就可以通过四个轮子的速度分解和叠加实现任意方向的移动、旋转。直观理解如下图（注意，麦轮安装必须按一定规则，辊子的中轴线都要通过车的正中心）
运动正逆解推导
**符号说明：**s系为固定坐标系，b为车体系，w为每个轮子的坐标系，$\phi$是车体系与固定系的夹角，$\gamma_i$是辊子与轮子的夹角，对于麦轮来说是$45°$，对于全向轮来说是$90°$，后面可以看到它们的不同。$x_i,y_i,\beta_i$描述了w系在b系中的相对位姿。
机器人在s系的角速度和线速度可以由$[\dot\phi, \dot x, \dot y]^T$表示，由这个状态推导出每个轮子的旋转角速度$u_i$的完整公式如下。 $$ u_i=\frac{1}{r_{i}}\left[1 \space {\tan \gamma_{i}}\right]\left[\begin{array}{cc} \cos \beta_{i} & \sin \beta_{i} \\ -\sin \beta_{i} & \cos \beta_{i} \end{array}\right]\left[\begin{array}{ccc} -y_{i} & 1 & 0 \\ x_{i} & 0 & 1 \end{array}\right]\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi \end{array}\right]\left[\begin{array}{l} \dot{\phi} \\ \dot{x} \\ \dot{y} \end{array}\right] $$ 从右向左看，第一个是旋转矩阵，将速度从s系转到b系。不过，注意这其实是一个分块的矩阵，因为是绕着z轴旋转，所以角速度不变（相等）。右下角2*2是绕着z轴旋转的旋转矩阵的x，y部分。
复习一下旋转矩阵的定义，求一个向量在A系中的表示到在B系的表示的旋转矩阵的求法：$P^B=R^B_AP^A$，$R^B_A$是A在B中的表示。这里的话由s系到b系，应该求s系在b系中的表示，见下图。技巧在于可以先求这个定义角为正的旋转顺序，然后如果需要求转置再求转置。
第二个是平移矩阵，将速度从b系移动到w系（但是没有旋转），这个不太理解。
第三个是旋转矩阵，将速度转一个$\beta_i$，使得$v_x$和driving velocity相同方向，便于后续分解。这里是由“虚线系”转到w系，应该求虚线系在w系中的表示。因为$\beta_i$角是w系相对于虚线系为正，所以虚线系相对于w系也要转置一次。
第四个是由下图分解得出，可以知道如何由$x$和$y$方向的速度表示driving velocity，即每个轮子最终旋转的线速度，然后除以轮半径，就可以得出角速度$u$。注意，这里是向非正交坐标系（$v_{drive}, v_{slide}$）分解，要从坐标基的最原始定义出发，即向坐标轴投影。所以这里$v_{slide}=v_y/cos\gamma$，而并不是乘。
求解四个矩阵相乘的结果如下（是一个1*3的行向量） $$ h_{i}(\phi)=\frac{1}{r_{i} \cos \gamma_{i}}\left[\begin{array}{c} x_{i} \sin \left(\beta_{i}+\gamma_{i}\right)-y_{i} \cos \left(\beta_{i}+\gamma_{i}\right) \\ \cos \left(\beta_{i}+\gamma_{i}+\phi\right) \\ \sin \left(\beta_{i}+\gamma_{i}+\phi\right) \end{array}\right]^{\mathrm{T}} $$ 这样一来，对于给定的固定系下的机器人速度信息$\dot q=[\dot\phi, \dot x, \dot y]^T$，每个轮子$i$只需乘上对应的$h_i(\phi)$即可得到该转的轮速（1*3与3*1相乘，是一个标量）。
我们将所有轮子写在一起，$u=[u_1,u_2,…,u_m]^T$，$H(\phi)=[h_1(\phi), h_2(\phi), …, h_m(\phi)]^T$，则有$u=H(\phi)\dot q$，即 $$ u=H(\phi)\dot q = \left[\begin{array}{c} h_1(\phi) \\ h_2(\phi)\\ \vdots \\ h_m(\phi) \end{array}\right] \left[\begin{array}{c} \dot{\phi} \\ \dot{x} \\ \dot{y} \end{array}\right] $$ 为了简化，通常使用车体系而非固定系中的速度$\mathcal V_b$作为变换的起点，于是省略掉第一个矩阵（即$\phi=0$）。 $$ u=H(0)\mathcal V_b = \left[\begin{array}{c} h_1(0) \\ h_2(0)\\ \vdots \\ h_m(0) \end{array}\right] \left[\begin{array}{c} {\omega_{bz{}}} \\ {v_{bx}} \\ {v_{by}} \end{array}\right] $$ 不管以上哪个式子，我们都得到了逆运动学公式，即如何安排每个轮子的轮速，让车达到期望的twist（包含了速度和角速度的描述机器人速度信息的量称作twist）。
以全向轮和麦轮经典结构为例进行计算
对此从线性方程组的角度做一些探讨。
插播：复习线性方程组的解
$Ax=b$的几何意义：$Ax$是取A所有列的线性组合，即A的列空间，要与b相等是需要b位于A的列空间中。而列秩指的是A独立的列的个数，即列空间的维数。
下面讨论此方程组的解与A的列秩的关系，摘自知乎一篇文章 ，MIT Linear Algebra也有。
对于$u=H(0)\mathcal{V}_b$，$\mathcal{V}_b$一定是3*1，但H和u随着轮子数变化而变化。
①3个轮子的情况，H是3*3的方阵
- 给定一个u，如果H是列满秩的，则$\mathcal{V}_b$有唯一解，如果H不是列满秩，即r < n，意味着没有取$\mathcal{V}_b$全部的分量来组合，即$\mathcal{V}_b$中有自由变量，所以会有无穷多个解。但有无穷多解的物理意义是什么呢？
②4个轮子的情况，H是4*3的“Tall and thin”矩阵。
- 给定一个u，如果H是列满秩的，则$\mathcal V_b$是可能无解的（如果u选择不当）或者有唯一解（u选择恰当，落在H的列空间中）。如果H不是列满秩，会有0或无穷多解。所以u也是不能任意选择的，比如要想沿着x方向直行，即$\mathcal V_b=[0,v_{bx},0]^T$，此时只有$H$阵的第二列发挥作用，计算得知必须满足$u1=u2=u3=u4$的约束。
后记
对于运动学有了更深的理解，虽然很多可能不会用上，但学的过程感觉还是挺开心。也复习了一些线性代数和物理的知识。
PS：Goldmark的数学公式渲染在加大括号方面有奇怪的问题，导致我排查了好久，比如
|
|
因为我用MathPixTool生成的公式总是带着很多大括号，所以有一些渲染失败了。而且令人头疼的是这个bug并不总是能触发。如果以后发现公式渲染失败，记得排查大括号的问题。另外，加\tag也可能导致渲染失败。
参考资料
- Dudek and Jenkin, Computational Principles of Mobile Robotics 【比较入门】
- 一篇总结性的博客
- 理解Ackermann视频 （B站也有对应的）
- 麦克纳姆轮实现pdf ，比较底层
麦克纳姆轮运动学推导麦克纳姆轮及全向轮类比推导- Engineering Educator Academy 的视频 【Nice】
- MIT Linear Algeria 5th edition【Nice】