基于MPC的自动泊车路径跟踪控制方案详解

1. 项目概述：基于运动学的自动泊车MPC路径跟踪方案

这个开源项目实现了一套完整的自动泊车路径跟踪控制系统，核心采用模型预测控制（MPC）算法，基于车辆运动学模型进行轨迹规划与跟踪。不同于传统PID控制，MPC通过滚动优化和反馈校正机制，能够更好地处理系统约束和非线性特性，特别适合自动泊车这种需要精确控制的应用场景。

项目最大的特点是"开箱即用"——所有代码模块完整封装，无需复杂配置即可运行测试。系统会实时输出路径跟踪误差曲线和前轮转角变化图，这对算法调试和性能评估非常有用。我在实际测试中发现，这套方案对标准泊车场景的横向位置误差能控制在5cm以内，完全满足实际应用需求。

2. 核心原理与技术架构

2.1 车辆运动学建模基础

项目采用经典的自行车模型作为运动学基础，这是大多数自动驾驶控制系统的标准选择。模型假设车辆只有前轮转向，后轮固定，且忽略轮胎侧偏等动力学因素。其状态方程可表示为：

code复制ẋ = v * cos(θ + β)
ẏ = v * sin(θ + β)
θ̇ = (v / L) * sin(β)
β = arctan((lr / L) * tan(δ))

其中(x,y)是后轴中心坐标，θ是航向角，v是车速，δ是前轮转角，L是轴距，lr是后轴到质心的距离。这个模型虽然简化，但对低速泊车场景已经足够精确。

2.2 MPC控制器设计要点

MPC的核心在于滚动优化和反馈校正。项目中的控制器每50ms执行一次以下流程：

1. 获取当前车辆状态（位置、航向等）
2. 基于运动学模型预测未来N步的状态轨迹
3. 求解优化问题，最小化跟踪误差和控制量变化
4. 只执行第一步控制量，下一周期重新优化

优化问题的代价函数通常包含：

• 路径跟踪误差（横向+航向）
• 控制量（前轮转角）大小
• 控制量变化率（保证转向平顺）

注意：预测时域N的选择很关键。太短会导致"短视"，太长会增加计算负担。实测发现对于泊车场景，N=10（对应0.5秒）是个不错的平衡点。

3. 代码实现与关键模块解析

3.1 项目文件结构说明

代码库采用模块化设计，主要包含以下核心文件：

code复制├── main.py              # 主运行入口
├── vehicle_model.py