无人车自动驾驶：基于模型预测控制的自动避障Matlab代码

无人车自动驾驶自动避障模型预测控制 matlab 代码

当无人车学会"预判"：用模型预测控制玩转自动避障

深夜的测试场里，一辆无人车正以30km/h的速度冲向障碍物。就在距离只剩5米时，它突然向左急转，轮胎与地面摩擦发出刺耳声响，最终划出一道完美的弧线完成避让------这背后藏着模型预测控制（MPC）的"预判"魔法。

预测未来三步的控制器

传统PID控制像在玩"打地鼠"，出现偏差才手忙脚乱地修正。MPC则像围棋高手，提前计算未来3-5秒的轨迹。我们先用一个简化的二自由度车辆模型：

matlab 复制代码

% 车辆动力学模型（离散化）
dt = 0.1; % 采样时间
A = [1 dt 0  0;
     0 1  0  0;
     0 0  1  dt;
     0 0  0  1];
B = [0.5*dt^2 0;
     dt       0;
     0        0.5*dt^2;
     0        dt];

这个状态方程把车辆位置(x,y)、速度(vx,vy)打包成向量，通过矩阵运算预测未来状态。就像游戏里的"子弹时间"，控制器能在虚拟时空中试错不同操作方案。

避障的核心套路

我们在目标函数里埋了两个彩蛋：既要追踪参考路径，又要和障碍物保持安全距离。看看这个优化问题的设置：

matlab 复制代码

% 构建MPC优化问题
Q = diag([10, 1, 10, 1]); % 状态权重
R = 0.1*eye(2);          % 控制量权重
obstacle_penalty = 1000;  % 障碍物惩罚项

for k = 1:N
    cost = cost + (X(:,k)-xref)'*Q*(X(:,k)-xref) + U(:,k)'*R*U(:,k);
    % 障碍物约束
    distance = norm(X(1:2,k)-obstacle_pos);
    cost = cost + obstacle_penalty/(distance^2 + 0.1);
end

这里用了个巧妙的"软约束"：当车辆靠近障碍物时，惩罚项会像弹簧一样急剧增大，迫使控制器另寻他路。比起硬约束，这种方法避免优化无解，实测中更鲁棒。

Matlab实战：会画彩虹的避障车

跑完优化后，用下面这段代码可视化轨迹。注意那个会移动的红色禁区，它模拟了动态障碍物：

matlab 复制代码

% 动态更新轨迹可视化
figure;
h_robot = plot(x_traj,y_traj,'b-o','LineWidth',2);
hold on;
h_obstacle = plot(obstacle_pos(1),obstacle_pos(2),'r*','MarkerSize',15);

for t = 1:length(x_traj)
    set(h_robot,'XData',x_traj(1:t),'YData',y_traj(1:t));
    % 让障碍物随机移动
    if mod(t,3)==0
        obstacle_pos = obstacle_pos + randn(2,1)*0.2;
        set(h_obstacle,'XData',obstacle_pos(1),'YData',obstacle_pos(2));
    end
    drawnow;
end

运行时会看到蓝色轨迹像被磁铁吸引般绕过红色障碍，有时还会出现"假动作"------先假装右转诱使障碍物移动，再突然左转突围。这种博弈策略完全由优化自动生成。

黎明前的调试陷阱

实测时踩过这样的坑：某次避让时车辆突然高频抖动。检查发现是预测时域过长导致求解器陷入局部最优。后来加上控制量变化率的约束才解决：

matlab 复制代码

% 在优化问题中添加控制量变化率约束
for k = 2:N
    constraints = [constraints, ...
        -0.5 <= U(:,k)-U(:,k-1) <= 0.5]; % 方向盘转角变化率限制
end

这提醒我们：理论上的完美模型，落地时总要和传感器延迟、执行器响应等现实因素肉搏。不过看到无人车最终丝滑地绕开障碍时，那种"代码终于驯服钢铁"的成就感，或许就是工程师的浪漫吧。