基于CarSim与Simulink联合仿真的低速自动驾驶控制算法实战：MPC、Stanley与Pure Pursuit对比与优化

1次阅读

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

低速自动驾驶（如园区接驳、仓储物流等场景）对控制算法提出了特殊要求：

高精度路径跟踪 ：狭窄通道和频繁启停需要厘米级跟踪精度
抗延迟能力 ：低速工况下执行器延迟（如制动响应）对系统影响更显著
舒适性约束 ：加速度变化率需平缓，避免急刹 / 猛冲

传统 PID 控制在非线性强、时变特性明显的低速场景中表现局限，主要表现为：

转弯时稳态误差大
参数鲁棒性差，换车需重新调参
难以处理路径曲率突变

算法	控制思想	适用场景	计算复杂度
MPC	滚动优化未来多步控制量	高动态 / 多约束场景	高
Stanley	前轮偏角 = 航向误差 + 横向误差补偿	结构化道路中低速跟踪	低
Pure Pursuit	基于前视距离的曲率控制	平滑路径跟踪	极低

精度优先 ：选择 MPC（需硬件支持实时计算）
快速部署 ：Stanley（调参直观）
资源受限 ：Pure Pursuit（嵌入式设备友好）

软件版本匹配 ：
CarSim 2020.0+
MATLAB R2019b+
安装 CarSim S-Function 模块

接口配置流程 ：

% 加载 CarSim 数据库
cs_load_database('LowSpeedAuto.sim');
% 设置 Simulink 接口参数
set_param(gcs, 'Solver', 'ode4', 'FixedStep', '0.01');

信号映射关系 ：
CarSim 输出：车辆位姿、轮速等
Simulink 输入：方向盘 / 油门 / 制动指令

function [steer, accel] = MPC_Controller(refPath, vehicleState)
    % 定义预测时域和控制时域
    p = 20;  % Prediction horizon
    m = 5;   % Control horizon

    % 构建代价函数（示例）cost = @(u) sum((refPath.Y(1:p) - predictY(u)).^2) + 0.1*sum(u.^2);

    % 调用 fmincon 求解
    options = optimoptions('fmincon','Display','off');
    u_opt = fmincon(cost, zeros(m,1), [], [], [], [], -1, 1, [], options);

    % 输出第一控制量
    steer = u_opt(1);
    accel = 0.3;  % 固定低速巡航
end

function steer = StanleyController(e_y, e_psi, v)
    % 参数定义
    k = 0.3;      % 增益系数
    soft_max = 25; % 最大前轮转角 (deg)

    % 计算转向角
    steer_raw = e_psi + atan(k * e_y / (v + 0.1));  % 防除零
    steer = min(max(steer_raw, -deg2rad(soft_max)), deg2rad(soft_max));
end