基于跟踪轮与IMU的机器人弧线路径里程计定位误差排查

问题描述 #

通过垂直跟踪轮（检测前后移动）、水平跟踪轮（检测左右移动）+ IMU搭建里程计系统，每10ms计算一次机器人位移实现实时定位：

• 跟踪轮转动量转英寸：vertical_rotation_sensor.get_position() / 5729.57795131（水平轮同理）
• IMU姿态转弧度：imu.get_rotation() * M_PI / 180
• 垂直轮偏移跟踪中心-10英寸，水平轮偏移4.21875英寸

直线运动时误差符合预期（前进24英寸，x=24.03、y=0.05），但旋转一周回到起点后误差达数英寸（x=0.49、y=-4.45），轮子位移与姿态变化计算已验证正常，需排查定位误差原因并修正。

原代码如下：

//设置跟踪轮常量
const double tracking_wheel_offset_vertical = -10; //跟踪轮距机器人跟踪中心的垂直偏移量（英寸）
const double tracking_wheel_offset_horizontal = 4.21875; //跟踪轮距机器人跟踪中心的水平偏移量（英寸）

//设置自主模式初始值
double x = 0; //全局X坐标
double y = 0; //全局Y坐标
double delta_local_y = 0; //弧的垂直弦长/跟踪中心的垂直位移
double delta_local_x = 0; //弧的水平弦长/跟踪中心的水平位移

//更新机器人预估位置的函数
void update_pose() {
    //初始化前一次计算的数值为当前值
    double previous_total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
    double previous_total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
    double previous_theta = imu.get_rotation() * M_PI / 180;

    while (true) {
        //延迟10ms以节省计算资源
        pros::delay(10);

        //获取跟踪轮的当前位移（英寸）和机器人当前姿态（弧度制）
        double total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
        double total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
        double theta = imu.get_rotation() * M_PI / 180;

        //计算跟踪轮位移变化量和机器人姿态变化量
        double delta_wheel_movement_vertical = total_wheel_movement_vertical - previous_total_wheel_movement_vertical;
        double delta_wheel_movement_horizontal = total_wheel_movement_horizontal - previous_total_wheel_movement_horizontal;
        double delta_theta = theta - previous_theta;

        //更新前一次计算的数值，用于下一次循环
        previous_total_wheel_movement_vertical = total_wheel_movement_vertical;
        previous_total_wheel_movement_horizontal = total_wheel_movement_horizontal;
        previous_theta = theta;

        //计算弧的弦长/跟踪中心的位移量
        if (fabs(delta_theta) != 0) {
            delta_local_y = 2 * sin(delta_theta / 2) * (delta_wheel_movement_vertical / delta_theta + tracking_wheel_offset_vertical);
            delta_local_x = 2 * sin(delta_theta / 2) * (delta_wheel_movement_horizontal / delta_theta + tracking_wheel_offset_horizontal);
        } else {
            delta_local_y = delta_wheel_movement_vertical;
            delta_local_x = delta_wheel_movement_horizontal;
        }

        //计算弧弦的角度/跟踪中心的移动方向
        double average_theta = previous_theta - delta_theta / 2;

        //根据弦长和角度更新全局坐标
        x += delta_local_y * cos(average_theta) - delta_local_x * sin(average_theta);
        y += delta_local_y * sin(average_theta) + delta_local_x * cos(average_theta);

        //在屏幕上打印跟踪与调试信息
        pros::lcd::print(0, "X: %f", x);
        pros::lcd::print(1, "Y: %f", y);
        pros::lcd::print(2, "Theta: %f", theta * 180 / M_PI);
        pros::lcd::print(3, "Total Local X Movement: %f", total_wheel_movement_horizontal);
        pros::lcd::print(4, "Total Local Y Movement: %f", total_wheel_movement_vertical);
    }
}

误差原因分析 #

1. 平均姿态角计算错误：当前代码用previous_theta - delta_theta / 2计算平均角度，正确的平均姿态应为previous_theta + delta_theta / 2——时间段内的中间姿态是起始姿态加上姿态变化的一半，而非减去。
2. 坐标系转换逻辑颠倒：全局坐标更新时，delta_local_x和delta_local_y的旋转矩阵顺序错误，导致位移方向在旋转时出现偏差。
3. 零角度判断精度不足：用fabs(delta_theta) != 0判断无旋转，IMU噪声会导致微小角度变化被误判为旋转，触发不必要的弧长计算。
4. IMU角度溢出未处理：当IMU角度从359°跳变到0°时，delta_theta会出现-359°的异常值，直接计算会导致位移结果错误。

修正方案 #

1. 核心逻辑修正 #

• 调整平均姿态角计算：改为average_theta = previous_theta + delta_theta / 2，确保使用时间段中间姿态转换坐标。
• 修正旋转矩阵顺序：调整全局坐标更新公式，匹配本地位移到全局位移的标准转换逻辑。
• 设置角度阈值：用fabs(delta_theta) > 1e-6替代绝对零判断，过滤IMU噪声。
• 添加角度溢出处理：将跨0°的角度变化修正为合理范围（如-359°转为+1°）。

修正后的完整代码 #

//设置跟踪轮常量
const double tracking_wheel_offset_vertical = -10; //跟踪轮距机器人跟踪中心的垂直偏移量（英寸）
const double tracking_wheel_offset_horizontal = 4.21875; //跟踪轮距机器人跟踪中心的水平偏移量（英寸）
const double THETA_EPSILON = 1e-6; //角度变化阈值，过滤IMU噪声

//设置自主模式初始值
double x = 0; //全局X坐标
double y = 0; //全局Y坐标
double delta_local_y = 0; //弧的垂直弦长/跟踪中心的垂直位移
double delta_local_x = 0; //弧的水平弦长/跟踪中心的水平位移

//更新机器人预估位置的函数
void update_pose() {
    //初始化前一次计算的数值为当前值
    double previous_total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
    double previous_total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
    double previous_theta = imu.get_rotation() * M_PI / 180;

    while (true) {
        //延迟10ms以节省计算资源
        pros::delay(10);

        //获取跟踪轮的当前位移（英寸）和机器人当前姿态（弧度制）
        double total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
        double total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
        double theta = imu.get_rotation() * M_PI / 180;

        //计算跟踪轮位移变化量和机器人姿态变化量
        double delta_wheel_movement_vertical = total_wheel_movement_vertical - previous_total_wheel_movement_vertical;
        double delta_wheel_movement_horizontal = total_wheel_movement_horizontal - previous_total_wheel_movement_horizontal;
        double delta_theta = theta - previous_theta;

        //处理IMU角度溢出（如从359度跳到0度）
        if (delta_theta > M_PI) {
            delta_theta -= 2 * M_PI;
        } else if (delta_theta < -M_PI) {
            delta_theta += 2 * M_PI;
        }

        //更新前一次计算的数值，用于下一次循环
        previous_total_wheel_movement_vertical = total_wheel_movement_vertical;
        previous_total_wheel_movement_horizontal = total_wheel_movement_horizontal;
        previous_theta = theta;

        //计算弧的弦长/跟踪中心的位移量
        if (fabs(delta_theta) > THETA_EPSILON) {
            double radius_y = delta_wheel_movement_vertical / delta_theta + tracking_wheel_offset_vertical;
            delta_local_y = 2 * sin(delta_theta / 2) * radius_y;
            
            double radius_x = delta_wheel_movement_horizontal / delta_theta + tracking_wheel_offset_horizontal;
            delta_local_x = 2 * sin(delta_theta / 2) * radius_x;
        } else {
            delta_local_y = delta_wheel_movement_vertical;
            delta_local_x = delta_wheel_movement_horizontal;
        }

        //计算弧弦的角度/跟踪中心的移动方向
        double average_theta = previous_theta + delta_theta / 2;

        //根据弦长和角度更新全局坐标（修正旋转矩阵顺序）
        x += delta_local_x * cos(average_theta) - delta_local_y * sin(average_theta);
        y += delta_local_x * sin(average_theta) + delta_local_y * cos(average_theta);

        //在屏幕上打印跟踪与调试信息
        pros::lcd::print(0, "X: %f", x);
        pros::lcd::print(1, "Y: %f", y);
        pros::lcd::print(2, "Theta: %f", theta * 180 / M_PI);
        pros::lcd::print(3, "Total Local X Movement: %f", total_wheel_movement_horizontal);
        pros::lcd::print(4, "Total Local Y Movement: %f", total_wheel_movement_vertical);
    }
}

额外优化建议 #

• 校准跟踪轮转换系数：通过实际移动已知距离，验证并微调5729.57795131这个脉冲转英寸的系数。
• 定期校准IMU：避免长时间累积姿态误差，可在机器人启动或空闲时执行姿态校准。
• 增加滤波处理：对跟踪轮位移和IMU角度添加低通滤波，进一步抑制噪声影响。

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