基于matlab+simulink联合仿真模型自动驾驶汽车的建模、轨迹规划和非线性控制技术

今天，世界上最具挑战性的话题之一是自动驾驶汽车的问题。公司正在大力投资这项技术的开发，并对该主题进行了广泛的研究，包括特斯拉和谷歌。 自动驾驶汽车有几个部分;最初，它必须通过其传感器（例如超声波接近传感器、用于图像处理的摄像头、GPS 卫星通信模块以及速度和加速度传感器）接收来自环境的反馈。 然后，通过数据集成方法，这些信息到达控制器，控制器的主要任务是将所需的输入应用于系统操作员，以接收所需的输出（例如油门、制动器、齿轮调整和转向角）。

移动机器人建模
动态模型
系统的动力学模型有很多方法，一些模型考虑发动机的动力学、制动系统的动力学以及侧倾和台阶的运动，而另一些模型只代表车辆的动力学。在本节中，研究了动态四轮驱动模型，该模型考虑了汽车的六个自由度（考虑汽车的所有可能运动）。
要理解描述系统的方程组，有必要了解车辆在 X-Y 平面上的运动，其中包括车辆的侧倾和步进子系统、推进动力学、制动动力学和发动机。最后，利用得到的运动模型，可以得到系统描述矩阵。 为了实现动力学方程，我们需要在每个轴上实现力和扭矩的平衡。因此，X 轴定义为汽车的纵轴，Y 轴定义为横向轴，Z 轴定义为垂直于道路。这个 XYZ 框架位于汽车的重心，以下简称 CoG。为简单起见，本项目中不考虑 Z 轴。

运动学模型和动力学模型之间的主要区别之一是假设零滑移假设和考虑较小的侧向力。两者都假设了低速的概念，因此可以说当汽车低速行驶时，电影模型提供了可接受的结果。两者之间的其他区别包括该模型没有考虑其他模型之间的质量和惯性。为简单起见，本节基于两轮运动学模型提出了方程。

泰勒展开法已用于线性化动力学和运动学系统。 由于动态系统复杂度高，利用运动学系统生成问题控制器，然后使用动态模型对所得控制器进行最终测试。

以下文字及示例代码仅供参考

以下是基于 MATLAB + Simulink 的 自动驾驶汽车建模、轨迹规划与非线性控制 的完整联合仿真方案，涵盖从车辆动力学建模、路径/轨迹规划（Path Planning）、到控制器设计（如 MPC、PID、Stanley 控制器）的全流程实现。

---

🧠 一、系统目标

本项目旨在构建一个完整的 自动驾驶汽车仿真平台，实现以下功能：

1. 车辆建模：使用自行车模型（Bicycle Model）描述车辆动力学；
2. 轨迹规划：基于参考路径生成时间最优或曲率连续的轨迹；
3. 非线性控制：使用 MPC（Model Predictive Control） 或 Stanley 控制器 实现车道保持与路径跟踪；
4. 可视化与评估：绘制车辆运动轨迹、横向误差、控制输入等关键指标。

---

📦 二、所需工具箱

确保安装以下 MATLAB 工具箱：

• Simulink
• Simscape Multibody（可选）
• Model Predictive Control Toolbox
• Control System Toolbox
• Navigation Toolbox / Robotics System Toolbox
• Optimization Toolbox

---

🚗 三、车辆动力学建模（Vehicle Dynamics）

✅ 使用 Bicycle Model（自行车模型）

% 车辆参数
m = 1575;        % 质量 (kg)
Iz = 2870;       % 绕 z 轴转动惯量 (kg·m²)
lf = 1.2;        % 前轴到质心距离 (m)
lr = 1.6;        % 后轴到质心距离 (m)
Cf = 80000;      % 前轮胎侧偏刚度 (N/rad)
Cr = 80000;      % 后轮胎侧偏刚度 (N/rad)

% 纵向速度 vx (假设为常数)
vx = 20;         % m/s

% 状态方程：x_dot = A*x + B*u
A = [0, 1, 0, 0;
     0, -(Cf+Cr)/(m*vx), (Cf+Cr)/m, (Cr*lr - Cf*lf)/(m*vx);
     0, 0, 0, 1;
     0, (Cr*lr - Cf*lf)/(Iz*vx), (Cf*lf - Cr*lr)/Iz, -((Cf*lf^2)+(Cr*lr^2))/(Iz*vx)];

B = [0;
     Cf/m;
     0;
     (Cf*lf)/Iz];

C = eye(4);
D = zeros(4,1);

sys_vehicle = ss(A,B,C,D);

---

🛣️ 四、轨迹规划（Trajectory Planning）

✅ 参考路径生成（Frenet 坐标系下）

% 定义参考路径（例如直线或曲线）
s_ref = 0:0.1:100;
x_ref = s_ref;
y_ref = sin(s_ref * 0.1);   % 弯道示例

% 计算路径点的航向角和曲率
psi_ref = atan2(diff(y_ref), diff(x_ref));
kappa_ref = gradient(psi_ref, 0.1);

✅ 时间最优轨迹生成（使用 Optimization Toolbox）

% 使用优化方法生成平滑轨迹
options = optimoptions('fmincon','Display','iter','Algorithm','sqp');
traj_opt = fmincon(@(x) trajectoryCost(x, x_ref, y_ref), ...
                   x0, [], [], [], [], lb, ub, @(x) trajectoryConstraints(x));

---

🎯 五、控制器设计（Controller Design）

1. MPC 控制器（推荐）

Ts = 0.1;          % 控制周期
p = 10;            % 预测步长
m = 3;             % 控制步长

mpcobj = mpc(sys_vehicle, Ts, p, m);

% 设置权重
mpcobj.Weights.OutputVariables = [10 0 1 0];   % ey 最重要
mpcobj.Weights.ManipulatedVariablesRate = 0.1;

% 输入限制
mpcobj.MV.Min = -0.5;    % rad
mpcobj.MV.Max = 0.5;
mpcobj.MV.RateMin = -0.2;
mpcobj.MV.RateMax = 0.2;

2. Stanley 控制器（适用于低速场景）

function delta = stanley_control(x, y, psi, v, path_x, path_y, path_psi)
    k = 0.5;  % 控制增益

    % 查找最近路径点
    [~, idx] = min((path_x - x).^2 + (path_y - y).^2);
    
    % 横向偏差
    e_y = (y - path_y(idx)) * cos(path_psi(idx)) - (x - path_x(idx)) * sin(path_psi(idx));

    % 航向角偏差
    e_psi = wrapToPi(psi - path_psi(idx));

    % Stanley 控制律
    delta = e_psi + atan(k * e_y / (v + 0.1));  % 防止除零
end

---

🧩 六、Simulink 联合仿真模型（autonomous_car.slx）

模型结构如下：

[Reference Path Generator] -->
[Path Planner] -->
[Controller (MPC or Stanley)] -->
[Vehicle Dynamics (Bicycle Model)] -->
[Outputs: x, y, psi, vyaw, etc.] -->
[Scope / Visualization]

主要模块说明：

模块	功能
MATLAB Function Block	实现 Stanley 控制逻辑
MPC Controller Block	执行预测控制算法
State Space Block	表示车辆动力学模型
XY Graph / Scope	显示车辆轨迹与控制效果

---

📊 七、结果可视化脚本（后处理）

figure;
plot(x_real, y_real, 'b-', 'LineWidth', 1.5);
hold on;
plot(x_ref, y_ref, 'r--', 'LineWidth', 1.5);
legend('实际轨迹', '参考路径');
title('车辆路径跟踪效果');
xlabel('X (m)'); ylabel('Y (m)');
grid on;

figure;
plot(tout, lateral_error, 'g-', 'LineWidth', 1.5);
title('横向误差随时间变化');
xlabel('时间 (s)'); ylabel('e_y (m)');
grid on;

---