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

小鱼

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

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

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

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

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

原理介绍

待补充

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. 总结

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