麦克纳姆轮底盘-正反向运动学-里程估计

此篇博客转自本作者在古月居的博客：https://www.guyuehome.com/33978

前言：

大概在大一的时候由于Robocon比赛就开始调试麦克纳姆轮底盘。恰好,最近有个项目需要用到这个，在此重温了下当初做的内容。对麦克纳姆轮底盘的速度分解问题，网上有很多推导已经详细说明，但都是自身坐标系下的。这对执行有些任务是不够的，因此在这里进行推导和总结。另外，总结了使用麦轮底盘进行里程估计的方法。

由于本人比较懒，所以就在接下来的推导中使用手写的笔记。所推导的内容已经在淘宝买的微宏机器人的麦克纳姆轮底盘上进行了验证。

在世界坐标系中进行任务规划，需要定义一个世界参考坐标系{W}，紧接着在定义车身的坐标系{B}，对于右前方的轮子中心定义为{b}（这里只定义一个轮子，其他轮子道理相同）。具体如下图（图中紫色区域圈出来的麦轮中心实际就为{b}的中心，这里写不下了，所以表述在了旁边。）

逆向运动学:

可以在世界坐标系中给与一个目标位移速度和偏航角角速度，堆叠起来形成一个向量，那么可以很容易想到将这个任务向量转化到车身坐标系{B}下，这需要一个齐次的矩阵来映射，具体的表述为

可以得到在{B}下的期望速度为V_B。由于每个轮子和车身通过机构以一定几何关系分布，所以麦轮也需要满足这个速度V_B。进一步的，需要将V_B转化到轮子坐标系下，那么以右上角的轮子为例，转化可以表述为

可以的到V_b。定义轮子旋转形成的线速度为V_r,可以很明显的发现V_r没有办法直接表现出V_b,这可以由麦轮中自由维度和驱动维度解决，理论上就是这两个维度速度矢量形成的V_b，但是这里为了转化到V_r上，采用另一种表述方法。当然也可以计算出自由维度和驱动维度的运行速度（但这是没有必要的）。由于麦轮的小轮倾斜α角度为45°，可以得到V_b转换为V_r表示为

最后总结上述的转换可以表达为一个较长的式子，得到车轮的线速度。这是一种逆向运动学。

这里进行补充下前面式子中的旋转矩阵R，可以表示为以下

由于麦轮底盘有四个轮子，那么堆叠上述的关系可以表达为以下简单的式子

这个式子所表述的思想非常的简单。启发的来看，在任意个轮子的时候这种方法同样适用。更加进一步的，这种方法适用于分布式的底盘，这将不仅限于麦克纳姆轮底盘，例如还有全向轮底盘，舵轮底盘（需要在{b}下增加一个旋转角度）也同样适用。所以，从宏观来看，这是一种统一的方法。

另外，许多小伙伴在进阶到一定程度时，都想如何让全向移动底盘可以边跑边转（在满足位移轨迹的前提下，还可以按照指定速率旋转），上述方法提供了参考思路，这点我的学弟们已经验证了有效性。

正向运动学:

里程估计是正向运动学积分得来的。在每届的Robocon比赛中里程估计（全场定位）都是通过一个两个正交排放的增量式编码器来累计的，这样子的好处就是底盘打滑时仍然可以进行里程估计。然而，有些时候，可能没有额外的设备，因此需要从轮子上的编码器进行里程计算。这点，我在网上找了下几乎没有对此的总结。首先，更加清晰的表述下我们用到的麦轮底盘（这里与最上面的实物图是对应的）

对于麦克纳姆轮底盘的正向运动学，可以由逆向运动学的一般表达式得到。对于微宏机器人的麦克纳姆轮底盘逆向运动学的一般式子可以很容易总结到规律（其他排布的克纳姆轮底盘同样道理），为以下表现形式（速度均为线速度）

进一步的需要将其转化的世界坐标系{W}下,那么在速度矢量前加一个旋转矩阵映射即可。

这是在世界坐标系下的速度矢量，只要对其进行积分即可。

由于微宏机器人的底层代码和硬件都做的非常好，m/s的速度估计非常准确，所以直接添加这部分功能，然后进行测试，实验效果是非常喜人的。代码跑的实时系统FreeRTOS，控制频率为100Hz。为了说明里程累积的准确率，在室内跑 2X8 的矩形路径，7圈后回到起点的定位误差约等于0.25m。

最后需要说下的就是，这种里程估计的方法要求机器人不能打滑。由于麦轮有四个轮子，从机械的角度上来看，他们中不会全部充分着地，维宏机器人在车轴（X方向）中间添加了一个活动铰链，来解决这个问题。还好机械比较稳，不然太大的不平都会是里程累积的严重扰动因素。最后，车子的重心需要尽量在中间位置，否则前轮或者后轮打滑，都会影响里程。

具体的代码（基于维宏机器人的历程修改的，只添加了一个文件）由于某些原因，晚点放到我的Github上。

具体如果有误，还望指出。