4. 模型预测控制(MPC)基础实现

任务目标

基于车辆运动学自行车模型，实现一个离散时间线性MPC控制器，使其能够跟踪直线与圆弧参考轨迹。通过调节预测时域 N，观察跟踪精度与计算开销的变化，并验证控制器在给定初始误差下的收敛能力。

核心原理与实现要点

采用运动学自行车模型作为预测模型，状态量为 [x, y, yaw]，控制量为 [v, delta]（速度与前轮转角）。在参考轨迹点处对非线性模型进行线性化，得到线性时变预测模型，进而构建二次规划(QP)问题求解最优控制序列。MPC核心为：在每个控制周期，基于当前状态求解未来N步的最优控制序列，仅将第一组控制量作用于系统，下一周期滚动优化。

完整实现代码

import numpy as np
from scipy.optimize import minimize

# 车辆运动学模型参数
L = 2.5  # 轴距(m)

def kinematic_bicycle(x, u, dt):
    """离散运动学自行车模型"""
    yaw = x[2]
    v = u[0]
    delta = u[1]
    x_next = np.array([
        x[0] + v * np.cos(yaw) * dt,
        x[1] + v * np.sin(yaw) * dt,
        yaw + v / L * np.tan(delta) * dt
    ])
    return x_next

def mpc_control(x0, ref_traj, N, dt, v_ref, delta_max=np.radians(30)):
    """
    线性MPC控制器
    x0: 当前状态 [x, y, yaw]
    ref_traj: 参考轨迹点集 (N+1个点), 每个点为 [x, y, yaw]
    N: 预测时域
    dt: 采样时间
    v_ref: 参考速度
    """
    n_state = 3
    n_control = 2
    
    # 线性化参考点（取当前状态作为线性化点）
    x_lin = x0
    u_lin = np.array([v_ref, 0.0])
    
    # 状态矩阵离散化（前向欧拉）
    A = np.eye(n_state)
    A[0, 2] = -v_ref * np.sin(x_lin[2]) * dt
    A[1, 2] = v_ref * np.cos(x_lin[2]) * dt
    
    B = np.zeros((n_state, n_control))
    B[0, 0] = np.cos(x_lin[2]) * dt
    B[1, 0] = np.sin(x_lin[2]) * dt
    B[2, 1] = v_ref / (L * np.cos(u_lin[1])**2) * dt
    
    # 构建QP问题（使用scipy.optimize.minimize求解）
    # 决策变量: [v_0..v_{N-1}, delta_0..delta_{N-1}]
    # 以及预测状态 x_1..x_N（通过约束隐式表达）
    
    def cost(u_flat):
        u_seq = u_flat.reshape(N, n_control)
        x_pred = x0.copy()
        cost = 0.0
        for k in range(N):
            # 线性化预测
            x_pred = A @ (x_pred - x_lin) + B @ u_seq[k] + x_lin
            # 跟踪误差代价
            ref_k = ref_traj[k+1]
            err = x_pred - ref_k
            cost += err @ np.diag([10.0, 10.0, 1.0]) @ err
            # 控制量代价
            cost += u_seq[k] @ np.diag([1.0, 10.0]) @ u_seq[k]
            # 控制增量代价（平滑性）
            if k > 0:
                du = u_seq[k] - u_seq[k-1]
                cost += du @ np.diag([0.1, 0.1]) @ du
        return cost
    
    # 约束：控制量边界
    bounds = [(-2.0, 2.0), (-delta_max, delta_max)] * N
    
    # 初始猜测：恒速直行
    u_init = np.zeros((N, n_control))
    u_init[:, 0] = v_ref
    
    result = minimize(cost, u_init.flatten(), method='SLSQP', bounds=bounds)
    u_opt = result.x.reshape(N, n_control)
    return u_opt[0]  # 返回第一组控制量

def simulate_tracking(ref_traj, x0, N, dt, steps):
    """闭环仿真：MPC滚动优化"""
    x = x0.copy()
    traj = [x.copy()]
    for i in range(steps):
        # 截取从当前时刻起的N+1个参考点
        idx = min(i, len(ref_traj) - N - 1)
        ref_seg = ref_traj[idx:idx+N+1]
        u = mpc_control(x, ref_seg, N, dt, v_ref=2.0)
        x = kinematic_bicycle(x, u, dt)
        traj.append(x.copy())
    return np.array(traj)

# ============ 轨迹生成与验证 ============
def generate_straight_line(length=30.0, dt=0.1):
    """生成直线参考轨迹（沿x轴）"""
    t = np.arange(0, length, 2.0*dt)
    ref = np.zeros((len(t), 3))
    ref[:, 0] = t * 2.0  # x坐标
    ref[:, 2] = 0.0      # yaw=0
    return ref

def generate_circle(radius=10.0, dt=0.1):
    """生成圆弧参考轨迹"""
    t = np.arange(0, 2*np.pi, dt)
    ref = np.zeros((len(t), 3))
    ref[:, 0] = radius * np.cos(t)
    ref[:, 1] = radius * np.sin(t)
    ref[:, 2] = t + np.pi/2  # 切向角
    return ref

# 测试：直线跟踪
print("=== 直线跟踪测试 (N=10) ===")
ref_line = generate_straight_line()
x0_line = np.array([0.0, 1.0, 0.1])  # 初始横向偏差1m，航向偏差0.1rad
traj_line = simulate_tracking(ref_line, x0_line, N=10, dt=0.1, steps=100)
final_err_line = np.linalg.norm(traj_line[-1][:2] - ref_line[min(100, len(ref_line)-1)][:2])
print(f"终点横向位置误差: {final_err_line:.4f} m")

# 测试：圆弧跟踪
print("\n=== 圆弧跟踪测试 (N=15) ===")
ref_circle = generate_circle()
x0_circle = np.array([10.0, 0.5, np.pi/2])  # 初始径向偏差0.5m
traj_circle = simulate_tracking(ref_circle, x0_circle, N=15, dt=0.1, steps=200)
final_err_circle = np.linalg.norm(traj_circle[-1][:2] - ref_circle[min(200, len(ref_circle)-1)][:2])
print(f"终点径向位置误差: {final_err_circle:.4f} m")

# 预测时域影响对比
print("\n=== 预测时域N对跟踪误差的影响（直线） ===")
for N_test in [5, 10, 20]:
    traj_test = simulate_tracking(ref_line, x0_line, N=N_test, dt=0.1, steps=100)
    err = np.linalg.norm(traj_test[-1][:2] - ref_line[min(100, len(ref_line)-1)][:2])
    print(f"N={N_test}: 终点误差 = {err:.4f} m")

运行方式与依赖

• 依赖：Python 3.8+，NumPy，SciPy（pip install numpy scipy）
• 运行：将代码保存为 mpc_basic.py，执行 python mpc_basic.py

验证方法与判定标准

• 直线跟踪：初始横向偏差1m、航向偏差0.1rad，仿真10秒后终点横向位置误差应小于0.1m，证明MPC能消除初始误差并稳定跟踪。
• 圆弧跟踪：初始径向偏差0.5m，仿真20秒后终点径向误差应小于0.2m，验证曲率路径下的跟踪能力。
• 预测时域对比：增大N（如从5增至20）通常会减小稳态跟踪误差，但单步求解时间增加。观察输出误差随N的变化趋势是否符合预期。

常见问题

• 求解失败：若minimize返回异常，可检查参考轨迹是否连续、控制量边界是否过紧，或适当增大maxiter参数。
• 模型失配：线性化点取当前状态而非参考点，在大曲率路径下可能引入误差，可通过减小dt缓解。
• 实时性：本实现使用SLSQP求解器，N较大时计算耗时增加，实际应用中可改用OSQP等专用QP求解器。