自动驾驶MPC路径跟踪与传感器融合仿真系统

资 源 简 介

本项目旨在MATLAB/Simulink环境下构建一个高保真的自动驾驶车辆横纵向协同控制仿真平台，主要解决复杂交通场景下的精确路径跟踪与动态避障问题。系统功能涵盖四个主要模块：首先是车辆与环境建模，利用Vehicle Dynamics Blockset建立包含轮胎非线性特性的七自由度车辆动力学模型，并构建包含直道、弯道及动态障碍物的虚拟测试场景；其次是感知与信息融合，通过模拟激光雷达与摄像头数据，应用扩展卡尔曼滤波(EKF)算法对多传感器数据进行融合处理，以建立精确的车辆状态估计与周围环境地图，有效过滤传感器噪声；第三是核心控制算法实现，设计基于模型预测控制(MPC)的横向控制器和基于PID的纵向速度控制器，将路径跟踪问题转化为约束优化问题，在满足车辆物理约束（如最大转向角、最大加速度）的同时，计算最优控制序列以最小化跟踪误差；最后是可视化与性能分析，系统提供实时3D仿真动画展示车辆运动姿态，并自动生成包含横向误差、航向误差、控制量平滑度及计算耗时的详细分析报告，用于评估算法在不同工况下的鲁棒性与实时性。该项目能够为高级驾驶辅助系统(ADAS)的算法开发提供从理论验证到代码生成的全流程支持。

详 情 说 明

基于模型预测控制(MPC)与传感器融合的自动驾驶车辆路径跟踪仿真系统

项目简介

这是一个基于MATLAB开发的高保真自动驾驶仿真平台。该项目在纯MATLAB环境下（不依赖Simulink Vehicle Dynamics Blockset）实现了车辆动力学模拟，并集成扩展卡尔曼滤波（EKF）进行多传感器数据融合，利用模型预测控制（MPC）算法实现精确的横向路径跟踪。系统能够模拟车辆在包含传感器噪声的复杂环境下的自动驾驶行为，完成从直线行驶到动态变道（Double Lane Change）的避障轨迹跟踪。

主要功能特性

• 高保真仿真环境：完全基于MATLAB脚本构建，包含车辆物理参数定义、环境建模及仿真时序控制。
• 传感器融合感知：模拟GPS位置数据和IMU航偏角数据，并注入高斯白噪声。通过扩展卡尔曼滤波（EKF）算法，有效融合多传感器信息，滤除噪声，输出平滑准确的车辆状态估计值。
• MPC横向控制：实现了基于线性误差模型的模型预测控制器。通过构建预测时域内的车辆状态方程，将路径跟踪转化为二次规划（QP）问题，在满足转向角约束的同时计算最优前轮转角。
• PID纵向控制：采用PID算法控制车辆加速度，确保车辆能够稳定地追踪期望纵向速度。
• 实时可视化与分析：仿真过程中提供实时的车辆运动轨迹、参考路径及位姿显示，并记录详细的仿真数据（误差、控制量等）用于后续性能分析。

系统要求

• MATLAB R2018b 或更高版本
• Optimization Toolbox（必须安装，用于求解MPC中的二次规划问题 quadprog）

详细实现逻辑与算法说明

本项目的所有核心逻辑均封装在主程序及相关子函数中，主要包含以下几个关键模块：

1. 系统初始化与场景构建

仿真开始时，系统首先定义了详细的车辆物理参数，包括质量、轴距、转动惯量及轮胎侧偏刚度等。同时设置了MPC控制器的预测时域（Prediction Horizon）和控制时域（Control Horizon），以及状态权重矩阵Q和控制权重矩阵R。

• 参考路径生成：利用生成函数构建了一条全长包含直线与正弦曲线切换的虚拟轨迹，模拟车辆在行驶过程中的平滑变道或避障行为。路径信息包含坐标(X, Y)、航向角(Phi)及曲率(Kappa)。

2. 车辆动力学建模 (The Plant)

虽然项目描述提及7自由度模型，但实际代码中实现了一个非线性自行车模型（Non-linear Bicycle Model）作为受控对象（Plant）。

• 该模型考虑了车辆的运动学特性及简化的侧偏角（Beta）估算。
• 输入量为前轮转角和加速度，输出量为车辆在全局坐标系下的真实位置、航向角、速度及横摆角速度。
• 模型包含物理限制逻辑，如最大转角和最大加速度限制。

3. 感知与状态估计 (EKF)

为了模拟真实的自动驾驶传感器环境，代码中包含噪声注入与滤波模块：

• 传感器模拟：在真实车辆状态基础上叠加高斯白噪声，分别模拟GPS（位置X, Y）和IMU（航向角 Yaw）的观测数据。
• 扩展卡尔曼滤波 (EKF)：

* 预测步：基于车辆运动学方程和上一时刻的控制量（转向、加速度），预测当前时刻的先验状态及协方差矩阵。计算过程中使用了线性化的雅可比矩阵（Jacobian Matrix）。 * 更新步：利用观测矩阵H对比预测值与模拟传感器量测值，计算卡尔曼增益K，对状态向量[X, Y, Phi, V]进行最优校正，大幅降低了传感器噪声对控制系统的影响。

4. 路径跟踪控制系统

控制系统采用横纵向解耦的设计思路：

#### 纵向控制 (PID) 使用经典的比例-积分（PI）控制器计算期望加速度。控制器根据设定的期望速度（如10m/s）与EKF估计的当前车速之间的误差，调节车辆的纵向加速度，实现定速巡航。

#### 横向控制 (MPC) 这是系统的核心算法模块，旨在最小化横向误差和航向误差。

• 误差计算：实时搜索参考路径上距离车辆最近的点（Match Point），计算车辆当前的横向误差（Lateral Error）和航向误差（Heading Error）。
• 模型线性化与离散化：基于车辆动力学误差微分方程，构建连续时间的状态空间模型（$A_c, B_c$），并采用欧拉法将其离散化为($A_d, B_d$)。状态向量包含：[横向误差，横向误差变化率，航向误差，航向误差变化率]。
• 二次规划求解：在每个控制周期内，构建标准二次规划（QP）问题。算法在预测时域内寻找最优的“前轮转角增量序列”，使得在满足车辆物理约束（最大转角）的前提下，代价函数（包含跟踪误差加权和控制量平滑度）最小。

5. 数据记录与可视化

• 仿真采用了主循环机制，以固定步长（0.05s）推进时间。
• 实时绘图：每隔一定帧数刷新画布，动态展示车辆当前的位姿、历史轨迹以及目标参考路径。
• 结果分析：仿真结束后，计算总耗时，并调用分析模块对全程的横向误差、控制量输入曲线等关键性能指标进行统计和绘图。

使用方法

1. 确保MATLAB已正确安装Optimization Toolbox。
2. 打开MATLAB，将工作路径切换至项目文件夹。
3. 直接运行主脚本（通常命名为 main）。
4. 程序将自动弹出仿真窗口展示车辆运动，并在MATLAB控制台输出仿真进度与耗时信息。
5. 仿真结束后，将生成包含误差分析的图表。