第3章不等式约束处理

第三章：不等式约束处理——障碍函数法、松弛变量法、投影法

各位同学，咱们今天聊点实在的。轨迹规划里最让人头疼的是什么？我个人觉得，就是那些不等式约束。你辛辛苦苦算出一条平滑轨迹，结果一检查，速度超限了，或者离障碍物太近了——全白干。

不等式约束说白了就是「不能超过某个值」。比如飞行器的最大速度、最小转弯半径、禁飞区边界。这些约束不像等式约束那么好伺候，它们有边界，有方向，处理起来需要点技巧。

我这些年做项目，总结下来最常用的就三招：障碍函数法、松弛变量法、投影法。咱们一个一个拆开讲。

先说说障碍函数法。这个方法的核心思想很简单：不让轨迹靠近约束边界。一旦靠近，目标函数的值就会急剧增大，像一堵无形的墙把你推开。

数学上长这样：

min f(x) + μ * Σ B(g_i(x))

其中 B(g_i(x)) 是障碍函数，g_i(x) ≥ 0 是不等式约束
μ 是权重系数，控制障碍的「硬度」

常用的障碍函数有两种：

类型	表达式	特点
对数障碍	-log(g(x))	边界附近增长平缓，适合精度要求不高的场景
倒数障碍	1/g(x)	边界附近增长极快，适合硬约束

我在项目中遇到过一个问题：用对数障碍函数处理飞行器的速度约束，结果在高速段轨迹总是「贴着」速度上限走。后来换成倒数障碍，效果立竿见影。为什么？因为倒数障碍在边界附近更「硬」，优化器不敢轻易靠近。

核心要点：障碍函数法适合约束边界「不可逾越」的场景。但要注意，μ 的取值很关键。μ 太大，问题会变得病态；μ 太小，约束可能被忽略。

我的习惯：先用较大的 μ 快速找到一个可行解，然后逐步减小 μ 来优化目标。这叫「同伦法」，说白了就是先求有，再求好。

障碍函数法有个硬伤：如果初始轨迹就不满足约束，优化过程可能直接崩溃。这时候就需要松弛变量法了。

松弛变量的思路是：给每个不等式约束配一个「缓冲垫」。约束太紧的时候，允许它暂时超一点，但超出的部分要付出代价。

数学形式：

原约束：g(x) ≥ 0
松弛后：g(x) + s ≥ 0, s ≥ 0
目标函数增加惩罚项：λ * s 或 λ * s²

这里的 s 就是松弛变量。它的大小反映了约束被违反的程度。λ 是惩罚系数，控制你有多「容忍」违反约束。

你想想看，这像什么？像你开车时压了实线，摄像头拍到了就罚款。罚款金额（λ）越高，你越不敢压线。但万一真压了，至少还能继续开，不会直接翻车。

我建议在实际工程中，松弛变量法可以作为「兜底方案」。特别是当你的初始轨迹是通过某种近似方法生成的，很可能一开始就不满足所有约束。这时候用松弛变量法，优化器能先找到一个「勉强可行」的解，再逐步收紧。

注意：松弛变量不能滥用。如果某个约束的松弛变量在优化结束后仍然很大，说明这个约束可能不合理，或者你的问题本身无解。我曾经在一个项目中，花了三天时间调参数，最后发现是约束条件写错了——禁飞区的半径少了一个零。

最后说投影法。这个方法最直观：先不管约束，算出一条轨迹，然后把它「投影」到可行域内。

投影的数学定义：

P_C(x) = argmin_{y ∈ C} ||x - y||²

其中 C 是可行域，P_C(x) 是 x 在 C 上的投影

说白了，就是在所有满足约束的点里，找一个离当前点最近的。这个操作可以嵌入到优化算法的每一步迭代中。

举个例子，假设你的速度约束是 v ≤ 10 m/s。优化器算出一个速度 v = 12 m/s，投影操作就是直接把它「剪」到 10 m/s。简单粗暴，但有效。

投影法最大的优点是实现简单。对于盒约束（比如上下界），投影就是一个 clamp 操作，一行代码搞定。

# 速度约束投影
v_projected = np.clip(v, 0, v_max)

# 位置约束投影（矩形禁飞区）
x_projected = np.clip(x, x_min, x_max)
y_projected = np.clip(y, y_min, y_max)

但投影法也有局限。对于复杂的约束（比如凸多边形禁飞区），投影本身就是一个优化问题，计算量不小。而且投影法只保证最终结果满足约束，中间过程可能一直在「跳来跳去」，收敛性不如前两种方法。

我的经验：投影法最适合「约束简单、实时性要求高」的场景。比如无人机在飞行中需要实时调整轨迹，用投影法做约束处理，计算量小，响应快。但如果是离线规划，我建议用障碍函数法或松弛变量法，精度更高。

咱们用一张表总结一下：

下面这张图展示了三种方法在轨迹规划中的位置和关系：

说了这么多，到底该用哪种？我个人的经验是：

嗯，这里要注意一点：这三种方法不是互斥的。我在实际项目中经常把它们组合使用。比如用投影法做快速修正，再用障碍函数法做精细优化。灵活搭配，才是王道。

好了，不等式约束处理就讲到这里。记住一句话：约束不是敌人，是你的朋友。处理好了，它们能帮你找到更安全、更高效的轨迹。