C语言调用RoverCmd控制火星车路径遍历Matplotlib轨迹不连贯求助

前置参数校准 #

首先确认你已经明确所有运动学映射规则，避免基础数值错误：

• RoverCmd(left, right)的参数和轮速映射：参数100对应1m/s轮速，单次调用执行时长固定为0.1s，因此单轮单次调用移动距离为参数值 * 0.001m
• 必须提前获取火星车轮距L（左右轮中心点的水平距离，单位m），这是差速转向计算的核心参数，缺失会直接导致轨迹漂移

核心控制逻辑实现 #

推荐使用纯追踪算法实现路径点跟踪，逻辑简单且适配差速运动模型，实现步骤如下：

1. 维护火星车实时位姿：每次调用RoverCmd后更新当前坐标(x,y)和航向角θ（x轴正方向为0，逆时针为正），更新公式如下：

#define DT 0.1f // 单次控制周期
#define MAX_SPEED 1.0f // 最高速度1m/s
#define WHEEL_SPAN 0.2f // 替换为你实际测量的轮距，单位m

// 输入：左右轮的RoverCmd参数，输出更新后的位姿
void update_pose(int left_cmd, int right_cmd, float *x, float *y, float *theta) {
    float v_left = left_cmd * 0.01f; // 转成m/s单位的轮速
    float v_right = right_cmd * 0.01f;
    float v = (v_left + v_right) / 2.0f;
    float omega = (v_right - v_left) / WHEEL_SPAN;

    if (fabs(omega) < 1e-6) { // 直线运动场景
        *x += v * cos(*theta) * DT;
        *y += v * sin(*theta) * DT;
    } else { // 转向运动场景
        float R = v / omega;
        *x += R * (sin(*theta + omega * DT) - sin(*theta));
        *y += R * (-cos(*theta + omega * DT) + cos(*theta));
        *theta += omega * DT;
    }
    // 角度归一化到[-π, π]防止溢出
    *theta = atan2(sin(*theta), cos(*theta));
}

2. 计算目标轮速参数：

• 先计算当前位姿到目标点(tx, ty)的朝向差α = atan2(ty - *y, tx - *x) - *theta
• 计算当前到目标点的直线距离d = sqrt(pow(ty - *y, 2) + pow(tx - *x, 2))
• 计算转向曲率k = 2 * sin(α) / d
• 计算左右轮速并转成RoverCmd参数：

float expect_v = 0.8f; // 设定前进速度，留20%余量给转向差速
float v_left = expect_v - k * WHEEL_SPAN / 2.0f;
float v_right = expect_v + k * WHEEL_SPAN / 2.0f;

// 参数钳位到[-100,100]范围
int left_cmd = (int)round(v_left * 100);
int right_cmd = (int)round(v_right * 100);
left_cmd = left_cmd > 100 ? 100 : (left_cmd < -100 ? -100 : left_cmd);
right_cmd = right_cmd > 100 ? 100 : (right_cmd < -100 ? -100 : right_cmd);

3. 到达判定：设置合理的到达阈值，比如d < 0.05m时就判定到达当前路径点，切换下一个目标，避免在目标点附近反复震荡。

常见误差排查 #

• 先做单场景校准测试：连续调用10次RoverCmd(100,100)，看实际位移是否为1m，确认速度映射正确；调用RoverCmd(50,-50)31次左右，看是否原地旋转180度，确认轮距参数正确
• 检查航向角方向：确认左轮速度大于右轮时航向角是否对应增加（左转），如果方向相反要调整角速度计算的正负号
• 如果使用了外部定位，确认定位数据的更新频率和RoverCmd的调用频率对齐，避免用过时的位姿计算控制量

内容的提问来源于stack exchange，提问作者Thunder Thigh