车辆三自由度运动学模型；
Carsim/Simulink联合仿真；
无人驾驶车辆模型预测控制（2.1）；
包括Carsim的设置、控制信号数据、PPT文件、cpar件、车辆运动分析图（适用于word两栏布局）；

Simulink 中 MATLAB Function 模块所需的 MPC 核心算法代码。这段代码实现了 3自由度车辆动力学模型（纵向、横向、横摆） 的离散化、预测矩阵构建以及二次规划求解。

MPC 控制器核心代码 (MATLAB Function)

轮转角。

function delta_k = MPC_Controller(x_current, ref_trajectory, params)
%#codegen
% 无人驾驶车辆MPC控制器
% 输入:
% x_current: [X, Y, psi, Vx, Vy, r] 当前车辆状态 (Carsim输出)
% ref_trajectory: [X_ref, Y_ref, psi_ref, Vx_ref] 参考轨迹点 (N_p步)
% params: 结构体，包含车辆参数和MPC权重

% --- 1. 参数提取 ---
m = params.m;         % 质量
Iz = params.Iz;       % 转动惯量
lf = params.lf;       % 质心到前轴距离
lr = params.lr;       % 质心到后轴距离
Cf = params.Cf;       % 前轮侧偏刚度
Cr = params.Cr;       % 后轮侧偏刚度

N_p = params.N_p;     % 预测时域
N_c = params.N_c;     % 控制时域
dt = params.dt;       % 采样时间

% 权重矩阵 (根据PPT中的优化目标调整)
Q = params.Q;         % 状态误差权重 [Y_err, psi_err]
R = params.R;         % 控制量权重 [delta]

% --- 2. 获取当前工作点 ---
Vx = x_current(4);
Vy = x_current(5);
r = x_current(6);
psi = x_current(3);

% 限制最小速度以防止除零
if Vx < 0.1
    Vx = 0.1;
end

% --- 3. 建立线性时变状态空间模型 (LTV) ---
% 状态变量: xi = [Vy; r] (横向动力学部分)
% 注意：X, Y, psi 的运动学部分单独处理或包含在增广矩阵中

% 状态矩阵 A (基于自行车模型线性化)
% d_Vy = -(Cf+2Cr)/(Vx)Vy - (Vx + (Cflf-Crlr)/(Vx))r + (Cf/m)delta
% d_r  = -(Cflf-Crlr)/(IVx)Vy - (Cflf^2+Crlr^2)/(IVx)r + (Cflf/Iz)*delta

a11 = -(Cf + 2Cr) / (m * Vx);
a12 = -Vx - (Cflf - Crlr) / (m * Vx);
a21 = -(Cflf - Crlr) / (Iz * Vx);
a22 = -(Cflf^2 + Crlr^2) / (Iz * Vx);

b11 = 2*Cf / m;
b21 = Cflf / Iz;

A_cont = [a11, a12; a21, a22];
B_cont = [b11; b21];

% 离散化 (欧拉法或零阶保持)
I_2 = eye(2);
A_k = I_2 + A_cont * dt;
B_k = B_cont * dt;

% --- 4. 构建增广状态空间 (包含误差积分项以提高跟踪精度) ---
% 这里简化处理，直接构建预测矩阵 Phi 和 Theta

% 构建预测矩阵 (简化版，实际应用中建议预先计算或使用稀疏矩阵)
% xi(k+1) = A_xi(k) + B_ku(k)

% 初始化预测矩阵
% 这里仅展示核心逻辑，实际代码需根据N_p展开

% 假设我们要控制的是横向误差 e_y 和航向误差 e_psi
% 需要在 Simulink 外部计算误差，或者在此处通过坐标变换计算

% --- 5. 坐标变换计算误差 (Frenet坐标系近似) ---
% 从 ref_trajectory 获取当前参考点
X_ref = ref_trajectory(1);
Y_ref = ref_trajectory(2);
psi_ref = ref_trajectory(3);

% 全局坐标转车体/误差坐标
% e_y = -(X - X_refsin(psi_ref) + (Y - Y_ref)cos(psi_ref)
% e_psi = psi - psi_ref

% 注意：x_current(1)是X, x_current(2)是Y
e_y = -(x_current(1) - X_refsin(psi_ref) + (x_current(2) - Y_ref)cos(psi_ref);
e_psi = x_current(3) - psi_ref;

% 增广状态向量 chi = [e_y; e_psi; Vy; r]
% 由于代码长度限制，此处直接构建QP问题矩阵

% --- 6. 构建二次规划 (QP) 问题 ---
% min J = U' * H * U + 2 * f' * U

% 这里使用简单的显式求解 (适用于短预测时域) 或调用 quadprog (需代码生成支持)
% 假设 N_p = 10, N_c = 3

% 构造 Hessian 矩阵 H 和 梯度向量 f
% 此处为伪代码逻辑，实际需展开矩阵乘法
% Phi: 状态转移矩阵堆叠
% Theta: 控制矩阵堆叠

% 简化的单步预测示例 (实际应循环 N_p 次):
% x_pred = A_k * x_curr + B_k * u
% cost = (x_pred - ref)' * Q * (x_pred - ref) + u' * R * u

% --- 7. 求解 (使用简单的矩阵求逆或内点法) ---
% 在实际嵌入式或Simulink代码生成中，通常使用固定大小的矩阵运算

% 示例：假设我们只用运动学模型进行简化预测以生成代码框架
% x_dot = Vx * e_psi + Vy
% e_psi_dot = r
% r_dot = ... (动力学)

% 为了提供可用的代码，以下是基于运动学模型的MPC核心矩阵构建
% 状态: [e_y; e_psi]
% 输入: delta

A_mpc = [0, Vx; 0, 0];
B_mpc = [0; Vx / (lf + lr)]; % 简化模型

A_d = eye(2) + A_mpc * dt;
B_d = B_mpc * dt;

% 构建 Theta 和 Phi
Phi = zeros(2*N_p, 2);
Theta = zeros(2*N_p, N_c);

for i = 1:N_p
    if i == 1
        Phi(i-1:2i, :) = A_d;
        Theta(i-1:2i, 1) = B_d;
    else
        Phi(i-1:2i, :) = Phi((i-1)-1:2(i-1), :) * A_d;
        for j = 1:min(i, N_c)
            idx_r = 2*i-1;
            idx_c = j;
            prev_idx = 2*(i-1)-1;
            Theta(idx_r:idx_r+1, idx_c) = Theta(prev_idx:prev_idx+1, idx_c) + B_d;
        end
    end
end

% 目标函数矩阵
H = Theta' * kron(eye(N_p), Q) * Theta + kron(eye(N_c), R);

% 误差向量
error_vec = [e_y; e_psi];
f_vec = (error_vec' * Phi' * kron(eye(N_p), Q) * Theta)';

% 求解 U = -Hf (无约束情况下的解析解)
% 实际应用中建议加入约束并使用 active-set 或 ipopt
U_opt = -H  f_vec;

% 输出第一个控制量
delta_k = U_opt(1);

end

Carsim 设置要点

为了配合上述代码，Carsim 的设置需要遵循以下步骤：

车辆选择：选择标准的 B-Class Hatchback 或 SUV 模型。
输入变量：确保 Simulink 传递给 Carsim 的变量包含：
Steer_SW (方向盘转角，需将代码输出的 delta 转换为方向盘角度，通常乘以转向传动比，如 16)。
Brake_Pressure (制动压力，如果做纵向控制)。
Throttle_Percent (油门开度，如果做纵向控制)。
输出变量：从 Carsim 提取给 Simulink 的变量必须包含：
X_Spr (质心X坐标)
Y_Spr (质心Y坐标)
Yaw_Spr (横摆角)
Vx_Spr (纵向速度)
Vy_Spr (侧向速度)
YawVel_Spr (横摆角速度)

车辆运动分析图 (Word两栏布局建议)

如果您需要制作 PPT 或 Word 文档中的分析图，建议使用以下布局：

左侧栏：
图1：轨迹跟踪对比图。绘制 X-Y 平面坐标，一条线为参考路径（虚线），一条线为实际路径（实线），标记出“横向误差”。
图2：控制输入图。绘制方向盘转角随时间变化的曲线。
右侧栏：
图3：速度响应图。绘制纵向速度和侧向速度的变化，验证车辆是否稳定。
图4：误差收敛图。单独绘制横向误差和航向误差随时间收敛到0的曲线。

PPT 展示的是 车辆运动学模型 的推导过程，特别是基于后轴中心点作为参考点的运动学方程。

PPT 右下角的公式 7 给出了最终的状态方程，即控制量为 [v_{vr}, delta_f] 时的系统模型。

以下是基于该 PPT 推导逻辑编写的 MATLAB 代码，包含了模型仿真和绘图功能，可以直接运行以复现车辆在该模型下的运动轨迹。

MATLAB 仿真代码

这段代码实现了 PPT 中公式 7 的离散化仿真，模拟了车辆在给定速度和转向角输入下的轨迹。

%% 车辆运动学模型仿真 (基于PPT公式7)
% 对应PPT内容：车辆运动模型推导及最终状态方程

clear; clc; close all;

% — 1. 参数初始化 —
dt = 0.01; % 仿真步长 (s)
T = 20; % 总仿真时间 (s)
t = 0:dt:T; % 时间向量
L = 2.8; % 轴距 (m)，图中为 L

% — 2. 初始状态 —
% 状态向量 State = [X_r, Y_r, phi] (后轴中心坐标 + 航向角)
X_r = 0;
Y_r = 0;
phi = 0; % 初始航向角 (rad)

% 用于存储轨迹
trajectory = zeros(length(t), 3);
trajectory(1, 😃 = [X_r, Y_r, phi];

% — 3. 定义控制输入 (控制量) —
% 对应PPT中的控制量 [v_vr, delta_f]
% 模拟一个简单的转弯工况：速度恒定，转向角阶跃
v_vr = 10 * ones(size(t)); % 后轴速度 10 m/s (约36km/h)
delta_f = zeros(size(t)); % 前轮转角

% 在 2秒 到 8秒 之间进行转向
idx_steer = find(t >= 2 & t 圆弧 -> 直线”的形状。
右图：航向角 varphi 随时间的变化。在转向期间，航向角会线性增加（因为速度和转角恒定，角速度也是恒定的）。