MINCO 学习笔记：Minimum Control Effort 轨迹生成

Minimum Control effort trajectory — ZJU FAST Lab，TRO 2022

核心思想#

给定一组中间路点和时间分配，用解析映射直接算出满足连续性的最优多项式轨迹。

两层结构#

1
外层：L-BFGS 迭代
2
      优化变量：中间路点 q_i + 虚拟时间 τ_i
3
      每步调用一次内层
4

5
内层：解线性方程组（不迭代，一步出结果）
6
      输入：当前 (q_i, T_i)
7
      输出：多项式系数 c

关键点： 内层不是优化，是直接解方程。给定 (q, T) 就出 c，一一对应。

轨迹表示#

M 段分段多项式，每段 degree-7（MinSnap）或 degree-3（MinJerk）：

1
p(t) = c0 + c1*t + c2*t^2 + ... + c7*t^7
2

3
优化变量只有两组：
4
  q_1, ..., q_{M-1}   中间路点位置
5
  T_1, ..., T_M       每段时长
6
（系数 c 由这两组变量解析算出，不是优化变量）

内层：拉格朗日解系数#

目标函数#

最小化 snap（4 阶导数）积分：

1
minimize  c^T * Q * c
2

3
Q 是由多项式基函数的高阶导数内积构成的常数矩阵
4
只和段时长 T_i 有关，和路点位置无关

为什么只有高阶系数？ 低阶系数 c0、c1、c2 已被端点边界条件钉死，不是自由度。

等式约束#

段间连续性 + 路点位置约束，全是线性的：

1
A * c = b
2

3
A：由多项式端点处的 [1, t, t^2, ...] 取值组成，只依赖 T_i
4
b：由路点 q_i 和边界条件组成

引入拉格朗日乘子#

1
L(c, λ) = c^T*Q*c + λ^T*(A*c - b)
2

3
令偏导为零（KKT 条件）：
4
  ∂L/∂c = 0  →  2*Q*c + A^T*λ = 0
5
  ∂L/∂λ = 0  →  A*c = b
6

7
整理成线性方程组：
8
  [2Q   A^T] [c]   [0]
9
  [A    0  ] [λ] = [b]
10

11
直接解出 c 和 λ，λ 用完即弃

连续性为什么永远满足#

每次 q_i 更新后，重新代入方程组解出新的 c，新 c 天然满足所有连续性约束。连续性是解析映射的内嵌不变量，不管路点怎么变都不会断。

外层：L-BFGS 优化总 Loss#

总 Loss 组成#

1
Loss = snap 积分（平滑代价）
2
     + 障碍物惩罚（ESDF 距离）
3
     + 可行性惩罚（速度/加速度超限）
4
     + 时间正则（防止时间无限拉长）

梯度链式传播#

1
Loss
2
 ↓ 对采样点 pos/vel/acc 求梯度
3
采样点梯度
4
 ↓ 多项式基函数（beta0, beta1, beta2）
5
系数梯度 gdC
6
 ↓ MINCO 解析 Jacobian（calGrads_PT）
7
路点梯度 gdP  +  时间梯度 gdT
8
 ↓ 虚拟时间映射导数
9
虚拟时间梯度 grad_τ
10
 ↓ L-BFGS 更新
11
新的 q_i 和 τ_i

虚拟时间参数化#

问题： T_i > 0 是有约束的，L-BFGS 处理不了。

解法： 引入无约束变量 τ_i，通过单调映射保证 T_i 恒正：

1
T_i = exp(τ_i)          ← T_i 恒正，τ_i 无约束
2

3
梯度链式换算：
4
  ∂Loss/∂τ_i = ∂Loss/∂T_i × exp(τ_i)

作用： 时间分配和路点在同一个 Loss 里联合优化（空间-时间联合优化）。障碍物附近自动拉长时间减速，开阔处自动压缩时间加速。