小车误差校准仿真与算法研究

1. 背景介绍

在二维平面内，我们定义了世界坐标系 W 和小车坐标系 C。小车在行走过程中会存在位置和角度上的误差，而这些误差会影响小车到达目标位置的精度。为了校正小车的误差，我们引入了Ex、Ey 和角度误差 \Delta\theta 的概念。通过相机获得这些误差值，我们可以计算出小车在世界坐标系中的实际位置。

本次仿真旨在验证公式计算的小车实际坐标与理论值之间的偏差，以及通过调整误差参数 Ex、Ey 和 \Delta\theta 来观察对偏差的影响。

2. 公式推导

当小车到达目标直线 Q 时，我们引入如下误差参数：

• X方向误差 Ex ：小车在 X 方向上的位置偏差。
• Y方向误差Ey ：小车在 Y 方向上的位置偏差。
• 角度误差 \Delta\theta：小车的 X 轴相对于目标直线 ( Q ) 之间的角度偏差。

基于以上误差参数，我们可以推导出小车在世界坐标系下的实际位置 (X_{\text{实际}}, Y_{\text{实际}}) 和角度 \theta_{\text{实际}}，推导出的误差校准公式如下：

3. MATLAB 仿真代码

为了验证公式的准确性，我们通过 MATLAB 创建了一个仿真界面。以下是完整的 MATLAB 仿真代码：

function car
    % 创建主窗口
    fig = uifigure('Name', '小车仿真', 'Position', [100, 100, 800, 600]);
    
    % 创建“开始仿真”按钮
    btn = uibutton(fig, 'push', 'Text', '开始仿真', ...
                   'Position', [350, 550, 100, 30], ...
                   'ButtonPushedFcn', @(btn, event) startSimulation());

    % 创建显示小车参数的标签
    lblTheoretical = uilabel(fig, 'Position', [600, 480, 200, 50], 'Text', '理论位置:');
    lblActual = uilabel(fig, 'Position', [600, 430, 200, 50], 'Text', '上帝观测实际坐标:');
    lblAngle = uilabel(fig, 'Position', [600, 380, 200, 50], 'Text', '实际角度:');
    lblCalculated = uilabel(fig, 'Position', [600, 330, 200, 50], 'Text', '公式计算实际坐标:');
    
    % 设置全局变量用于存储仿真数据
    global X_car_actual Y_car_actual theta_actual;
    
    % 创建仿真绘图区域
    ax = uiaxes(fig, 'Position', [50, 50, 500, 500]);
    hold(ax, 'on');
    axis(ax, 'equal');
    xlabel(ax, 'X');
    ylabel(ax, 'Y');
    title(ax, '小车仿真');

    
    % 目标直线Q
    lineQ_y = 9; % 直线 Q 的 Y 坐标
    lineQ_x_center = 4; % 直线 Q 的中点在 X 方向的坐标
    plot(ax, [-5, 15], [lineQ_y, lineQ_y], 'k--', 'DisplayName', '目标直线 Q');
    
    % 小车的初始状态
    X_car_estimated = 0;
    Y_car_estimated = 0;
    theta = 0;
    
    % 编码器误差和角度误差（假设）
    Ex = 0.8; % X方向的误差
    Ey = -1.5; % Y方向的误差
    delta_theta = deg2rad(5); % 角度误差（5度）


    % 按钮点击事件处理函数
    function startSimulation()
        % 动态更新小车的状态
        X_car_actual = lineQ_x_center + Ex;
        Y_car_actual = lineQ_y + Ey;
        theta_actual = theta + delta_theta;
        
        % 小车检测到与目标直线Q的误差
        detected_Ex = Ex;
        detected_Ey = Ey;
        detected_angle_error = theta_actual;
        
        % 利用改进的误差校准公式，计算小车的实际位置
        [X_calibrated, Y_calibrated, theta_calibrated] = calculateActualPosition(lineQ_x_center, lineQ_y, detected_Ex, detected_Ey, detected_angle_error);
        
        % 更新显示文本
        lblTheoretical.Text = sprintf('理论位置: (%.2f, %.2f)', lineQ_x_center, lineQ_y);
        lblActual.Text = sprintf('上帝观测实际坐标: (%.2f, %.2f)', X_car_actual, Y_car_actual);
        lblAngle.Text = sprintf('实际角度: %.2f 度', rad2deg(theta_actual));
        lblCalculated.Text = sprintf('公式计算实际坐标: (%.2f, %.2f)', X_calibrated, Y_calibrated);
        
        % 显示实际位置
        cla(ax);
        hold(ax, 'on');
        plot(ax, [-5, 15], [lineQ_y, lineQ_y], 'k--', 'DisplayName', '目标直线 Q');
        plot(ax, lineQ_x_center, lineQ_y, 'ro', 'MarkerSize', 10, 'DisplayName', '理论位置');
        plot(ax, X_car_actual, Y_car_actual, 'bx', 'MarkerSize', 10, 'DisplayName', '上帝观测实际坐标');
        quiver(ax, X_car_actual, Y_car_actual, cos(theta_actual), sin(theta_actual), 1, 'k', 'LineWidth', 2, 'DisplayName', '小车朝向');
        plot(ax, X_calibrated, Y_calibrated, 'gs', 'MarkerSize', 10, 'DisplayName', '公式计算实际坐标');
        legend(ax);
    end

    % 计算小车的实际位置和角度的函数（基于旋转矩阵进行误差校准）
    function [X_actual, Y_actual, theta_actual] = calculateActualPosition(X_target, Y_target, Ex, Ey, delta_theta)
        % 使用旋转矩阵来进行校正
        rotation_matrix = [cos(delta_theta), -sin(delta_theta); sin(delta_theta), cos(delta_theta)];
        rotation_matrix_ni = [cos(delta_theta), sin(delta_theta); -sin(delta_theta), cos(delta_theta)];
         

        error_vector_world = rotation_matrix_ni * [Ex; Ey];
        ex = error_vector_world(1);
        ey = error_vector_world(2);


        % 将误差 (Ex, Ey) 转换为世界坐标系下的误差
        error_vector_world = rotation_matrix * [ex; ey];
        Ex_W = error_vector_world(1);
        Ey_W = error_vector_world(2);
        
        % 计算校准后的实际位置
        X_actual = X_target + Ex_W;
        Y_actual = Y_target + Ey_W;
        
        % 计算修正后的角度
        theta_actual = delta_theta;
    end
end

算法仿真结果