代码收藏家 robomaster麦轮运动解算
1.资源与代码
1.1 参考文章
本文主要参考的三篇文章如下：
麦轮运动特性分析：
https://mp.weixin.qq.com/s?biz=MzI3MTIyMjQwNQ==&mid=2247483844&idx=1&sn=a8c93bc62c80d61502f0083b466e7eec&chksm=eac4522eddb3db3835452993bc71088de1ea799d5bc9efb1c030b371a69b74f4bd6502d08f91&scene=21#wechat_redirect
麦轮运动分析基础上的整个机器人的运动分析：
https://mp.weixin.qq.com/s/GWhlXsuY6QYyoZydaSYpjQ
1.2 验证代码
//（轴距+轮距）/2 /57.3
static float rotate_ratio_f = ((WHEELBASE+WHEELTRACK)/2.0f - GIMBAL_OFFSET)/RADIAN_COEF;
static float rotate_ratio_b = ((WHEELBASE+WHEELTRACK)/2.0f + GIMBAL_OFFSET)/RADIAN_COEF;
static float wheel_rpm_ratio = 60.0f/(PERIMETER*CHASSIS_DECELE_RATIO);
int16_t wheel_rpm[4];
wheel_rpm[0] = (+vx - vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[1] = (+vx + vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[2] = (-vx + vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
wheel_rpm[3] = (-vx - vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
最后希望达成的目的是，通过计算来得出wheel_rpm各轮的速度值，来验证代码的正确性。
2. 麦轮特性与单轮分析
这一部分主要是参考文章
https://mp.weixin.qq.com/s?biz=MzI3MTIyMjQwNQ==&mid=2247483844&idx=1&sn=a8c93bc62c80d61502f0083b466e7eec&chksm=eac4522eddb3db3835452993bc71088de1ea799d5bc9efb1c030b371a69b74f4bd6502d08f91&scene=21#wechat_redirect
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-oNTHxZX1-1649517566223)(…/img/RM麦轮运动解算/640.png)]
2.1单轮受力分析
与普通的轮子转动后得到的速度是竖直的不同，麦轮转动后得到的速度是斜向的，为什么会这样？因为麦轮的轮毂上有辊子的存在，并且最重要的是辊子是以45°的方式进行安装的。以上图b中的麦轮为例，当轮毂转动的时候，辊子轮流接地，与地面进行摩擦，我们可以单独地分析接地的那一个辊子的受力。
以轮子向前转为例，轮子前转的时候可以将接地的辊子当成一个静止的点进行摩擦，地面会给辊子一个向前的摩擦力。将这个摩擦力沿辊子、垂直辊子方向进行分解。
分解成平行于辊子的力和垂直于辊子的力，其中垂直于辊子的力会导致辊子进行转动，这是滚动摩擦，是非常小的，因此对轮子的运动不会产生什么作用，我们可以认为这个方向上的力会因为辊子的滚动而被化掉了。
但是因为辊子有物理的限位，不会在平行于辊子轴的方向上进行滚动，因此平行于辊子轴的力产生的是滑动摩擦。这个力是对我们的轮子的运动起到至关重要的作用的力，这个力的方向自然也就是轮子在向前滚动时会移动的朝向。也就是说，图中的该麦轮在向前滚的时候会向左前方运动。
为什么可以将垂直于辊子方向上的力认为成不对轮子运动产生影响的力？
①假如辊子轴线和轮毂轴线平行（如图 3.1），轮毂受电机驱动而主动转动，则地面作用于辊子的摩擦力全为滚动摩擦力，滚动摩擦力全部用于驱动辊子飞速转动，但麦轮本身并不会有丝毫的前进或后退。这个现象就如滚子轴承（或行星齿轮），外圈固定，内圈疯狂转动，但被滚子转动给抵消掉了，外圈自身并不会运动。
②我们日常滑轮滑时，如果让两只轮滑鞋方向始终保持平行运动，我们肯定是原地不动的，只能让别人来推动前进，所以我们要让两只滑冰鞋呈一定夹角（外八字）而相对运动，这样我们才能这种“夹角”借力逐渐“提升”速度，这个力就是平行于轮子轴线的分力，这个分力和惯性会使得我们“向前”运动，而垂直于轮子轴线的分力只是促使轮子滚动运动合力中一个部分，并不“推动”我们“向前”运动。
所以垂直于轮毂轴线的分力并不用于主动驱动机器人运动，而是被辊子消耗掉了。
2.2 单轮速度与转速关系
上面进行了轮子受力的分析，速度与力是一一对应的关系，因此速度的分析与力完全相同。如下：
由于45°的角度关系，我们可以很轻易地通过轮子的转速来计算出辊子的速度，当然反之亦然。但是要注意的是V轮是线速度，而我们一般所说的电机的转速是rpm，也就是每分钟多少转。我们通过这个“真正的转速rpm”可以得到的是角速度，角速度到线速度还要进行一步转换。这里的转化，在代码中也会有所体现。这里我们先以线速度来进行运算，即 V辊 = V轮*cos45°
3. 麦轮底盘运动分析。
麦轮底盘之所以可以全向运动是因为通过两种不同的麦轮（A、B轮）通过特定的安装方式（最常见的是O型安装方法），再经过对应轮子进行对应方向的转动来产生斜向的速度，而斜向的速度可以被分解成竖直方向和水平方向上的速度，通过四个轮子在两个方向上的速度的叠加与相互抵消来进行全向运动。
具体的进行各方向运动时各轮子的转动方向的分析在下面链接文章中的第2部分有详细地介绍。
https://mp.weixin.qq.com/s/GWhlXsuY6QYyoZydaSYpjQ
4. 底盘逆运动学解算
4.1 逆运动学
正运动学：通过各个轮子的转速来计算出底盘运动的速度与方向
逆运动学：通过所需要的底盘运动的方向，计算出每个轮子应该有怎样的转速与转向才能使得底盘以我们期望的方式进行运动。
在进行逆运动学解算的时候我们可以将底盘的运动看成是由三个方向的运动进行叠加而成的，竖直方向的运动，水平方向的运动，旋转的运动。 这三个量也是我们在代码中可以很方便地得到的，在编程的时候会非常的方便。
我们要做的就是假设有一个底盘的速度（竖直、水平方向上的速度）和旋转速度，通过这个速度来算出每个轮子的辊子平行于轴方向上的速度，进而通过辊子速度与轮子转速的关系来得出每个轮子的转速。
下面以O型安装方式为例来进行分析与演算。
4.2 解算过程
解算过程：
1. 假设一个底盘的速度Vc，那么每一个轮子上都会存在这样一个速度Vc
2. 假设一个底盘加速度W
3. 通过刚体旋转公式得到对应轮子的旋转速度Vw
4. 轮子的Vc与Vw进行速度的合成得到轮子满足目标运动所需的速度Vx（V0,V1,V2,V3）
5. 将Vx沿辊子轴方向分解得到Vx平行
6. 根据根据辊子沿辊子轴平行方向速度与轮子的转速的关系，得到轮子转速
写在前面：
可能在分析的时候会有疑问，为什么要让Vx分解得到V平行？V平行不是提供实际的速度的么？
不对，因为我们现在是逆运动学，我们的目的实际上是用V平行与V垂直来表示V0（也就是实际需要的速度）。因为我们唯一知道的可以得到轮子转速的关系是v平行与轮子转速的关系，因此我们只要知道在期望的运动中v平行是个什么状态，我们就可以得到轮子的转速。
将四个轮从右上顺时针编号。
对0号轮分析：
对1号轮分析：
对2号轮分析：
对3号轮分析：
最后得出的各轮对应的速度如下：
5.联系代码
我们来把式子中的量转换成我们代码中的量
在转换的时候我们首先需要明确我们有哪些量，以及这些量的单位和式子中的量的单位。
- 我们拥有电机的转速，我们最后要的得到的量也是转速，但是转速的单位是rpm：转每分钟。因此我们需要将线速度转换成rpm单位
- 式子中w的单位是rad/s，即每秒的弧度。但是我们所知道的旋转的角度是通过imu得到的，单位是度每秒
- 电机具有转速比，我们能控制的转速与实际的转速有一个倍数的关系。以RM3508电机为例，转速比是19:1。
- 我们知道的是轮距与轴距，但是式子中的t与b是半轮距与半轴距
首先rad/s转deg/s：
(rad/s)/57.3 = deg/s
对应我们代码中的
rotate_ratio_f = ((WHEELBASE+WHEELTRACK)/2.0f - GIMBAL_OFFSET)/RADIAN_COEF; //其中GIMBAL_OFFSET恒为0
所以：vw * rotate_ratio_f = vw * (WHEELBASE+WHEELTRACK)/2.0f/RADIAN_COEF;
然后m/s或者mm/s（因为代码中用的单位是mm）转rpm：
mm/s = 60/轮子周长 rpm
再加上减速比则为：
mm/s = 60/(轮子周长*减速比) rpm
对应代码中的
wheel_rpm_ratio = 60.0f/(PERIMETER*CHASSIS_DECELE_RATIO);
所以我们将计算出的式子转换成代码如下：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-TzsO0p4Z-1649517566230)(…/img/RM麦轮运动解算/2-16495154947223.png)]
来对比一下代码
wheel_rpm[0] = (vx - vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[1] = (vx + vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[2] = (-vx + vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
wheel_rpm[3] = (-vx - vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
我们的代码是这样的：
wheel_rpm[0] = (-vx + vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[1] = (vx + vy - vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[2] = (-vx + vy - vw * rotate_ratio_b) * wheel_rpm_ratio;
wheel_rpm[3] = (vx + vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
我们惊奇地发现，费这么大劲，md符号对不上……
为什么？不卖关子，原因出在电机安装上。 因为车上的四个电机不是一顺的，正常情况下一侧是顺的，另一侧是反的。
在我们这台车上，1,2号电机是正常的，也就是我们计算出的转速是正的，给到电机时轮子就会正转。但是0,3号电机是反的，这是安装需要导致的。也就是我们计算出的转速是正的，我们需要反一下才可以使电机达到正转的效果。
因此我们需要将wheel_rpm[0]和wheel_rpm[3]前加上负号即
wheel_rpm[0] = (vx - vy - vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[1] = (vx + vy - vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[2] = (-vx + vy - vw * rotate_ratio_b) * wheel_rpm_ratio;
wheel_rpm[3] = (-vx - vy - vw * rotate_ratio_b) * wheel_rpm_ratio;
我们会发现，符号还不照。为啥？ 通过观察差别，发现vx和vy前的符号都一样了，只是vw前全反了。
为什么会造成这样的结果？ 方向反了？
因为代码中的vw本身是自带正负号的，而我们计算的时候是以逆时针旋转来看的，如果顺时针的话vw * rotate_ratio_f这一大坨会在前面加一个负号，因此我们如果计算通用的情况的话，应该用正号，具体正负，由imu传来的vw来控制
因此vw的系数要是+
所以我们最终得到的代码应该是：
wheel_rpm[0] = (vx - vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[1] = (vx + vy + vw * rotate_ratio_f) * wheel_rpm_ratio;
wheel_rpm[2] = (-vx + vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
wheel_rpm[3] = (-vx - vy + vw * rotate_ratio_b) * wheel_rpm_ratio;
Bingo！
