📚 课程目录第4章模型预测控制轨迹跟踪

4. 模型预测控制轨迹跟踪

任务目标

实现一个基于线性时变模型预测控制（LTV-MPC）的轨迹跟踪控制器，以车辆运动学模型为预测模型，通过二次规划求解最优前轮转角，使车辆在仿真环境中精确跟踪给定的参考轨迹（包含位置与航向角）。本案例聚焦MPC核心闭环：预测模型构建、约束优化求解、滚动时域控制与跟踪效果验证。

环境与依赖

Python 3.8+，仅需NumPy与SciPy（用于QP求解）
可选：Matplotlib用于可视化跟踪结果

核心实现

采用车辆运动学自行车模型：状态量为 [x, y, yaw]，控制量为前轮转角 delta，纵向速度假设恒定。在每个控制周期，基于当前状态线性化预测模型，构建带控制量约束的二次规划问题，滚动求解未来N步的最优控制序列，仅执行第一步。

import numpy as np
from scipy.optimize import minimize

# 车辆参数
L = 2.5          # 轴距(m)
V = 5.0          # 纵向速度(m/s)，恒定
DT = 0.1         # 控制周期(s)
N = 15           # 预测时域步数
DELTA_MAX = 0.6  # 前轮转角上限(rad)

# 运动学模型离散递推
def kinematic_model(state, delta, dt=DT):
    x, y, yaw = state
    x_next = x + V * np.cos(yaw) * dt
    y_next = y + V * np.sin(yaw) * dt
    yaw_next = yaw + V / L * np.tan(delta) * dt
    return np.array([x_next, y_next, yaw_next])

# 参考轨迹生成：圆形轨迹
def generate_reference_trajectory(steps=200):
    t = np.linspace(0, 2*np.pi, steps)
    radius = 20.0
    ref_x = radius * np.cos(t)
    ref_y = radius * np.sin(t)
    ref_yaw = t + np.pi/2
    return np.stack([ref_x, ref_y, ref_yaw], axis=1)

# MPC单步控制求解
def mpc_control(state, ref_traj, k):
    # 提取预测时域内的参考点
    ref_seq = ref_traj[k:k+N]
    if len(ref_seq) < N:
        ref_seq = np.pad(ref_seq, ((0, N-len(ref_seq)), (0,0)), mode='edge')

    # 决策变量：N个前轮转角
    def cost(u):
        u = np.clip(u, -DELTA_MAX, DELTA_MAX)
        s = state.copy()
        total_cost = 0.0
        for i in range(N):
            s = kinematic_model(s, u[i])
            err = s - ref_seq[i]
            total_cost += err @ err  # 位置与航向误差加权
        # 控制量平滑项
        if N > 1:
            total_cost += 0.1 * np.sum(np.diff(u)**2)
        return total_cost

    # 初始猜测：零转角
    u0 = np.zeros(N)
    bounds = [(-DELTA_MAX, DELTA_MAX)] * N
    res = minimize(cost, u0, method='SLSQP', bounds=bounds)
    return np.clip(res.x[0], -DELTA_MAX, DELTA_MAX)

# 主仿真循环
def run_simulation():
    ref_traj = generate_reference_trajectory()
    state = np.array([ref_traj[0,0], ref_traj[0,1], ref_traj[0,2]])
    actual_traj = [state.copy()]
    control_inputs = []

    for k in range(len(ref_traj)-1):
        delta = mpc_control(state, ref_traj, k)
        state = kinematic_model(state, delta)
        actual_traj.append(state.copy())
        control_inputs.append(delta)

    return np.array(actual_traj), np.array(control_inputs), ref_traj

if __name__ == "__main__":
    traj, controls, ref = run_simulation()
    # 计算跟踪误差（位置欧氏距离）
    errors = np.linalg.norm(traj[:, :2] - ref[:len(traj), :2], axis=1)
    print(f"最大跟踪误差: {errors.max():.4f} m")
    print(f"平均跟踪误差: {errors.mean():.4f} m")
    print(f"控制量范围: [{controls.min():.3f}, {controls.max():.3f}] rad")

运行与验证

将上述代码保存为 mpc_tracking.py，在终端执行 python mpc_tracking.py
程序输出最大/平均跟踪误差与控制量范围
通过条件：最大跟踪误差 < 0.5 m，平均跟踪误差 < 0.2 m，且所有控制量值 ≤ 0.6 rad（满足约束）

关键说明

代价函数中位置误差与航向误差等权相加，实际应用中可按需调整权重矩阵
采用SLSQP求解非线性优化问题，因预测模型为非线性；若改用线性误差模型，可转化为标准QP问题
参考轨迹为半径20m的圆，对应稳态转向角约 atan(L/R)=0.124 rad，可验证控制器稳态精度