基于NMPC算法实现的车辆路径跟踪控制，建立车辆动力学模型，

资深码侬

于 2025-03-31 07:00:00 发布

阅读量841

点赞数 27

分类专栏：路径规划文章标签：算法

本文链接：https://blog.youkuaiyun.com/2501_90314346/article/details/146772634

版权

路径规划专栏收录该内容

54 篇文章

订阅专栏

基于NMPC算法实现的车辆路径跟踪控制，建立车辆动力学模型，跟踪具有参考速度的期望路径
通过CarSim和Simulink联合仿真，仿真效果良好
图为车辆路径图，纵向速度，前轮转角图
有对应的参考论文及Carsim配置cpr文件
在这里插入图片描述

文章目录

基于非线性模型预测控制（NMPC）的车辆路径跟踪控制是一种高效的控制方法，能够处理复杂的非线性系统和约束。以下是一个完整的实现方案，包括车辆动力学模型、NMPC控制器设计、CarSim 和 Simulink 的联合仿真配置以及代码示例。

1. 车辆动力学模型

我们使用自行车模型（Bicycle Model）来描述车辆的动力学行为：

状态变量：

( x ): 车辆纵向位置
( y ): 车辆横向位置
( \psi ): 航向角
( v ): 纵向速度
( \dot{\psi} ): 横摆角速度

输入变量：

( \delta ): 前轮转角
( a ): 加速度

动力学方程：

[
\begin{aligned}
\dot{x} &= v \cos(\psi) \
\dot{y} &= v \sin(\psi) \
\dot{\psi} &= \frac{v}{L} \tan(\delta) \
\dot{v} &= a
\end{aligned}
]
其中，( L ) 是车辆轴距。

2. NMPC 控制器设计

NMPC 通过优化目标函数来生成控制输入，同时考虑系统约束。

目标函数：

[
J = \sum_{k=0}^{N-1} \left( |x_k - x_{ref,k}|^2 + |y_k - y_{ref,k}|^2 + |\psi_k - \psi_{ref,k}|^2 + |\delta_k|^2 + |a_k|^2 \right)
]

约束条件：

车辆动力学约束
输入约束：( |\delta| \leq \delta_{max}, |a| \leq a_{max} )
状态约束：( v_{min} \leq v \leq v_{max} )

NMPC 实现：

使用 MATLAB 的 fmincon 或 MPC 工具箱实现优化问题。

3. CarSim 和 Simulink 联合仿真

CarSim 配置：

在 CarSim 中定义车辆参数（如质量、轴距、轮胎特性等）。
导出车辆的状态变量（如车速、横摆角速度、位置等）到 Simulink。
接收 Simulink 的控制信号（如前轮转角和加速度）。

Simulink 模型结构：

参考轨迹生成模块：
- 使用五次多项式或三次样条插值生成参考路径和速度。
NMPC 控制模块：
- 输入：参考轨迹、车辆当前状态。
- 输出：前轮转角 ( \delta ) 和加速度 ( a )。
CarSim 接口模块：
- 将 NMPC 输出传递给 CarSim，并接收车辆状态反馈。

— 在这里插入图片描述

4. 代码实现

4.1 参考轨迹生成

function [x_ref, y_ref, v_ref] = generate_reference_trajectory(initial_pos, target_pos, initial_speed, target_speed, time)
    % 输入参数：
    % initial_pos: 初始位置 [x0, y0]
    % target_pos: 目标位置 [xf, yf]
    % initial_speed: 初始速度 (m/s)
    % target_speed: 目标速度 (m/s)
    % time: 时间序列 (秒)

    t = time;
    T = max(t);
    a0_x = initial_pos(1);
    a1_x = initial_speed;
    a2_x = 0;
    a3_x = (10*(target_pos(1)-initial_pos(1)) - (4*T*initial_speed + 6*T*target_speed)) / T^3;
    a4_x = (-15*(target_pos(1)-initial_pos(1)) + (7*T*initial_speed + 8*T*target_speed)) / T^4;
    a5_x = (6*(target_pos(1)-initial_pos(1)) - (3*T*initial_speed + 3*T*target_speed)) / T^5;

    b0_y = initial_pos(2);
    b1_y = 0;
    b2_y = 0;
    b3_y = (10*(target_pos(2)-initial_pos(2))) / T^3;
    b4_y = (-15*(target_pos(2)-initial_pos(2))) / T^4;
    b5_y = (6*(target_pos(2)-initial_pos(2))) / T^5;

    v_ref = initial_speed + (target_speed - initial_speed) * t / T;

    x_ref = a0_x + a1_x*t + a2_x*t.^2 + a3_x*t.^3 + a4_x*t.^4 + a5_x*t.^5;
    y_ref = b0_y + b1_y*t + b2_y*t.^2 + b3_y*t.^3 + b4_y*t.^4 + b5_y*t.^5;
end

4.2 NMPC 控制器

function [delta, a] = nmpc_controller(x_ref, y_ref, psi_ref, v_ref, x_car, y_car, psi_car, v_car, L, N, dt)
    % 输入参数：
    % x_ref, y_ref, psi_ref, v_ref: 参考轨迹
    % x_car, y_car, psi_car, v_car: 当前车辆状态
    % L: 车辆轴距
    % N: 预测时域长度
    % dt: 时间步长

    % 定义优化问题
    options = optimoptions('fmincon', 'Display', 'off');
    u0 = [0; 0]; % 初始猜测
    lb = [-pi/4; -5]; % 输入下界
    ub = [pi/4; 5];   % 输入上界

    % 目标函数
    cost_func = @(u) nmpc_cost(u, x_ref, y_ref, psi_ref, v_ref, x_car, y_car, psi_car, v_car, L, N, dt);

    % 优化求解
    [u_opt, ~] = fmincon(cost_func, u0, [], [], [], [], lb, ub, [], options);

    % 输出控制量
    delta = u_opt(1);
    a = u_opt(2);
end

function cost = nmpc_cost(u, x_ref, y_ref, psi_ref, v_ref, x_car, y_car, psi_car, v_car, L, N, dt)
    % 提取控制输入
    delta = u(1);
    a = u(2);

    % 初始化状态
    x = x_car;
    y = y_car;
    psi = psi_car;
    v = v_car;

    % 计算代价
    cost = 0;
    for k = 1:N
        % 动力学更新
        x = x + v * cos(psi) * dt;
        y = y + v * sin(psi) * dt;
        psi = psi + (v / L) * tan(delta) * dt;
        v = v + a * dt;

        % 跟踪误差
        cost = cost + (x - x_ref(k))^2 + (y - y_ref(k))^2 + (psi - psi_ref(k))^2 + 0.1*delta^2 + 0.01*a^2;
    end
end