CarSim仿真程序中的MPC自动驾驶轨迹跟踪控制：从原理到实践

1次阅读

共计 2304 个字符，预计需要花费 6 分钟才能阅读完成。

在自动驾驶轨迹跟踪领域，传统 PID 控制存在明显局限性：

预测能力缺失 ：PID 是纯反馈控制，无法预判未来轨迹变化，面对急弯或连续 S 形路段时容易超调
约束处理困难 ：难以直接融入轮胎力饱和、转向角限制等物理约束
耦合问题 ：横向和纵向控制通常需要独立设计，协调性较差

而 MPC（模型预测控制）凭借以下优势成为更优选择：

前馈 + 反馈结合 ：基于动力学模型预测未来多步状态
显式处理约束 ：将控制量、状态量约束直接写入优化问题
多目标优化 ：通过权重矩阵平衡跟踪精度与控制消耗

\begin{cases}
x_{k+1} = x_k + v_k \cos(\psi_k) \Delta t \\
y_{k+1} = y_k + v_k \sin(\psi_k) \Delta t \\
\psi_{k+1} = \psi_k + \frac{v_k}{L_f} \delta_k \Delta t \\
v_{k+1} = v_k + a_k \Delta t
\end{cases}

其中 $(x,y)$ 为车辆坐标，$\psi$ 为航向角，$\delta$ 为前轮转角，$L_f$ 为轴距

\min \sum_{i=1}^{N_p} \| y_{ref} - y_i \|_Q^2 + \sum_{i=0}^{N_c-1} \| \Delta u_i \|_R^2

$N_p$ 为预测时域，$N_c$ 为控制时域，$Q/R$ 为权重矩阵

在 CarSim 中配置 VS_Comm 模块输出车辆状态（X/Y/Yaw/Vel）
将 MPC 控制器编译为 S -Function（需 VS2015 以上）
通过 Simulink Real-Time 模块连接 CarSim RT

[VS_API]
采样周期 = 0.02s   # 建议与控制器保持一致
输出通道 = 4,5,14,18  # X,Y,Yaw,Vx

import casadi as ca

# 构建 QP 问题
opti = ca.Opti()
X = opti.variable(N_p, 4)  # 状态变量
U = opti.variable(N_c, 2)  # 控制量 (加速度, 转向)

# 目标函数
cost = 0
for i in range(N_p):
    cost += (X[i,0]-xref[i])**2 * Q[0]  # X 误差
    cost += (X[i,1]-yref[i])**2 * Q[1]  # Y 误差

# 动力学约束
for k in range(N_p-1):
    x_next = vehicle_model(X[k,:], U[k//step_ratio,:])
    opti.subject_to(X[k+1,:] == x_next )

# 控制量约束
opti.subject_to(opti.bounded(-3, U[:,0], 3) )  # 加速度限制
opti.subject_to(opti.bounded(-0.6, U[:,1], 0.6) )  # 转向角限制

# 求解
opti.solver('ipopt')
sol = opti.solve()

#include <osqp/osqp.h>

// 构造 QP 矩阵
Eigen::SparseMatrix<double> P(2*N_c, 2*N_c);
Eigen::VectorXd q = Eigen::VectorXd::Zero(2*N_c);

// 填充 P 矩阵（示例）for(int i=0; i<N_c; i++){P.insert(i,i) = R_accel;
    P.insert(N_c+i, N_c+i) = R_steer;
}

// OSQP 求解
OSQPSolver solver;
OSQPSettings settings;
osqp_set_default_settings(&settings);
settings.eps_abs = 1e-5;

OSQPData *data = osqp_data_form(P, q, A, l, u);
osqp_setup(&solver, data, &settings);
osqp_solve(&solver);