利用CasADi实现非完整约束小车的MPC轨迹跟踪控制

1. 从定点到轨迹：为什么你的小车需要更聪明的控制器？

大家好，我是老张，在机器人控制领域摸爬滚打了十几年。今天想和大家聊聊一个非常实际的问题：我们好不容易用CasADi和MPC（模型预测控制）让一辆非完整约束的小车（比如差速轮式机器人）能稳稳地停在一个目标点上，这感觉就像教会了它“刹车”和“瞄准”。但现实任务中，哪有那么多“站桩输出”的场景？更多时候，我们需要小车沿着一条预设的路径，比如“8”字形、圆形，或者避障生成的复杂曲线，平滑、准确地运动。这就是轨迹跟踪要解决的问题。

简单来说，定点控制是“终点思维”，只要最后能到那个点，中间怎么走、走得多晃荡，控制器可能不太关心。而轨迹跟踪是“过程思维”，它要求小车在每一时刻，都尽可能地贴合一条随时间变化的参考路径。这其中的挑战一下子就上来了：非完整约束（简单理解就是小车不能像螃蟹一样横着走）让它的运动学变得耦合且非线性；MPC虽然能处理多步预测和约束，但如何把一条连续的轨迹“喂”给控制器，并让它实时计算出一系列最优的控制指令，这中间的坑可不少。

我见过很多朋友在实现了定点控制后，想当然地直接把目标点换成轨迹上的下一个点，结果小车走得跟喝醉了一样，要么剧烈震荡，要么根本跟不上。问题的核心在于，轨迹跟踪不是简单的“串行定点控制”。它需要控制器具备“预见性”，不仅要看当前偏离了多少，还要预测未来几步的轨迹走向，并提前做出柔和的调整。这就像老司机开车，不是盯着车头一寸寸地修正，而是目光放远，预判道路的弯曲，提前打好方向盘。

基于CasADi这个强大的优化工具，我们可以相对优雅地构建这个“预见性”控制器。它能把复杂的非线性优化问题，用符号计算的方式表述出来，然后调用IPOPT这样的求解器高效计算。接下来的内容，我会手把手带你，将之前那个定点控制的MPC框架，升级为一个鲁棒、高效的轨迹跟踪控制器。我会分享我实际调试中的参数设置经验、代码重构的关键点，以及如何避免那些让仿真崩溃的常见陷阱。无论你是做学术研究，还是正在开发实际的移动机器人项目，相信这些实战经验都能让你少走弯路。

2. 核心升级：为MPC控制器注入“轨迹”灵魂

原来的定点控制MPC，其优化问题的“目标”在代码里很明确：让预测状态 X 逼近一个固定的 desired_states。对于轨迹跟踪，这个目标必须动态变化起来。我们需要在每一个控制周期，为MPC预测时域内的每一个未来时刻，都提供一个期望的状态参考点。这构成了一个参考轨迹序列。

2.1 重新定义优化问题：从静态目标到动态轨迹

首先，我们要在思想上完成转变。在定点控制中，代价函数大概是这样的： cost = sum( (X[k] - X_desired)^T * Q * (X[k] - X_desired) + ... )， 其中 X_desired 在整个预测时域内是不变的。

对于轨迹跟踪，我们需要一个随时间变化的参考序列 X_ref[k]。假设我们有一条由时间参数化的参考轨迹，它给出了每个时刻的期望位姿 (x_ref(t), y_ref(t), theta_ref(t))。那么，在离散的MPC框架下，对于预测时域的第 k 步（对应未来时间 t + k*dt），其参考状态就是 X_ref[k] = [x_ref(t+k*dt), y_ref(t+k*dt), theta_ref(t+k*dt)]^T。

因此，轨迹跟踪MPC的代价函数核心项变为： cost = sum( (X[k] - X_ref[k])^T * Q * (X[k] - X_ref[k]) + ... )

这意味着，我们需要在每次求解优化问题时，不仅传入当前状态，还要传入一整个参考轨迹序列 X_ref[0], X_ref[1], ..., X_ref[N]（N为预测步数）。在代码实现上，这要求我们对之前 NMPC1 类的构造函数和 opti_solution 函数接口进行修改。

2.2 代码结构改造：让参考轨迹“流”起来

在原来的 nmpc1.h/cpp 中，优化参数 opt_para 只包含了当前状态和固定的期望状态、期望控制。现在，我们需要将其扩展，以容纳整个参考轨迹。一个高效且清晰的做法是，将 opt_para 重新定义为： opt_para = [current_states, X_ref_0, X_ref_1, ..., X_ref_N, U_ref_0, ..., U_ref_{N-1}] 其中 X_ref_k 是三维向量，U_ref_k 是二维向量。这样，在构建代价函数时，我们就可以方便地索引到对应时刻的参考值。

但是，直接这样拼接会导致优化参数维度变得很大（3 + 3*(N+1) + 2*N），可能影响求解效率。另一种更常见的工程实践是，不在优化参数中传递整个轨迹，而是在每次求解前，根据当前时间和轨迹参数，实时计算出预测时域内的参考序列，并将其作为已知参数传入。这样，优化参数 p 可以设计为 p = [current_states, traj_params, t_current]，其中 traj_params 是描述轨迹的参数（如圆心、半径、速度等）。在构建代价函数时，我们在CasADi的符号层面，利用这些参数和 t_current 计算出每个预测步的 X_ref。

为了更直观，我们先采用第一种“传递完整序列”的方法进行讲解，因为它逻辑更直接。后续我们可以讨论第二种更优雅的方法。我们需要修改 set_my_nmpc_solver 函数中 opt_para 的定义，以及 opti_solution 函数中填充参数 parameters 的逻辑。

// 在 nmpc1.h 的类定义中，可能需要调整成员变量来存储或计算参考轨迹
class NMPC1 {
private:
    // ... 其他成员 ...
    // 新增：用于生成或存储参考轨迹的参数或函数
    std::vector<Eigen::Vector3f> m_reference_trajectory; // 或者用函数生成
public:
    // 接口需要改变，不再接受单一的 desired_states，而是接受参考轨迹或轨迹参数
    void opti_solution(Eigen::Matrix<float,3,1> current_states,
                       const std::vector<Eigen::Vector3f>& ref_trajectory, // 传入参考轨迹序列
                       const std::vector<Eigen::Vector2f>& ref_controls = {}); // 可选的参考控制序列
};

在 opti_solution 的实现里，我们需要将 ref_trajectory（长度至少为 N+1）和可选的 ref_controls（长度至少为 N）展平，填充到 parameters 向量中，然后传递给求解器。

2.3 轨迹生成器：为控制器提供“路书”

一个灵活的轨迹跟踪系统，离不开一个独立的轨迹生成模块。这个模块负责根据高级指令（如“以0.5m/s速度走半径为3米的圆”）生成一条时间、位置、姿态（甚至速度）都连续的参考轨迹 ref_trajectory 和 ref_controls。

对于非完整小车，理想的参考轨迹不仅要给出位置 (x_ref, y_ref)，最好还能给出航向角 theta_ref 以及对应的前向速度 v_ref 和角速度 omega_ref。这样，MPC的跟踪目标会更准确。例如，对于一个圆形轨迹：

• x_ref(t) = R * cos(w*t)
• y_ref(t) = R * sin(w*t)
• theta_ref(t) = atan2(y_ref_dot, x_ref_dot) = w*t + pi/2 (切线方向)
• v_ref(t) = sqrt(x_ref_dot^2 + y_ref_dot^2) = R*w (恒定)
• omega_ref(t) = w (恒定)

我们可以创建一个 TrajectoryGenerator 类，它根据轨迹类型和参数，提供 get_reference(t) 方法，返回对应时刻的参考状态和控制量。在MPC的主循环中，在每个控制周期开始，我们调用这个生成器，获取从当前时间 t 到 t+N*dt 的一系列参考点，然后传递给MPC控制器。

// 一个简单的轨迹生成器示例
class TrajectoryGenerator {
public:
    enum TrajType { CIRCLE, LINE, LEMNISCATE }; // 轨迹类型：圆、直线、8字形
    TrajectoryGenerator(TrajType type, float param1, float param2, float speed);
    // 获取从时间t开始，持续N个步长的参考序列
    void get_reference_sequence(float t, float dt, int N,
                                 std::vector<Eigen::Vector3f>& ref_states,
                                 std::vector<Eigen::Vector2f>& ref_controls);
private:
    TrajType m_type;
    float m_R; // 半径
    float m_omega; // 角速度
    // ... 其他参数
};

将轨迹生成与MPC控制解耦，使得我们的系统非常灵活，可以轻松切换不同的跟踪路径，也为后续处理动态调整（比如遇到障碍物临时修改轨迹）打下了基础。

3. 实战：一步步实现轨迹跟踪MPC

理论说再多，不如一行代码。让我们基于原始的定点控制代码，进行一场“外科手术式”的升级。我会把关键改动点和背后的思考都讲清楚。

3.1 第一步：重构NMPC求解器定义

首先，打开 nmpc1.cpp 中的 set_my_nmpc_solver 函数。这是定义优化问题的地方，我们需要修改优化参数 opt_para 的维度。

原先的 opt_para 维度是 2*n_states + n_controls（即 3+3+2=8）。现在，假设预测步长为 N，我们需要传入 N+1 个参考状态点（每个点3维）和 N 个参考控制量（每个点2维）。因此，新的维度是 3 + 3*(N+1) + 2*N。为了清晰，我们分段定义：

// 在 set_my_nmpc_solver 函数内
int n_states = 3;
int n_controls = 2;
int N = m_predict_step; // 预测步长

// 优化参数 p 的结构：[当前状态(3), 参考状态序列(3*(N+1)), 参考控制序列(2*N)]
int n_opt_para = n_states + n_states*(N+1) + n_controls*N;
casadi::SX opt_para = casadi::SX::sym("opt_para", n_opt_para);

// 从 opt_para 中提取当前状态
casadi::SX current_states = opt_para(casadi::Slice(0, n_states));
// 初始状态赋值
X(casadi::Slice(), 0) = current_states;

// 在构建代价函数的循环中，我们需要提取对应时刻的参考值
casadi::SX cost_fun = 0;
for(int k=0; k<N; k++) {
    // 提取第k步的参考状态，注意索引偏移
    int state_ref_start = n_states + k*n_states;
    casadi::SX states_ref_k = opt_para(casadi::Slice(state_ref_start, state_ref_start+n_states));
    
    // 提取第k步的参考控制（如果有的话），控制参考的索引接在状态参考后面
    int ctrl_ref_start = n_states + n_states*(N+1) + k*n_controls;
    casadi::SX controls_ref_k = opt_para(casadi::Slice(ctrl_ref_start, ctrl_ref_start+n_controls));
    
    // 状态误差和控制误差
    casadi::SX states_err = X(casadi::Slice(), k) - states_ref_k;
    casadi::SX controls_err = U(casadi::Slice(), k) - controls_ref_k;
    
    // 加权求和，Q和R是权重矩阵
    cost_fun = cost_fun + casadi::SX::mtimes({states_err.T(), m_Q, states_err})
                        + casadi::SX::mtimes({controls_err.T(), m_R, controls_err});
}
// 通常还会对终端状态（第N步）加一个额外的惩罚，这里省略了

这样，我们就定义了一个能够接受完整参考轨迹序列的优化问题。代价函数会努力让预测的状态序列 X 和控制的序列 U 分别逼近传入的参考序列。

3.2 第二步：修改求解接口与主循环

接下来，修改 opti_solution 函数的签名和内部实现。它现在需要接收两个向量：参考状态序列和参考控制序列。

void NMPC1::opti_solution(Eigen::Matrix<float,3,1> current_states,
                           const std::vector<Eigen::Vector3f>& ref_states, // 长度应为 N+1
                           const std::vector<Eigen::Vector2f>& ref_controls) // 长度应为 N
{
    // 1. 参数验证
    if(ref_states.size() < m_predict_step+1 || ref_controls.size() < m_predict_step) {
        std::cerr << "参考轨迹序列长度不足！" << std::endl;
        return;
    }
    
    // 2. 构建参数向量 parameters
    std::vector<float> parameters;
    // 2.1 放入当前状态
    for(int j=0; j<n_states; j++) parameters.push_back(current_states[j]);
    // 2.2 放入参考状态序列 (N+1 个点)
    for(int k=0; k<=m_predict_step; k++) {
        for(int j=0; j<n_states; j++) {
            parameters.push_back(ref_states[k][j]);
        }
    }
    // 2.3 放入参考控制序列 (N 个点)
    for(int k=0; k<m_predict_step; k++) {
        for(int j=0; j<n_controls; j++) {
            parameters.push_back(ref_controls[k][j]);
        }
    }
    
    // 3. 设置求解参数（边界约束 m_args["lbx"], m_args["ubx"] 和之前一样）
    m_args["p"] = parameters;
    m_args["x0"] = m_initial_guess; // 初始猜测解，可以用上次求解结果移位得到，即 warm-start
    
    // 4. 求解
    m_res = m_solver(m_args);
    
    // 5. 解析结果，更新初始猜测，计算预测轨迹等（与之前类似）
    // ... 
    std::vector<float> res_control_all(m_res.at("x"));
    // 提取第一个控制量作为当前输出
    m_control_command << res_control_all[0], res_control_all[m_predict_step]; // 假设U是[v0, v1,..., v_{N-1}, omega0, omega1,..., omega_{N-1}]的排列
    
    // 6. Warm-start: 为下一次求解提供更好的初始值
    // 将本次求解得到的控制序列去掉第一个，最后一个重复一次（或置零），作为下次的初始猜测
    std::vector<float> new_guess;
    for(int i=1; i<m_predict_step; i++) new_guess.push_back(res_control_all[i]); // v
    new_guess.push_back(res_control_all[m_predict_step-1]); // 补上一个v，可以用最后一个值或0
    for(int i=m_predict_step+1; i<2*m_predict_step; i++) new_guess.push_back(res_control_all[i]); // omega
    new_guess.push_back(res_control_all[2*m_predict_step-1]); // 补上一个omega
    m_initial_guess = new_guess;
}

在主函数 main.cpp 中，我们需要在循环的每一步：

1. 根据当前仿真时间，从轨迹生成器获取未来 N+1 个参考状态点和 N 个参考控制量。
2. 调用新的 nmpc.opti_solution(current_states, ref_states, ref_controls)。
3. 应用计算出的控制量，更新小车状态。

// 在主循环中
TrajectoryGenerator traj_gen(TrajectoryGenerator::CIRCLE, 5.0, 0.5); // 半径5m，角速度0.5 rad/s
while(sim_time < total_time) {
    // 1. 生成参考轨迹序列
    std::vector<Eigen::Vector3f> ref_states_seq;
    std::vector<Eigen::Vector2f> ref_controls_seq;
    traj_gen.get_reference_sequence(sim_time, ts, predict_step, ref_states_seq, ref_controls_seq);
    
    // 2. MPC求解
    nmpc.opti_solution(states, ref_states_seq, ref_controls_seq);
    controls = nmpc.get_controls();
    
    // 3. 更新真实小车模型
    car.state_update(controls);
    states = car.get_states();
    
    // 4. 记录、绘图...
    // ...
}

3.3 第三步：调试与参数整定经验分享

代码写完了，一跑仿真，小车可能直接飞了或者原地打转。别慌，这是常态。MPC的调试，参数整定占了80%的功夫。这里分享几个我踩过的坑和心得：

1. 权重矩阵 Q 和 R：这是调参的核心。Q 惩罚状态误差，R 惩罚控制量变化。

   • Q 矩阵：对于轨迹跟踪，x, y 的权重通常比 theta 大，因为位置误差更直观。但 theta 的权重不能为0，否则小车可能以奇怪的姿态到达目标点。我的经验起步值：Q = diag([10.0, 10.0, 2.0])。
   • R 矩阵：这个参数决定了控制器的“激进”程度。R 越大，控制器越不愿意使用大的控制量，运动越平滑，但跟踪可能滞后。R 太小，控制量会剧烈变化，可能超出物理极限或导致震荡。起步值：R = diag([0.1, 0.01])。前向速度 v 的权重通常比角速度 omega 的权重大，因为速度变化带来的能量消耗或冲击通常更值得关注。
   • 调整策略：先给一个较大的 R 保证稳定，然后逐渐减小，直到跟踪响应速度满足要求但又不震荡。然后微调 Q 中各项的比例，改善跟踪精度。

2. 预测时域 N 和采样时间 dt：这是一对耦合参数。N*dt 是预测范围。

   • 预测范围：太短，控制器“短视”，对于弯曲的轨迹可能反应不过来，容易超调。太长，计算负担重，且对遥远未来的预测可能不准确。一般让预测时间能覆盖小车惯性带来的主要动态过程。对于低速小车，1~3 秒的预测范围是个不错的起点。
   • 采样时间 dt：与你的控制器实际运行周期一致。dt 小，控制更精细，但计算更频繁；dt 大，计算压力小，但控制粗糙。通常 0.1~0.5 秒对于这类路径跟踪是合理的。
   • 经验公式：N = 预测范围 / dt。例如，想要2秒的预测，dt=0.2s，则 N=10。N 不宜过大，超过20后求解时间会显著增长，可能无法满足实时性。

3. 控制量约束：必须设置，这是物理模型的硬限制。在 opti_solution 中设置的 lbx 和 ubx 要对应小车的最大速度 v_max 和最大角速度 omega_max。约束设得太紧，控制器可能找不到可行解；设得太松，失去了意义。就按你小车模型的真实能力来设。

4. Warm-start：上面代码中第6步提到的“热启动”极其重要！把上一次求解的最优控制序列，移位后作为本次优化的初始猜测，可以大幅减少求解器的迭代次数，提高计算速度，并增强收敛性。这是实现实时MPC的关键技巧。

5. 可视化调试：一定要把预测轨迹、参考轨迹和实际轨迹同时画出来。这是最直接的调试工具。如果预测轨迹和参考轨迹在开头就偏离很大，说明当前控制问题可能无解或权重设置不当。如果实际轨迹总是滞后于参考轨迹，可能是 R 太大或 Q 太小。

4. 处理动态环境与实时路径调整

在实际应用中，比如自动驾驶或动态避障，参考轨迹不可能是预先完全确定、一成不变的。环境中的障碍物会移动，全局路径规划器可能会在途中发送新的路径点。这就要求我们的轨迹跟踪MPC具备一定的在线重规划能力。

4.1 滚动时域与实时轨迹更新

MPC天然的滚动时域特性已经为此奠定了基础。在每个控制周期，我们只执行优化序列中的第一个控制指令，然后基于新的状态和最新的环境信息，重新规划未来时域内的轨迹并求解。因此，处理动态变化的核心就变成了：如何快速生成或更新传入MPC的参考轨迹序列 ref_states_seq 和 ref_controls_seq。

一个常见的架构是分层控制：

• 上层：轨迹生成/重规划层。频率较低（比如10Hz）。它接收全局目标、当前位姿以及感知到的动态障碍物信息，生成或更新一条短期（覆盖MPC预测时域）的、无碰撞的、满足运动学约束的参考轨迹。这个轨迹可以是多项式曲线、样条曲线，甚至是另一条更快的“局部规划器”产生的。
• 下层：MPC跟踪层。频率较高（比如20-50Hz）。它接收上层传来的短期参考轨迹，进行精细的跟踪控制，并输出底层的电机指令。

在我们的代码框架里，TrajectoryGenerator 类就需要升级。它不能只按固定公式生成轨迹，而要能接受来自上层的“路径点”序列，并实时拟合出一条平滑的、带时间信息的轨迹。

4.2 应对突发状况：软约束与松弛变量

即使有重规划，也可能出现突发情况，使得在当前的约束下，严格跟踪参考轨迹变得不可能（比如一个突然出现在正前方的障碍物）。这时，硬性的状态约束可能会导致优化问题无可行解，控制器“卡死”。

为了解决这个问题，我们可以引入松弛变量。例如，我们在代价函数中，对轨迹跟踪误差的惩罚是“软”的，但我们可以对避免碰撞的约束设置一个“软”边界。更常见的做法是，在优化问题中为某些关键的状态约束（比如与障碍物的距离）添加松弛变量，并将松弛变量的大小也纳入代价函数进行惩罚。这样，当无法完全满足约束时，求解器会允许一定的违反，但会尽量最小化违反的程度，从而保证问题总是有解，控制器能给出一个“尽力而为”的次优解。

在CasADi中实现软约束，通常是在优化变量中增加松弛变量 s，并将约束改写为 g(x) <= s，同时在目标函数中添加一项 rho * s^2，其中 rho 是一个很大的权重，迫使 s 尽可能接近0（即尽可能满足原约束）。

// 在构建优化问题时，举例：添加一个与障碍物距离的软约束
casadi::SX s = casadi::SX::sym("s", 1); // 松弛变量
// 将松弛变量也加入优化变量 opt_var
opt_var = casadi::SX::vertcat({casadi::SX::reshape(U.T(), -1, 1), s});

// 定义障碍物距离约束函数 g(x) <= s
casadi::SX obstacle_distance = ... // 计算小车位置与障碍物的距离
casadi::SX g_constraint = obstacle_distance - safe_margin; // 应大于0

// 在nlp问题中添加约束
nlp_prob["g"] = g_constraint; // 约束 g(x) - s <= 0
nlp_prob["lbg"] = -casadi::inf; // 下界为负无穷
nlp_prob["ubg"] = 0;           // 上界为0，即 g(x) <= s

// 在代价函数中增加对松弛变量的惩罚
cost_fun = cost_fun + rho * s * s;

这种方法增加了问题的维度，但极大地提高了控制器的鲁棒性。在实际动态环境中，这是非常必要的安全措施。

4.3 计算效率优化：让控制器跑得更快

实时性是MPC应用的灵魂。除了前面提到的Warm-start，还有几个技巧可以提升效率：

1. 减少预测步长 N：在满足性能要求的前提下，尽可能用小一点的 N。
2. 使用更快的求解器：IPOPT功能强大但有时较慢。对于凸问题或可以线性化的问题，可以尝试OSQP或qpOASES等二次规划求解器。CasADi也支持切换后端求解器。
3. 代码生成：CasADi一个杀手级功能是代码生成。你可以将定义好的NLP问题，用 nlpsol 的 codegen 功能生成高度优化的C代码。编译后，求解速度会有数量级的提升，非常适合部署到嵌入式系统或对实时性要求极高的场景。

   // 在定义求解器后，可以生成C代码
   m_solver.generate_dependencies("nmpc_solver.c");
   // 然后编译成一个独立的函数，反复调用
   

4. 简化模型：如果精度允许，可以考虑对小车模型进行适当的线性化或简化，将NMPC（非线性MPC）转化为LMPC（线性MPC），计算量会大大降低。

轨迹跟踪MPC的实现，是一个从理论到实践的完整闭环。它要求我们不仅理解优化和控制理论，还要有扎实的编程能力和工程调试经验。当你看到自己编写的小车，流畅地沿着预定的“8”字或复杂曲线运动，并能对外界干扰做出稳健反应时，那种成就感是无与伦比的。希望这篇结合了原始代码和实战经验的长文，能帮你顺利跨过从定点控制到轨迹跟踪这道坎。记住，多调参，多看图，耐心是工程师最好的美德。如果在实现中遇到具体问题，欢迎随时交流讨论。