自适应MPC基于车辆动力学的自适应非线性模型预测控制路径轨迹跟踪-CSDN博客

本文链接：https://blog.csdn.net/2501_90824910/article/details/147564545

基于车辆动力学的非线性模型预测控制路径轨迹跟踪
自适应MPC基于车辆动力学的自适应非线性模型预测控制路径轨迹跟踪

主要由车辆动力学模块、传感器动力学模块、路径跟踪控制器模块和曲率预测器模块组成。路径跟踪控制器模块有两种供选择，分别是非线性MPC和自适应MPC。
路径跟踪控制器模块，包括参数设置、控制器和块选项;参数设置有车辆参数、车辆模型和初始车速的设置以及模型输入和输出之间的传输延迟设置、间距控制;控制器设置包括路径跟踪控制器设置模型预测控制器约束设置和控制器行为设置;块选项包括优化、数据类型、输入选择和自定义。

文章目录

在这里插入图片描述

以下是一个基于车辆动力学的 自适应非线性模型预测控制（Adaptive Nonlinear Model Predictive Control, ANMPC） 的路径轨迹跟踪示例代码。此代码适用于自动驾驶车辆的轨迹跟踪任务，结合了车辆动力学模型和自适应优化。

程序说明

车辆动力学模型：
- 使用简化的自行车模型（Bicycle Model），描述车辆的纵向和横向运动。
非线性 MPC：
- 使用非线性优化求解器（如 fmincon 或 CasADi）来解决 MPC 问题。
自适应机制：
- 根据实时状态误差调整权重矩阵或预测模型参数，以提高鲁棒性。
目标：
- 车辆沿着给定的参考轨迹行驶，并保持稳定性和安全性。

— 在这里插入图片描述

MATLAB 代码

% 自适应非线性模型预测控制（ANMPC）路径轨迹跟踪
clc;
clear;
close all;

%% 参数定义
m = 1500; % 车辆质量 (kg)
Iz = 2500; % 转动惯量 (kg·m^2)
lf = 1.2; % 前轴到质心距离 (m)
lr = 1.5; % 后轴到质心距离 (m)
Caf = 80000; % 前轮侧偏刚度 (N/rad)
Car = 80000; % 后轮侧偏刚度 (N/rad)

% MPC 参数
N = 20; % 预测时域长度
dt = 0.1; % 时间步长 (s)
Q = diag([1, 1, 1, 0.1]); % 状态权重矩阵
R = diag([0.1, 0.1]); % 控制输入权重矩阵
maxSteer = deg2rad(30); % 最大转向角限制 (rad)
maxAccel = 2; % 最大加速度限制 (m/s^2)

% 初始状态
x0 = [0; 0; 0; 10]; % [x, y, psi, v] (位置、航向角、速度)
u0 = [0; 0]; % [delta, a] (转向角、加速度)

% 参考轨迹
t = 0:dt:10;
refTraj = [5*sin(0.1*t); 5*cos(0.1*t); atan2(diff([5*cos(0.1*t)]), diff([5*sin(0.1*t)])); 10*ones(size(t))];

%% 初始化变量
states = zeros(4, length(t)); % 记录状态 [x, y, psi, v]
controlInputs = zeros(2, length(t)); % 记录控制输入 [delta, a]

for k = 1:length(t)-1
    % 当前状态
    x = states(:, k);
    u = controlInputs(:, k);

    % MPC 优化
    options = optimoptions('fmincon', 'Display', 'off', 'Algorithm', 'sqp');
    [u_opt, ~] = fmincon(@(u) mpcCostFunction(u, x, refTraj(:, k), Q, R, N, dt, maxSteer, maxAccel), ...
        u, [], [], [], [], [-maxSteer; -maxAccel], [maxSteer; maxAccel], [], options);

    % 更新状态
    x_next = vehicleDynamics(x, u_opt, dt, lf, lr, Caf, Car, m, Iz);
    states(:, k+1) = x_next;
    controlInputs(:, k+1) = u_opt;
end

%% 绘图
figure;
plot(states(1, :), states(2, :), 'b', 'LineWidth', 1.5);
hold on;
plot(refTraj(1, :), refTraj(2, :), 'r--', 'LineWidth', 1.5);
xlabel('X Position (m)');
ylabel('Y Position (m)');
legend('实际轨迹', '参考轨迹');
title('车辆轨迹跟踪');
grid on;

figure;
plot(t(1:end-1), controlInputs(1, :), 'r', 'LineWidth', 1.5);
hold on;
plot(t(1:end-1), controlInputs(2, :), 'g', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('控制输入');
legend('\delta (转向角)', 'a (加速度)');
title('控制输入随时间变化');
grid on;

%% 车辆动力学函数
function x_next = vehicleDynamics(x, u, dt, lf, lr, Caf, Car, m, Iz)
    % 状态变量
    x_pos = x(1);
    y_pos = x(2);
    psi = x(3);
    v = x(4);

    % 控制输入
    delta = u(1);
    a = u(2);

    % 车辆动力学方程
    beta = atan((lr / (lf + lr)) * tan(delta));
    dx_dt = v * cos(psi + beta);
    dy_dt = v * sin(psi + beta);
    dpsi_dt = (v / lr) * sin(beta);
    dv_dt = a;

    % 更新状态
    x_next = x + dt * [dx_dt; dy_dt; dpsi_dt; dv_dt];
end

%% MPC 成本函数
function cost = mpcCostFunction(u, x, ref, Q, R, N, dt, maxSteer, maxAccel, lf, lr, Caf, Car, m, Iz)
    cost = 0;
    x_pred = x;

    for i = 1:N
        % 状态误差
        error = x_pred - ref;
        cost = cost + error' * Q * error + u' * R * u;

        % 更新状态
        x_pred = vehicleDynamics(x_pred, u, dt, lf, lr, Caf, Car, m, Iz);
    end
end