牵引-挂车轨迹跟踪控制算法

本文档根据当前工程中的 mpc.h 与 mpc.cpp 整理，用于说明当前牵引车-挂车系统轨迹跟踪控制器的算法结构、状态定义、误差模型、约束条件与执行流程。

这份文档描述的是“代码里实际实现的控制器”，不是论文中后端轨迹优化器本身。后端规划器输出连续参考轨迹，控制器将其离散采样后，利用模型预测控制（MPC）在线求解速度与转角命令，实现对牵引-挂车系统的闭环跟踪。

1. 控制器目标

当前控制器的核心任务是：

1. 接收后端优化器发布的牵引-挂车参考轨迹；
2. 根据当前车辆状态，从参考轨迹中截取一个有限预测窗口；
3. 利用牵引-挂车离散运动学模型，预测未来若干步状态；
4. 最小化预测状态与参考状态之间的误差；
5. 在满足速度、转角和控制增量约束的前提下，求得当前时刻控制命令。

控制输出为 Ackermann 形式的两维控制量：

$$u=\left[\begin{array}{c}v\\ \delta \end{array}\right]$$

其中：

• $v$ 为牵引车纵向速度；
• $\delta$ 为牵引车转向角。

2. 代码入口与模块关系

控制器主类为 MPC，实现位置如下：

• 类定义：mpc.h
• 主要实现：mpc.cpp

控制闭环涉及以下几个对象：

• planner::ArcTrailerTraj
  后端优化器输出的连续参考轨迹消息。
• TrajAnalyzer
  将连续轨迹转换为“按当前时刻可采样”的离散参考点序列。
• planner::TrailerState
  当前牵引车及各节挂车的状态观测。
• MPC
  构造优化问题、求解控制量并发布控制命令。

整体数据流为：

$$\text{ArcTrailerTraj}\to \text{TrajAnalyzer}\to \text{xref}\to \text{MPC}\to \text{cmd}(v,\delta )$$

3. 状态量与控制量定义

3.1 状态向量

当前控制器内部统一使用如下状态向量：

$$x={\left[\begin{array}{cccccc}{x}_0& y_0& {\theta }_0& {\theta }_1& \cdots & {\theta }_N\end{array}\right]}^T$$

其中：

• $x_0,y_0$：牵引车参考点位置，代码中对应牵引车后轴中心位置；
• ${\theta }_0$：牵引车航向角；
• ${\theta }_1\sim {\theta }_N$：从第一节挂车到第 $N$ 节挂车的航向角；
• $N$：挂车数量，对应代码宏 TRAILER_NUM。

因此状态维数为：

$$n_x=3+N$$

3.2 控制向量

控制量为：

$$u={\left[\begin{array}{cc}v& \delta \end{array}\right]}^T$$

其中：

• $v$：牵引车速度；
• $\delta$：牵引车转角。

控制维数为：

$$n_u=2$$

4. 当前控制器使用的误差模型

4.1 误差状态的定义方式

当前实现并没有单独建立一个“误差系统动力学模型”，而是直接在每个预测步上用参考状态与预测状态相减，构造状态误差：

$$\Delta x_k=x_k^{ref}-x_k^{pred}$$

这也是当前代码中 delta_x = ref_state - X(:, i) 的含义。

因此，真正被 MPC 加权优化的误差状态量为：

$$\Delta x_k={\left[\begin{array}{cccccc}{e}_x& e_y& e_{{\theta }_0}& e_{{\theta }_1}& \cdots & e_{{\theta }_N}\end{array}\right]}^T$$

它们分别表示：

• $e_x$：牵引车在世界坐标系下的 $x$ 方向位置误差；
• $e_y$：牵引车在世界坐标系下的 $y$ 方向位置误差；
• $e_{{\theta }_0}$：牵引车航向误差；
• $e_{{\theta }_1}\sim e_{{\theta }_N}$：各节挂车航向误差。

4.2 为什么误差模型是这些状态量

从实现看，控制器重点约束的是：

1. 牵引车是否走到参考位置；
2. 牵引车朝向是否对齐；
3. 每节挂车姿态是否与参考队形保持一致。

也就是说，当前控制器并没有显式把以下量作为独立误差状态写入优化变量：

• 横向误差 / 航向误差的 Frenet 形式；
• 铰接角误差；
• 曲率误差；
• 弧长误差。

这些关系并不是完全没有被考虑，而是通过以下两种方式隐式体现：

1. 挂车航向角本身已经反映了拖挂系统姿态链；
2. stateTrans() 中的耦合运动学会把牵引车与挂车之间的铰接关系传递到未来预测状态中。

因此，当前代码采用的是一种“直接状态跟踪型 MPC”，而不是“显式误差状态空间 MPC”。

5. 牵引-挂车离散运动学预测模型

5.1 模型作用

MPC 在每个控制周期都需要预测：如果未来若干步施加某组控制输入，系统状态会如何演化。这个预测由 mpc.h 中的 stateTrans() 完成。

5.2 牵引车状态更新

牵引车位置和航向更新为：

$$x_{0,k+1}=x_{0,k}+v_k\cos ({\theta }_{0,k})dt$$

$$y_{0,k+1}=y_{0,k}+v_k\sin ({\theta }_{0,k})dt$$

$${\theta }_{0,k+1}={\theta }_{0,k}+\frac{v_k\tan ({\delta }_k)}{L}dt$$

其中：

• $L$ 为牵引车轴距，对应 trailer.wheel_base；
• $dt$ 为 MPC 预测步长。

5.3 挂车航向传播

对第 $i$ 节挂车，代码中采用如下离散传播关系：

$${\theta }_{i,k+1}={\theta }_{i,k}+\frac{v_{i-1,k}\sin ({\theta }_{i-1,k}-{\theta }_{i,k})}{L_{head,i}}dt$$

其中：

• $L_{head,i}$ 为第 $i$ 节挂车与前一节车体之间的等效连接长度；
• $v_{i-1,k}$ 为上一节车体沿本体传播到当前连接点的速度。

速度沿挂接链逐节衰减：

$$v_{i,k}=v_{i-1,k}\cos ({\theta }_{i-1,k}-{\theta }_{i,k})$$

这意味着：

• 如果前后两节车体姿态接近一致，速度传播较完整；
• 如果前后夹角较大，则传递给后车体的有效速度会减小；
• 挂车姿态变化由整条拖挂系统的几何耦合共同决定。

5.4 模型离散化方式

当前实现采用前向欧拉法离散化，因此模型简单直接、在线计算开销较小，适合每个控制周期重复构造优化问题并求解。

6. 参考轨迹的生成与误差计算

6.1 参考轨迹来源

参考轨迹并非控制器内部生成，而是来自后端规划器输出的 ArcTrailerTraj。控制器在回调 rcvArcTrajCallBack() 中调用 traj_analyzer.setTraj(msg) 将其转化为可查询轨迹对象。

6.2 参考轨迹窗口截取

在每次定时器回调 cmdCallback() 中，控制器调用：

$$\text{xref}=\text{traj\_analyzer.getRefPoints}(N_{pre},dt)$$

这里得到的是一个滚动参考窗口：

$$\left\{x_0^{ref},x_1^{ref},\dots ,x_{N_{pre}-1}^{ref}\right\}$$

其中：

• $N_{pre}$ 为预测步数；
• $dt$ 为步长；
• 每个参考点都包含完整拖挂状态：
  $[x_0,y_0,{\theta }_0,{\theta }_1,\dots ,{\theta }_N]$。

控制器每次只优化这个有限窗口，而不是对整条长轨迹做直接控制。

6.3 为什么需要航向角展开

航向角通常以 $[-\pi ,\pi ]$ 表示。如果当前状态是 $+\pi$ 附近，而参考点刚好以 $-\pi$ 附近表示，那么虽然姿态几乎一致，但直接相减会得到接近 $2\pi$ 的虚假误差。

因此，smooth_yaw() 会对参考窗口中的牵引车与各节挂车航向角做角度展开，目的有两个：

1. 让第一个参考点尽量靠近当前真实状态；
2. 让后续参考点与前一个参考点角度连续。

处理后，MPC 看到的是连续航向序列，从而避免因为角度跳变导致控制发散或误判。

6.4 参考轨迹误差如何计算

第 $k$ 个预测步中，控制器先构造该步的参考状态：

$$x_k^{ref}={\left[\begin{array}{cccccc}{x}_{0,k}^{ref}& y_{0,k}^{ref}& {\theta }_{0,k}^{ref}& {\theta }_{1,k}^{ref}& \cdots & {\theta }_{N,k}^{ref}\end{array}\right]}^T$$

然后将其与预测状态相减：

$$\Delta x_k=x_k^{ref}-x_k^{pred}$$

对应到每一维，就是：

$$\Delta x_k=\left[\begin{array}{c}{x}_{0,k}^{ref}-x_{0,k}^{pred}\\ y_{0,k}^{ref}-y_{0,k}^{pred}\\ {\theta }_{0,k}^{ref}-{\theta }_{0,k}^{pred}\\ {\theta }_{1,k}^{ref}-{\theta }_{1,k}^{pred}\\ ⋮\\ {\theta }_{N,k}^{ref}-{\theta }_{N,k}^{pred}\end{array}\right]$$

这就是当前控制器中“参考轨迹计算误差”的本质过程。

需要注意的是，当前代码中参考输入被直接设定为：

$$u_k^{ref}={\left[\begin{array}{cc}0& 0\end{array}\right]}^T$$

因此输入误差项实际上为：

$$\Delta u_k=u_k^{ref}-u_k=-u_k$$

这意味着这部分代价主要起到“抑制过大控制输入”的作用，而不是严格跟踪某个规划器给出的参考速度或参考转角。

7. MPC 优化问题建模

7.0 从控制建模到优化输出的主线

当前代码中的 MPC 可以概括为下面这条建模链：

1. 定义牵引车-挂车系统状态向量
   $x=[x_0,y_0,{\theta }_0,{\theta }_1,\dots ,{\theta }_N{]}^T$
2. 定义控制输入
   $u=[v,\delta {]}^T$
3. 用离散运动学建立预测模型
   $x_{k+1}=f(x_k,u_k)$
4. 从后端轨迹截取有限长度参考窗口
   $x_k^{ref}$
5. 构造状态误差、输入误差与输入变化率代价
6. 加入动力学、输入和输入增量约束
7. 在线求解有限时域优化问题
8. 取最优控制序列的第一个控制量作为当前输出

因此，当前实现虽然形式上是标准时域 MPC，但它有三个非常明确的工程特征：

• 参考不是解析函数，而是后端轨迹离散采样得到的窗口；
• 预测模型不是线性误差模型，而是显式拖挂离散运动学；
• 控制器每轮求得整段控制序列，但只执行第一步控制量。

7.0.1 连续模型到离散模型

从物理系统角度看，牵引车-挂车首先满足连续时间运动学：

$$\dot{x}=f_c(x,u)$$

其中：

• $x$ 是拖挂系统状态；
• $u=[v,\delta {]}^T$ 是控制输入。

为了做滚动预测，代码没有直接求解连续时间最优控制问题，而是用固定采样周期 $dt$ 构造离散模型：

$$x_{k+1}=f(x_k,u_k)$$

这里的 $f(\cdot )$ 就是 stateTrans() 的实现结果。这样就把连续控制问题转成了一个有限步、有限维的非线性规划问题。

7.1 优化变量

对长度为 $N_{pre}$ 的预测窗口，优化变量为：

$$X=\left[x_0,x_1,\dots ,x_{N_{pre}-1}\right]$$

$$U=\left[u_0,u_1,\dots ,u_{N_{pre}-1}\right]$$

其中：

• $X\in {\mathbb{R}}^{(3+N)\times N_{pre}}$
• $U\in {\mathbb{R}}^{2\times N_{pre}}$

也就是说，求解器并不是只找“一个当前控制量”，而是在每个控制周期同时寻找：

1. 一条未来预测状态轨迹；
2. 一条未来控制序列。

可以把它写得更明确一些：

$$X=\left[\begin{array}{cccc}{x}_0& x_1& \cdots & x_{N_{pre}-1}\end{array}\right]$$

$$U=\left[\begin{array}{cccc}{u}_0& u_1& \cdots & u_{N_{pre}-1}\end{array}\right]$$

其中每一列都对应一个未来离散时刻。

7.1.1 参数与变量的区别

除了优化变量 $X$ 和 $U$，代码里还有一个关键参数：

$$X_0=x_{cur}$$

它表示当前时刻真实测得的车辆状态。这个量不是要求解器优化出来的，而是每次由里程计提供、直接传入优化器的已知参数。

因此当前问题更准确地说是：

$$\underset{X,U}{\min }J(X,U;X_0,x^{ref})$$

即：

• $X,U$ 是待优化变量；
• $X_0$ 和参考轨迹窗口 $x^{ref}$ 是给定参数。

7.2 初始状态约束

当前时刻观测到的真实状态为：

$$x_{cur}$$

MPC 将其作为参数传入优化器，并要求第一步预测状态由当前状态和第一个控制量传播得到：

$$x_0=f(x_{cur},u_0)$$

之后每一步满足：

$$x_k=f(x_{k-1},u_k),k=1,2,\dots ,N_{pre}-1$$

这就是代码中的动力学等式约束。

7.2.1 为什么代码里没有把当前状态写进 X 第一列

当前实现中，第一个预测状态直接写成：

$$x_0=f(x_{cur},u_0)$$

而不是把 X(:,0) 设成当前状态本身。也就是说，X(:,i) 表示的是“施加控制之后的第 $i$ 个预测状态”，不是“控制前的已知状态”。

这种写法有两个直接好处：

1. 当前状态永远由外部测量决定，不会被优化器错误修改；
2. 优化变量规模更紧凑，不必额外在 X 里放入一个固定列。

7.3 控制输入约束

控制输入上下界为：

$$u_{min}\le u_k\le u_{max}$$

具体包括：

• 速度上下限 min_speed、max_speed；
• 转角上下限 -trailer.max_steer、trailer.max_steer。

写成向量形式就是：

$$u_{min}=\left[\begin{array}{c}{v}_{min}\\ {\delta }_{min}\end{array}\right],u_{max}=\left[\begin{array}{c}{v}_{max}\\ {\delta }_{max}\end{array}\right]$$

并对每个预测步施加：

$$u_{min}\le u_k\le u_{max}$$

7.4 控制增量约束

为限制控制变化过快，代码对相邻时刻控制增量加入约束：

$$\Delta u_k=u_k-u_{k-1}$$

并要求：

$$\Delta u_{min}\le \Delta u_k\le \Delta u_{max}$$

其中第一个控制步使用上一轮实际执行的控制量 u_0 作为参考：

$$\Delta u_0=u_0-u_{last}$$

这使控制输出更加平滑，减少执行器抖动。

7.4.1 为什么增量约束很重要

如果只限制控制量本身，而不限制相邻时刻的变化，那么优化器可能会给出：

• 当前时刻大转角；
• 下一时刻又迅速反向大转角；
• 导致方向盘抖动或速度指令突变。

所以当前代码同时约束：

$$|u_k|$$

和

$$|u_k-u_{k-1}|$$

这会让控制结果更像真实车辆上可执行的命令序列。

7.5 目标函数

当前代码中的目标函数由三部分组成：

$$J=\sum _{k=0}^{N_{pre}-1}\left(\Delta x_k^{T}Q\Delta x_k+\Delta u_k^{T}R\Delta u_k+d{u}_k^T{R}_{d}d{u}_k\right)$$

其中：

• $\Delta x_k$：状态跟踪误差；
• $\Delta u_k$：控制输入相对零输入的偏差；
• $d{u}_k$：相邻时刻控制变化量；
• $Q$：状态误差权重矩阵；
• $R$：控制量权重矩阵；
• $R_d$：控制增量权重矩阵。

7.5.1 从单步代价到总目标函数

把每个预测步的代价单独写出来，可以得到单步 stage cost：

$${\ell }_k=\Delta x_k^{T}Q\Delta x_k+\Delta u_k^{T}R\Delta u_k+d{u}_k^T{R}_{d}d{u}_k$$

然后把整个预测窗口上的单步代价求和，就得到总目标函数：

$$J=\sum _{k=0}^{N_{pre}-1}{\ell }_k$$

这就是代码在 for (int i = 0; i < Npre; i++) 循环中不断累加 J 的数学含义。

7.5.2 当前 MPC 的完整优化形式

将代价函数、动力学约束、输入约束和输入增量约束合在一起，当前代码求解的问题可以写成：

$$\underset{X,U}{\min }\sum _{k=0}^{N_{pre}-1}\left(\Delta x_k^{T}Q\Delta x_k+\Delta u_k^{T}R\Delta u_k+d{u}_k^T{R}_{d}d{u}_k\right)$$

subject to

$$x_0=f(x_{cur},u_0)$$

$$x_k=f(x_{k-1},u_k),k=1,\dots ,N_{pre}-1$$

$$u_{min}\le u_k\le u_{max}$$

$$\Delta u_{min}\le d{u}_k\le \Delta u_{max}$$

其中

$$\Delta x_k=x_k^{ref}-x_k$$

$$\Delta u_k=u_k^{ref}-u_k$$

$$d{u}_k=\{\begin{array}{ll}{u}_0-u_{last},& k=0\\ u_k-u_{k-1},& k\ge 1\end{array}$$

这就是 getCmd() 真正在 CasADi 里构造出来的非线性规划问题。

7.6 从优化解到控制输出

求解器每轮得到的是一整段最优控制序列：

$$U^{\star }=\left[\begin{array}{cccc}{u}_0^{\star }& u_1^{\star }& \cdots & u_{N_{pre}-1}^{\star }\end{array}\right]$$

但 MPC 不会一次性执行完整序列，而只执行第一步：

$$u_{cmd}=u_0^{\star }$$

在代码中，这一步对应：

$$u_0^{\star }=U\_sol(:,0)$$

然后映射成实际控制命令：

$$\text{cmd.speed}=u_0^{\star }(0)$$

$$\text{cmd.steering\_angle}=u_0^{\star }(1)$$

这就是“从优化结果到控制输出”的直接推导。

7.6.1 为什么只取第一步控制

原因是 MPC 的核心思想就是：

1. 当前时刻根据当前状态求解未来一段最优序列；
2. 只执行第一步控制；
3. 下一控制周期重新根据新的真实状态再优化一次。

因为一旦车辆实际运动了一小段，真实状态和参考窗口都会变化，所以原来序列里的后续控制量：

$$u_1^{\star },u_2^{\star },\dots$$

不一定仍然最优。于是控制器始终采用“重求解 + 只执行首项”的滚动时域策略。

7.6.2 u_0、X_sol 和 U_sol 为什么要缓存

求解成功后，代码会缓存：

• u_0
• X_sol
• U_sol

它们分别有两个作用：

1. u_0
   用于下一轮优化时构造第一个输入增量约束
2. X_sol 和 U_sol
   用作下一轮 CasADi 求解器的 warm start 初值

因此当前控制器不是每次都从随机初值开始，而是利用上一轮最优解来加速收敛。

8. 权重矩阵的物理意义

8.1 状态权重矩阵 Q

代码中 Q 为对角阵。初始化时读取参数：

• mpc/matrix_q

当前实现中先读取四个基础权重：

$$[q_x,q_y,q_{{\theta }_0},q_{trailer}]$$

然后将最后一个挂车航向权重复制给其余挂车，因此：

$$Q=\text{diag}(q_x,q_y,q_{{\theta }_0},q_{trailer},\dots ,q_{trailer})$$

其物理意义为：

• $q_x,q_y$ 越大，越强调牵引车位置跟踪；
• $q_{{\theta }_0}$ 越大，越强调牵引车车头指向；
• $q_{trailer}$ 越大，越强调挂车整体姿态队形。

8.2 输入权重矩阵 R

R 对应速度和转角本身的惩罚：

$$R=\text{diag}(r_v,r_{\delta })$$

其作用是避免输出过大的速度或转角命令。

8.3 输入变化率权重矩阵 Rd

Rd 对应控制增量惩罚：

$$R_d=\text{diag}(r_{\Delta v},r_{\Delta \delta })$$

其作用是提高控制平滑性，减小突变指令。

9. 在线求解流程

在每个控制周期内，控制器执行流程如下：

1. 检查是否已经收到里程计和参考轨迹；
2. 从 TrajAnalyzer 中截取长度为 Npre 的参考窗口；
3. 若已到目标点或参考为空，则输出零控制；
4. 对参考窗口做角度展开 smooth_yaw()；
5. 在线构建 CasADi Opti 优化问题；
6. 加入状态变量、控制变量、动力学约束、输入约束和增量约束；
7. 构造总代价函数；
8. 使用上一轮解 X_sol、U_sol 作为 warm start；
9. 调用 sqpmethod 求解；
10. 取第一个控制量作为当前输出命令；
11. 发布参考轨迹和预测轨迹可视化结果。

这是一种典型的滚动时域优化控制结构，也就是 receding horizon control。

10. 停车与异常处理逻辑

10.1 到达目标点

当 traj_analyzer.at_goal == true 或参考窗口为空时，控制器：

• 输出 speed = 0
• 输出 steering_angle = 0
• 将内部缓存的 X_sol、U_sol 重置

这可以避免在目标附近继续输出多余动作。

10.2 求解失败

如果 CasADi 求解器抛出异常，当前实现不会让节点崩溃，而是直接忽略本次求解失败，保持已有控制状态。这种做法的优点是系统鲁棒性较好，但也意味着：

• 没有更细的失败分类与恢复机制；
• 没有专门的降级策略；
• 没有显式记录失败次数。

11. 参数说明

控制器从 ROS 参数服务器读取以下关键参数：

• mpc/dt
  预测时间步长。
• mpc/predict_steps
  预测步数。
• mpc/max_speed
  最大速度。
• mpc/min_speed
  最小速度。
• mpc/max_accel
  速度增量约束上限。
• mpc/max_dsteer
  转角增量约束上限。
• mpc/delay_num
  输出缓冲长度，目前代码中仅保留缓冲队列逻辑，未真正参与控制补偿。
• mpc/matrix_q
  状态误差权重。
• mpc/matrix_r
  控制量权重。
• mpc/matrix_rd
  控制增量权重。

11.1 当前配置文件中的默认参数

根据当前工程中的 controller.yaml，MPC 默认参数为：

mpc:
  dt: 0.1
  predict_steps: 10
  max_speed: 2.0
  min_speed: 0.0
  max_accel: 2.0
  max_dsteer: 1.5
  delay_num: 0
  matrix_q: [100.0, 100.0, 1.0, 0.1]
  matrix_r: [0.1, 0.01]
  matrix_rd: [0.01, 10.0]

由此可得当前默认预测域为：

$$T_{pre}=N_{pre}\cdot dt=10\times 0.1=1.0s$$

并且当前默认速度约束为：

$$0\le v\le 2.0m/s$$

这说明当前配置默认面向前进跟踪工况，并未启用倒车速度区间。

11.2 默认权重的控制倾向

当前默认权重对应：

$$Q=\text{diag}(100,100,1,0.1,\dots )$$

$$R=\text{diag}(0.1,0.01)$$

$$R_d=\text{diag}(0.01,10.0)$$

从这个配置可以读出当前控制器的调参倾向：

1. 位置跟踪优先级很高；
2. 牵引车航向误差权重明显低于位置误差；
3. 挂车航向误差权重进一步降低；
4. 转角变化率惩罚很强；
5. 速度变化率惩罚相对较弱。

因此，这组参数通常会使控制器表现为：

• 尽量把牵引车位置贴到参考轨迹上；
• 避免方向盘命令快速抖动；
• 对挂车姿态的调整更偏向平滑，而不是激进拉正。

11.3 调参建议

如果现场调试时出现不同问题，可以优先按下面思路调整：

• 牵引车位置误差较大
  增大 matrix_q 中的前两项。
• 车头方向总是偏得比较明显
  增大 matrix_q 中的 theta0 权重。
• 挂车姿态恢复慢或甩尾明显
  增大挂车航向权重。
• 转向抖动明显
  增大 matrix_rd 中转角变化项权重。
• 速度突变明显
  增大 matrix_rd 中速度变化项权重。
• 控制过于保守、响应慢
  适当减小 R 或 Rd，同时检查 max_accel、max_dsteer 是否过小。

需要特别注意的是，max_accel 和 max_dsteer 在当前实现里本质上是“单步输入增量上限”，它们和 dt 是耦合的。dt 改变后，这两个约束通常也应一起重调。

12. 工程实现补充说明

12.1 控制器工作时序

当前控制器的一个完整工作周期可以概括为：

1. 轨迹回调收到新的 ArcTrailerTraj；
2. 里程计回调更新当前牵引车和挂车状态；
3. 定时器触发 cmdCallback()；
4. 从当前时刻开始采样参考窗口 xref；
5. 对参考窗口做 smooth_yaw()；
6. 在线构建并求解 MPC；
7. 取第一步控制量作为实际输出；
8. 下一控制周期重复上述过程。

这就是标准的滚动时域控制流程。

12.2 伪代码

下面给出和当前代码一致的高层伪代码：

初始化:
  读取车辆参数和 MPC 参数
  构造 Q, R, Rd
  初始化 warm start 缓存 X_sol, U_sol
  初始化上一轮控制 u_0

收到参考轨迹:
  traj_analyzer.setTraj(msg)
  receive_traj = true

收到当前状态:
  x_cur = [x0, y0, theta0, theta1, ..., thetaN]
  has_odom = true

每次控制周期:
  if !has_odom or !receive_traj:
    return

  xref = getRefPoints(Npre, dt)

  if at_goal or xref.empty():
    输出零控制
    重置缓存
    return

  smooth_yaw(xref)

  建立优化变量 X, U
  for k = 0 ... Npre-1:
    构造参考状态 x_ref(k)
    加入动力学约束
    加入输入上下界约束
    加入输入增量约束
    累加状态误差、输入误差、输入变化率代价

  设置当前状态参数
  使用上一轮解 warm start
  求解

  if 求解成功:
    输出 U(:,0)
  else:
    维持当前失败处理策略

12.3 warm start 的作用

当前实现使用上一轮求解得到的 X_sol 和 U_sol 作为下一轮优化的初值。这样做的好处是：

1. 连续控制周期之间解通常变化不大；
2. 有助于减少求解器迭代次数；
3. 有助于提高在线求解稳定性。

12.4 delay_num 的当前状态

虽然代码中保留了 delay_num 和 output_buff，但当前实现还没有把这部分真正用于输入延迟补偿。现阶段它更像是一个预留接口，而不是已经完整生效的功能。

13. 当前实现的特点与局限

13.1 特点

当前控制器有以下特点：

1. 结构直接，容易理解与调试；
2. 预测模型与仿真模型保持一致；
3. 同时考虑了牵引车与挂车姿态误差；
4. 同时约束控制量大小与控制变化率；
5. 支持 warm start，适合在线滚动求解。

13.2 局限

从当前代码实现看，仍存在以下限制：

1. 误差是在世界坐标系下直接计算，未转换为路径坐标系下的横纵向误差；
2. 没有显式参考速度、参考转角，输入参考默认为零；
3. 没有显式终端代价和终端约束；
4. delay_num 缓冲逻辑尚未真正作用到控制补偿；
5. 没有加入更强的防折叠、铰接角边界或 jack-knife 保护约束；
6. 每次控制周期都重新搭建优化问题，存在一定额外开销。

14. 文档与源码对应关系

若需要继续结合源码阅读，建议按下面顺序查看：

1. mpc.cpp
   先看 init()、cmdCallback()、getCmd()，理解主流程。
2. mpc.h
   再看 stateTrans() 与 smooth_yaw()，理解模型与角度处理。
3. traj_anal.hpp
   理解参考轨迹如何按当前时刻采样成 xref。
4. trailer.hpp
   理解车辆几何参数和部分辅助状态变换。

15. 总结

当前工程中的牵引-挂车轨迹跟踪控制器，本质上是一个基于离散运动学模型的时域 MPC。它通过对后端规划轨迹进行局部采样，构造状态误差、输入惩罚和输入增量惩罚，在线求解牵引车速度与转角命令，从而实现对整条牵引-挂车系统轨迹的闭环跟踪。

从代码实现角度看，这个控制器最关键的三点是：

1. 状态跟踪误差定义为 参考状态 - 预测状态；
2. 误差状态量包括牵引车位置、牵引车航向和各节挂车航向；
3. 挂车之间的耦合关系通过离散运动学预测模型隐式进入优化问题。

因此，如果后续需要继续增强控制性能，通常会围绕以下方向展开：

• 改进误差定义方式；
• 引入更完整的参考输入；
• 增强终端约束与防折叠约束；
• 显式考虑执行器延迟与状态估计误差。