跳转至

底盘运动学正逆解算

写在前面

我的底盘控制思路为使用电机的速度模式，通过运动学正逆解算将任务的 X,Y 运动距离转化为四个轮子的转速，从而实现底盘运动。

根据电机编码器返还的值来计算当前底盘位置，从而实现位置闭环控制。 根据陀螺仪的角度来计算当前底盘的角度，从而实现角度闭环控制。

这样可以精细控制电机的运动状态，比如在运动过程中实现边旋转变前进。

底盘运动学正逆解算

关于麦克纳姆轮的运动学正逆解算可以参考：

但是如果想要变旋转边运动我们还需要引入旋转角度 \(\theta\) 影响

运动学正逆解的本质

先回顾一下：

  • 正解（Forward Kinematics）：已知轮子转速 → 算小车的速度或位置变化。

  • 逆解（Inverse Kinematics）：已知小车想要的速度或位移 → 算每个轮子应该转多少。

在这里，小车既能 前后移动，又能 左右移动，还可能 原地旋转。 所以我们有两套坐标系：

车体坐标系（局部坐标系）

  • 原点在车中心。
  • x 轴指前方，y 轴指左方。
  • 小车内部的控制、速度、轮子运动都在这个坐标系里计算。

全局坐标系（绝对坐标系）

  • 地图坐标，固定不动。
  • 小车在地图上的实际位置和方向。

旋转矩阵的作用

假设小车 车头朝东（x轴），它想向前走 1 米，在局部坐标系里就是 \((1, 0)\)。

  • 如果地图上北是 y 轴正方向、东是 x 轴正方向，车头朝东 → 不需要旋转。
  • 如果车头朝北（局部 x 轴指北） → “局部前进 1 米”在全局坐标里是 \((0, 1)\)。

也就是说：局部坐标系里的前方，在全局坐标系里不一定是同一个方向。

旋转矩阵就是用来把 局部坐标（车体坐标系） 转换到 全局坐标系 的工具，这样我们就可以使得运动过程中的旋转不会影响到我们走直线。

旋转矩阵的原理

二维旋转矩阵：

\[ \begin{bmatrix} x_\text{global} \\ y_\text{global} \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x_\text{local} \\ y_\text{local} \end{bmatrix} \]

假设：

  • 小车朝向 θ = 45°（前方指向东北）
  • 想在全局坐标系里 正东 + 正北方向同时移动，速度分别为 Vx_global = 1, Vy_global = 1

逆解步骤：

旋转矩阵把全局速度换算到局部坐标系：

\[ \begin{aligned} Vx_{local} &= Vx_{global}\cosθ + Vy_{global}\sinθ = 1*0.707 + 1*0.707 ≈ 1.414 \\ Vy_{local} &= -Vx_{global}\sinθ + Vy_{global}\cosθ = -1*0.707 + 1*0.707 ≈ 0 \end{aligned} \]
  • 结果：在小车自己的坐标系里，它只需要沿前方跑 1.414 米/秒，不需要横向移动
  • 旋转矩阵“帮你把全局想走的方向转换成车能直接理解的方向”。

然后加上 角速度 ω，再通过麦克纳姆轮逆解公式算四个轮子的转速。

这样小车就能 同时前进和旋转，边走边转。

代码

运动学逆解算代码：

C
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
void Kinematics_Mecanum_CalculateRPM(Kinematics_Struct *Kinematics)
{
    float linear_vel_x_mins;
    float linear_vel_y_mins;
    float angular_vel_z_mins;
    float tangential_vel;
    float x_rpm;
    float y_rpm;
    float tan_rpm;
    float angle_rad = 0;
    angle_rad = HWT101_Struct.CurrentAngle  * M_PI / 180.0f;//陀螺仪角度转弧度

    float vel_x =  Kinematics->Linear_X  * cosf(angle_rad) + Kinematics->Linear_Y  * sinf(angle_rad);
    float vel_y = -Kinematics->Linear_X  * sinf(angle_rad) + Kinematics->Linear_Y  * cosf(angle_rad);

    //convert cm/s to cm/min
    linear_vel_x_mins = vel_x * 60;
    linear_vel_y_mins = vel_y * 60;

    //convert rad/s to rad/min
    angular_vel_z_mins = Kinematics->Angular_Z * 0.6 ;

    tangential_vel = angular_vel_z_mins * (FR_WHEELS_DISTANCE / 2 + LR_WHEELS_DISTANCE / 2);

    x_rpm = linear_vel_x_mins / WHEEL_CIRCUMFERENCE;
    y_rpm = linear_vel_y_mins / WHEEL_CIRCUMFERENCE;
    tan_rpm = tangential_vel / WHEEL_CIRCUMFERENCE;

    //calculate for the target motor RPM and direction
    Kinematics->M1_RPM = x_rpm - y_rpm - tan_rpm;//M1电机目标转速 B

    Kinematics->M2_RPM = x_rpm + y_rpm + tan_rpm;//M2电机目标转速 C

    Kinematics->M3_RPM = x_rpm + y_rpm - tan_rpm;//M3电机目标转速 A

    Kinematics->M4_RPM = x_rpm - y_rpm + tan_rpm;//M4电机目标转速 D
}

对于正解代码来说，因为张大头电机返回的就是距离值，所以直接可以通过瞬时位移来计算当前速度，而不用再去求导了：

C
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
void Kinematics_Mecanum_GetVelocities(Kinematics_Struct *Kinematics)
{
    // 计算车体的线速度和角速度
    float angle_rad = 0;
    angle_rad =  HWT101_Struct.CurrentAngle * M_PI / 180.0f;

    // 基于麦克纳姆轮正解：车体坐标系下的线速度
    float vx = Kinematics->X -Measured_value.old_X;
    float vy = Kinematics->Y -Measured_value.old_Y;
    Measured_value.old_X = Kinematics->X;
    Measured_value.old_Y = Kinematics->Y;

    // 变换为全局坐标速度
    Kinematics->average_rps_x = vx * cosf(angle_rad) - vy * sinf(angle_rad);
    Kinematics->average_rps_y = vx * sinf(angle_rad) + vy * cosf(angle_rad);
    Kinematics->average_rps_a = (-Kinematics->M1_RPM + Kinematics->M2_RPM - Kinematics->M3_RPM + Kinematics->M4_RPM) / 4 / 60* WHEEL_CIRCUMFERENCE / (FR_WHEELS_DISTANCE / 2 + LR_WHEELS_DISTANCE / 2);
}

// 函数：计算小车位置
CarPosition_Struct calculateCarPosition(Kinematics_Struct *Kinematics)
{
    CarPosition_Struct Measured_Position;
    Measured_Position.Valid = SUCCESSRECEIVE;
    Measured_Position.x = 0;
    Measured_Position.y = 0;
    // 假设小车的初始位置为(0, 0)
    Measured_value.X += Kinematics->average_rps_x;
    Measured_value.Y += Kinematics->average_rps_y;

    float angle = Normalize_Angle(HWT101_Struct.CurrentAngle);

    if (angle == 0 || angle == 180 || angle == -180) {
        Measured_Position.x = Measured_value.X * 0.98;
        Measured_Position.y = Measured_value.Y * 0.965;
    } else if (angle == 90 || angle == -90) {
        Measured_Position.x = Measured_value.X * 0.965;
        Measured_Position.y = Measured_value.Y * 0.98;
    } else {
        Measured_Position.x = Measured_value.X * 0.98;
        Measured_Position.y = Measured_value.Y * 0.965;
    }

return Measured_Position;
}

因为我正解这一部分比较乱在很多地方都有引用，所以知道大概意思就好，注意电机读取的速度一定要快。

一些未经实验的东西

从理论上来说，我们可以通过运动学逆解得到的四个电机速度反带到运动学正解中，计算出当前小车位置。这样我们就可以通过二维卡尔曼滤波来增强整个系统的鲁棒性。

但是实际过程中，因为我们并没有做速度闭环而使用电机本身的速度闭环，所以在一开始时我们系统求出的速度和实际速度会有一个偏差，这个偏差会导致卡尔曼滤波发散。

我其实找到了一些解决方法，比如下面这篇文章中提到的，但是由于时间的关系我并实装在车上，感兴趣的同学可以参考：

关于位置、角度闭环

位置闭环用位置式PID这个没什么好说的。

角度闭环现在我使用的方式还是有一些问题的，主要原因是我直接将陀螺仪的角度进行比例缩放，用位置式 PID 将速度带入逆解公式的角速度中 ，这样会导致角速度在开始的时候是突变的，会直接按限值来运行。但是我也没找到比这个更好的方法。

完整代码