轨迹规划中的约束优化:从罚函数法到ADMM的解法谱系与工程选型
1. 约束优化问题的完整解法谱系:从理论到可落地的工程选择
约束优化在轨迹规划里有多重要,干过机器人运动规划、自动驾驶路径生成或者无人机航迹设计的朋友应该都有体会。机械臂要从A点移动到B点,关节角度不能超限,速度不能太快,力矩不能超出电机能力,末端还要避开障碍物——这些全部都是约束。一个轨迹规划问题,本质上就是一个带约束的优化问题。
很多初学者最大的困惑不是"优化是什么",而是"约束优化到底有多少种解法,每种解法适用于什么场景"。
这个问题问得非常好,因为它直接决定了你在实际工程项目里选哪条路线。我见过不少团队在同一个问题上反复折腾:一开始用惩罚函数法,调惩罚系数调到怀疑人生;后来换成内点法,结果初始点选不好直接发散;再后来试了SQP,发现非凸问题容易卡在局部解;最后才意识到这个问题的结构其实更适合用凸优化建模。每一次切换解法,都意味着大量代码重写和调试成本。
这篇文章把约束优化的主流解法做一个系统梳理,从最基础的罚函数法讲到现代求解器背后的内点法、SQP、增广拉格朗日法,再到针对性问题设计的ADMM和对偶分解,每个方法我会讲清楚数学原理、适用场景、优缺点,更重要的是讲清楚它在轨迹规划里的具体应用方式。
先说一个核心判断,后面会反复用到:
轨迹规划里的约束优化问题,绝大多数是中小规模(几十到几千个变量)、稠密或稀疏的连续优化问题,而且往往需要实时性。这决定了我们不能盲目套用通用求解器,而是要根据问题结构选择"足够好"的解法,而不是"理论上最优"的解法。
1.1 约束优化问题的数学本质与分类
先把问题形式化。约束优化问题的标准形式是:
min f(x) s.t. g_i(x) ≤ 0, i = 1, ..., m h_j(x) = 0, j = 1, ..., p
其中 x ∈ R^n 是决策变量,f(x) 是目标函数,g_i(x) 是不等式约束,h_j(x) 是等式约束。
在轨迹规划中,这个框架非常具体。假设我们要规划一条机械臂关节空间轨迹,让末端执行器在 T 个时间步内从起点运动到终点,那么:
- 决策变量 x 可以是所有时间步上的关节角度(n = 关节数 × T),或者更精炼地,是用多项式/B样条参数化的轨迹系数;
- 目标函数 f(x) 通常是能量消耗、时间、平滑度(如加速度的平方积分)、或者末端追踪误差;
- 不等式约束 g_i(x) 包括关节角度极限、速度极限、加速度极限、避障距离约束;
- 等式约束 h_j(x) 包括起始位置/速度条件、终点位置/速度条件、以及动力学方程约束。
从数学性质上,约束优化问题可以分为几类,每类的求解难度天差地别:
| 问题类型 | 数学性质 | 求解难度 | 典型轨迹规划场景 |
|---|---|---|---|
| 线性规划LP | f和约束全为线性 | 极容易 | 凸包内时间最优路径 |
| 二次规划QP | 目标二次、约束线性 | 容易 | 跟踪控制、最小加速度轨迹 |
| 二次约束二次规划QCQP | 目标二次、约束二次 | 中等 | 带速度锥约束的轨迹 |
| 凸非线性规划NLP | 目标凸、约束凸非线性 | 中等 | 避障距离约束、关节极限 |
| 非凸NLP | 存在非凸约束 | 困难 | 避障最优、奇异位形规避 |
| 混合整数规划MINLP | 含离散变量 | 极困难 | 任务分配+轨迹联合优化 |
这里必须多说一句:判断一个轨迹规划问题能不能"简单求解",第一件事就是看它的凸性。凸问题的局部最优解就是全局最优解,这是优化理论给我们的最大礼物。但很遗憾,现实世界的避障约束(如"末端点与障碍物距离≥安全距离")通常是非凸的,因为它排除了障碍物内部的连续区域,这个约束本身就是一个"洞"。
1.2 轨迹规划中约束的物理来源与数学表达
在具体讨论解法之前,先把轨迹规划问题里最常见的几类约束做一个梳理,因为后面每次提到"这个方法适合哪些约束",都需要回到这里的物理来源。
1. 关节与执行器极限约束
每个关节都有物理极限。关节角度极限 θ_min ≤ θ(t) ≤ θ_max 是最基础的运动学约束;关节速度极限 |θ'(t)| ≤ v_max 来自电机最大转速;关节加速度/力矩极限 |θ''(t)| ≤ a_max 或 |τ(t)| ≤ τ_max 来自电机最大转矩和传动机构强度。
这类约束通常可以直接写成线性不等式。如果轨迹用B样条表示,由于B样条的凸包性质,只要控制点在极限范围内部,整条曲线就保证在极限范围内。这个性质非常重要,它让我们可以把无限维的轨迹曲线约束转化为有限维的控制点约束。
2. 动力学约束
对于机械臂,动力学方程是 M(θ)θ'' + C(θ,θ')θ' + G(θ) = τ。如果我们要保证轨迹是动力学可行的,就需要在每个时间点上满足这个等式约束,同时力矩还要在电机极限范围内。
动力学约束是等式约束还是不等式约束取决于你怎么用。如果直接把力矩作为决策变量,动力学方程就是等式约束;如果只规划运动轨迹,然后反向校验力矩是否超限,那动力学约束就是一个"事后校验"环节,严格来说不算优化约束。
3. 避障约束
避障约束是整个轨迹规划里最"磨人"的约束。对于机械臂来说,常见的建模方式有三种:
- 球体包络:把机械臂连杆和障碍物都建模为球体,要求两球中心距离≥半径之和。数学上写成 ||p_i(x) - o_j|| ≥ r_i + r_j,这是一个非凸的二次约束。
- 凸多面体分离:要求机械臂的AABB(轴对齐包围盒)或OBB(有向包围盒)与障碍物的凸包之间存在分离平面,即 A' 存在法向量 n 使得 n·p_i > n·o_j。
- 构型空间障碍:对于二维平面规划,可以在构型空间里直接画出C-obstacle,然后要求在构型空间里避开这些区域。但三维以上的C-obstacle计算量极大,实际工程中很少直接使用。
避障约束非凸的根源在于 ||p_i(x) - o_j|| ≥ r 定义的是球的外部区域,这是一个补集,补集不可能是凸集。所以只要轨迹规划里带了避障约束,问题本质上就是非凸的,这一点会在很大程度上影响解法选择。
4. 任务约束
任务约束包括起点/终点位置约束、速度连续性约束、姿态约束等。这类约束通常是线性的,很容易处理,但如果涉及姿态(如要求末端在终点保持水平),由于旋转矩阵本身的非线性,往往会引入非凸约束。
2. 经典解法逐层拆解:罚函数法、障碍法与增广拉格朗日法
这一节讲的是约束优化领域最经典的"入门三件套"。如果你是从零开始学习约束优化,大概率第一个接触的方法就是罚函数法。这三个方法的共同思路是:把约束优化问题转化为一系列无约束优化问题来求解。区别在于转化方式不同,适用场景也不同。
2.1 罚函数法:简单粗暴但代价沉重的经典方法
罚函数法的核心思想非常直白:既然带约束难解,那我不如设计一个新的目标函数,让违反约束的行为受到"惩罚",然后去最小化这个新目标。
具体来说,对外罚函数法,定义增广目标函数:
F(x, μ) = f(x) + μ * Σ ψ(g_i(x)) + μ * Σ φ(h_j(x))
其中 ψ 和 φ 是惩罚函数,通常取 max(0, g_i(x))² 和 h_j(x)²,μ 是惩罚系数。当 μ → ∞ 时,任何违反约束的方案都会付出无穷大的代价,所以最优解必然满足约束。
内罚函数法(又叫障碍函数法)则不同,它从可行域内部出发,在边界处竖起一堵"无限高的墙":
F(x, μ) = f(x) - μ * Σ log(-g_i(x))
当 x 逼近可行域边界 (g_i(x) → 0⁻) 时,-log(-g_i(x)) → +∞,所以迭代点永远不会越界。障碍函数法只能处理不等式约束,等式约束需要额外处理。
罚函数法在轨迹规划里的问题非常明显:
第一,惩罚系数 μ 的选择极度困难。μ 太小时,结果可能严重违反约束(比如轨迹撞进障碍物);μ 太大时,F 的Hessian矩阵条件数急剧恶化,无约束优化变得极其缓慢。这个"病态条件数"是罚函数法的致命伤。
第二,只能得到近似解。不管 μ 取多大,最终求出来的解严格来说都会在约束边界外侧一点点,这个"一点点"在碰撞检测里可能就是几毫米的差距,足以导致实际执行时发生碰撞。
第三,求解效率低。每个 μ 对应一个无约束优化问题,而算出一个高精度的无约束解成本已经很高,外层还要迭代多次 μ → ∞,整体效率无法满足在线轨迹规划的需求。
我在实际项目中基本不会用纯罚函数法做轨迹规划,除非是做一些离线的一次性规划,或者给后面的求解器提供初始点。它的价值更多在于教学和理论推导,作为理解"用无约束近似约束"这个思想的起点。
2.2 增广拉格朗日法:工程上最耐用的"老黄牛"
增广拉格朗日法(Augmented Lagrangian Method, ALM)是我个人在轨迹规划项目里用得最多的方法之一,因为它很好地弥补了罚函数法的两个致命缺陷。
增广拉格朗日法的构造思路是:把拉格朗日函数的对偶项和罚函数二次项结合在一起。对等式约束 h(x) = 0,增广拉格朗日函数为:
L_A(x, λ, μ) = f(x) + λ_h * h(x) + (μ/2) * h(x)²
这里 λ_h 是拉格朗日乘子的估计值。关键迭代步骤是:
- 固定 λ,求解 min_x L_A(x, λ, μ)
- 更新乘子:λ ← λ + μ * h(x)
- 逐渐增大 μ
为什么这个方法比纯罚函数法好?关键在于那个 λ 更新步骤。罚函数法相当于永远用固定的 λ = 0 去求解,所以需要 μ → ∞ 才能满足约束;而增广拉格朗日法通过不断校正 λ,即使 μ 保持在一个中等水平,也能让约束误差快速趋于零。这个"乘子校正"机制极大缓解了病态条件数问题。
对于不等式约束 g(x) ≤ 0,需要使用一个变量代换技巧。把不等式约束转化为等式约束 g(x) + s² = 0(s 是松弛变量),或者直接对 g 做一个"裁剪"操作。实际实现中,最常用的形式是:
L_A(x, λ, μ) = f(x) + Σ [λ_i * g̃_i(x) + (μ/2) * g̃_i(x)²]
其中 g̃_i(x) = max(g_i(x), -λ_i/μ),这个 max 操作保证了对已满足的约束不施加惩罚,只惩罚违反约束的部分。
增广拉格朗日法在轨迹规划里的应用非常广泛。我之前做过一个焊接机械臂的轨迹优化项目,目标是最小化末端振动(即最小化加速度变化率),约束包括关节极限、速度和避障。当时用第三方非线性求解器总是卡在奇异位形附近,后来换成了ALM自己写求解器,配合宽限的线搜索策略,问题迎刃而解。
实测经验:ALM 对初始点要求不高,甚至可以从一个违反约束的不可行点出发,这在工程里非常实用。比如机械臂从当前关节角出发规划,当前位形可能已经靠近关节极限,甚至速度超标。ALM 的鲁棒性让它可以处理这些"脏"起点。
2.3 直接解法与间接解法的工程对照
把这一类方法放在一起对比,方便大家做选择:
| 方法 | 迭代逻辑 | 对初始点要求 | 约束处理精度 | 典型使用场景 |
|---|---|---|---|---|
| 外罚函数法 | 逐步增大μ | 低 | 近似(永远在边界外侧) | 教学演示、可行性初判 |
| 内罚函数法 | 从可行域内逼近边界 | 必须在可行域内部 | 精度可高 | 凸QP/凸NLP的求解内核 |
| 增广拉格朗日法 | 乘子校正+适度增μ | 低,可不可行点 | 高 | 中规模非凸轨迹优化 |
内罚函数法还应该多说一句:现代内点法(Interior-Point Method)虽然名字听起来像障碍函数法,但它的核心是路径跟踪技术——通过对数障碍函数构造中心路径,用牛顿法跟踪这条路径,同时迭代步长和障碍参数按规则更新。它不是简单地"设μ→∞",而是同时调整μ和迭代点的位置,保证每一步都有足够的下降和足够的可行域进度。这个现代变体我们在第3节展开讲。
3. 现代主流解法:内点法与序列二次规划(SQP)的硬核剖析
如果说第2节的方法是"古典"思路,那么内点法和SQP就是现代实际求解器调度应用的两大支柱。几乎所有主流商用求解器(如Gurobi、MOSEK、KNITRO、SNOPT、IPOPT、MATLAB的fmincon)的内核,都跑不出这两种框架。理解它们的本质区别,就知道该在什么问题上选什么求解器。
3.1 内点法的核心机制:中心路径与障碍参数的作用
现代内点法的理论基础是1950年代Fiacco和McCormick提出的障碍函数法思想,但真正让它变得强大的是1980年代Karmarkar在线性规划上的突破以及随后非线性的拓展。它的核心机制可以用一句话概括:把不等式约束转化为障碍函数项,然后沿着"中心路径"追踪到最优解。
考虑问题 min f(x), s.t. g(x) ≤ 0。引入非负松弛变量 s,把不等式变成等式约束 g(x) + s = 0,然后构造对数障碍目标:
min f(x) - μ * Σ log(s_i) s.t. g(x) + s = 0
这里 μ 叫障碍参数。当 μ 从大往小变化时,最优解的轨迹(在 x-s 空间中)叫做中心路径(central path)。理论可以证明,当 μ → 0 时,中心路径收敛到原问题的最优解。
内点法的迭代过程不是简单地让 μ 递减,而是要同时满足三层条件:
- 原始可行性:g(x) + s = 0
- 对偶可行性:∇f(x) + Σ λ_i ∇g_i(x) = 0
- 互补松弛:λ_i * s_i = μ(不再是0,而是等于障碍参数)
这三个条件的联立解就是中心路径上的点。算法每次迭代用牛顿法求解这个非线性方程组,然后判断是否满足原问题、对偶问题以及互补条件的残差,所有残差都在容差以内时即收敛。
为什么内点法在工程中表现如此稳定?因为它本质是一个"全程可行的牛顿法"——在每个迭代点,松弛变量 s 都被保证为正,这维护了目标函数的强凸性(对数项贡献了正定Hessian),从而让牛顿步的计算更加可靠。
在轨迹规划里,内点法的典型用途是求解凸QP和凸NLP。比如我们把避障约束做了凸化近似(这个技巧后面会详细讲),得到一个凸NLP,那么交给IPOPT这类内点法求解器,一般能在几十次迭代内收敛,速度和可靠性都令人满意。
注意:内点法对病态问题的处理能力有限。如果约束的雅各比矩阵接近奇异(比如轨迹中有两段完全重合的运动),牛顿步会变得不可靠,收敛速度会急剧下降。在轨迹规划里,要尽量避免约束冗余或几乎线性相关的情况。
3.2 SQP的原理:为什么它是非凸轨迹优化的"默认选择"
序列二次规划(Sequential Quadratic Programming, SQP)的思路和罚函数法、内点法都不同。它不试图把约束问题变成无约束问题,而是直接在每步迭代中构造一个二次规划子问题来逼近原问题。
SQP的迭代公式基于KKT条件的牛顿法推导。在第k步迭代点 (x_k, λ_k) 处,把拉格朗日函数 L(x, λ) = f(x) + Σ λ_i g_i(x) 做二阶Taylor展开,约束做一阶Taylor展开,得到QP子问题:
min (1/2) d' B_k d + ∇f(x_k)' d s.t. g_i(x_k) + ∇g_i(x_k)' d ≤ 0, i = 1, ..., m
其中 B_k 是拉格朗日函数的Hessian(或它的近似),d 是搜索方向。求解这个QP子问题得到 d 和新的乘子估计 λ_{k+1},然后通过线搜索或信赖域策略确定步长 α,更新 x_{k+1} = x_k + α d。
SQP的核心优势在于:
-
它是一个"逼近最优解结构"的方法。因为它直接逼近KKT条件,所以收敛后约束满足精度非常高,通常可以达到10⁻⁸甚至更高。这在轨迹规划里意味着轨迹可以精确地贴在约束边界上运行——比如刚好不碰障碍物,同时把运动时间压到最短。
-
非凸问题的处理能力强。只要QP子问题本身求解成功,SQP并不强制要求原问题是凸的。虽然收敛结果可能是局部最优(这是非凸问题的宿命),但它在实践中的表现通常远好于把非凸问题直接丢给内点法。
-
QP子问题的求解已经非常成熟。对于中小规模的QP,active-set方法和内点法都能高效求解,有些情况甚至可以在毫秒级完成。
SQP的软肋也很明显:它需要B_k(近似的二阶信息)是正定的才能保证QP子问题可解。当约束体系的线性化导致子问题不可行(比如约束之间存在矛盾)时,需要进行可行性恢复(feasibility restoration)操作,这一步做不好就会导致迭代发散。
在轨迹规划领域,SQP几乎是处理非凸轨迹优化的默认选择,这个判断基于三个理由:
- 轨迹优化的目标函数(如最小化加速度积分)通常是二次或接近二次的,SQP的二次逼近非常精准;
- 约束(关节极限、避障等)虽然在全局非凸,但在小邻域内的线性化质量足够好;
- 可以方便地传入初始轨迹(比如运动学规划的轨迹),SQP会在这个基础上做局部改进。
我之前做一个移动机械臂的轨迹优化,末端要完成一个抓取动作,同时底盘要绕开货架。整个问题是非凸的,因为避障约束和机械臂的奇异规避约束同时存在。当时用IPOPT在内点法框架下收敛了80多次迭代还时有失败,换了SQP求解器(用了SNOPT)之后,40次迭代左右稳定收敛,而且约束满足精度高了一个数量级。
3.3 内点法与SQP的对比表与选型逻辑
| 对比维度 | 内点法 | SQP |
|---|---|---|
| 算法框架 | 障碍参数+中心路径跟踪 | KKT条件的牛顿逼近 |
| 典型求解器 | IPOPT, KNITRO(IP), MOSEK(conic) | SNOPT, KNITRO(SQP), fmincon(SQP) |
| QP相关 | 内部有QP求解,但主要在凸问题上表现优 | 核心就是序列QP子问题 |
| 凸问题性能 | 极佳,几十次迭代以内 | 良,但可能因active-set切换稍慢 |
| 非凸问题 | 容易卡在局部最优或发散 | 更好,尤其有初始点 |
| 约束精度 | 高(可达1e-6~1e-8) | 极高(1e-8+) |
| 不精确梯度的敏感度 | 中等 | 较敏感(依赖QP方向质量) |
| 大规模稀疏问题 | 极佳(内点法天然适合稀疏线性代数) | 较佳但需处理大规模QP |
| 热启动(从旧解启动) | 不方便 | 较方便(可以保留active set) |
选型逻辑根据这个对比就很清晰了:
- 凸问题(目标凸、约束凸):优先内点法,IPOPT或者MOSEK都行。凸问题不需要SQP的非凸处理能力,内点法的收敛速度更快、更稳定。
- 非凸问题但有良好初始解:优先SQP。SNOPT这类SQP求解器对初始点比较敏感,但只要初始轨迹合理(比如来自RRT规划或梯形速度规划),效果远超内点法。
- 大规模稀疏问题(比如几万到几十万个变量的时间网格离散轨迹):内点法几乎不可替代,因为稀疏线性代数的优势决定了内存和计算量可控。
- 需要在线实时求解:这个要具体看问题规模。小型问题(几十个变量)用内点法或QP快解都可以;中大型问题想实时化,通常需要专门设计稀疏结构,甚至要用SQP+热启动+提前计算Hessian的预处理技巧。
4. 特殊结构与先进方法:KKT系统、对偶分解与ADMM
前面几节覆盖了"通用"解法。但轨迹规划里很多问题有特殊结构,如果无视结构直接套通用求解器,性能往往差一个数量级。这节讲如何利用问题结构"走捷径"。
4.1 KKT条件与最优性检验:判断你的解是不是真的最优
任何约束优化问题的解都必须满足KKT条件(在一阶可微的前提下)。KKT条件是判断解是否局部最优的必要条件(在约束品性成立时也是充分条件,对凸问题而言必要且充分)。它由三部分组成:
1. 平稳性条件(Stationarity): ∇f(x*) + Σ λ_i* ∇g_i(x*) + Σ ν_j* ∇h_j(x*) = 0
2. 原始可行性(Primal Feasibility): g_i(x*) ≤ 0, h_j(x*) = 0
3. 对偶可行性与互补松弛(Dual Feasibility & Complementary Slackness): λ_i* ≥ 0, λ_i* * g_i(x*) = 0
第三条的直观含义非常深刻:如果一个不等式约束在最优解处是"松"的(g_i(x*) < 0),那么它的拉格朗日乘子必须为零;如果一个约束是"紧"的(g_i(x*) = 0),它的乘子才可能非零。紧约束就是真正"卡住"最优解的约束——它们在KKT体系中起到了决定性作用。
在轨迹规划里,KKT条件的工程价值有两个:
第一,判断收敛质量。很多求解器返回的结果并不是严格满足KKT条件,而是满足"近似KKT"(比如对偶残差小于1e-6,互补残差小于1e-8)。这个近似度的含义应该心里有数。当我做高精度轨迹优化(比如半导体设备晶圆搬运的高加速度轨迹)时,我会自己写一个校验程序,检查返回解的KKT残差,确认约束是否真的被满足到期望精度。
第二,理解"卡住"解的约束是什么。通过查看最优解处的拉格朗日乘子,可以知道哪些约束是瓶颈。比如轨迹优化完,如果发现中间某个时间点的加速度约束乘子特别大,说明是加速度极限卡住了最优轨迹。这个信息对机械设计很有价值——告诉你做哪个硬件参数(比如换更大扭矩的电机)最能提升轨迹性能。
4.2 对偶分解:把复杂轨迹问题拆成可并行的小问题
对偶分解(Dual Decomposition)的核心思想是:如果一个约束优化问题可以被分解成几个子问题,并且这些子问题之间只通过"耦合约束"连接,那么可以在对偶空间里处理耦合约束,让各个子问题独立求解。
具体来说,考虑问题:
min f_1(x_1) + f_2(x_2) + ... + f_N(x_N) s.t. x_i ∈ X_i, Σ A_i x_i = b
这里的等式约束把 N 个子问题耦合在一起。构造拉格朗日函数:
L(x_1,...,x_N, λ) = Σ f_i(x_i) + λ' (Σ A_i x_i - b)
由于拉格朗日函数对 x 是可分离的,对固定 λ,min_x L 等价于分别求解 N 个独立子问题:
min_{x_i ∈ X_i} f_i(x_i) + λ' A_i x_i
然后在对偶空间里更新 λ(通常用次梯度法或者更现代的加速梯度法)。
对偶分解在轨迹规划里有一个特别经典的应用场景:多机器人协同轨迹规划。假设两台机械臂在一个共享工作空间里作业,各自的轨迹是自己的决策变量,而防止两台机械臂互相碰撞的约束就是耦合约束。对偶分解后,每台机械臂只需知道自己和一些 "虚拟力(λ)" 的信息,就可以独立规划轨迹,λ 的更新过程就是不断协调碰撞规避的过程。
这个方法的优势在于:
- 并行化:每个子问题可以放不同处理器或者不同线程并行计算;
- 隐私或模块化:各子系统的模型不需要共享,只要通过 λ 交换信息;
- 适合分布式架构:在车路协同或多机器人调度场景中非常自然。
劣势也相当明显:对偶分解的收敛速度取决于目标函数的强凸性质。如果 f_i 不够强凸,次梯度更新的收敛速度是 O(1/√k),慢得让人失去耐心。轨迹优化里的目标函数虽然一般是凸的,但往往不是强凸的(比如只最小化线性形式的位置误差),这时对偶分解可能会需要几百上千次迭代,实际应用受限。
工程上如果要使用对偶分解,我建议先对所有子问题做"正则化"处理——在目标函数里加一项 ε||x||²,虽然这会略微改变原问题的解,但能显著提升对偶迭代的收敛速度。代价是解不再是原问题的精确解,需要在精度和收敛速度之间做权衡。
4.3 ADMM:交替方向乘子法在轨迹规划中的应用网
ADMM(Alternating Direction Method of Multipliers)是对偶分解的"豪华升级版"。它融合了增广拉格朗日法的稳健性(惩罚项,不怕非强凸)和对偶分解的可分解性(交替更新,天然适合分块问题)。
考虑标准ADMM形式:
min f(x) + g(z) s.t. Ax + Bz = c
ADMM的迭代步骤是:
- x^{k+1} = argmin_x L_A(x, z^k, λ^k)
- z^{k+1} = argmin_z L_A(x^{k+1}, z, λ^k)
- λ^{k+1} = λ^k + ρ(Ax^{k+1} + Bz^{k+1} - c)
其中 L_A 是增广拉格朗日函数,ρ 是惩罚参数。这里x步和z步交替进行,每步都可能有一个解析解或一个快速的迭代求解。
ADMM在轨迹规划里最有价值的应用,是处理分布式MPC(模型预测控制)和具有结构分块的轨迹优化。
一个非常实际的应用场景是机械臂轨迹的光滑化与避障。把轨迹优化拆成两个子问题:一个是"平滑优化"(只考虑轨迹的平滑性,目标函数是加速度的积分),另一个是"避障调整"(只考虑轨迹点的避障距离要求)。两者通过等式的耦合约束连接:平滑轨迹与避障轨迹在离散时刻必须重合。ADMM在这两个子问题之间交替迭代,每次都只解决一个相对简单的子问题(一个带等式约束的QP,一个投影问题),最终得到一条既平滑又避障的轨迹。
另一个场景是硬件在环的实时规划。对于模型预测控制,每一步需要求解一个带约束的优化问题。如果把问题按时间维度分解(每个时间步的子问题),ADMM的并行特性可以让子问题在多个核上同时计算,从而实现毫秒级的在线求解。这是许多高性能自动驾驶、无人机集群规划采用ADMM作为内核的原因。
但ADMM也有明显的坑:
- ρ 参数敏感。ρ 选得不好,收敛可能很慢甚至震荡。现代做法是使用"自适应ADMM",在迭代中动态调节ρ,但这也意味着代码复杂度的提升。
- 需要在x步和z步的交替中保持耐心。ADMM的收敛通常在开始几次迭代很快,但后期线性收敛到高精度解的速度较慢。如果你的应用需要极高精度(比如严格满足约束),ADMM不如内点法。
我的经验是:ADMM非常适合"在线近似最优"的应用场景,它对精度要求不高、但实时性要求强、问题结构可以分块。而离线高精度规划,还是交给内点法或SQP。
4.4 特殊结构问题:如何识别你要用的"快捷键"
其实每次拿到一个轨迹规划问题,我都建议先做一个"结构审计",花30分钟分析一下问题的数学结构,比盲目选求解器和调参省下一周时间。审计清单如下:
- 变量是否可分离? 如果可以写成 Σ f_i(x_i) 的形式,且约束也可以按块分解(或只少量耦合),考虑ADMM。
- 约束中是否存在大量冗余? 如果有很多约束基本等价(比如某几个时间点约束只有细微差别),可以用active-set方法或预处理简化。
- 问题是凸还是非凸? 非凸是全局的(避障)还是局部的(某个特殊位形)?前者需要专门处理,后者靠SQP的良好初始点。
- 问题规模是多大? 小于1000变量可以直接用稠密内点法;上万变量必须考虑稀疏结构或分解方法。
- 是否需要在线求解? 如果需要毫秒级响应,考虑QP热启动、SQP+预计算Hessian、ADMM并行化。
5. 轨迹规划中的实战应用:从问题建模到求解器选型
理论部分讲完了,这节进入"落地"阶段。我会用几个具体的轨迹规划问题,演示约束优化怎么一步步建模、选解法、求解和验证。这一节的思路是:问题建模决定了问题类型,问题类型决定了解法选择。
5.1 案例一:机械臂关节空间最小加速度轨迹规划
问题描述:6自由度机械臂要从关节角 θ_s 运动到 θ_g,要求轨迹平滑(最小化加速度平方积分),且满足关节角度、速度、加速度极限约束。
第一种建模方式:B样条参数化
用B样条表示轨迹,决策变量是B样条的控制点。假设用k阶B样条,n个控制点。
目标函数:min Σ||θ(t)''||²,由于B样条的导数是B样条的导数,这个目标可以写成控制点的二次型,即目标函数是二次的。
约束:
- 关节角度极限:由于B样条的凸包性质,只要控制点在极限内,整条曲线就在极限内;
- 速度极限和加速度极限:同样用导数控制点,仍然是线性约束。
这样问题就变成了二次规划(QP):
min (1/2) p' H p + c' p s.t. A p ≤ b
其中 p 是控制点向量。QP的求解非常成熟,直接用内点法求解器(比如OSQP、qpOASES)求解即可,毫秒级完成。
第二种建模方式:时间离散化
把时间 [0,T] 离散成N个时间步,决策变量是所有时间步的关节角度 θ_1, ..., θ_N。目标函数是加速度的离散近似(用差分近似):min Σ (θ_{k+1} - 2θ_k + θ_{k-1})²。
约束是每个时间步的关节角度极限、速度极限(用差分近似)和加速度极限。这时问题仍然是一个QP,但变量数量是 6×N。如果N=100,就是600个变量,仍然是中等规模QP,可以直接求解。
解法推荐:QP用内点法或active-set法都是合适的。工程上我常用OSQP(用ADMM实现的内点法风格的QP求解器)或qpOASES(active-set),两者在在线规划中都有不错表现。关键看你的问题是需要"热启动"能力(qpOASES的active-set更擅长)还是"高精度"(OSQP更合适)。
5.2 案例二:避障约束下的非凸轨迹优化
问题描述:在案例一的基础上,增加避障约束。设机械臂末端在第k个时间步的位置为 p_k(θ_k),障碍物中心为 o,半径为 r,要求 ||p_k(θ_k) - o|| ≥ r + d_safe。这个约束是非凸的。
这个约束一旦加入,整个问题就不能用QP描述了,变成了非凸NLP。此时有几种处理策略:
策略A:直接用SQP求解器处理非凸问题
把非凸NLP丢给SNOPT或KNITRO的SQP求解器。前提是要有一个足够好的初始轨迹。这个初始轨迹可以来自:
- 先用无避障的QP规划一条平滑轨迹,虽然会穿过障碍物,但作为迭代起点;
- 用RRT之类的采样规划方法生成一条几何路径,再做时间参数化。
SQP会在初始轨迹的基础上,通过迭代把轨迹"推"出障碍物区域,同时尽量保持平滑性。这个方法在实践中效果很好,也是工程上最常用的方式。
但要注意:SQP是基于局部线性化的,如果初始轨迹穿入障碍物太深(比如末端在障碍物内部),SQP可能陷入一个"陷阱"——线性化后看不到可行的逃逸方向,迭代发散或卡住。此时可以先用一个宽松的"膨胀障碍物版本"(把r设置成r+margin)求解,然后用收缩的margin做几轮迭代,逐级逼近真实障碍物。
策略B:把非凸约束凸化(用SOC的近似)
对避障约束 ||p - o|| ≥ r,可以做一个凸化技巧。最经典的是在给定参考点 p_ref 的情况下,把非凸约束近似为线性约束:
(p_ref - o)' (p - p_ref) ≥ 0
这个线性约束说的是:p 必须停留在以 p_ref - o 为法向量的半空间内,即p在障碍物的"这一侧"。这个近似在 p_ref 离障碍物不太远时是合理的,但问题在于 p_ref 本身依赖于轨迹。
做法是迭代地求解:先给定轨迹参考点(来自上一步的轨迹),构造线性化避障约束,得到一个凸问题(比如QP或SOCP),求解后得到新轨迹,再更新参考点,循环直到收敛。这就是凸化-Callback方法(Sequential Convex Programming, SCP)。
SCP在实践中效果非常好,特别是在无人机和火箭着陆轨迹规划里广泛应用。它的优势是每步求解的凸问题都有全局最优解,整个迭代的收敛性比直接跑SQP更可控。
策略C:用 penalty 方法牺牲一点约束精度换计算速度
在极端实时场景(如反应式避障),可能没时间精确求解避障约束。此时可以放弃严格的避障约束,改为在目标函数里加入避障惩罚项:f(x) + β * Σ max(0, d_safe - ||p_k - o||)²。这个惩罚项是分段二次的,整个目标变成无约束问题或带简单约束的问题。
但正如前面说的,惩罚项的解可能略微违反约束,需要额外关卡做碰撞检测和轨迹微调。
我的实战推荐:对于离线可接受的规划时长,优先选择SCP(Sequential Convex Programming)或SQP;对于在线高实时性需求,用QP+避障惩罚+碰撞检测后处理。
5.3 案例三:多机器人协同轨迹规划
问题描述:两台机械臂要在共享工作空间协作搬运一个物体(比如抓取同一个大件物品),要求整体运动过程中两台机械臂不互相碰撞、也不与其他障碍物碰撞,同时各自满足关节极限。
两种建模思路殊途同归:
集中式方法:把两台机械臂的所有决策变量合并成一个大的变量(两台臂各自的控制点向量),然后把所有约束组合在一起。这样问题是一个大规模的非凸NLP(因为有互碰撞约束)。求解器用SQP或内点法。集中式的优点是精度高,缺点是计算量大,扩展性差(增加机械臂数量会导致求解时间指数增长)。
分布式方法:两台机械臂各自维护自己的轨迹,互碰撞约束通过"协商"处理。具体可以用对偶分解:定义耦合约束为 "两台机械臂之间距离≥安全距离",然后每台臂在本地求解自己的子问题时,把对方轨迹视为固定的"环境",同时考虑一个"虚拟力"(拉格朗日乘子)来修正轨迹。
分布式方法适合机械臂数量多的场景,但收敛性需要仔细分析。如果互碰撞很密集(两台臂在狭窄空间内高速靠近),分布式方法的收敛速度会明显下降,甚至振荡。
5.4 基于凸优化的避障约束凸化技巧
这是整个轨迹规划避障部分最实用的一节。因为很多读者问我:"避障约束明明是非凸的,为什么我看到很多论文里用凸优化求解带避障的轨迹规划?"
答案在于它们做了局部凸化。一个常用且非常有效的凸化方法是把避障约束写成SOC(二阶锥)形式。具体来说,如果把避障约束表示为 ||p - o|| ≥ r,这在非凸的,因为不等式符号是"≥"(非凸集)。但如果改成 ||p - o||² ≥ r²,这个仍然是非凸的。
但几何上,要把"球的外部"变成凸集,唯一的办法是把约束放松为"某个包含该半平面外部空间的凸区域"。最简单的技巧是用分离超平面:如果已知轨迹的参考点 p_ref 在某一个平面的一侧(不妨设 (p_ref - o)' n ≥ r),那么约束 (p - o)' n ≥ r 就是一个线性约束(凸),并且对于所有满足这个线性约束的点,它们都在障碍物的"这一侧"。
实际操作时,我们通过迭代更新 n 和 p_ref,每步把避障约束替换成线性约束,然后求解凸QP。这个方法的收敛性和工程可行性在大量机器人规划文献和开源项目(如GPMP2、TrajOpt)里得到了验证。
5.5 求解器选型速查表:结合你的问题矩阵
这张表是我在实际项目里反复验证过的快速选型参考:
| 问题类型 | 变量规模 | 在线/离线 | 推荐方法/求解器 | 备注 |
|---|---|---|---|---|
| 凸QP(无避障) | < 1000 | 在线 | OSQP, qpOASES | qpOASES适合热启动 |
| 凸QP(无避障) | > 1000 | 在线 | OSQP / GUROBI | 注意矩阵存储与稀疏化 |
| 凸SOCP(避障凸化) | 中 | 离线 | ECOS / MOSEK | 如果约束可以被凸化优先选 |
| 非凸NLP(避障真实) | 中 | 离线 | SNOPT / KNITRO | 初始轨迹质量是关键 |
| 非凸NLP(避障真实) | 大 | 离线 | IPOPT(内点法非线性) | 用稀疏结构 |
| 大规模分布式问题 | 大 | 在线 | ADMM | 注意ρ调参 |
| 实时MPC | 中 | 在线 | qpOASES / SQP | 不断热启动 |
6. 真实项目中的踩坑记录与调参经验
这一节分享几段真实的踩坑经历。这些东西在教科书里找不到,但在实际项目中几乎人人都要过一遍。
6.1 初始点选择:90%的优化失败都与它有关
很多初学者最开始使用非线性优化求解器时,随意选初始值(比如全零向量或者直接在可行域外乱选),结果求解器报错、发散或者返回一个奇怪的结果,就认为是求解器不够好。
实际上,在非凸轨迹规划问题中,初始点的作用怎么强调都不为过。SQP和SCP本质上都是局部方法:它们只在初始点的某个邻域内搜索最优解。如果初始轨迹严重违反约束——比如穿过障碍物中心、或者关节角速度超标几个数量级——SQP的QP子问题可能找不到可行方向,直接失败。
我常用的策略有三个:
- 分层规划:先用不考虑复杂约束的快速方法(比如GPU上的RRT或梯形速度规划)生成一条粗糙但几何可行的轨迹,作为NLP求解的初始点。
- 约束渐进:把所有不等式约束的"权限"分成几层。第一轮只施加关节极限约束和端点约束,求解得到一个平滑但可能撞障碍的轨迹;第二轮把避障约束添加进去,再求解。每一步的初始点都来自上一步的解,这比一步到位稳定得多。
- 膨胀障碍物法:把障碍物半径从 r 逐步缩小到真实半径,每轮用上一轮的解作为初始点。这个方法在避障轨迹优化里极其好用,几乎成了我的标准操作。
6.2 约束的数值缩放:一个非常容易被忽略的坑
轨迹优化里的约束往往量纲差异巨大。关节角度约束是 0~2π 量级,速度约束是 0~10 rad/s 量级,而加速度约束可能是 0~100 rad/s² 量级。如果不对它们做缩放,数值线性代数里的矩阵条件数会变得非常大,导致求解器收敛变得非常慢,甚至误判不可行。
解决办法是约束归一化。对每个约束除以一个典型量级,让所有约束在数值上都在 0.1~10 的范围内。举例:
- 关节角度约束:θ_min ≤ θ ≤ θ_max,可以写成 θ/θ_scale ∈ [a, b],其中 θ_scale 是某个典型关节角(比如π)。
- 速度约束:θ'/v_scale ∈ [-1, 1],v_scale 是最大速度。
- 加速度约束同理。
另一个需要缩放的量是目标函数。如果目标函数的值在1e-5量级,而约束在10量级,那么优化算法会过度关注约束满足,而几乎不优化目标函数。一个简单的启发式:先把目标函数和约束都粗略评估一下,然后把目标函数缩放使得最优解处的目标值在1~100量级。
这个"数值缩放"问题我栽过不止一次跟头。有一次跑一个自动化装配的轨迹优化,初始轨迹看起来不错,但最后一段轨迹总是微微越过速度约束。查了很久才发现是约束的数值单位不匹配:程序里速度约束用的是 rad/s,而轨迹计算时乘了一个错误的换算系数,单位差了一百倍。这类错误如果用缩放后的无量纲模型表达,很容易在调试时暴露。
6.3 求解时间的优化:从"够用"到"实时"
真实机器人项目里,"求解这个优化问题要花多久"往往是能不能部署的关键指标。下面是我在项目中的几条实际经验:
经验一:尽量把问题保持在线性约束+二次目标的结构内
如果目标是二次、约束是线性,那么问题就是QP,QP求解时间通常在毫秒级。一旦把约束改成非线性,求解时间可能会暴涨几十倍。所以很多时候我会故意"保守"地把非线性约束近似成线性约束,牺牲一点点最优性换取实时性。比如避障约束用安全半空间近似而不是精确的距离约束。
经验二:利用"问题结构"做预分解
一个问题如果有明显的稀疏结构(比如时间相邻步才有关联),用稀疏线性代数求解器(如MA57、HSL MA27)能比稠密求解器快一个数量级。IPOPT内置了稀疏线性代数,SQP求解器(如SNOPT)也支持稀疏QP。在代码层面,直接告诉求解器你的雅各比矩阵的稀疏结构(用稀疏矩阵存储),效果远超默认稠密存储。
经验三:热启动的意义被低估了
在MPC或者反馈控制场景里,每一步的优化问题和上一步的优化问题其实非常接近(决策变量相似、约束相似)。如果把上一步的解作为这一步的初始点,再配合active-set方法保留上次的active set,QP求解往往能在几次迭代内收敛。实测下来,用 qpOASES 的热启动功能,能比冷启动快5-10倍。这种优化在轨迹跟踪控制里是非常关键的:每次控制周期只有1-10ms,冷启动根本来不及。
6.4 不可行问题的诊断与处理
有时候求解器返回"infeasible"(不可行)。这里的坑在于,求解器告诉你不可行,可能有两种完全不同的原因:
- 问题本身真的不可行:比如起点和终点之间存在无法满足的约束组合(关节极限限制了某段路径,导致不可能同时满足起点和终点条件)。
- 数值问题导致"伪不可行":约束之间有微小的数值冲突(比如要求加速度恰好等于某值的同时又要求加速度≤一个略小的值),或者约束缩放不当导致数值求解器判断失误。
处理方式:
- 如果是"真的不可行",先检查约束是否逻辑一致。比如起点关节角 θ_s 如果在关节极限之外,那问题当然无解。这时候需要放宽约束(扩大关节极限的容忍度,或者允许终点存在小误差)。
- 如果是"伪不可行",最有效的办法是引入松弛变量。对每个约束加入一个小的松弛量 ε,目标函数里额外加一个 M*||ε||² 项。这样问题永远可行,同时如果原问题真的可行,松弛量会被优化到接近零。这个方法在工程上非常实用。
注意:松弛变量不是银弹。如果 M 选得太大,问题会变得数值病态;如果 M 太小,约束会被明显违反。一种稳妥的做法是先求解原问题,如果不收敛再逐步引入松弛,而不是一开始就全部加松弛。
6.5 避障约束的离散化陷阱
轨迹规划里避障约束通常在离散时间点上施加,即只在 N 个时间点检查"末端点离障碍物够不够远"。这个离散化做法有一个隐患:即便所有离散点都满足避障约束,两个相邻离散点之间的轨迹段仍可能穿过障碍物。
这个问题的本质是连续约束的离散化近似。解决办法有几个方向:
- 加稠:增加离散点密度。但这会显著增加变量数量,拖慢求解速度。
- 加保守:把障碍物膨胀一圈(半径加上一个安全余量),让实际轨迹和障碍物保持更大的距离。简单有效,缺点是可行域变小。
- 使用高阶插值:如果轨迹用B样条表示且控制点满足避障约束,由于凸包性质,整条曲线(包括离散点之间的部分)都在控制点的凸包内。如果障碍物在凸包外部,可以保证整条轨迹远离障碍物。这是一个非常精细的优势,推荐在轨迹参数化时考虑使用B样条。
6.6 参数敏感性分析:哪些坑你是躲不掉的
轨迹优化里有一类参数是"必须调"但"永远调不完"的:惩罚系数、松弛权重、内点法容差、SQP子问题终止阈值。我建议每个项目在最终交付前做一个参数敏感性分析,而不是"调到一个看起来不错的参数后就直接用"。
方法很简单:在你标称参数附近扰动几个百分点,记录优化结果的变化(轨迹形状、总时间、约束余量)。如果结果对参数扰动非常敏感(比如调1%就导致轨迹大幅改变),说明问题本身或者建模方式有问题,甚至可能问题不是良态的。这时候与其继续调参,不如回头改进建模——比如更换更合适的参数化方式、增加束约束等。
我还发现一个普遍规律:绝大多数轨迹优化失败案例,不是求解器不够强,而是建模阶段埋的雷足够多。 一个把约束写对、缩放合理、初始点可选好的问题,交给内点法或SQP,通常都能稳稳收敛。
7. 几个容易混淆的概念与工程澄清
这一节把经常被问到的几个概念问题集中回答一下。这些问题在技术社区里总能看到重复提问。
7.1 凸优化算法 vs 非凸优化算法:选型的真正分界线
我遇到过非常多的开发者,认为"凸优化 = 能全局最优的高端方法",而"非凸优化就是局部方法"。还有人说"只要用了内点法,就一定有全局最优",这是完全错误的理解。
内点法是一个算法框架,它既可以求解凸优化问题,也可以求解非凸优化问题(IPOPT就是非线性内点法)。区别在于:
- 当问题是凸的,内点法的中心路径理论能保证收敛到全局最优;
- 当问题是非凸的,内点法的KKT系统可能有多解(局部极值),它收敛到哪一个取决于初始点和迭代路径。
所以选型逻辑应该清晰:
- 如果问题本身是凸的(比如凸QP),不管用什么方法都能全局最优;
- 如果问题非凸,无论用什么方法都只能保证局部最优。这时候核心策略应该围绕"获得一个足够接近全局最优的初始点"展开,而不是指望某个算法自带"全局搜索"能力。
全局优化方法(如分支定界、粒子群、遗传算法、模拟退火)确实可以处理非凸问题,但在轨迹规划的实际应用中很少采用,因为:
- 计算代价巨大;
- 轨迹规划的可行域通常是连通的,局部方法在好的初始点下能获得足够好的解;
- 轨迹规划通常需要在线性,全局优化几乎不可能满足实时性。
7.2 等式约束处理的"隐式方法"与"显式方法"
轨迹规划里的动力学约束(如 M(θ)θ'' + C(θ,θ')θ' + G(θ) = τ)如何处理,直接决定了求解难度。
显式处理:把θ, θ', θ'', τ都作为决策变量,动力学方程作为等式约束。变量多,约束多,但结构清晰。
隐式处理:把τ用直接运动规划消去(假设我们只规划θ,τ由动力学方程唯一确定),然后约束只关于θ及其导数。变量少,但动力学约束变成隐含的,KKT条件的推导会更复杂。
在很多实际轨迹规划项目里,人们用第三种方式:先规划轨迹,再用动力学方程反向计算所需的力矩,校验是否在电机极限内。这种方法严格来说不是约束优化,而是"预测-校验-调整"的方法。它的计算量远小于真正的约束优化,但无法保证力矩约束在整个过程中都被满足。如果发现力矩超限,就需要重新规划或调整轨迹。
在工程上,如果你需要精确的力矩约束(比如高负载机械臂、精密装配),我建议直接用显式处理,牺牲一些变量数,换取KKT系统更简单、求解更稳定。如果只是安防巡检类机器人,力矩约束不是瓶颈,用验算方法就好。
7.3 "全局最优"在轨迹规划中的现实意义
最后聊聊"全局最优"这个被过度神话的概念。
轨迹规划里的全局最优,是指在所有可行轨迹中找出一条代价最小的。从数学上,这个定义很完美;从工程上,你要问自己三个问题:
- 目标函数真的能完全代表你想要的吗? 很多时候"最小化时间"或者"最小化能量"是一个简化建模,并不完全映射真实的工程需求。全局最优的轨迹未必是实际执行起来最舒服的轨迹。
- 模型本身有误差吗? 动力学模型有摩擦系数误差、关节间隙、负载不确定性。在一个不精确模型下找到的"全局最优"轨迹,在真实机器人上执行时远不如一个带安全裕度的次优轨迹。
- 计算预算够吗? 全局最优通常意味着大量计算,这对在线系统几乎不可行。
所以我的真实建议是:把"优化"当作轨迹规划的辅助工具,而不是唯一答案。先用一个稳健的几何规划方法(RRT、PRM、图搜索)找到一条高安全性的路径,然后用局部优化(QP或SQP)把轨迹做得更平滑、更高效。这样得到的解虽然不是全局最优,但工程上是"足够好且足够稳"的。
8. 未来趋势与实践建议:从求解器到端到端学习
轨迹规划中的约束优化,技术演进方向正在变得非常有趣。这里简单梳理几个值得关注的方向,并附上我在实践中的判断。
8.1 在线学习与数据驱动约束优化
这一两年学术界和工业界都在探索如何把数据融入约束优化。比如:
- 通过数据校准约束参数:动力学约束中的摩擦系数、关节间隙等参数,实际上很难精确建模。可以用执行数据(关节角度、力矩、振动)通过系统辨识更新模型参数,然后把更准确的模型带入优化。
- 用学习生成初始解:针对特定任务(比如从A到B的标准抓取动作),可以用模仿学习或强化学习训练一个快速轨迹生成网络,作为约束优化求解前的初始解。这相当于用学习"猜"一个很接近最优解的起点,然后由优化求解器精确收敛。这个组合思路在实践中非常有效,能显著加速非线性优化求解。
8.2 实时求解器与嵌入式优化的进展
在嵌入式系统(如MCU、FPGA)上实时求解约束优化是一个热点方向。现代QP求解器(如qpOASES、OSQP、HPIPM)已经可以做到:
- 在1ms内求解中小规模QP;
- 利用硬件加速的稀疏线性代数;
- 通过"提前计算"和"热启动"把每次求解的迭代次数降到几十次以内。
但要注意,实时求解的代价通常是对问题结构的极大利用。也就是说,你需要针对你的特定问题(固定的维度、固定的约束布局)做定制化求解器,而不是求一个通用接口。很多商业产品(如高动态无人机飞控)就是这么干的:控制周期内有限时间内必须算出下一个控制量,所以它们的代码本质上是一个专门的、针对特定问题结构的QP求解器。
8.3 关于组合优化与混合整数规划的轨迹规划
最后一个值得一提的方向:当轨迹规划不止涉及连续变量,还涉及离散决策时(例如"选择哪个路径点序列""是否进入避障模式""多任务之间的时序选择"),问题会变成混合整数规划(MIP)。
但MIP在轨迹规划中的实际应用还是比较受限的,主要难点在于:
- MIP的求解复杂度极高(NP-hard);
- 轨迹规划的连续变量和离散变量之间的耦合非常紧密,切比雪夫的松弛和线性化往往效率低下;
- 实时性通常不满足。
我的建议是:除非问题规模非常小(比如10个以内的离散决策),否则尽量避免直接用MIP求解器。更实际的路线是两层分解:先用图搜索或任务规划器决定离散结构(选哪条路径),然后用连续优化来做平滑和精调。这也是目前大多数机器人平台采用的技术路线。
最后聊一点个人的体会。
做了这么多年轨迹规划,我最大的感受是:约束优化不是一道"算法选型题",而是一道"数学建模题"。同样的机械臂运动需求,有人可以写成凸QP,毫秒级求出稳定解;有人写成非凸NLP,调参调试几个星期,最后还可能因为初始点不好而失败。
差别往往就在"怎么建模"这一个环节。把决策变量选对(用B样条控制点还是时间离散点、用关节空间还是任务空间),把约束写对(写成线性还是保持非线性、成组还是保持分离、用精确表达还是合理的凸近似),初始点如何生成(RRT还是分层规划),在动手写代码之前想清楚这几点,后面所有求解环节都会顺畅得多。
希望这篇文章能让读者在遇到一个新的轨迹规划问题时,少走一些我在初期走过的弯路。有问题欢迎在评论区交流,尤其是实际项目中遇到的建模和求解器调参问题,这些经验往往比课本上的理论更有价值。