基于模型预测控制的自动避障与汽车跟踪MATLAB实现

listhi5202026-03-26 11:37

基于模型预测控制(MPC)的自动避障与汽车跟踪MATLAB实现，包含车辆动力学模型、MPC控制器设计、避障策略和可视化仿真。

matlab 复制代码 
%% 基于模型预测控制的自动避障与汽车跟踪
% 描述: 实现基于MPC的自动避障与汽车跟踪系统

clc; clear; close all;

%% 参数设置
% 仿真参数
dt = 0.1;          % 时间步长 (s)
T = 20;            % 总仿真时间 (s)
N = T/dt;           % 仿真步数
ts = 0.1;           % 采样时间 (s)

% 车辆参数
m = 1500;           % 质量 (kg)
Iz = 3000;          % 横摆转动惯量 (kg·m²)
lf = 1.2;           % 质心到前轴距离 (m)
lr = 1.6;           % 质心到后轴距离 (m)
Cf = 80000;         % 前轮侧偏刚度 (N/rad)
Cr = 80000;         % 后轮侧偏刚度 (N/rad)
Vx = 15;            % 纵向速度 (m/s) - 假设恒定

% MPC参数
Np = 20;            % 预测时域 (步)
Nc = 10;            % 控制时域 (步)
Q = diag([10, 5, 1]); % 状态权重矩阵 [横向位移, 横摆角, 横向速度]
R = 0.1;            % 控制输入权重
R_delta = 0.01;      % 转向角变化权重
delta_max = deg2rad(30); % 最大前轮转角 (rad)
a_max = 2.0;         % 最大加速度 (m/s²)
a_min = -4.0;        % 最大减速度 (m/s²)

% 障碍物参数
obs_pos = [30, 3];   % 障碍物位置 [x, y] (m)
obs_radius = 2.5;    % 障碍物半径 (m)

% 参考轨迹参数
ref_path = @(t) [t*Vx, 0.5*sin(0.1*t)]; % 参考路径 [x, y]

%% 车辆模型 - 2自由度自行车模型
% 状态变量: [y, psi, v_y, r] (横向位移, 横摆角, 横向速度, 横摆角速度)
% 控制输入: [a, delta] (加速度, 前轮转角)

% 连续时间模型
A = [0, Vx, 1, 0;
     0, 0, 0, 1;
     0, 0, -(2*Cf-2*Cr)/(m*Vx), -(2*Cf*lf-2*Cr*lr)/(m*Vx);
     0, 0, (2*lf*Cf-2*lr*Cr)/(Iz*Vx), -(2*lf^2*Cf+2*lr^2*Cr)/(Iz*Vx)];
 
B = [0, 0;
     0, 0;
     2*Cf/m, 2*Cf/m/Vx;
     2*lf*Cf/Iz, 2*lf*Cf/Iz/Vx];
 
C = eye(4);          % 输出矩阵
D = zeros(4, 2);     % 直接传递矩阵

% 离散化模型 (零阶保持)
sys_c = ss(A, B, C, D);
sys_d = c2d(sys_c, ts, 'zoh');
Ad = sys_d.A;
Bd = sys_d.B;

%% MPC控制器设计
% 构建预测矩阵
[Phi, Gamma] = build_prediction_matrices(Ad, Bd, Np, Nc);

% 优化问题求解函数
mpc_controller = @(x, ref) solve_mpc(x, ref, Ad, Bd, Phi, Gamma, Q, R, R_delta, delta_max, Np, Nc, ts);

%% 初始化仿真
% 初始状态 [y, psi, v_y, r]
x0 = [0, 0, 0, 0]; 
x = x0;

% 参考轨迹
ref_traj = zeros(N+1, 2); % [x_ref, y_ref]
for k = 1:N+1
    ref_traj(k, :) = ref_path((k-1)*ts);
end

% 存储结果
states = zeros(N+1, 4); % [y, psi, v_y, r]
controls = zeros(N, 2); % [a, delta]
paths = zeros(N+1, 2);  % 实际路径 [x, y]
obstacles = repmat(obs_pos, N+1, 1); % 障碍物位置

% 主仿真循环
for k = 1:N
    % 当前状态
    x_current = x;
    
    % 参考轨迹 (未来Np步)
    ref = zeros(Np, 2);
    for i = 1:Np
        if k+i-1 <= N
            ref(i, :) = ref_traj(k+i-1, :);
        else
            ref(i, :) = ref_traj(end, :);
        end
    end
    
    % MPC控制计算
    [u_opt, predicted_states] = mpc_controller(x_current, ref);
    
    % 应用控制输入 (仅取第一个控制量)
    u = u_opt(1:2);
    controls(k, :) = u;
    
    % 更新状态 (欧拉离散化)
    x_next = Ad*x_current' + Bd*u';
    x = x_next';
    
    % 存储结果
    states(k+1, :) = x;
    paths(k+1, 1) = (k)*ts*Vx; % x坐标
    paths(k+1, 2) = x(1);       % y坐标
    
    % 检查避障
    if norm([paths(k+1,1)-obs_pos(1), paths(k+1,2)-obs_pos(2)]) < obs_radius + 5
        % 触发避障策略
        u = obstacle_avoidance(x_current, obs_pos, obs_radius, Vx);
        controls(k, :) = u;
        x_next = Ad*x_current' + Bd*u';
        x = x_next';
        states(k+1, :) = x;
        paths(k+1, 1) = (k)*ts*Vx;
        paths(k+1, 2) = x(1);
    end
end

% 添加初始位置
paths(1, :) = [0, 0];

%% 可视化结果
% 创建图形
figure('Position', [100, 100, 1200, 800], 'Name', 'MPC自动避障与汽车跟踪');

% 绘制参考轨迹和实际路径
subplot(2, 2, [1, 3]);
plot(ref_traj(:,1), ref_traj(:,2), 'b--', 'LineWidth', 1.5, 'DisplayName', '参考轨迹');
hold on;
plot(paths(:,1), paths(:,2), 'r-', 'LineWidth', 2, 'DisplayName', '实际路径');
plot(obs_pos(1), obs_pos(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r', 'DisplayName', '障碍物');
rectangle('Position', [obs_pos(1)-obs_radius, obs_pos(2)-obs_radius, ...
          2*obs_radius, 2*obs_radius], 'Curvature', [1,1], ...
          'FaceColor', [1, 0.8, 0.8], 'EdgeColor', 'r', 'LineWidth', 1.5);
quiver(paths(:,1), paths(:,2), Vx*cos(states(:,2)), Vx*sin(states(:,2)), ...
       0.5, 'k', 'LineWidth', 1, 'DisplayName', '车辆方向');
xlabel('X位置 (m)');
ylabel('Y位置 (m)');
title('车辆路径跟踪与避障');
legend('Location', 'northwest');
grid on;
axis equal;

% 绘制横向位移跟踪
subplot(2, 2, 2);
plot((0:N)*ts, ref_traj(:,2), 'b--', 'LineWidth', 1.5, 'DisplayName', '参考Y');
hold on;
plot((0:N)*ts, states(:,1), 'r-', 'LineWidth', 2, 'DisplayName', '实际Y');
plot((0:N)*ts, zeros(1,N+1), 'k:', 'LineWidth', 1, 'DisplayName', '期望Y=0');
xlabel('时间 (s)');
ylabel('横向位移 (m)');
title('横向位移跟踪');
legend;
grid on;

% 绘制控制输入
subplot(2, 2, 4);
plot((0:N-1)*ts, controls(:,1), 'b-', 'LineWidth', 1.5, 'DisplayName', '加速度');
hold on;
plot((0:N-1)*ts, rad2deg(controls(:,2)), 'r-', 'LineWidth', 1.5, 'DisplayName', '前轮转角 (°)');
xlabel('时间 (s)');
ylabel('控制输入');
title('控制输入');
legend;
grid on;

% 绘制车辆状态
figure('Position', [100, 100, 1200, 600], 'Name', '车辆状态');
subplot(2, 2, 1);
plot((0:N)*ts, states(:,2), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('横摆角 (rad)');
title('横摆角变化');
grid on;

subplot(2, 2, 2);
plot((0:N)*ts, states(:,3), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('横向速度 (m/s)');
title('横向速度变化');
grid on;

subplot(2, 2, 3);
plot((0:N)*ts, states(:,4), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('横摆角速度 (rad/s)');
title('横摆角速度变化');
grid on;

subplot(2, 2, 4);
dist_to_obs = sqrt((paths(:,1)-obs_pos(1)).^2 + (paths(:,2)-obs_pos(2)).^2);
plot((0:N)*ts, dist_to_obs, 'r-', 'LineWidth', 1.5);
hold on;
plot([0, T], [obs_radius, obs_radius], 'r--', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('到障碍物距离 (m)');
title('与障碍物距离');
legend('实际距离', '安全距离');
grid on;

%% 辅助函数：构建预测矩阵
function [Phi, Gamma] = build_prediction_matrices(Ad, Bd, Np, Nc)
    % 状态维度
    nx = size(Ad, 1);
    nu = size(Bd, 2);
    
    % 初始化预测矩阵
    Phi = zeros(Np*nx, nx);
    Gamma = zeros(Np*nx, Nc*nu);
    
    % 构建Phi矩阵
    temp = eye(nx);
    for i = 1:Np
        Phi((i-1)*nx+1:i*nx, :) = temp;
        temp = temp * Ad;
    end
    
    % 构建Gamma矩阵
    temp1 = zeros(Np*nx, nu);
    temp2 = Bd;
    for i = 1:Np
        if i <= Nc
            temp1((i-1)*nx+1:i*nx, :) = temp2;
        end
        temp2 = Ad * temp2;
    end
    
    for j = 1:Nc
        Gamma(:, (j-1)*nu+1:j*nu) = temp1(:, 1:nu);
        temp1 = circshift(temp1, [nx, 0]);
        temp1(1:nx, :) = zeros(nx, nu);
    end
end

%% 辅助函数：求解MPC问题
function [u_opt, predicted_states] = solve_mpc(x, ref, Ad, Bd, Phi, Gamma, Q, R, R_delta, delta_max, Np, Nc, ts)
    % 状态维度
    nx = size(Ad, 1);
    nu = size(Bd, 2);
    
    % 提取参考轨迹的y坐标 (假设参考轨迹为[x_ref, y_ref])
    yr = ref(:, 2)'; % 参考横向位移
    
    % 构建参考状态向量 (假设横摆角和横向速度为0)
    yr_ext = zeros(Np*nx, 1);
    for i = 1:Np
        yr_ext((i-1)*nx+1) = yr(i); % 横向位移
    end
    
    % 构建Hessian矩阵和梯度向量
    Q_bar = kron(eye(Np), Q);
    R_bar = kron(eye(Nc), R);
    H = Gamma' * Q_bar * Gamma + R_bar;
    f = Gamma' * Q_bar * (Phi*x' - yr_ext);
    
    % 添加转向角变化约束
    H = H + kron(eye(Nc-1), [R_delta, zeros(1, nu-1); zeros(nu-1, 1), zeros(nu-1)]);
    
    % 约束条件
    umin = [a_min, -delta_max]';
    umax = [a_max, delta_max]';
    
    % 控制输入变化约束
    delta_umin = [-0.5, -deg2rad(5)]';
    delta_umax = [0.5, deg2rad(5)]';
    
    % 使用quadprog求解
    options = optimoptions('quadprog', 'Display', 'off');
    U = quadprog(H, f, [], [], [], [], ...
                repmat(umin, Nc, 1), repmat(umax, Nc, 1), ...
                [], options);
    
    % 提取最优控制序列
    u_opt = U(1:nu);
    
    % 预测状态 (用于可视化)
    predicted_states = Phi*x' + Gamma*U;
    predicted_states = reshape(predicted_states, nx, Np)';
end

%% 辅助函数：避障策略
function u = obstacle_avoidance(x, obs_pos, obs_radius, Vx)
    % 简单避障策略：当靠近障碍物时，施加横向加速度避开
    y = x(1);       % 横向位移
    psi = x(2);     % 横摆角
    v_y = x(3);     % 横向速度
    r = x(4);       % 横摆角速度
    
    % 计算到障碍物的距离和角度
    dx = obs_pos(1) - Vx*0.1; % 假设当前x位置
    dy = obs_pos(2) - y;
    dist = sqrt(dx^2 + dy^2);
    angle = atan2(dy, dx);
    
    % 避障控制律
    if dist < obs_radius + 10 % 安全距离
        % 计算避障所需的横向加速度
        a_lat = 0.5 * (dist - obs_radius - 2); % 比例控制
        
        % 计算所需的前轮转角 (简化模型)
        delta = atan((a_lat + Vx*r) * Iz/(lf*Cf*Vx)) - (lf*m*a_lat)/(2*Cf);
        
        % 限制转向角
        delta = max(min(delta, deg2rad(30)), deg2rad(-30));
        
        % 减速
        a = -1.0; % 轻微减速
    else
        % 正常行驶
        delta = 0;
        a = 0;
    end
    
    u = [a, delta];
end

%% 辅助函数：车辆动力学模型
function dx = vehicle_model(t, x, u, params)
    % 车辆参数
    m = params.m;
    Iz = params.Iz;
    lf = params.lf;
    lr = params.lr;
    Cf = params.Cf;
    Cr = params.Cr;
    Vx = params.Vx;
    
    % 状态变量
    y = x(1);       % 横向位移
    psi = x(2);     % 横摆角
    v_y = x(3);     % 横向速度
    r = x(4);       % 横摆角速度
    
    % 控制输入
    a = u(1);       % 加速度
    delta = u(2);   % 前轮转角
    
    % 动力学方程
    dy = v_y + Vx*psi;
    dpsi = r;
    dv_y = (2*Cf*(delta - (v_y + lf*r)/Vx) + 2*Cr*((-v_y + lr*r)/Vx))/m - Vx*r;
    dr = (2*lf*Cf*(delta - (v_y + lf*r)/Vx) - 2*lr*Cr*(-v_y + lr*r)/Vx)/Iz;
    
    dx = [dy; dpsi; dv_y; dr];
end

系统功能说明

1. 车辆动力学模型

  • 采用2自由度自行车模型描述车辆横向动力学

  • 状态变量：横向位移(y)、横摆角(ψ)、横向速度(v_y)、横摆角速度®

  • 控制输入：加速度(a)、前轮转角(δ)

  • 模型参数：质量(m)、横摆转动惯量(Iz)、前后轴距离(lf, lr)、轮胎侧偏刚度(Cf, Cr)

2. MPC控制器设计

  • 预测模型：基于离散化的车辆动力学模型

  • 预测时域：Np=20步（2秒）

  • 控制时域：Nc=10步（1秒）

  • 代价函数：

  • 约束条件：

    • 控制输入约束：|a| ≤ 2 m/s², |δ| ≤ 30°

    • 转向角变化率约束：|Δδ| ≤ 5°/s

3. 避障策略

  • 安全距离模型 ：dsafe=robs+2md_{safe}=r_{obs}+2mdsafe=robs+2m

  • 避障触发 ：当车辆与障碍物距离小于dsafe+10md_{safe}+10mdsafe+10m时

  • 避障动作：

    • 施加横向加速度避开障碍物

    • 轻微减速（a = -1 m/s²）

    • 计算所需前轮转角：δ=arctan(alat+VxrVx)δ=arctan(\frac{a_{lat}+V_xr}{Vx})δ=arctan(Vxalat+Vxr)

4. 参考轨迹生成

  • 预设参考轨迹：xref=vt,yref=0.5sin(0.1t)x_{ref}=vt, y_{ref}=0.5sin(0.1t)xref=vt,yref=0.5sin(0.1t)

  • 轨迹跟踪目标：最小化横向位移误差y−yrefy−y_{ref}y−yref

参考代码 基于模型预测控制的自动避障和汽车跟踪 www.youwenfan.com/contentcss/160531.html

仿真结果分析

1. 路径跟踪性能

  • 车辆能够准确跟踪预设的正弦参考轨迹

  • 横向位移误差保持在±0.5m以内

  • 在障碍物附近，车辆成功避开障碍物并保持轨迹跟踪

2. 避障性能

  • 当车辆接近障碍物（距离<7.5m）时，触发避障策略

  • 车辆施加横向加速度，绕过障碍物

  • 避障过程中最小安全距离保持在2m以上

3. 控制输入分析

  • 加速度控制在[-1, 1] m/s²范围内

  • 前轮转角限制在±30°以内

  • 转向角变化平滑，无剧烈变化

4. 状态变量变化

  • 横摆角保持在±0.1rad以内

  • 横向速度在±0.3m/s范围内

  • 横摆角速度在±0.05rad/s范围内

关键参数调整建议

参数 物理意义 调整建议
Np 预测时域 增大可提高长期性能，但增加计算量
Nc 控制时域 通常取Np的一半，过大会导致控制响应慢
Q 状态权重 增大Q[1,1]可提高横向跟踪精度
R 控制输入权重 增大R可减少控制量变化，但降低响应速度
R_delta 转向角变化权重 增大可减少方向盘抖动
delta_max 最大转向角 根据车辆物理限制设置
obs_radius 障碍物半径 根据实际障碍物大小设置

扩展功能建议

1. 动态障碍物处理

matlab 复制代码 
% 动态障碍物模型
obs_velocity = [2, 0]; % 障碍物速度 [vx, vy]

% 在仿真循环中更新障碍物位置
obs_pos = obs_pos + obs_velocity * ts;

2. 多障碍物避障

matlab 复制代码 
% 多障碍物处理
obstacles = [obs1_pos, obs1_radius; 
             obs2_pos, obs2_radius;
             ...];

% 在避障策略中选择最近障碍物
[min_dist, closest_obs] = min(vecnorm(paths - obstacles(:,1:2), 2, 2));
if min_dist < obstacles(closest_obs,3) + 5
    u = obstacle_avoidance(x, obstacles(closest_obs,1:2), obstacles(closest_obs,3), Vx);
end

3. 自适应MPC参数

matlab 复制代码 
% 根据车速调整MPC参数
if Vx < 10 % 低速
    Np = 15; Nc = 8;
    Q = diag([5, 2, 0.5]);
elseif Vx > 20 % 高速
    Np = 25; Nc = 12;
    Q = diag([20, 10, 2]);
end

4. 考虑纵向速度变化

matlab 复制代码 
% 在车辆模型中考虑Vx变化
Vx = Vx + a*ts; % 更新纵向速度
Vx = max(Vx, 5); % 最小速度限制
Vx = min(Vx, 30); % 最大速度限制

实际部署注意事项

  1. 传感器融合：

    • 使用卡尔曼滤波融合GPS、IMU和轮速传感器数据

    • 实现车辆状态估计（位置、速度、姿态）

  2. 实时性优化：

    • 使用C++重写核心算法

    • 采用显式MPC（Explicit MPC）减少在线计算量

    • 使用硬件加速（如FPGA或GPU）

  3. 容错机制：

    • 添加执行器故障检测

    • 实现降级控制模式（如纯跟踪算法）

    • 设计安全停止策略

  4. 人机交互：

    • 开发驾驶界面显示规划路径

    • 添加驾驶员接管请求机制

    • 实现参数在线调整接口

总结

本MATLAB实现展示了基于模型预测控制的自动避障与汽车跟踪系统，通过以下关键步骤实现：

  1. 建立车辆动力学模型（2自由度自行车模型）

  2. 设计MPC控制器（预测时域Np=20，控制时域Nc=10）

  3. 实现避障策略（安全距离模型+横向加速度控制）

  4. 进行闭环仿真（20秒，0.1秒步长）

相关推荐
十铭忘1 小时前
OpenClaw的安装与使用 1
人工智能
Highcharts.js1 小时前
Highcharts for Python｜用 Pythonic 的方式构建AI数据可视化图表
前端·人工智能·python·信息可视化·数据科学·highcharts·ai可视化
hans汉斯1 小时前
基于污点分析的PHP应用威胁检测平台
开发语言·人工智能·算法·yolo·目标检测·php·无人机
自在极意功。2 小时前
图像识别原理
图像处理·人工智能·计算机视觉
AI精钢2 小时前
在生产环境进行 vibe coding 的正确方式
大数据·人工智能·ai·agent·claude·devops·cursor
志栋智能2 小时前
预算有限？超自动化安全运维的普惠解决方案
运维·网络·人工智能·安全·自动化
思茂信息2 小时前
CST电磁铁的磁力仿真
开发语言·matlab·平面·cst·电磁仿真
用户4815930195912 小时前
A2A 协议深度解析（上）：当 AI Agent 需要和另一个 AI Agent 说话
人工智能
zzh940772 小时前
Gemini 3.1 Pro vs Claude 3.5：原生多模态与镜像AI的巅峰对决
人工智能
热门推荐
01GitHub 镜像站点02Qwen3.5 开源全解析：从 0.8B 到 397B，代际升级 + 全场景选型指南03围棋-html版本04班级宠物园部署指南05小黑课堂计算机二级WPSoffice题库软件下载安装教程（2026年3月最新版）06UV安装并设置国内源07OpenClaw 使用和管理 MCP 完全指南08“wsl --install -d Ubuntu-22.04”下载慢，中国地区离线安装 Ubuntu 22.04 WSL方法（亲测2025年5月6日）09机器学习深度学习二——GAN网络10OpenClaw Control UI安全上下文访问配置