【FishBot四驱版】9.3.4 运动学正逆解的实现（麦克纳姆轮版）

[小鱼]

9.3.4 运动学正逆解的实现（麦克纳姆轮版）

在前面机器人导航键盘控制时，我们下发的是线速度和角速度信息，并不会直接对轮子转速进行直接控制，这就需要我们把角速度和线速度转换成机器人轮子的速度，我们把这一过程称为运动学逆解，反之通过轮子的实际转速计算机器人的线速度和角速度的过程就是运动学正解。接着我们对运动学正逆解进行推导。

麦克纳姆轮（Mecanum Wheel）是一种特殊设计的轮子，通过改变轮子上滚轮的倾斜方向，可以实现全向移动能力。麦克纳姆轮机器人能够同时控制三个自由度：沿 ( x ) 轴的线速度 ( v_x )、沿 ( y ) 轴的线速度 ( v_y )、以及绕 ( z ) 轴的角速度 ( \omega )。为了实现这些自由度的控制，我们需要通过运动学正解和逆解完成速度与轮子转速的转换。

运动学逆解：将机器人期望的线速度和角速度转换为四个轮子的转速。

运动学正解：通过轮子实际的转速计算机器人整体的线速度和角速度。

原理介绍

一开始小鱼接触这个轮子的时候也很懵，后来抓住一个要点才明悟，这里把武功秘籍传授与你。

麦轮上的小轮子转动方向就是力的方向。

记住这句话，然后看图，小鱼店里卖的小车，麦轮是这样装的（注意小轮子）：

向前（X方向）运动受力原理：

当我们需要让小车前进的时候，就有下面的图：

• 白色箭头：电机前进方向
• 红色箭头：小轮子产生的力的方向
• 蓝色箭头：红色力方向在前进方向分力
• 绿色箭头：红色力方向在横向的分立

如果轮子转速相同，则红色箭头的力大小相同，绿色箭头的力会被相互抵消（M1抵消M2的，M3抵消M4的），这样就只剩下前进方向的力了。
假设机器人前进方向为V_x，每个轮子的转速是V_Mx(x是轮子的编号)，根据轮子转动的方向（向V_x正方向为正），此时就有：

V_M1 = V_x
V_M2 = V_x
V_M3 = V_x
V_M4 = V_x

向左(Y方向)平移受力原理：

假设机器人向右方向为V_y的负方向，每个轮子的转速是V_Mx(x是轮子的编号)，根据轮子转动的方向（向V_x正方向为正），此时就有：

V_M1 = -V_y
V_M2 = V_y
V_M3 = V_y
V_M4 = -V_y

逆时针旋转受力分析：

---

2. 运动学推导

2.1 麦克纳姆轮的数学建模

假设机器人具有四个麦克纳姆轮，编号为 1 至 4，分别位于机器人的四个角。定义如下参数：

• ( a )：机器人中心到轮子沿 ( x ) 方向的距离；
• ( b )：机器人中心到轮子沿 ( y ) 方向的距离；
• ( r )：轮子的半径；
• ( \omega_1, \omega_2, \omega_3, \omega_4 )：四个轮子的角速度。

每个轮子的线速度可以分解为 ( x ) 和 ( y ) 两个方向上的分量，加上角速度分量。通过分析轮子受力，我们得到以下速度关系：

2.2 运动学逆解公式

根据上述公式，给定机器人期望的 ( v_x )、( v_y )、( \omega )，我们可以计算出各轮子的速度：

38acab4-im

2.3 运动学正解公式

通过已知四个轮子的速度，求解机器人整体的运动状态：

3. 代码实现

以下是基于上述公式的运动学正解和逆解的代码实现。

3.1 运动学逆解

void Kinematics::kinematic_inverse(float linear_x_speed, float linear_y_speed, float angular_speed,
                                   float &out_wheel_speed1, float &out_wheel_speed2, float &out_wheel_speed3, float &out_wheel_speed4)
{
    const float a = 108.0f;  // 机器人中心到轮子沿x方向的距离
    const float b = 88.5f;   // 机器人中心到轮子沿y方向的距离

    out_wheel_speed1 = linear_x_speed - linear_y_speed - angular_speed * (a + b);
    out_wheel_speed2 = linear_x_speed + linear_y_speed + angular_speed * (a + b);
    out_wheel_speed3 = linear_x_speed + linear_y_speed - angular_speed * (a + b);
    out_wheel_speed4 = linear_x_speed - linear_y_speed + angular_speed * (a + b);

    // Debug 输出
    // Serial.printf("out_wheel_speed[%f,%f,%f,%f]\n", out_wheel_speed1, out_wheel_speed2, out_wheel_speed3, out_wheel_speed4);
}

3.2 运动学正解

void Kinematics::kinematic_forward(float wheel1_speed, float wheel2_speed, float wheel3_speed, float wheel4_speed,
                                   float &linear_x_speed, float &linear_y_speed, float &angular_speed)
{
    const float a = 108.0f;  // 机器人中心到轮子沿x方向的距离
    const float b = 88.5f;   // 机器人中心到轮子沿y方向的距离

    // 计算机器人线速度和角速度
    linear_x_speed = (wheel1_speed + wheel2_speed + wheel3_speed + wheel4_speed) / 4.0f;
    linear_y_speed = (-wheel1_speed + wheel2_speed + wheel3_speed - wheel4_speed) / 4.0f;
    angular_speed = float(-wheel1_speed + wheel2_speed - wheel3_speed + wheel4_speed) / (4.0f * (a + b));

    // Debug 输出
    // Serial.printf("angular_speed:%f wheel_speed[%f,%f,%f,%f]\n",angular_speed, wheel1_speed, wheel2_speed, wheel3_speed, wheel4_speed);
}

---

4. 总结

本文基于麦克纳姆轮的特点推导了运动学正解和逆解公式，并结合代码实现了相应的逻辑。通过运动学逆解，机器人能够根据给定的线速度和角速度生成各轮子的转速；而通过运动学正解，可以从轮子速度反推出机器人的运动状态。这些方法为全向移动机器人的控制提供了理论和实践支持。