基于模型预测控制的车辆路径跟踪

基于模型预测控制的车辆路径跟踪

一、车辆路径跟踪问题定义

车辆路径跟踪的目标是通过控制方向盘转角δ和加速度a，使车辆质心位置(x,y)和航向角φ精确跟踪参考轨迹，同时满足车辆动力学约束和乘坐舒适性要求。

1. 核心挑战

2. 性能指标

指标 数学表达 典型要求
横向位置误差 < 0.1m (高速)
航向角误差 < 0.03rad
控制平滑性 变化率限制内
计算实时性 单步求解时间 < 10ms (100Hz)

二、车辆建模

1. 运动学模型（低速场景）

适用于车速<5m/s的泊车、低速跟车场景。

状态向量：
控制输入：

其中L为轴距。

2. 动力学模型（高速场景）

考虑轮胎侧偏特性，适用于高速过弯、紧急避障。

二自由度自行车模型：
状态：
控制：

轮胎模型（线性）：

轮胎模型（非线性，Pacejka魔术公式）：

3. 模型离散化

采用前向欧拉法或4阶龙格-库塔法：

function x_next = discrete_vehicle_model(x, u, Ts, model_type)
    if strcmp(model_type, 'kinematic')
        % 运动学模型离散化
        L = 2.7; % 轴距(m)
        X = x(1); Y = x(2); phi = x(3); v = x(4);
        a = u(1); delta = u(2);
        
        % 前向欧拉离散
        X_next = X + Ts * (v * cos(phi));
        Y_next = Y + Ts * (v * sin(phi));
        phi_next = phi + Ts * (v * tan(delta) / L);
        v_next = v + Ts * a;
        
        x_next = [X_next; Y_next; phi_next; v_next];
        
    elseif strcmp(model_type, 'dynamic')
        % 动力学模型离散化（4阶龙格-库塔）
        k1 = vehicle_ode(x, u);
        k2 = vehicle_ode(x + Ts/2*k1, u);
        k3 = vehicle_ode(x + Ts/2*k2, u);
        k4 = vehicle_ode(x + Ts*k3, u);
        
        x_next = x + Ts/6 * (k1 + 2*k2 + 2*k3 + k4);
    end
end

function dx = vehicle_ode(x, u)
    % 车辆动力学连续模型
    m = 1500;   % 质量(kg)
    Iz = 2500;  % 转动惯量(kg·m²)
    lf = 1.2;   % 前轴到质心距离(m)
    lr = 1.5;   % 后轴到质心距离(m)
    Cf = 80000; % 前轮侧偏刚度(N/rad)
    Cr = 100000;% 后轮侧偏刚度(N/rad)
    
    vx = x(1); vy = x(2); omega = x(3);
    delta = u(1); Fx = u(2);
    
    % 轮胎侧偏角
    alpha_f = delta - (vy + lf*omega)/vx;
    alpha_r = -(vy - lr*omega)/vx;
    
    % 轮胎力（线性模型）
    Fyf = Cf * alpha_f;
    Fyr = Cr * alpha_r;
    
    % 状态导数
    dvx = vy*omega + (Fx - Fyf*sin(delta))/m;
    dvy = -vx*omega + (Fyf*cos(delta) + Fyr)/m;
    domega = (lf*Fyf*cos(delta) - lr*Fyr)/Iz;
    
    dx = [dvx; dvy; domega];
end

三、MPC控制器设计

1. 优化问题构建

function [u_opt, info] = mpc_controller(x_current, ref_trajectory, params)
    % 输入：
    %   x_current: 当前状态 [X; Y; phi; v; ...]
    %   ref_trajectory: 参考轨迹 [N×3] - [X_ref, Y_ref, phi_ref]
    %   params: MPC参数结构体
    
    % 提取参数
    N = params.prediction_horizon;     % 预测时域
    Ts = params.sampling_time;         % 采样时间
    Q = params.state_weight;           % 状态权重矩阵
    R = params.control_weight;         % 控制权重矩阵
    S = params.control_rate_weight;    % 控制变化率权重
    
    % 决策变量：控制序列 U = [u0, u1, ..., u_{N-1}]
    % u = [加速度; 前轮转角]
    U = sdpvar(2, N);  % 2个控制输入 × N步
    
    % 初始化状态预测序列
    X_pred = zeros(length(x_current), N+1);
    X_pred(:,1) = x_current;
    
    % 构建目标函数和约束
    cost = 0;
    constraints = [];
    
    % 预测循环
    for k = 1:N
        % 状态预测
        X_pred(:,k+1) = discrete_vehicle_model(X_pred(:,k), U(:,k), Ts, params.model_type);
        
        % 计算跟踪误差
        if k <= size(ref_trajectory, 1)
            ref_state = ref_trajectory(k,:)';
            error = X_pred(1:3,k+1) - ref_state(1:3);
        else
            % 超出参考轨迹长度时使用最后一个参考点
            error = X_pred(1:3,k+1) - ref_trajectory(end,:)';
        end
        
        % 添加跟踪误差代价
        cost = cost + error' * Q * error;
        
        % 添加控制量代价
        cost = cost + U(:,k)' * R * U(:,k);
        
        % 添加控制变化率代价（k>1时）
        if k > 1
            delta_u = U(:,k) - U(:,k-1);
            cost = cost + delta_u' * S * delta_u;
        end
        
        % 添加约束
        % 1. 控制量约束
        constraints = [constraints, 
            params.delta_min <= U(2,k) <= params.delta_max,   % 转向角限制
            params.a_min <= U(1,k) <= params.a_max];          % 加速度限制
        
        % 2. 控制变化率约束
        if k > 1
            constraints = [constraints,
                -params.delta_rate_max <= U(2,k)-U(2,k-1) <= params.delta_rate_max,
                -params.a_rate_max <= U(1,k)-U(1,k-1) <= params.a_rate_max];
        end
        
        % 3. 车辆动力学约束（轮胎摩擦圆）
        if strcmp(params.model_type, 'dynamic')
            % 计算轮胎垂向载荷
            Fz_f = m*g*lr/(lf+lr) - m*a*h/(lf+lr);
            Fz_r = m*g*lf/(lf+lr) + m*a*h/(lf+lr);
            
            % 轮胎力约束（摩擦圆）
            Fx_max_f = mu * Fz_f;
            Fy_max_f = mu * Fz_f;
            constraints = [constraints,
                (U(1,k)/Fx_max_f)^2 + (Fyf/Fy_max_f)^2 <= 1];
        end
        
        % 4. 路径边界约束（避障）
        if ~isempty(params.obstacles)
            for obs = 1:length(params.obstacles)
                obs_pos = params.obstacles(obs).position;
                obs_radius = params.obstacles(obs).radius;
                dist = norm(X_pred(1:2,k+1) - obs_pos);
                constraints = [constraints, dist >= obs_radius + params.safety_margin];
            end
        end
    end
    
    % 终端代价（确保预测时域末端稳定）
    terminal_error = X_pred(1:3,N+1) - ref_trajectory(min(N, end),:)';
    cost = cost + terminal_error' * params.Q_terminal * terminal_error;
    
    % 设置求解器选项
    ops = sdpsettings('solver', 'ipopt', 'verbose', params.verbose, ...
                      'usex0', params.warm_start);
    
    % 求解优化问题
    diagnostics = optimize(constraints, cost, ops);
    
    % 提取结果
    if diagnostics.problem == 0
        u_opt = value(U(:,1));  % MPC：只取第一个控制量
        info.solved = true;
        info.cost = value(cost);
        info.iterations = diagnostics.solveroutput.iter;
        info.solve_time = diagnostics.solvertime;
    else
        warning('MPC求解失败，使用备用控制器');
        u_opt = backup_controller(x_current, ref_trajectory(1,:));
        info.solved = false;
    end
    
    % 可选：存储预测轨迹用于热启动
    if params.warm_start
        params.last_solution = value(U);
    end
end

2. 参数整定指南

% MPC参数配置示例
params.prediction_horizon = 15;        % 预测步数
params.control_horizon = 5;            % 控制步数（通常≤预测步数）
params.sampling_time = 0.1;            % 采样时间(s)

% 权重矩阵（需要根据车辆速度和跟踪精度要求调整）
params.state_weight = diag([10, 10, 5]);    % [横向误差, 纵向误差, 航向误差]
params.control_weight = diag([0.1, 0.5]);   % [加速度, 转向角]
params.control_rate_weight = diag([0.05, 1]); % 控制变化率权重

% 约束条件
params.delta_max = deg2rad(30);        % 最大转向角 ±30°
params.delta_min = -params.delta_max;
params.delta_rate_max = deg2rad(15);   % 转向角变化率限制
params.a_max = 2.5;                    % 最大加速度(m/s²)
params.a_min = -3.5;                   % 最大减速度
params.a_rate_max = 1.0;               % 加速度变化率限制

% 车辆参数
params.vehicle.m = 1500;               % 质量(kg)
params.vehicle.Iz = 2500;              % 转动惯量
params.vehicle.lf = 1.2;               % 前轴到质心距离
params.vehicle.lr = 1.5;               % 后轴到质心距离
params.vehicle.Cf = 80000;             % 前轮侧偏刚度
params.vehicle.Cr = 100000;            % 后轮侧偏刚度
params.vehicle.mu = 0.9;               % 路面摩擦系数

3. 线性时变MPC（LTV-MPC）

对于高速场景，采用线性时变模型提升实时性：

function [A, B] = linearize_vehicle_model(x_ref, u_ref, params)
    % 在工作点(x_ref, u_ref)线性化车辆模型
    
    % 提取参数
    m = params.m; Iz = params.Iz;
    lf = params.lf; lr = params.lr;
    Cf = params.Cf; Cr = params.Cr;
    
    vx = x_ref(1); vy = x_ref(2); omega = x_ref(3);
    delta = u_ref(1);
    
    % 计算雅可比矩阵
    % 状态矩阵A
    A = zeros(3,3);
    
    % ∂f1/∂vx
    A(1,1) = 0;
    A(1,2) = omega;
    A(1,3) = vy;
    
    % ∂f2/∂vy
    A(2,1) = -omega;
    A(2,2) = -(Cf*cos(delta) + Cr)/(m*vx);
    A(2,3) = -vx - (lf*Cf*cos(delta) - lr*Cr)/(m*vx);
    
    % ∂f3/∂omega
    A(3,1) = 0;
    A(3,2) = -(lf*Cf*cos(delta) - lr*Cr)/(Iz*vx);
    A(3,3) = -(lf^2*Cf*cos(delta) + lr^2*Cr)/(Iz*vx);
    
    % 控制矩阵B
    B = zeros(3,2);
    
    % ∂f/∂δ
    B(1,2) = 0;
    B(2,2) = Cf*cos(delta)/m - Cf*sin(delta)*(vy+lf*omega)/(m*vx);
    B(3,2) = lf*Cf*cos(delta)/Iz - lf*Cf*sin(delta)*(vy+lf*omega)/(Iz*vx);
    
    % ∂f/∂Fx
    B(1,1) = 1/m;
    B(2,1) = 0;
    B(3,1) = 0;
    
    % 离散化
    [A_d, B_d] = c2d(A, B, params.Ts);
end

参考代码 基于模型预测控制的车辆路径跟踪 www.youwenfan.com/contentcnt/160645.html

四、仿真实现

1. MATLAB/Simulink仿真框架

% 主仿真脚本
function simulate_vehicle_mpc()
    % 初始化
    clear; clc; close all;
    
    % 仿真参数
    Ts = 0.1;                 % 控制周期(s)
    T_total = 30;             % 总仿真时间(s)
    N_sim = floor(T_total/Ts);
    
    % 生成参考轨迹（双移线）
    [ref_traj, road_width] = generate_double_lane_change(T_total, Ts);
    
    % 车辆初始状态
    x0 = [ref_traj(1,1); ref_traj(1,2); ref_traj(1,3); 20/3.6; 0; 0]; % [X,Y,φ,v,β,ω]
    
    % MPC控制器参数
    mpc_params = setup_mpc_parameters();
    
    % 初始化记录数组
    x_history = zeros(length(x0), N_sim+1);
    u_history = zeros(2, N_sim);
    error_history = zeros(3, N_sim);
    solve_time_history = zeros(1, N_sim);
    
    x_history(:,1) = x0;
    current_x = x0;
    
    % 主仿真循环
    for k = 1:N_sim
        % 获取当前参考轨迹段（预测时域内）
        ref_start = min(k, size(ref_traj,1));
        ref_end = min(k+mpc_params.prediction_horizon, size(ref_traj,1));
        current_ref = ref_traj(ref_start:ref_end, :);
        
        % 记录当前跟踪误差
        error_history(:,k) = [current_x(1)-ref_traj(k,1);
                              current_x(2)-ref_traj(k,2);
                              current_x(3)-ref_traj(k,3)];
        
        % MPC求解
        t_start = tic;
        [u_opt, info] = mpc_controller(current_x, current_ref, mpc_params);
        solve_time = toc(t_start);
        
        solve_time_history(k) = solve_time * 1000; % 转换为ms
        
        % 应用控制量（考虑执行器延迟和饱和）
        u_applied = apply_actuator_limits(u_opt, u_history(:,max(1,k-1)), Ts);
        
        % 车辆状态更新（使用高保真模型）
        current_x = high_fidelity_vehicle_model(current_x, u_applied, Ts);
        
        % 记录
        x_history(:,k+1) = current_x;
        u_history(:,k) = u_applied;
        
        % 实时可视化（每10步更新一次）
        if mod(k,10) == 0
            visualize_simulation(x_history(:,1:k+1), ref_traj(1:k+1,:), ...
                                 error_history(:,1:k), k*Ts);
        end
    end
    
    % 性能分析
    analyze_performance(x_history, ref_traj, u_history, solve_time_history);
end

2. CarSim/Simulink联合仿真

对于高保真仿真，建议使用CarSim+Simulink：

  1. CarSim配置：

    • 选择车辆模型（如C-Class Sedan）
    • 设置道路环境（曲率、摩擦系数）
    • 定义输出信号（车速、横摆角速度、质心位置等）

  2. Simulink接口：

% S-Function接口示例
function sys = carsim_interface(t, x, u, flag)
    persistent carsim_socket;
    
    if flag == 0
        % 初始化
        carsim_socket = tcpip('localhost', 12345);
        fopen(carsim_socket);
        sys = [6, 0, 2, 6, 0, 0]; % 状态数、输入数、输出数
        
    elseif flag == 1
        % 状态更新（由CarSim计算）
        % 发送控制量u到CarSim
        fwrite(carsim_socket, u, 'double');
        
        % 接收CarSim状态
        car_state = fread(carsim_socket, 6, 'double');
        sys = car_state;
        
    elseif flag == 3
        % 输出
        sys = x;
        
    elseif flag == 9
        % 终止
        fclose(carsim_socket);
    end
end

3. 性能评估指标

function analyze_performance(x_history, ref_traj, u_history, solve_time)
    % 计算关键性能指标
    
    % 1. 跟踪误差统计
    pos_error = sqrt((x_history(1,:)-ref_traj(:,1)').^2 + ...
                     (x_history(2,:)-ref_traj(:,2)').^2);
    max_pos_error = max(pos_error);
    rms_pos_error = rms(pos_error);
    
    heading_error = abs(x_history(3,:) - ref_traj(:,3)');
    max_heading_error = max(heading_error);
    
    % 2. 控制平滑性
    delta_u = diff(u_history, 1, 2);
    control_jerk = sum(abs(delta_u(:)));
    
    % 3. 实时性
    avg_solve_time = mean(solve_time);
    max_solve_time = max(solve_time);
    real_time_ratio = avg_solve_time / (Ts*1000); % 应<1
    
    % 4. 稳定性分析（李雅普诺夫）
    % 计算误差收敛速率
    
    % 显示结果
    fprintf('===== MPC路径跟踪性能评估 =====\n');
    fprintf('跟踪精度：\n');
    fprintf('  最大位置误差：%.3f m\n', max_pos_error);
    fprintf('  RMS位置误差：%.3f m\n', rms_pos_error);
    fprintf('  最大航向误差：%.3f deg\n', rad2deg(max_heading_error));
    
    fprintf('\n控制品质：\n');
    fprintf('  控制量变化总和：%.3f\n', control_jerk);
    fprintf('  转向角使用率：%.1f%%\n', 100*max(abs(u_history(2,:)))/deg2rad(30));
    
    fprintf('\n实时性：\n');
    fprintf('  平均求解时间：%.2f ms\n', avg_solve_time);
    fprintf('  最大求解时间：%.2f ms\n', max_solve_time);
    fprintf('  实时性比率：%.2f (目标<1)\n', real_time_ratio);
    
    if real_time_ratio < 1
        fprintf('  ✓ 满足实时性要求\n');
    else
        fprintf('  ✗ 实时性不足，需优化\n');
    end
end

五、案例研究

1. 双移线测试（ISO 3888-2）

2. 高速过弯

3. 紧急避障

4. 实车测试结果

某自动驾驶项目实测数据：

场景 MPC PID LQR
城市道路跟踪 误差0.15m 误差0.25m 误差0.20m
高速匝道 误差0.20m 失稳 误差0.35m
湿滑路面 误差0.25m 振荡 误差0.40m
计算资源 85% CPU 30% CPU 60% CPU

六、工程实践建议

1. 模型选择指南

2. 实时性优化策略

  1. 代码生成：使用MATLAB Coder生成C代码，部署到dSPACE或NI实时系统

  2. 求解器选择：

    • 高精度：IPOPT（NMPC）
    • 快速：qpOASES（LMPC）
    • 嵌入式：OSQP或ECOS

  3. 降低维度：

    • 减少预测时域N（通常10-20）
    • 控制时域M=3-5（小于N）
    • 采用输入参数化（如多项式）

3. 鲁棒性增强

4. 调试与验证流程

  1. 离线仿真：MATLAB/Simulink验证算法逻辑
  2. 硬件在环：dSPACE验证实时性和接口
  3. 车辆在环：转向机器人、油门机器人测试
  4. 实车测试：封闭场地→开放道路渐进验证

七、总结

基于MPC的车辆路径跟踪通过模型预测、滚动优化，在保证安全约束的前提下实现精确跟踪。关键成功因素包括：

  1. 模型精度：根据工况选择合适的车辆模型；
  2. 参数整定：平衡跟踪精度、控制平滑性和实时性；
  3. 约束处理：合理设置物理限制和安全边界；
  4. 实时优化：选择高效求解器和代码优化技术。

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

关于我们湘ICP备19023172号-4