RoboMaster电控入门（5）麦克纳姆轮底盘控制

底盘运动解算

麦克纳姆轮是瑞典麦克纳姆公司的专利，其特点是轮子上安装了很多辊子，轮毂轴和辊子之间夹角为45°（有时也可以是其他度数，视使用目的而定），由于搭载了麦轮的四轮底盘可以很容易的实现全向移动，使底盘具有极佳的灵活性，而和传统的全向轮相比，麦克纳姆轮可以直接按照传统四轮底盘的方式进行安装，相对而言更加方便，所以目前绝大部分的Robomaster参赛队都使用麦克纳姆轮作为底盘全向运动的方案。

关于麦克纳姆轮的资料有很多，这里推荐一个写的比较好的知乎专栏

https://zhuanlan.zhihu.com/p/20282234

一个典型的RM机器人四轮底盘由电机，底板，悬挂等构成，底盘安装在底盘的四角，呈矩形分布，麦克纳姆轮的辊子方向会影响其运动性能，一般采用如下图所示，四个麦轮的辊子延长线都过底盘中心的安装方法。

底盘解算的主要目的是将底盘的运动期望解算至电机，因为电机才是我们最后进行控制的执行机构，为了控制底盘的方向和速度，实际上最后依然是要依靠控制电机的转向和转速来实现。

为此需要对底盘进行运动学分析，将底盘速度期望解算成四个电机的转速期望。如下图中，建立底盘坐标系XY，其中底盘速度期望设为

\[\vec{V} = \left[ V_{x},V_{y},\omega_{v} \right]
\]

给四个电机标为1，2，3，4号（对应于电调ID），设n号电机的速度期望为\({V_{n}}\)

运动解算的目标是得到以\(V_{x}\),\(V_{y}\)和\(\omega_{v}\)作为参量描述电机n的\(V_{n}\)，电机的\(V_{n}\)以底盘朝X正方形前进时的转动方向为正方向。

解算的方法很多，这里介绍最简单的一种方式，即将三个正交的底盘运动方向\(V_{x}\),\(V_{y}\)和\(\omega_{v}\)进行线性叠加的方式，这种方法的好处在于可以直接通过实验的方式就能获得结果——只需要将装好的底盘按照各个方向推一推就可以知道了。

首先假设底盘速度期望只有沿X方向的分量

\[\vec{V} = \left[ V_{x},0,0 \right]
\]

显然此时需要四个电机沿X向同速旋转，可以得到四个电机的转速期望

\[\begin{cases}
V_{1} = V_{x} * k_{x} \\[2ex]
V_{2} = V_{x} * k_{x} \\[2ex]
V_{3} = V_{x} * k_{x} \\[2ex]
V_{4} = V_{x} * k_{x}
\end{cases}
\]

再假设底盘速度期望只有沿Y方向分量，即底盘横向平移

\[\vec{V} = \left[ 0,V_{y},0 \right]
\]

则此时电机转速需要做出正负的区分

\[\begin{cases}
V_{1} = -V_{y}* k_{y} \\[2ex]
V_{2} = V_{y}*k_{y} \\[2ex]
V_{3} = -V_{y}*k_{y} \\[2ex]
V_{4} = V_{y}*k_{y}
\end{cases}
\]

最后假设底盘速度期望只有\(\omega_{v}\)，即底盘原地旋转

\[\vec{V} = \left[ 0,0,\omega_{v} \right]
\]

则此时电机转速为

\[\begin{cases}
V_{1} = -\omega_{v}* k_{\omega} \\[2ex]
V_{2} = -\omega_{v}* k_{\omega} \\[2ex]
V_{3} = \omega_{v}* k_{\omega} \\[2ex]
V_{4} = \omega_{v}* k_{\omega}
\end{cases}
\]

对以上公式进行叠加后就可以得到在底盘速度期望为

\[\vec{V} = \left[ V_{x},V_{y},\omega_{v} \right]
\]

对应的电机转速为

\[\begin{cases} V_{1} = V_{x} * k_{x}-V_{y}* k_{y} -\omega_{v}* k_{\omega}\\[2ex] V_{2} = V_{x} * k_{x} + V_{y}*k_{y} -\omega_{v}* k_{\omega}\\[2ex] V_{3} = V_{x} * k_{x} -V_{y}*k_{y} +\omega_{v}* k_{\omega}\\[2ex] V_{4} = V_{x} * k_{x} + V_{y}*k_{y} +\omega_{v}* k_{\omega}\end{cases}
\]

以上推导过程也可以通过严格的动力学分析完成，最后得到的结果是一致的。最后还是以官方代码为例看一下这个解算过程的代码实现。

在mecanum.c下我们可以找到解算函数mecanum_calculate(struct mecanum *mec)中的解算部分

  float wheel_rpm[4];
  float max = 0;

  wheel_rpm[0] = (-mec->speed.vx - mec->speed.vy - mec->speed.vw * rotate_ratio_fr) * wheel_rpm_ratio;
  wheel_rpm[1] = (mec->speed.vx - mec->speed.vy - mec->speed.vw * rotate_ratio_fl) * wheel_rpm_ratio;
  wheel_rpm[2] = (mec->speed.vx + mec->speed.vy - mec->speed.vw * rotate_ratio_bl) * wheel_rpm_ratio;
  wheel_rpm[3] = (-mec->speed.vx + mec->speed.vy - mec->speed.vw * rotate_ratio_br) * wheel_rpm_ratio;

会发现这段代码和我们公式推导的结果有一点出入，这是由于轮子的标号不一致以及电机正方向规定不一致导致的，我们将推得的公式和代码之间重新对应一下，关系如下：

公式	代码	实际位置
\(-V_{4}\)	wheel_rpm[0]	右前
\(V_{1}\)	wheel_rpm[1]	左前
\(V_{2}\)	wheel_rpm[2]	左后
\(-V_{3}\)	wheel_rpm[3]	右后

可见在代码实现时，解算公式要由轮子安装方式，实际的电调设置以及电机正方向决定。

为了方便，一般写完一套代码之后，共用这套代码的所有机器人都要用一样的安装方式和电调设置方式。

闭环控制

光完成底盘运动解算是不够的，如果用只做了解算的底盘拿去开，会发现这个底盘相当的“飘逸”，按一会方向键然后松手，底盘不会立马刹住，而是会“漂移”一段距离才能停下来，这是因为目前电机都是开环控制的，不能及时响应期望的变化，这个时候就需要给电机添加PID控制器了。关于PID控制器的算法原理及实现查看我上一次的博客

https://www.cnblogs.com/sasasatori/p/11672918.html

底盘的控制流程一般是通过遥控器获取底盘运动期望----->底盘运动期望解算至电机转速期望----->电机转速闭环控制

值得一提的是，在通过遥控器获取底盘运动期望时，由于底盘控制周期（一般1-2ms）往往远小于遥控器数据的刷新周期14ms，所以在底盘控制进程看来，遥控器的输入是阶跃的，这样会使底盘开起来有卡顿感，不够流畅，此外速度突变也容易导致底盘出现超功率。为此需要对输入信号进行平滑处理，处理的方式有很多，比如数字滤波或者斜坡函数。

下图为底盘控制进程获得的遥控器原始数据。

经过低通滤波平滑之后

通过斜坡函数处理

小陀螺

小陀螺是近几年兴起的一股技术风潮，自从中国矿业大学掏出这一黑科技，并取得全国四强的好成绩之后，众多学校纷纷效仿。

使用小陀螺的机器人可以提高对方的瞄准难度，降低被打中装甲板的概率，提高生存率，此外由于云台可以在底盘不动的情况下指向任意位置，也有了更高的灵活性。

小陀螺的实现关键是导电滑环，底盘到云台的过线必须经过导电滑环才能避免在底盘360°旋转时发生线缠住的问题。目前RM选购滑环的主要厂家有森瑞普， 默孚龙等。我们在备赛时，这两家的产品都使用过，但是最后用在步兵车上的默孚龙的滑环是出了一些问题的，滑环在旋转到不同角度时压降不一致，使传感器的供电不稳定，发生自动复位，导致了比较严重的后果。

下面的图片来自森瑞普官网上的新闻

https://www.senring.cn/robomaster/20190609.html

选购导电滑环前需要提前设计好整车的硬件拓扑结构，根据该结构计算需要通过滑环的型号，线数，电源线所需要的电流上限等，再结合机械方面的需求，比如如果要做下供弹可能需要过孔滑环，以及需不需要带法兰等。确认好这些需求之后去联系厂家，厂家会有客服以及选型手册帮助完成选型。

选滑环的坑是比较多的，所以当设计方案要使用滑环时，最好做好迭代一两次的心理准备。

另外由于麦克纳姆轮的特性，我们是可以实现在底盘360°旋转的同时，依然正常的前后左右平动的。

由于底盘在360°转动，我们以图传朝向为正方向，建立云台坐标系。假设底盘坐标系S1，云台坐标系S2，为了实现该功能，我们需要将S2下的速度期望转换到S1坐标系下，假设S2下速度期望为

\[\vec{V_{S2}} = \left[ V_{x2},V_{y2} \right]
\]

S1下速度期望为

\[\vec{V_{S1}} = \left[ V_{x1},V_{y1} \right]
\]

S2和S1之间的夹角为\(\theta\)，可以得到如下公式

\[\left[ \begin{array}{} V_{x1} \\ V_{y1} \end{array}\right]=\left[ \begin{array}{} \cos{\theta}&-\sin{\theta} \\ \sin{\theta}&\cos{\theta} \end{array}\right]\left[ \begin{array}{} V_{x2} \\ V_{y2} \end{array}\right]
\]

即

\[\begin{cases} V_{x1} = V_{x2}*\cos{\theta} - V_{y2}*\sin{\theta} \\ V_{y1} = V_{x2}*\sin{\theta} + V_{y2}*\cos{\theta}\end{cases}
\]

我们在底盘小陀螺模式下通过这样一个公式将云台坐标系期望解算到底盘坐标系期望之后，就和普通的底盘控制过程一模一样了。具体的代码实现就不贴了，总共也就几行就可以写出来，\(\sin\)和\(\cos\)可以#include "math.h"后直接调用。

结语

本次教程主要介绍了麦克纳姆轮底盘的运动解算原理以及代码实现，基本上结合之前的几讲，一个能动的底盘就能够写出来了，但是底盘的主要难度是在和云台结合起来之后，会有更多的运动模式，比如底盘跟随云台，底盘和云台独立，扭屁股，小陀螺等，另外软件功率控制以及操作手感的提升，也是一些值得研究的问题。