基于MPC的车辆圆形轨迹跟踪优化实践

1. 项目背景与核心挑战

在自动驾驶和机器人运动控制领域，轨迹跟踪一直是个经典但极具挑战性的问题。我最近在做一个基于模型预测控制（MPC）的轨迹跟踪项目，目标是让车辆能够精准跟踪圆形轨迹，同时以后轴中心作为参考基准。这个看似简单的需求，在实际实现过程中遇到了不少意料之外的坑。

圆形轨迹跟踪在泊车场景、环形道路测试中非常常见，而后轴基准则是车辆动力学建模的标准做法。但将二者结合时，传统PID控制会出现超调大、收敛慢的问题，而MPC虽然理论上更优，参数调不好反而会让车辆"画龙"。经过两周的调试和算法迭代，终于实现了厘米级的跟踪精度，这里把完整方案和踩坑经验分享给大家。

2. 系统建模与参考系选择

2.1 车辆动力学模型构建

我们采用经典的自行车模型（Bicycle Model）作为基础，这是大多数MPC控制器的标准选择。模型假设：

• 忽略悬架动态，车辆简化为前后两轮
• 前轮转向角δ较小（小于15°）
• 纵向速度vx保持恒定（约2m/s）

状态方程如下：

code复制ẋ = vx * cos(θ + β)
ẏ = vx * sin(θ + β)
θ̇ = (vx / Lr) * sin(β)
β = arctan( (Lr / (Lf+Lr)) * tan(δ) )

其中Lf、Lr分别是前后轴到质心的距离，β是滑移角。

关键细节：实际编码时要将连续模型离散化，采样时间Δt建议取0.05-0.1s。太大会丢失动态细节，太小会增加计算负担。

2.2 后轴基准的特殊处理

大多数论文以质心为参考点，但实际车辆控制更关注后轴中心（特别是倒车场景）。我们需要做坐标系转换：

1. 将期望轨迹点(x_d,y_d)转换到后轴坐标系：

code复制x'_d = x_d - Lr*cosθ
y'_d = y_d - Lr*sinθ

2. 计算跟踪误差时，使用转换后的坐标与当前后轴位置(x_r,y_r)的偏差：

code复制e = sqrt( (x'_d - x_r)² + (y'_d - y_r)² )

实测发现，不进行这个转换会导致半径5m的圆轨迹出现10cm以上的稳态误差。

3. MPC控制器设计

3.1 成本函数设计

成本函数是MPC的核心，我们采用分阶段加权策略：

python复制cost = 0
# 跟踪误差项
cost += 100 * (x_ref - x_pred)^2  
cost += 100 * (y_ref - y_pred)^2
# 控制量变化率项
cost += 1 * (δ_k - δ_{k-1})^2  
# 终端状态惩罚
cost += 500 * (x_ref_N - x_N)^2

调参心得：前两项系数需要比文献推荐值大3-5倍，因为后轴基准放大了误差。但过大会导致控制量振荡，需要配合下面的松弛因子。

3.2 约束条件设置

除了常规的转向角限制（±30°），我们特别添加了：

• 路径偏移松弛变量ε，允许暂时性偏离：

  code复制-0.1 ≤ y_err - ε ≤ 0.1
  ε ≥ 0
  

• 转向速率限制（±15°/s），防止电机过载

在CVXPY中实现如下：

python复制constraints = [
    x[1:] == A @ x[:-1] + B @ u,
    u >= -np.deg2rad(30),
    u <= np.deg2rad(30),
    delta_u >= -np.deg2rad(15)*dt,
    delta_u <= np.deg2rad(15)*dt 
]

3.3 实时线性化技巧

非线性MPC计算量太大，我们采用每步重新线性化的策略：

1. 在当前状态点(x0,u0)泰勒展开
2. 计算雅可比矩阵A,B
3. 求解线性化后的QP问题

实测在树莓派4B上，预测时域N=10时单次求解仅需8ms。

4. 圆形轨迹生成与跟踪

4.1 轨迹参数化方法

圆形轨迹采用角度参数化：

python复制def generate_circle(R, center, N):
    theta = np.linspace(0, 2*np.pi, N)
    x = center[0] + R * np.cos(theta)
    y = center[1] + R * np.sin(theta)
    return np.vstack([x, y]).T

注意点：步长不宜过密，建议每0.2m取一个点。太密会导致MPC过早切换下一个目标点。

4.2 前视距离动态调整

我们发现固定前视距离(Look-ahead)在弯道表现差，改为速度自适应：

code复制L_d = min(1.5, max(0.8, 0.3*vx))

其中1.5m是上限，防止急弯失稳；0.8m是下限，保证控制灵敏度。

5. 实车调试问题排查

5.1 典型问题与解决方案

5.2 关键参数调试记录

经过50多次迭代测试，最优参数组合为：

• 预测时域：N=12
• 控制时域：M=5
• 状态权重：Q=diag([100,100,10])
• 控制权重：R=0.1
• 采样时间：Δt=0.08s

在直径10m的圆形路径上测试，最终实现了：

• 平均跟踪误差：2.3cm
• 最大误差：4.7cm
• 转向角变化率：≤12°/s

6. 性能优化技巧

6.1 热启动加速求解

利用上一帧的解作为初始猜测，可减少40%求解时间：

python复制# 第一次求解
problem.solve(solver=ECOS)
# 后续帧
problem.solve(solver=ECOS, warm_start=True, init_vals=prev_solution)

6.2 状态估计补偿

当GPS更新频率(10Hz)低于控制频率(12.5Hz)时，采用运动学预测：

code复制x_k = x_{k-1} + v*dt*cos(θ+β)
y_k = y_{k-1} + v*dt*sin(θ+β)
θ_k = θ_{k-1} + v*dt*sin(β)/Lr

这个简单的补偿让跟踪误差降低了32%。

7. 扩展应用与改进方向

当前系统在以下场景表现良好：

• 低速（<5m/s）园区物流车
• 自动泊车路径跟踪
• 农业机械定轨作业

下一步计划：

1. 增加路面坡度补偿项
2. 融合视觉的轨迹预测
3. 开发参数自整定模块

这套方案已经成功应用在三个实际项目中，核心代码片段如下（完整实现见GitHub仓库）：

python复制class MPCController:
    def __init__(self, N=10, dt=0.1):
        self.N = N  # 预测时域
        self.dt = dt  # 采样时间
        self.setup_optimization()
    
    def solve(self, x0, ref_traj):
        # 更新初始约束
        self.constraints[0] = self.x[:,0] == x0
        
        # 更新参考轨迹
        self.cost = 0
        for k in range(self.N):
            self.cost += quad_form(self.x[:,k]-ref_traj[k], self.Q)
            
        # 求解优化问题
        self.problem.solve()
        return self.u[:,0].value

最后分享一个调试心得：当跟踪效果不理想时，先检查参考轨迹在后轴坐标系下的投影是否正确，这个问题浪费了我整整两天时间。建议在rviz中同时显示原始轨迹和转换后轨迹进行验证。