一起自学SLAM算法：6.1 底盘运动学模型

2023-05-16

连载文章，长期更新，欢迎关注：

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

  6.1 底盘运动学模型

        6.2 底盘性能指标

        6.3 典型机器人底盘搭建

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

轮式机器人底盘按照转向方式的不同，可以分为两轮差速模型、四轮差速模型、阿克曼模型、全向模型等。

两轮差速模型是最简单的底盘模型，通过左右两个轮子的转动速度差实现转弯。
四轮差速模型跟两轮差速模型很相似，也是通过左边轮子的和右边轮子的转动速度差实现转弯，只不过是四个轮子，同一边的轮子速度是同步的，也就是左前轮和左后轮速度始终一样，右前轮和右后轮速度也始终一样。
阿克曼模型就是汽车的转向模型，通过控制前面两个轮子的转向实现转弯。
全向模型采用带有横滚轴的特殊轮子构造的模型，这种轮子除了能正常旋转外，轮胎接地面还可以侧向滑动，全向模型运动更灵活，底盘的运动速度的方向不受约束。

两轮差速模型、四轮差速模型、阿克曼模型也称为运动受约束模型，全向模型也称为运动不受约束模型。介绍到的这几种模型都是轮式底盘模型，除此之外还有其他一些底盘模型，比如双足底盘模型、四足底盘模型、六足底盘模型、多足与轮式复合底盘模型等，不过这些只需要了解即可，并不要求掌握。

6.1.1 两轮差速模型

两轮差速模型可以说是最简单的底盘模型，在底盘的左右两边平行安装两个由电机驱动的动力轮，考虑到至少需要3点才能稳定支撑，底盘上还需要安装用于支撑的万向轮。

最简单的就是在底盘前边装1个万向轮，后边2个动力轮，组成3轮结构，如图6-1a所示。这种结构理论上可以实现转弯半径为0的原地旋转，但是原地旋转时的速度瞬心位于两动力轮轴线中点位置，而不是底盘的几何中心，在自主导航过程中，最小避让空间为 ${r}'$ 半径范围的区域，显然 ${r}'$ 大于 $r$ ，底盘旋转所需要的避让空间会更大。

3轮结构中，除了底盘旋转所需要的避让空间更大之外，还存在侧翻的问题，当底盘上的载重分布不均匀时，转弯时很容易翻车，改进方法是多装几个万向轮。如图6-1b所示，前后各装1个万向轮，在底盘中轴线上左右位置各装1个动力轮，组成4轮结构。也可以在底盘的4个角各装1个万向轮，在底盘中轴线上左右位置各装1个动力轮，组成6轮结构，如图6-1c所示。图6-1b和图6-1c结构的优点是原地旋转时的速度瞬心与底盘几何中心是重合的，底盘旋转所需要的避让空间可以达到理论最小，并且不容易侧翻。由于4轮和6轮结构存在接地点大于3的多点接触地面问题，也就是当地面不平坦时，有的轮子会出现悬空。为了解决多点接触地面问题，4轮和6轮结构需要在2个主动轮上加装悬挂系统，而悬挂系统将使整个底盘的结构设计变得更加复杂。

因此，要结合实际的应用场景，合理的选择图6-1中的3种结构，在满足要求的前提下，尽量降低底盘设计复杂度。下面通过对前向运动学、逆向运动学和轮式里程计的内容分析，详细介绍两轮差速模型的原理。

图6-1 两轮差速模型

1.前向运动学

底盘中各个动力轮的速度可以由编码器值计算得到，那么通过各个轮子的速度求底盘整体的运动速度的过程，即前向运动学。已知左动力轮的线速度为 $V_{L}$ ，右动力轮的线速度为 $V_{R}$ ，动力轮的线速度可以用编码器计数值计算，如式（6-1）所示。

底盘整体的运动速度由速度瞬心上的线速度和角速度描述，三维空间中刚体运动速度由6维向量 $[v_{x},v_{y},v_{z},\omega _{x},\omega _{y},\omega _{z}]$ 描述。由于这里讨论的底盘只在二维平面上运动，并且两轮差分底盘不发生侧向滑动，底盘采用右手坐标系，底盘中只有向前的线速度 $v_{x}$ 和 $z$ 轴的角速度 $\omega _{z}$ 为非0项。通过前向运动学，在已知左、右动力轮的线速度为 $V_{L}$ 和 $V_{R}$ ，利用图6-2所示的几何关系，便可以求出底盘的整体速度 $v_{x}$ 和 $\omega _{z}$ 。

图6-2 两轮差速运动学

在很短的时间t内，底盘可以看成绕O点做圆周运动，两个圆周的半径分别为 $\rho$ 和 $\rho +d$ ，其中d为两个轮子之间的轴距，两个轮子走过的路程分别为 $s_{L}$ 和 $s_{R}$ 转过的角度为 $\varphi$ ，底盘速度瞬心走过的路程为s， $s_{L}$ 和 $s_{R}$ 满足式（6-2）的关系。

将式（6-2）中的两个等式相减可以求出 $\varphi$ ，利用 $\varphi$ 继而可以求出s，求解如式（6-3）所示。

由于运动时间t很小，利用极限的思想， $\varphi$ 趋近于0，s也趋近于0，那么s可以等效为底盘速度瞬心轨迹圆弧切线方向的一小段位移，有了这个近似假设条件，用s和 $\varphi$ 直接除以t就可以求出底盘的整体速度 $v_{x}$ 和 $\omega _{z}$ 。

2.逆向运动学

前向运动学是利用各个轮子的速度求解底盘整体的速度，逆向运动学是前向运动学的逆过程，即利用底盘整体的速度求解各个轮子的速度。逆向运动学计算就比较简单了，直接由式（6-4）做矩阵求逆运算，就能得到，如式（6-5）所示。

3.轮式里程计

机器人主机向底盘发送运动控制量 $v_{x}$ 和 $\omega _{z}$ ，经过逆向运动学公式（6-5）解算就可以得到底盘中每个轮子应该运行的目标速度，通过电机控制主板的PID算法实现每个轮子转速控制，关于轮子转速控制的详细过程，请参考4.4.3节的内容。

底盘在运动过程中，通过轮式编码器可以反馈每个轮子的转速，经过前向运动学公式（6-4）解算就可以得到底盘的整体速度 $v_{x}$ 和 $\omega _{z}$ 了。而利用这个反馈速度 $v_{x}$ 和 $\omega _{z}$ ，就可以由前一时刻底盘的位姿 $P_{k-1}$ 推算出当前时刻底盘的位姿 $P_{k}$ ，这个推算过程也叫航迹推演算法，由于底盘不是严格的质点，所以底盘的位姿代表的是底盘速度瞬心在世界坐标系的位姿。

利用轮式编码器反馈，经过航迹推演算法，得到底盘当前时刻位姿，整个过程就是机器人中常说的轮式里程计。如图4-70所示，底盘ROS驱动将里程计信息发布在话题/odom，并从话题/cmd_vel订阅速度控制信息。

很多初学者常常将轮式里程计与编码器、电机控制主板、底盘等概念混为一谈，所以需要特别注意。可以看出，轮式里程计其实是编码器、底盘运动学模型、航迹推演算法等综合出来的产物。也就是说，轮式里程计并不是某种传感器，而是利用轮式编码器计算底盘位姿的一种算法。

下面具体分析一下轮式里程计的解算过程，如图6-3所示。在机器人学中，通常采用右手坐标系，底盘的正前方为x轴正方向、底盘的正上方为z轴正方向、底盘的正左方向为y轴正方向、航向角 $\theta$ 以x轴为0度角并逆时针方向增大。一般以底盘上电时刻，底盘的位姿处建立里程计坐标系（odom），随时间推移底盘的实时位姿 $P_{0}$ 、 $P_{1}$ 、 $P_{2}$ 、 $P_{3}$ 、...、 $P_{n}$ 连接起来就形成了底盘的航行轨迹。同时，底盘自身的坐标系（robot）建立在其速度瞬心处。

图6-3 两轮差速里程计

其实，建立起robot坐标系中的 $v_{x}$ 和 $\omega _{z}$ 速度量与odom坐标系中的底盘位姿 $P_{k}$ 之间的关系，就能解算出轮式里程计。在二维平面中移动的底盘，可以用 $P=[X,Y,\theta ]^{T}$ 表示底盘的坐标和航向，即底盘的位姿，那么位姿 $P$ 满足式（6-6）所示的微分方程。

将 $\dot{P}$ 在时间t上做积分，就可以求出底盘实时的位姿，也就是里程计信息。实际情况下，需要在离散时间域上做计算，由于底盘相邻两位姿相隔时间 $\Delta t$ 很小，积分运算可以用式（6-7）的累加运算来替代。

由于里程计提供底盘在odom坐标系中的位姿，而odom坐标系跟底盘上电启动时所处的地方有关，也就是说里程计中的底盘位姿是一个相对值。另外，里程计随时间的推移存在较大的累积误差，所以一般都是取不同时刻之间的里程计差值应用于其他算法之中。由于轮式里程计只能提供底盘的局部位姿，并且存在较大累积误差，所以还需要结合后面章节中的地图及SLAM重定位技术来获取全局位姿。

由式（6-1）、式（6-4）、式（6-6）和式（6-7）得知，里程计值跟轮子的半径R和左右轮的轴距d有关系。由于制造安装误差的存在，参数R和d会存在一定的偏差。为了减小底盘固有参数R和d对里程计精度的影响，在首次使用底盘前需要对里程计进行标定。最常用到的标定方法是试探法，比如设定1米的直线距离，通过观察底盘实际行走直线的距离与设定距离的偏差，手动调整参数R，直到底盘直线行走的距离与设定距离吻合；参数d的标定也是类似的，比如设定360°转动角，通过观察底盘实际自转转过的角度与设定角度的偏差，手动调整参数d，直达转动角度与设定角度吻合。

6.1.2 四轮差速模型

四轮差速模型跟两轮差速模型非常相似，并且在载重能力和越野性能上比两轮差速模型更有优势。跟两轮差速底盘一样，四轮差速底盘也是靠左右两边轮子的转速差实现转弯，只不过每一边都是两个轮子，同一边的两个轮子的转速是完全一样的，即前轮和后轮的速度是同步的，因此也把四轮差速模型称为同步驱动模型。实现前轮与后轮速度同步的方式主要有3种，分别是履带连接方式、链条连接方式和电控方式。履带连接方式中，是直接将前轮和后轮用履带做连接，在机械上实现前后轮速度同步，如图6-4a所示；链条连接方式中，是用链条将前轮和后轮的输出轴连接在一起，也是在机械上实现前后轮速度同步，如图6-4b所示；在玩具和简易底盘等低成本场合，为了避免设计复杂的机械结构，常常使用4个独立的电机控制4个轮子，并通过控制主板实现前轮和后轮的速度同步，这就是电控方式，如图6-4c所示。

图6-4 四轮差速模型

如图6-4a所示的履带底盘，因接触地面面积的增大而具有很好的越野性能。同时也有很大的弊端，比如履带底盘原地自转时，常常认为履带上前轮和后轮连线中点位置处为等效的接地点，这样就可以将底盘模型等效为两轮差速模型来处理。显然这种简单的等效是不靠谱的，比如当地面存在稍微的起伏时，履带与地面的接触点可能出现在任何位置，这样就会造成底盘的前向运动学和逆向运动学失效。因此在对轮式里程计精度要求较高的应用场景，很少选用履带底盘，而是非履带底盘。

如图6-4b和图6-4c所示的四轮底盘，是各个轮子单点接触地面，所以轮式里程计精度比履带底盘中的要高。在有些对底盘控制和里程计精度要求不是特别高的场合，也常常将这种四轮底盘简单等效为两轮差速模型来处理。等效的条件是轮子不发生侧向滑动，如图6-5所示，当底盘的长宽比 $\frac{b}{a}$ 足够大时，也就是前、后轮距离很近，左、右轮距离很远，底盘自转时才会足够孙畅，保证轮子不发生侧向滑动。

图6-5 四轮差速模型简单等效

在实际情况中，不可能把底盘设计成长宽比很大的样子，也就是说上面的等效条件很难满足。也就是说在实际的四轮差速模型中，转弯的时候轮子会存在侧向滑动。为了得到更高精度的里程计，就需要在四轮差速模型的侧向滑动情况下，对其前向运动学、逆向运动学和轮式里程计进行讨论。

1.前向运动学

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.逆向运动学

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.轮式里程计

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.轨迹跟踪问题

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

6.1.3 阿克曼模型

四轮差速底盘具有很好的载重和越野性能，但缺点是转弯时轮子会发生一定程度的侧向滑动。虽然在侧向滑动不明显的情况下，可以用两轮差速模型来等效。无论如何，这种侧向滑动让底盘控制稳定性、里程计解算、轨迹跟踪等问题变得更加复杂。阿克曼底盘通过前轮的机械转向，让底盘中的4个轮子在基本不发生侧向滑动的情况下能顺畅的转弯。不过也有缺点，阿克曼底盘不能原地旋转，也就是最小转弯半径不为0，想像一下汽车在狭小的地方倒车入库场景就不难理解了，车辆需要反复左右调整方向盘才能将车停入指定车位。

在直线行驶时，4个车轮的轴线互相平行。在进行转弯时，需要让4个车轮只沿车轮切线方向前进而不发生侧移，以避免侧移给车轮带来的巨大磨损。即4个轮子转弯弧线必须处于共同的圆心ICR之上，这个时候前轮内侧转向角 $\delta _{in}$ 会大于外侧转向角 $\delta _{out}$ 。在阿克曼转向几何中，利用梯形四连杆的相等曲柄控制前轮的转向，实现 $\delta _{in}$ 和 $\delta _{out}$ 需要的约束关系，避免车轮发生侧移，让4个车轮能顺畅地转弯，如图6-7所示。

图6-7 阿克曼转向几何

从图6-7a可以知道，转向机构是ABCD四连杆结构，A和B分别为左、右轮的销点，C和D为曲柄的连接点，曲柄AD和BC是等长的，E是后轮中轴点，在前轮转向角为0时，C、D位于ABE等腰三角形的两腰等位点上，这就是阿克曼转向梯形结构。关于阿克曼转向梯形结构如何实现 $\delta _{in}$ 和 $\delta _{out}$ 约束的数学原理证明，在机械原理和汽车结构研究领域已经有大量的文献可考，这里就不再赘述了，大家只需要记住阿克曼转向梯形结构设计的结论就行了。

值得注意的是，在实际应用中阿克曼有多种变种结构，如图6-8所示。

图6-8 几种常见阿克曼结构

当车辆低速行驶转弯时，轮胎受到的侧向摩擦力很小可以认为是0，这时候适合采用图6-8a所示的转向结构，其中的 $\delta _{in}$ 大于 $\delta _{out}$ ，也称理想阿克曼结构；当高速过弯时，轮胎受到的侧向摩擦力就不能忽略了，由于内轮转向角更大，内轮也就更容易发生侧滑，这就需要适当减小内轮转向角与外轮转向角的差值，避免内轮侧滑，这时候适合采用图6-8b所示的转向结构，其中的 $\delta _{in}$ 等于 $\delta _{out}$ ，也称平行阿克曼结构；在赛车等特殊场合，需要车辆具有极致的过弯性能，尽量缩短过弯时间以赢得比赛，这时候就需要增大外轮的侧向摩擦力，以提供更大的向心力让车辆在更小的转弯半径上完成转弯，这时候适合采用图6-8c所示的转向结构，其中的 $\delta _{in}$ 小于 $\delta _{out}$ ，也称反阿克曼结构。家用汽车转弯介于低速和高速之间，其转向结构介于理想阿克曼结构和平行阿克曼结构之间。而机器人运动速度相比于汽车就慢得多了，其转向结构一般都是理想阿克曼结构，因此下面的讨论将围绕理想阿克曼结构展开。

按照驱动方式的不同，阿克曼底盘又分为前驱、后驱和四驱。前驱就是利用前轮提供动力来驱动底盘运动，由于在转弯过程中，左前驱动轮和右前驱动轮的转速是不一样的，还需要用差速器将发动机输出的动力按不同的比例分配给两个驱动轮，如图6-9a所示；后驱就是利用后轮提供动力来驱动底盘运动，同样由于在转弯过程中，左后驱动轮和右后驱动轮的转速是不一样的，需要用差速器将发动机输出的动力按不同的比例分配给两个驱动轮，如图6-9b所示；四驱就是4个轮子都提供动力驱动底盘运动，除了前轮轴上的差速器和后来轴上的差速器，还需要中央差速器来解决前轮和后来转速不同的问题，如图6-9c所示。

图6-9 阿克曼底盘的驱动方式

采用不同的驱动方式，其对应的前向运动学、逆向运动学和里程计解算也将不同。由于前驱中，阿克曼转向结构和差速器都分布在前轮，设计上变得更复杂，所以在机器人中一般选择在结构设计上更加简单的后驱方式。

在汽车中，差速器是一种特殊的齿轮组结构，能根据转弯时左、右轮受到地面侧向摩擦力大小上的差别，自动将发动机输出的转速按照相应的比例分配到左、右轮上，并且发动机输出的转速M与左轮转速（ML）、右轮转速（MR）始终保持2*M=ML+MR的关系，如图6-10所示。

图6-10 通用差速器结构

考虑到差速器复杂的齿轮结构设计，在机器人底盘中，通常采用两个独立的电机驱动左右轮，并配合电机驱动板调速程序来替代纯机械式的差速器，控制前轮转向的阿克曼结构由舵机带动来实现转向，如图6-11所示。因此下面的前向运动学、逆向运动学和里程计相关内容，将在这种简易阿克曼后驱底盘中展开讲解。

图6-11 简易阿克曼后驱底盘

1.前向运动学

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.逆向运动学

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.轮式里程计

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.轨迹跟踪问题

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

6.1.4 全向模型

为了引出本节的全向底盘，我们需要先对底盘的自由度进行一下讨论。在二维平面上运动的底盘，可以用速度向量 $[v_{x},v_{y},\omega _{z}]$ 来表示底盘的运动， $v_{x}$ 和 $v_{y}$ 是底盘线速度在x和y两个坐标轴方向的分量， $\omega _{z}$ 是底盘绕z轴在平面旋转的角速度。如果速度向量 $[v_{x},v_{y},\omega _{z}]$ 中的三个分量之间的取值相互独立，这样的底盘运动速度不受约束，也称为不受约束底盘。在两轮差速或四轮差速底盘中， $v_{x}$ 和 $\omega _{z}$ 都由左、右轮速度 $V_{L}$ 和 $V_{R}$ 计算，如式（6-4）所示，由于 $V_{L}$ 和 $V_{R}$ 的取值是相互独立的，以 $V_{L}$ 和 $V_{R}$ 作为向量的基，那么 $v_{x}$ 和 $\omega _{z}$ 点乘结果为0，也就是说 $v_{x}$ 和 $\omega _{z}$ 在向量基 $V_{L}$ 和 $V_{R}$ 中是正交的，即 $v_{x}$ 与 $\omega _{z}$ 是相互独立的。不过由于两轮差速或四轮差速底盘无法朝轮子的轴方向运动，即速度分量 $v_{y}$ 始终为0，这样的底盘运动速度就受到了约束，也称为受约束底盘。在阿克曼底盘中，除了速度分量 $v_{y}$ 始终为0的约束外，由于底盘最小转弯半径 $R_{min}$ 不为0，那么还受到约束 $\frac{v_{x}}{\omega _{z}}=R,\left | R \right |\in [R_{min},+\infty )$ 的限制，也称为受约束底盘。为了更直观地说明，将速度向量 $[v_{x},v_{y},\omega _{z}]$ 的可能取值在三维空间中表示出来，如图6-14所示。

图6-14 速度向量约束

底盘的速度受到约束的原因是普通轮子不能侧向移动，只有切向移动一个自由度。如果把轮子换成包含切向和侧向两个自由度的轮子，底盘速度就不受约束，也就成了全向底盘。在业界，把这种具有两个自由度的轮子称为麦克纳姆轮，其结构是在轮表面装上滚柱，依据滚柱的不同安装方式，麦克纳姆轮又分为不同的型号。以45°方向安装滚柱的麦克纳姆轮是最常见的型号，如图6-15所示。不过由于45°安装的滚柱之间存在缝隙，轮表面截面其实是多边形，而不是理想的圆形，这就导致轮子转动过程中会上下振动。因此，这种轮子必须加装悬挂系统。

图6-15 滚柱45°型麦克纳姆轮

为了避免由于滚柱之间的缝隙而导致轮子转动过程中上下振动的问题，可以将滚柱以90°方向安装，如图6-16所示。如图6-16a所示，以90°方向安装的滚柱并排交替排列，这样轮子在转动过程中，轮轴心与地面的高度就是恒定值，就不会发生振动了。不过由于滚柱是并排交替安装的，导致轮表面接地点并不固定，在地面上走出的轨迹是矩形波形状。而轮式里程计的解算，假定左、右轮轴距d是常数，即轮表面的接地点为固定点。为了让轮表面的接地点为固定点，需要将滚柱安装在同一排面上，如图6-16b所示。这种轮子的滚柱是一大一小交叉排列的，这种轮子不可能设计的太小，安装制造也比较复杂，价格不菲。

图6-16 滚柱90°型麦克纳姆轮

将4个滚柱45°型麦克纳姆轮平行安装在底盘中，当4个轮子电机锁死时，底盘可以自锁在原地不动，如图6-17a所示。而将4个滚柱90°型麦克纳姆轮平行安装在底盘中，就无法让底盘自锁在原地不动了，当所有电机锁死时，底盘横向是可以被推动了，如果底盘停止在斜坡上，底盘会往下滑动。因此滚柱90°型麦克纳姆轮必须要成一定角度排列，如图6-17b所示。也可以按图6-17c所示排列，能少使用一个电机。

图6-17 全向底盘

图6-17b和6-17c所示底盘，无论底盘朝哪个方向移动，必定有轮子是按照非切线方向移动的，这样使得电机的动力转化效率无法发挥出来，底盘的移动速度大打折扣。因此，推荐使用图6-17a所示的全向底盘，下面就以这种全向底盘为例，对前向运动学、逆向运动学和里程计来展开讲解。

1.前向运动学

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.逆向运动学

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.轮式里程计

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

6.1.5 其他模型

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

源码仓库

Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation
Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

【1】张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)