车辆碰撞避免的网络模型预测控制（MPC）MATLAB仿真

yongui47834

483人浏览 · 2026-04-08 09:07:24

yongui47834 · 2026-04-08 09:07:24 发布

一、系统概述

网络模型预测控制（Networked MPC）在车辆碰撞避免中结合了车联网（V2X）通信与模型预测控制，实现多车协同避撞。系统通过V2X网络交换车辆状态信息，MPC控制器基于全局信息优化控制策略，确保安全避撞。

核心架构：

V2X通信网络：车辆间实时交换位置、速度、加速度信息
集中式/分布式MPC：基于网络信息进行协同决策
碰撞避免约束：动态安全距离、碰撞锥约束
轨迹预测：基于LSTM等深度学习模型预测障碍车轨迹

二、系统建模

2.1 车辆动力学模型（自行车模型）

状态变量：

$x=[X,Y,ψ,v,β,r]^T$

$(X, Y)$ ：全局坐标系位置
$ψ$ ：横摆角（航向角）
$v$ ：车速
$β$ ：质心侧偏角
$r$ ：横摆角速度

控制输入：

$u=[a,δ_f]^T$

$a$ ：纵向加速度
$δ_f$ ：前轮转角

非线性动力学方程：

function dx = vehicle_dynamics(t, x, u, params)
    % 参数提取
    m = params.m;       % 质量 (kg)
    Iz = params.Iz;     % 绕z轴转动惯量 (kg·m^2)
    lf = params.lf;     % 前轴到质心距离 (m)
    lr = params.lr;     % 后轴到质心距离 (m)
    Cf = params.Cf;     % 前轮侧偏刚度 (N/rad)
    Cr = params.Cr;     % 后轮侧偏刚度 (N/rad)
    
    % 状态变量
    X = x(1); Y = x(2); psi = x(3);
    v = x(4); beta = x(5); r = x(6);
    
    % 控制输入
    a = u(1);           % 纵向加速度
    delta_f = u(2);     % 前轮转角
    
    % 轮胎侧偏角
    alpha_f = beta + lf*r/v - delta_f;
    alpha_r = beta - lr*r/v;
    
    % 轮胎侧向力（线性模型）
    Fyf = Cf * alpha_f;
    Fyr = Cr * alpha_r;
    
    % 动力学方程
    dX = v * cos(psi + beta);
    dY = v * sin(psi + beta);
    dpsi = r;
    dv = a;
    dbeta = (Fyf + Fyr)/(m*v) - r;
    dr = (lf*Fyf - lr*Fyr)/Iz;
    
    dx = [dX; dY; dpsi; dv; dbeta; dr];
end

2.2 离散化模型（欧拉法）

function x_next = discrete_vehicle_model(x, u, params, Ts)
    % 前向欧拉离散化
    dx = vehicle_dynamics(0, x, u, params);
    x_next = x + Ts * dx;
end

三、网络模型预测控制器设计

3.1 集中式网络MPC架构

系统结构：

classdef NetworkedMPC
    properties
        N_vehicles;          % 车辆数量
        vehicle_models;      % 各车动力学模型
        Np; Nc;              % 预测时域、控制时域
        Q; R; S;             % 权重矩阵
        Ts;                  % 采样时间
        comm_delay;          % 通信延迟
        safety_margin;       % 安全裕度
    end
    
    methods
        function obj = NetworkedMPC(N_vehicles, params)
            % 初始化网络MPC控制器
            obj.N_vehicles = N_vehicles;
            obj.Ts = params.Ts;
            obj.comm_delay = params.comm_delay;
            obj.safety_margin = params.safety_margin;
            
            % 初始化各车MPC参数
            obj.Np = 20; obj.Nc = 10;
            obj.Q = diag([10, 10, 1, 1, 5, 5]);  % 状态权重
            obj.R = diag([0.1, 0.1]);            % 控制输入权重
            obj.S = diag([0.01, 0.01]);          % 输入变化率权重
        end
    end
end

3.2 碰撞避免约束建模

安全距离模型（时变安全距离）：

$d_{safe}(t)=d_{min}+τ⋅v_{rel}(t)$

$d_{min}$ ：最小静态安全距离
$τ$ ：反应时间
$v_{rel}$ ：相对速度

碰撞锥约束：

function [A_collision, b_collision] = collision_cone_constraint(ego_state, obs_state, params)
    % 碰撞锥约束：确保自车与障碍车在未来Np步内不会进入碰撞锥
    
    % 提取状态
    ego_pos = ego_state(1:2); ego_vel = ego_state(4);
    obs_pos = obs_state(1:2); obs_vel = obs_state(4);
    
    % 相对位置和速度
    rel_pos = ego_pos - obs_pos;
    rel_vel = ego_vel - obs_vel;
    
    % 碰撞锥条件：相对速度方向应远离障碍车
    % (rel_pos + rel_vel * t) 的模长应大于安全距离
    A_collision = [];
    b_collision = [];
    
    for k = 1:params.Np
        t = k * params.Ts;
        % 预测位置
        ego_pred = ego_pos + ego_vel * t;
        obs_pred = obs_pos + obs_vel * t;
        
        % 距离约束
        dist = norm(ego_pred - obs_pred);
        safe_dist = params.d_min + params.tau * norm(rel_vel);
        
        % 线性化约束（在当前位置线性化）
        if dist < safe_dist * 2  % 只在接近时添加约束
            A_collision = [A_collision; (ego_pred - obs_pred)'/dist];
            b_collision = [b_collision; safe_dist - dist + (ego_pred - obs_pred)'*(ego_pred - obs_pred)/dist];
        end
    end
end

3.3 基于LSTM的障碍车轨迹预测

classdef LSTMPredictor
    % LSTM轨迹预测器
    properties
        lstm_net;           % LSTM网络
        seq_length;         % 序列长度
        pred_horizon;       % 预测时域
    end
    
    methods
        function obj = LSTMPredictor(seq_length, pred_horizon)
            % 初始化LSTM预测器
            obj.seq_length = seq_length;
            obj.pred_horizon = pred_horizon;
            
            % 构建LSTM网络（简化示例）
            layers = [
                sequenceInputLayer(4)  % [x, y, v, psi]
                lstmLayer(128)
                fullyConnectedLayer(64)
                dropoutLayer(0.2)
                fullyConnectedLayer(4*pred_horizon)  % 输出预测轨迹
                regressionLayer
            ];
            
            options = trainingOptions('adam', ...
                'MaxEpochs', 100, ...
                'MiniBatchSize', 32, ...
                'Plots', 'training-progress');
            
            % 注：实际需要训练数据训练网络
            obj.lstm_net = layers;
        end
        
        function pred_traj = predict(obj, history_traj)
            % 预测障碍车未来轨迹
            % history_traj: 历史轨迹 [seq_length × 4]
            % pred_traj: 预测轨迹 [pred_horizon × 4]
            
            % 使用训练好的LSTM网络进行预测
            % 这里简化处理，实际应调用训练好的网络
            pred_traj = zeros(obj.pred_horizon, 4);
            
            % 简单线性外推（实际应用应使用训练好的LSTM）
            last_state = history_traj(end, :);
            for i = 1:obj.pred_horizon
                pred_traj(i, :) = last_state + i * [last_state(3)*cos(last_state(4)), ...
                                                    last_state(3)*sin(last_state(4)), ...
                                                    0, 0];
            end
        end
    end
end

四、MATLAB仿真实现

4.1 主仿真程序

%% 车辆碰撞避免的网络模型预测控制仿真
clear; close all; clc;

%% 1. 系统参数设置
% 车辆参数
params.m = 1500;       % 质量 (kg)
params.Iz = 2500;      % 转动惯量 (kg·m^2)
params.lf = 1.2;       % 前轴到质心距离 (m)
params.lr = 1.5;       % 后轴到质心距离 (m)
params.Cf = 80000;     % 前轮侧偏刚度 (N/rad)
params.Cr = 80000;     % 后轮侧偏刚度 (N/rad)

% MPC参数
params.Ts = 0.1;       % 采样时间 (s)
params.Np = 20;        % 预测时域 (2秒)
params.Nc = 10;        % 控制时域 (1秒)

% 安全参数
params.d_min = 3.0;    % 最小安全距离 (m)
params.tau = 1.5;      % 反应时间 (s)
params.safety_margin = 0.5;  % 安全裕度 (m)

% 网络参数
params.comm_delay = 0.05;    % 通信延迟 (s)
params.packet_loss = 0.05;   % 丢包率

%% 2. 场景设置
% 车辆数量
N_vehicles = 3;

% 初始状态
% 车辆1：自车
x0_ego = [0; 0; 0; 20; 0; 0];  % [X, Y, ψ, v, β, r]

% 车辆2：前方障碍车
x0_obs1 = [50; 1.5; 0; 18; 0; 0];  % 同向行驶，稍慢

% 车辆3：对向来车
x0_obs2 = [100; -3.5; pi; 22; 0; 0];  % 对向行驶

% 参考轨迹（自车保持车道中心线）
ref_traj = zeros(6, params.Np);
for k = 1:params.Np
    ref_traj(1, k) = x0_ego(4) * k * params.Ts;  % X方向匀速
    ref_traj(2, k) = 0;                          % Y方向保持车道中心
    ref_traj(3, k) = 0;                          % 航向角保持
    ref_traj(4, k) = 20;                         % 速度保持
end

%% 3. 初始化控制器和预测器
% 创建网络MPC控制器
mpc_controller = NetworkedMPC(N_vehicles, params);

% 创建LSTM轨迹预测器
lstm_predictor = LSTMPredictor(10, params.Np);

% 历史轨迹缓冲区
history_buffer = cell(N_vehicles, 1);
for i = 1:N_vehicles
    history_buffer{i} = zeros(10, 4);  % 存储最近10个时刻的轨迹
end

%% 4. 仿真初始化
T_sim = 10;                     % 总仿真时间 (s)
N_sim = T_sim / params.Ts;      % 仿真步数

% 状态历史记录
x_history = zeros(6, N_vehicles, N_sim+1);
u_history = zeros(2, N_vehicles, N_sim);

% 初始状态
x_history(:, 1, 1) = x0_ego;
x_history(:, 2, 1) = x0_obs1;
x_history(:, 3, 1) = x0_obs2;

% 控制输入初始值
u_prev = zeros(2, N_vehicles);

%% 5. 主仿真循环
fprintf('开始网络MPC碰撞避免仿真...\n');
for k = 1:N_sim
    % 当前时间
    t = (k-1) * params.Ts;
    
    % 1. V2X通信：交换车辆状态信息（考虑延迟和丢包）
    vehicle_states = zeros(6, N_vehicles);
    for i = 1:N_vehicles
        % 模拟通信延迟和丢包
        if rand() > params.packet_loss
            % 有延迟的状态信息
            delay_steps = round(params.comm_delay / params.Ts);
            if k > delay_steps
                vehicle_states(:, i) = x_history(:, i, k-delay_steps);
            else
                vehicle_states(:, i) = x_history(:, i, 1);
            end
        else
            % 丢包情况，使用上一时刻状态
            vehicle_states(:, i) = x_history(:, i, max(1, k-1));
        end
    end
    
    % 2. 轨迹预测（基于LSTM）
    pred_trajectories = cell(N_vehicles, 1);
    for i = 1:N_vehicles
        % 更新历史缓冲区
        current_state = [vehicle_states(1:2, i)', vehicle_states(4, i), vehicle_states(3, i)];
        history_buffer{i} = [history_buffer{i}(2:end, :); current_state];
        
        % LSTM预测未来轨迹
        pred_trajectories{i} = lstm_predictor.predict(history_buffer{i});
    end
    
    % 3. 集中式MPC优化（自车控制）
    ego_state = x_history(:, 1, k);
    obs_states = vehicle_states(:, 2:end);
    obs_preds = {pred_trajectories{2}, pred_trajectories{3}};
    
    % 求解MPC优化问题
    [u_opt, feasible] = centralized_mpc_solver(ego_state, obs_states, obs_preds, ...
                                                ref_traj, u_prev(:, 1), params);
    
    if feasible
        u_history(:, 1, k) = u_opt;
        u_prev(:, 1) = u_opt;
    else
        % 优化不可行，启用紧急避撞策略
        fprintf('时间 %.1fs: MPC不可行，启用紧急避撞策略\n', t);
        u_history(:, 1, k) = emergency_avoidance(ego_state, obs_states, params);
    end
    
    % 4. 障碍车控制（简单跟驰模型）
    for i = 2:N_vehicles
        if i == 2  % 前车：ACC跟驰
            u_history(:, i, k) = acc_following(x_history(:, i, k), ...
                                                x_history(:, 1, k), params);
        else  % 对向来车：保持原状态
            u_history(:, i, k) = [0; 0];
        end
    end
    
    % 5. 更新车辆状态
    for i = 1:N_vehicles
        x_next = discrete_vehicle_model(x_history(:, i, k), ...
                                        u_history(:, i, k), params, params.Ts);
        x_history(:, i, k+1) = x_next;
    end
    
    % 6. 碰撞检测
    collision_detected = check_collision(x_history(:, :, k+1), params);
    if collision_detected
        fprintf('警告：时间 %.1fs 检测到碰撞风险！\n', t+params.Ts);
        % 可添加紧急制动等处理
    end
    
    % 显示进度
    if mod(k, 10) == 0
        fprintf('时间: %.1f s, 自车位置: (%.1f, %.1f), 速度: %.1f m/s\n', ...
                t+params.Ts, x_history(1, 1, k+1), x_history(2, 1, k+1), x_history(4, 1, k+1));
    end
end
fprintf('仿真完成！\n');

%% 6. 集中式MPC求解器
function [u_opt, feasible] = centralized_mpc_solver(ego_state, obs_states, obs_preds, ...
                                                    ref_traj, u_prev, params)
    % 集中式MPC优化求解
    
    % 优化变量：U = [a(0), δ(0), ..., a(Nc-1), δ(Nc-1)]
    Nc = params.Nc;
    U0 = zeros(2*Nc, 1);  % 初始猜测
    
    % 约束
    lb = repmat([-3; -0.5], Nc, 1);   % 加速度限制[-3,3] m/s²，转角限制[-0.5,0.5] rad
    ub = repmat([3; 0.5], Nc, 1);
    
    % 输入变化率约束
    A_ineq = []; b_ineq = [];
    if Nc > 1
        % Δa约束：|a(k)-a(k-1)| ≤ 1 m/s²
        % Δδ约束：|δ(k)-δ(k-1)| ≤ 0.1 rad
        A_du = kron([eye(Nc-1), zeros(Nc-1, 1)] - [zeros(Nc-1, 1), eye(Nc-1)], eye(2));
        b_du = repmat([1; 0.1], Nc-1, 1);
        A_ineq = [A_ineq; A_du; -A_du];
        b_ineq = [b_ineq; b_du; b_du];
    end
    
    % 碰撞避免约束
    [A_collision, b_collision] = collision_constraints(ego_state, obs_states, obs_preds, params);
    A_ineq = [A_ineq; A_collision];
    b_ineq = [b_ineq; b_collision];
    
    % 优化选项
    options = optimoptions('fmincon', 'Display', 'off', 'Algorithm', 'sqp', ...
                          'MaxIterations', 100, 'MaxFunctionEvaluations', 1000);
    
    % 求解优化问题
    try
        [U_opt, ~, exitflag] = fmincon(@(U) mpc_cost_function(U, ego_state, ref_traj, u_prev, params), ...
                                       U0, A_ineq, b_ineq, [], [], lb, ub, [], options);
        
        if exitflag > 0
            u_opt = U_opt(1:2);  % 取第一个控制输入
            feasible = true;
        else
            u_opt = u_prev;
            feasible = false;
        end
    catch
        u_opt = u_prev;
        feasible = false;
    end
end

%% 7. MPC代价函数
function J = mpc_cost_function(U, x0, ref_traj, u_prev, params)
    % MPC代价函数计算
    
    Np = params.Np; Nc = params.Nc;
    Q = params.Q; R = params.R; S = params.S;
    
    x = x0;
    J = 0;
    
    % 控制序列扩展（控制时域外保持最后值）
    U_ext = reshape(U, 2, Nc);
    if Nc < Np
        U_ext = [U_ext, repmat(U_ext(:, end), 1, Np-Nc)];
    end
    
    for k = 1:Np
        % 控制输入
        u = U_ext(:, k);
        
        % 状态更新
        x = discrete_vehicle_model(x, u, params, params.Ts);
        
        % 跟踪误差代价
        J = J + (x - ref_traj(:, min(k, size(ref_traj, 2))))' * Q * (x - ref_traj(:, min(k, size(ref_traj, 2))));
        
        % 控制输入代价
        J = J + u' * R * u;
        
        % 输入变化率代价
        if k > 1
            du = u - U_ext(:, k-1);
            J = J + du' * S * du;
        end
    end
end

%% 8. 碰撞约束生成
function [A_collision, b_collision] = collision_constraints(ego_state, obs_states, obs_preds, params)
    % 生成碰撞避免约束
    
    Np = params.Np;
    N_obs = size(obs_states, 2);
    A_collision = [];
    b_collision = [];
    
    % 自车预测轨迹（简化，假设控制输入为零）
    ego_pred = predict_ego_trajectory(ego_state, params);
    
    for i = 1:N_obs
        % 障碍车预测轨迹
        if ~isempty(obs_preds) && length(obs_preds) >= i
            obs_pred = obs_preds{i};
        else
            % 简单恒定速度预测
            obs_pred = predict_constant_velocity(obs_states(:, i), params);
        end
        
        % 为每个预测步添加距离约束
        for k = 1:Np
            % 预测位置
            ego_pos = ego_pred(1:2, k);
            obs_pos = obs_pred(k, 1:2)';
            
            % 安全距离（考虑相对速度）
            rel_vel = ego_state(4) - obs_states(4, i);
            safe_dist = params.d_min + params.tau * abs(rel_vel) + params.safety_margin;
            
            % 当前距离
            current_dist = norm(ego_state(1:2) - obs_states(1:2, i));
            
            % 线性化约束（在当前位置）
            if current_dist < safe_dist * 3  % 只在接近时添加约束
                dir_vector = (ego_pos - obs_pos) / norm(ego_pos - obs_pos);
                A_collision = [A_collision; dir_vector', zeros(1, 2*params.Nc-2)];
                b_collision = [b_collision; safe_dist - norm(ego_pos - obs_pos)];
            end
        end
    end
end

%% 9. 紧急避撞策略
function u_emergency = emergency_avoidance(ego_state, obs_states, params)
    % 紧急避撞策略（当MPC不可行时）
    
    % 计算到最近障碍车的距离和方向
    min_dist = inf;
    nearest_obs = 1;
    
    for i = 1:size(obs_states, 2)
        dist = norm(ego_state(1:2) - obs_states(1:2, i));
        if dist < min_dist
            min_dist = dist;
            nearest_obs = i;
        end
    end
    
    % 紧急策略：制动+转向
    if min_dist < params.d_min * 2
        % 紧急制动
        a = -5;  % 最大制动加速度
        % 转向避让（远离障碍车）
        obs_dir = obs_states(1:2, nearest_obs) - ego_state(1:2);
        turn_dir = sign(obs_dir(2));  % 横向避让方向
        
        if abs(turn_dir) < 0.1
            turn_dir = 1;  % 默认向右避让
        end
        
        delta = 0.3 * turn_dir;  % 紧急转向角
    else
        % 温和避让
        a = -2;
        delta = 0.1;
    end
    
    % 限制控制输入
    a = max(min(a, 3), -5);      % 加速度限制[-5,3] m/s²
    delta = max(min(delta, 0.5), -0.5); % 转角限制[-0.5,0.5] rad
    
    u_emergency = [a; delta];
end

%% 10. ACC跟驰模型
function u_acc = acc_following(ego_state, front_state, params)
    % 自适应巡航控制（前车）
    
    % 车距
    dx = front_state(1) - ego_state(1);
    dv = front_state(4) - ego_state(4);
    
    % 期望车距（时距1.5秒）
    desired_gap = max(5, ego_state(4) * 1.5);
    
    % ACC控制律
    if dx < desired_gap * 0.8
        % 太近，减速
        a = -2 * (desired_gap - dx)/desired_gap - 0.5 * dv;
    elseif dx > desired_gap * 1.2
        % 太远，加速
        a = 1 * (dx - desired_gap)/desired_gap;
    else
        % 保持速度
        a = 0;
    end
    
    % 保持车道中心
    lane_center = 0;
    lane_offset = ego_state(2) - lane_center;
    delta = -0.1 * lane_offset;  % 简单的PD控制
    
    % 限制控制输入
    a = max(min(a, 2), -3);
    delta = max(min(delta, 0.3), -0.3);
    
    u_acc = [a; delta];
end

%% 11. 碰撞检测
function collision = check_collision(states, params)
    % 检测车辆间碰撞
    
    N_vehicles = size(states, 2);
    collision = false;
    
    for i = 1:N_vehicles-1
        for j = i+1:N_vehicles
            pos_i = states(1:2, i);
            pos_j = states(1:2, j);
            dist = norm(pos_i - pos_j);
            
            % 考虑车辆尺寸（简化矩形）
            vehicle_length = 5;  % 车长
            vehicle_width = 2;   % 车宽
            
            if dist < sqrt((vehicle_length/2)^2 + (vehicle_width/2)^2)
                collision = true;
                return;
            end
        end
    end
end

%% 12. 轨迹预测函数
function ego_pred = predict_ego_trajectory(ego_state, params)
    % 预测自车轨迹（简化，假设零输入）
    
    Np = params.Np;
    ego_pred = zeros(6, Np);
    x = ego_state;
    
    for k = 1:Np
        % 假设零控制输入
        u = [0; 0];
        x = discrete_vehicle_model(x, u, params, params.Ts);
        ego_pred(:, k) = x;
    end
end

function obs_pred = predict_constant_velocity(obs_state, params)
    % 恒定速度预测（障碍车）
    
    Np = params.Np;
    obs_pred = zeros(Np, 4);
    
    for k = 1:Np
        t = k * params.Ts;
        obs_pred(k, 1) = obs_state(1) + obs_state(4) * cos(obs_state(3)) * t;
        obs_pred(k, 2) = obs_state(2) + obs_state(4) * sin(obs_state(3)) * t;
        obs_pred(k, 3) = obs_state(4);  % 速度
        obs_pred(k, 4) = obs_state(3);  % 航向角
    end
end

4.2 结果可视化

%% 13. 结果可视化
figure('Position', [100, 100, 1400, 800]);

% 子图1：车辆轨迹
subplot(2, 3, 1);
hold on; grid on;
colors = ['b', 'r', 'g'];
for i = 1:N_vehicles
    plot(squeeze(x_history(1, i, :)), squeeze(x_history(2, i, :)), ...
         [colors(i) '-'], 'LineWidth', 1.5, 'DisplayName', sprintf('车辆%d', i));
end
xlabel('X位置 (m)'); ylabel('Y位置 (m)');
title('车辆轨迹');
legend('Location', 'best');
axis equal;

% 标记碰撞风险区域
for k = 1:10:N_sim
    ego_pos = x_history(1:2, 1, k);
    for i = 2:N_vehicles
        obs_pos = x_history(1:2, i, k);
        dist = norm(ego_pos - obs_pos);
        if dist < 20
            plot([ego_pos(1), obs_pos(1)], [ego_pos(2), obs_pos(2)], 'k--', 'LineWidth', 0.5);
        end
    end
end

% 子图2：自车速度
subplot(2, 3, 2);
t_plot = (0:N_sim) * params.Ts;
plot(t_plot, squeeze(x_history(4, 1, :)), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('速度 (m/s)');
title('自车速度');
grid on;

% 子图3：控制输入
subplot(2, 3, 3);
t_control = (0:N_sim-1) * params.Ts;
plot(t_control, squeeze(u_history(1, 1, :)), 'r-', 'LineWidth', 1.5);
hold on;
plot(t_control, squeeze(u_history(2, 1, :)), 'g-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('控制输入');
title('自车控制输入');
legend('加速度 (m/s²)', '前轮转角 (rad)', 'Location', 'best');
grid on;

% 子图4：车距
subplot(2, 3, 4);
distances = zeros(N_sim+1, N_vehicles-1);
for k = 1:N_sim+1
    for i = 2:N_vehicles
        distances(k, i-1) = norm(x_history(1:2, 1, k) - x_history(1:2, i, k));
    end
end
plot(t_plot, distances, 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('车距 (m)');
title('自车与障碍车距离');
legend('与前车距离', '与对向来车距离', 'Location', 'best');
grid on;
yline(params.d_min, 'r--', '最小安全距离', 'LineWidth', 1.5);

% 子图5：横向位置
subplot(2, 3, 5);
plot(t_plot, squeeze(x_history(2, 1, :)), 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('横向位置 (m)');
title('自车横向位置');
grid on;
yline(0, 'k--', '车道中心线', 'LineWidth', 1.5);

% 子图6：安全裕度
subplot(2, 3, 6);
safety_margin_actual = zeros(N_sim+1, 1);
for k = 1:N_sim+1
    min_dist = inf;
    for i = 2:N_vehicles
        dist = norm(x_history(1:2, 1, k) - x_history(1:2, i, k));
        if dist < min_dist
            min_dist = dist;
        end
    end
    safety_margin_actual(k) = min_dist - params.d_min;
end
plot(t_plot, safety_margin_actual, 'm-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('安全裕度 (m)');
title('实际安全裕度');
grid on;
yline(0, 'r--', '安全边界', 'LineWidth', 1.5);

%% 14. 性能指标计算
fprintf('\n========== 性能指标 ==========\n');

% 1. 最小安全距离
min_safe_distance = min(min(distances));
fprintf('最小安全距离: %.2f m (要求: >%.1f m)\n', min_safe_distance, params.d_min);

% 2. 碰撞避免成功率
collision_count = 0;
for k = 1:N_sim+1
    if check_collision(x_history(:, :, k), params)
        collision_count = collision_count + 1;
    end
end
avoidance_rate = 100 * (1 - collision_count / (N_sim+1));
fprintf('碰撞避免成功率: %.1f%%\n', avoidance_rate);

% 3. 控制输入平滑度
acc_jerk = diff(squeeze(u_history(1, 1, :))) / params.Ts;
steer_rate = diff(squeeze(u_history(2, 1, :))) / params.Ts;
fprintf('加速度变化率(RMS): %.3f m/s³\n', rms(acc_jerk));
fprintf('转向角变化率(RMS): %.3f rad/s\n', rms(steer_rate));

% 4. 跟踪误差
tracking_error = zeros(N_sim+1, 1);
for k = 1:N_sim+1
    ref_idx = min(k, size(ref_traj, 2));
    tracking_error(k) = norm(x_history(1:2, 1, k) - ref_traj(1:2, ref_idx));
end
fprintf('位置跟踪误差(RMS): %.3f m\n', rms(tracking_error));

% 5. 计算时间分析（假设）
fprintf('平均MPC求解时间: <50 ms (设计要求)\n');
fprintf('最大通信延迟: %.0f ms\n', params.comm_delay*1000);

%% 15. 保存结果
save('networked_mpc_results.mat', 'x_history', 'u_history', 'params', ...
     'ref_traj', 'distances', 'tracking_error');

参考代码论文+程序车辆碰撞避免的网络模型预测控制的MATLAB仿真 www.youwenfan.com/contentcst/160593.html

五、关键技术与创新点

5.1 网络化MPC架构优势

协同避撞：多车信息共享，实现全局最优避撞策略
预测精度提升：基于V2X的轨迹预测比单车传感器更准确
计算分布式：可扩展至大规模车联网场景

5.2 LSTM轨迹预测集成

% 实际LSTM训练代码框架
function train_lstm_predictor()
    % 加载车辆轨迹数据集
    load('vehicle_trajectory_data.mat');  % 包含历史轨迹数据
    
    % 数据预处理
    XTrain = {}; YTrain = {};
    for i = 1:length(trajectories)
        seq = trajectories{i};
        XTrain{end+1} = seq(1:end-params.pred_horizon, :);
        YTrain{end+1} = seq(end-params.pred_horizon+1:end, :);
    end
    
    % 构建LSTM网络
    layers = [
        sequenceInputLayer(4)
        lstmLayer(128, 'OutputMode', 'sequence')
        dropoutLayer(0.2)
        lstmLayer(64, 'OutputMode', 'last')
        fullyConnectedLayer(4*params.pred_horizon)
        regressionLayer
    ];
    
    % 训练选项
    options = trainingOptions('adam', ...
        'MaxEpochs', 150, ...
        'MiniBatchSize', 32, ...
        'ValidationData', {XVal, YVal}, ...
        'Plots', 'training-progress');
    
    % 训练网络
    net = trainNetwork(XTrain, YTrain, layers, options);
    
    % 保存模型
    save('lstm_predictor.mat', 'net');
end