分层MPC控制器的实时车辆最优控制和避障

分层MPC控制器的实时车辆最优控制和避障

一、分层MPC控制架构设计

1. 核心思想

分层模型预测控制（Hierarchical MPC）通过将复杂车辆控制问题分解为多时间尺度、多目标层级，实现实时性与最优性的平衡：

优势：降低单步优化维度（上层低维路径规划+下层高维跟踪），满足实时性（总计算时间<100ms），同时兼顾全局最优与局部动态性能。

2. 系统架构

┌─────────────────────────────────────────────────────────────┐
│                    分层MPC控制器                             │
├─────────────────┬─────────────────┬─────────────────┤
│    上层（规划层）  │    协调层（可选）  │    下层（跟踪层）  │
│  - 路径规划      │  - 参考轨迹校验  │  - 动力学跟踪    │
│  - 避障决策      │  - 约束协调      │  - 执行器控制    │
│  - 参考轨迹生成  │  - 性能评估      │  - 实时扰动抑制  │
└─────────────────┴─────────────────┴─────────────────┘
                              │
                              ▼
┌─────────────────────────────────────────────────────────────┐
│                车辆动力学系统（被控对象）                     │
│  - 二自由度车辆模型（横向+纵向）                              │
│  - 执行器模型（转向、油门/刹车）                              │
│  - 传感器（GPS、IMU、激光雷达、摄像头）                      │
└─────────────────────────────────────────────────────────────┘

二、上层规划层：路径规划与避障决策

1. 车辆运动学模型（规划层简化）

上层采用自行车模型（Kinematic Bicycle Model）描述车辆运动，降低计算复杂度：

其中：为车辆位置，为航向角，为速度，为轴距，为前轮转角，为加速度。

离散化（采样时间）：

2. 避障约束处理

（1）静态障碍物（如道路边界、固定物）

\sqrt{(x_v(k+i) – x_o(k+i))^2 + (y_v(k+i) – y_o(k+i))^2} \geq d_{safe}, \quad \forall i=1,…,N_p

J_p = \sum{i=0}{N_pp-1} \left( |x(i)-x{goal}|Q + |a(i)|R \right) + \sum{i=0}{N_pp} | \text{障碍物距离}(i) |S

\begin{aligned}
m(\dot{v}x – v_y \dot{\theta}) &= F{xf} \cos\delta + F{yr} – F{drag} \
m(\dot{v}y + v_x \dot{\theta}) &= F{xf} \sin\delta + F{yf} + F{yr} \
I_z \ddot{\theta} &= L_f(F{xf} \sin\delta + F{yf}) – L_r F_{yr}
\end{aligned}

J_c = \sum{i=0}{N_pc-1} \left( |x(i)-x{ref}(i)|Q + |u(i)-u{ref}(i)|R + | \Delta u(i) |S \right)
$$
其中$x=[x,y,\theta,v]^T$为状态向量，$u=[\delta,a]^T$为控制输入，$\Delta u(i)=u(i)-u(i-1)$为控制增量。

约束：

3. 实时优化求解

下层跟踪需高频更新（50-100Hz），采用显式MPC或快速QP求解器（如OSQP、qpOASES）：

四、MATLAB/Simulink实现框架

1. 上层规划层代码（路径规划与避障）

function [ref_traj, success] = upper_layer_planning(vehicle_state, goal, obstacles, Ts_p)
    % 上层规划层：路径规划与避障
    % 输入：vehicle_state-当前车辆状态[x,y,θ,v]，goal-目标点[x_g,y_g]，obstacles-障碍物列表，Ts_p-规划采样时间
    % 输出：ref_traj-参考轨迹[N_p^p×(x,y,θ,v,a,δ)]，success-规划成功标志
    
    % 参数设置
    N_p_p = 20;       % 规划时域（步）
    Q = diag([10,10,1,1]);  % 状态权重
    R = diag([0.1, 0.01]);  % 控制权重
    d_safe = 0.5;     % 安全距离
    
    % 初始化优化变量（控制序列）
    U_p = sdpvar(2, N_p_p);  % [δ,a]序列
    X_p = sdpvar(4, N_p_p+1); % 状态序列[x,y,θ,v]
    
    % 初始状态约束
    constraints = [X_p(:,1) == vehicle_state'];
    
    % 动力学约束（自行车模型）
    for i = 1:N_p_p
        constraints = [constraints, 
                       X_p(:,i+1) == bicycle_model(X_p(:,i), U_p(:,i), Ts_p)];
    end
    
    % 避障约束（静态障碍物）
    for i = 1:N_p_p+1
        for obs = obstacles
            % 障碍物为圆形：(x_o,y_o,r_o)
            dist = sqrt((X_p(1,i)-obs.x)^2 + (X_p(2,i)-obs.y)^2);
            constraints = [constraints, dist >= obs.r + vehicle_width/2 + d_safe];
        end
    end
    
    % 目标约束（终点接近目标点）
    constraints = [constraints, norm(X_p(1:2,end)-goal(1:2)) <= 1.0];
    
    % 代价函数
    cost = 0;
    for i = 1:N_p_p
        state_err = X_p(:,i) - [goal(1); goal(2); goal(3); goal(4)];
        cost = cost + state_err'*Q*state_err + U_p(:,i)'*R*U_p(:,i);
    end
    
    % 求解优化问题
    options = sdpsettings('solver', 'ipopt', 'verbose', 0);
    sol = optimize(constraints, cost, options);
    
    if sol.problem == 0
        ref_traj = [X_p(1,:); X_p(2,:); X_p(3,:); X_p(4,:); U_p(2,:); U_p(1,:)]';
        success = true;
    else
        ref_traj = [];
        success = false;
    end
end

function x_next = bicycle_model(x, u, Ts)
    % 自行车模型离散化
    x_pos = x(1); y_pos = x(2); theta = x(3); v = x(4);
    delta = u(1); a = u(2);
    L = 2.8;  % 轴距(m)
    
    x_next = [x_pos + v*Ts*cos(theta) - v*Ts*sin(theta)*tan(delta)/L;
              y_pos + v*Ts*sin(theta) + v*Ts*cos(theta)*tan(delta)/L;
              theta + v*Ts*tan(delta)/L;
              v + a*Ts];
end

2. 下层跟踪层代码（动力学跟踪）

function [u_opt, x_pred] = lower_layer_tracking(vehicle_state, ref_traj, Ts_c)
    % 下层跟踪层：动力学跟踪
    % 输入：vehicle_state-当前状态，ref_traj-上层参考轨迹，Ts_c-跟踪采样时间
    % 输出：u_opt-最优控制量[δ,a]，x_pred-预测状态
    
    % 参数设置
    N_p_c = 10;       % 跟踪时域（步）
    Q = diag([100, 100, 10, 1]);  % 状态误差权重
    R = diag([0.5, 0.1]);  % 控制权重
    S = diag([0.1, 0.05]); % 控制增量权重
    
    % 提取参考轨迹（前N_p_c步）
    x_ref = ref_traj(1:4, 1:N_p_c)';  % 参考状态序列
    u_ref = ref_traj(5:6, 1:N_p_c)';  % 参考控制序列
    
    % 初始化优化变量
    U_c = sdpvar(2, N_p_c);  % 控制序列[δ,a]
    X_c = sdpvar(4, N_p_c+1); % 预测状态序列
    
    % 初始状态约束
    constraints = [X_c(:,1) == vehicle_state'];
    
    % 动力学约束（二自由度模型）
    for i = 1:N_p_c
        constraints = [constraints, 
                       X_c(:,i+1) == dynamic_model(X_c(:,i), U_c(:,i), Ts_c)];
    end
    
    % 代价函数
    cost = 0;
    for i = 1:N_p_c
        state_err = X_c(:,i) - x_ref(i,:)';
        cost = cost + state_err'*Q*state_err + (U_c(:,i)-u_ref(i,:)')'*R*(U_c(:,i)-u_ref(i,:)');
        if i > 1
            delta_u = U_c(:,i) - U_c(:,i-1);
            cost = cost + delta_u'*S*delta_u;
        end
    end
    
    % 执行器约束
    constraints = [constraints, -deg2rad(30) <= U_c(1,:) <= deg2rad(30)];  % 转向角±30°
    constraints = [constraints, -5 <= U_c(2,:) <= 3];  % 加速度-5~3m/s²
    
    % 求解优化问题（用OSQP加速）
    options = sdpsettings('solver', 'osqp', 'verbose', 0);
    sol = optimize(constraints, cost, options);
    
    if sol.problem == 0
        u_opt = value(U_c(:,1));  % 取第一步控制量
        x_pred = value(X_c);
    else
        u_opt = [0; 0];  % 失败时用零控制
        x_pred = vehicle_state;
    end
end

function x_next = dynamic_model(x, u, Ts)
    % 二自由度动力学模型（简化版，实际需考虑轮胎力）
    m = 1500; I_z = 2500; L_f = 1.2; L_r = 1.6;
    v_x = x(4); delta = u(1); a = u(2);
    
    % 简化侧向力模型（忽略侧偏角非线性）
    F_yf = -2*C_f*(x(3) - v_x*delta/L_f);  % 前轮侧向力
    F_yr = -2*C_r*(x(3) - 0);  % 后轮侧向力（C_f,C_r为侧偏刚度）
    
    x_next = [x(1) + (v_x*cos(x(3)) - x(2)*sin(x(3)))*Ts;
              x(2) + (v_x*sin(x(3)) + x(2)*cos(x(3)))*Ts;
              x(3) + (L_f*F_yf - L_r*F_yr)/I_z*Ts;
              v_x + a*Ts];
end

3. 主控制循环（分层协调）

function hierarchical_mpc_control()
    % 分层MPC主控制循环
    Ts_p = 0.1;   % 上层规划周期（10Hz）
    Ts_c = 0.02;  % 下层跟踪周期（50Hz）
    sim_time = 30; % 仿真时间(s)
    
    % 初始化车辆状态
    vehicle_state = [0; 0; 0; 5];  % [x,y,θ,v] (m,m,rad,m/s)
    goal = [100; 0; 0; 10];  % 目标点[x,y,θ,v]
    obstacles = [struct('x',50,'y',0,'r',2), struct('x',30,'y',-5,'r',1.5)];  % 圆形障碍物
    
    % 主循环
    t = 0;
    while t < sim_time
        % 上层规划（每Ts_p更新一次）
        if mod(t, Ts_p) < Ts_c
            [ref_traj, success] = upper_layer_planning(vehicle_state, goal, obstacles, Ts_p);
            if ~success
                error('上层规划失败！');
            end
        end
        
        % 下层跟踪（每Ts_c更新一次）
        [u_opt, x_pred] = lower_layer_tracking(vehicle_state, ref_traj, Ts_c);
        
        % 应用控制量，更新车辆状态（用动力学模型仿真）
        vehicle_state = dynamic_model(vehicle_state, u_opt, Ts_c);
        
        % 数据记录与可视化
        record_data(t, vehicle_state, ref_traj, u_opt);
        t = t + Ts_c;
    end
    
    % 绘制结果
    plot_results();
end

参考代码 分层MPC控制器的实时车辆最优控制和避障 www.youwenfan.com/contentcst/160715.html

五、关键技术与优化

1. 实时性保障

2. 避障鲁棒性

3. 多目标优化

六、仿真与实验结果

1. 性能指标

2. 典型场景

七、总结

分层MPC控制器通过上下层解耦实现车辆实时最优控制与避障：

专注于matlab/simulink,电子电路,编程

关于我们湘ICP备19023172号-4