基于CarSim与Simulink联合仿真的MPC/Stanley/Pure Pursuit算法对比与实现 – 低速自动驾驶入门指南

1次阅读

共计 2209 个字符，预计需要花费 6 分钟才能阅读完成。

环境配置复杂：CarSim 与 Simulink 的联合仿真需要正确配置 S -Function 接口、车辆动力学参数匹配（如质量 /Mass、转动惯量 /Moment of Inertia 等）
算法选择困难：MPC（模型预测控制 /Model Predictive Control）、Stanley（斯坦利控制器）和 Pure Pursuit（纯追踪算法）在低速场景（<5m/s）下的表现差异不直观
调试效率低下：参数调优缺乏系统性方法（如 Stanley 的前视距离 /Look-ahead Distance 与速度的关系）

算法类型	计算复杂度	适用速度范围	对道路曲率适应性	典型应用场景
MPC	高	全速域	极强	高精度路径跟踪
Stanley	中	中低速	较强	园区物流车
Pure Pursuit	低	低速	一般	停车场自动泊车

CarSim 配置（需确保 CarSim 2020 以上版本）：
在 Vehicle Dynamics 模块设置采样时间（Sample Time）为 0.01s
输出接口选择 Simulink 模式，勾选关键信号：
- 车辆位置 /Vehicle Position（X/ Y 坐标）
- 横摆角 /Yaw Angle
- 车速 /Velocity

Simulink 接口配置（关键 MATLAB 脚本）：

% 加载 CarSim S-Function（需先配置 CarSim 数据库路径）[~,~,~] = dos('carsim_build -setup');
cs_block = 'carsim_lib/CarSim S-Function';
add_block(cs_block, 'model/carsim_interface');

% 设置联合仿真参数（单位统一为 SI 制）set_param('model/carsim_interface', 'Ts', '0.01');
set_param('model/carsim_interface', 'OutputBusMode', 'on');

function [steer] = MPC_Controller(refPath, vehicleState)
    % 定义预测时域（Prediction Horizon）和控制时域（Control Horizon）Np = 20;  % 20 步预测
    Nc = 5;   % 5 步控制

    % 构建代价函数（Cost Function）权重矩阵
    Q = diag([10, 1, 5]);  % 跟踪误差 / 横向误差 / 航向误差权重
    R = 0.1;              % 控制量权重

    % 调用 quadprog 求解优化问题（需预先建立车辆动力学模型）options = optimoptions('quadprog', 'Display', 'off');
    [u, ~] = quadprog(H, f, A, b, [], [], lb, ub, [], options);
    steer = u(1);  % 取第一个控制量作为方向盘转角
end

% Stanley 参数调优原则：前视距离与车速成正比
lookAheadDist = min(3, max(0.5, 0.3 * abs(velocity)));

% 横向误差计算（Cross Track Error）[nearestIdx, ~] = dsearchn(refPath(:,1:2), vehiclePos);
cte = norm(vehiclePos - refPath(nearestIdx,1:2));

% 航向误差补偿（Heading Error Compensation）headingError = refPath(nearestIdx,3) - yawAngle;
steer = atan(k * cte / velocity) + headingError;

动力学参数不匹配：CarSim 中的轮胎模型（如 Pacejka 参数）需与 Simulink 控制器假设一致
仿真步长设置：建议固定步长（Fixed-step）设置为 0.01s，变步长易导致离散系统不稳定
单位系统混乱 ：CarSim 默认使用英制单位，需在Setup > Units 中切换为 SI 制
初始状态对齐：仿真开始时车辆位置需与参考路径起点重合（误差 <0.1m）
信号延迟处理：实际车辆存在执行器延迟，需在 Simulink 中添加 Transport Delay 模块（典型值 100-200ms）

flowchart TD
    A[生成参考路径] --> B[初始化车辆状态]
    B --> C{算法选择}
    C -->|MPC| D[调用 QP 求解器]
    C -->|Stanley| E[计算 CTE]
    C -->|Pure Pursuit| F[寻找预瞄点]
    D/E/F --> G[记录跟踪误差]
    G --> H[绘制对比曲线]