简介：路径规划是机器人自主导航的核心技术之一。当环境中存在雷达探测区、禁飞区等威胁区域时，路径不仅要避免碰撞，还需满足动力学约束和实时性要求。这类问题天然具有非凸性，直接求解困难。凸优化凭借全局最优解和高效求解能力，成为处理此类问题的有力工具。本文从凸优化的基本原理出发，介绍将威胁规避约束转化为凸问题的三种工程方法：惩罚函数、凸安全走廊和混合整数凸优化。重点展示一种基于A*粗路径生成凸走廊，再以CVXPY建模、OSQP求解二次规划（QP）的完整流程，使无人机在满足速度、加速度限制的同时安全绕开威胁区。该方法计算速度快、稳定性好，适用于无人机离线规划与在线重规划场景。同时总结了仿真中走廊不连通、目标函数非凸等易出错的细节，为路径规划工程实践提供参考。 前两天整理硬盘，翻出一个标注着“使用凸优化在存在威胁区域的情况下对无人机进行路径规划.zip”的压缩包，那是之前复现过的路径规划项目。压缩包不算大，但里面夹杂着论文截图、MATLAB代码和一份放飞自我的实验结果记录。今天想把这套东西的来龙去脉摊开说清楚：所谓威胁区域下的无人机路径规划，到底是怎么把“别飞进雷达探测区、禁飞区”这种躲猫猫要求，翻译成一个凸优化问题并求解的。如果你是做机器人或无人机路径规划的学生、工程师，或者想了解凸优化在真实场景里怎么落地，这篇东西应该能给你一些参考。
1. 威胁区把问题变成了什么：从非凸约束到可求解模型
1.1 问题定义不是“绕开障碍物”这么简单
无人机路径规划，最常见说法是找一条从起点到终点的路径，不碰障碍物。但一旦把“威胁区域”放进问题里，目标往往就变了：不只是“不碰”，而是“尽量别靠近”，或者“穿过威胁区的暴露时间要短”。这个差别很重要，直接决定了用什么优化框架。
威胁区域在工程上通常被抽象成圆形或凸多边形。比如一个圆形威胁区，可以写成：
C_j = { p : ||p - c_j|| <= r_j }
其中因为一旦进入这个区域，代价很大甚至直接失败，所以路径需要尽可能避开。但与此同时，你还要让路径尽量短、飞行时间尽量少、控制能量尽量小。于是最自然的数学表达是：
- 输入：起点 p0，终点 pT，威胁区集合 {C_j}；
- 输出：连续轨迹 γ(t)；
- 目标：最小化飞行时间、路径长度或威胁暴露；
- 约束：γ(0)=p0，γ(T)=pT，且对所有 t 和 j 满足 γ(t) ∉ C_j。
这里最大的难点就藏在最后一条约束里。||p - c_j|| >= r_j左边是凸函数，但约束是“凸函数大于等于常数”。它的可行集是圆的外部，而圆的外部不是凸集。你站在航线上看，这条约束等于是在整个平面上挖了一个洞，洞周围的可行域是弯折的，凹进去的。严格说，整个问题是非凸的。
1.2 凸优化的“真香”来自哪里
那为什么大家还是前赴后继地用凸优化？答案非常实际：一旦问题能写成凸优化，求解器给的是全局最优解，而且计算快、稳定性好。
凸优化的两个核心条件是：目标函数是凸函数，可行域是凸集。凸函数只有一个全局最低点，没有一堆局部极小值来坑你。你可以把它想象成一个只有单个底部的大碗，从碗沿任何位置松手，球都会滚到同一个碗底。非凸问题则像布满小坑的高尔夫球场，球很容易滚进某个浅坑就出不来，你甚至不知道这个坑是不是最深的。
对于无人机这种讲究实时性的场景，这个性质太重要了。离线规划慢一点没关系，在线重规划必须毫秒级。凸优化配合 OSQP、Gurobi 这类成熟求解器，很多小规模问题能在几毫秒内求解。这对后续做机载计算、动态重规划都很有价值。
1.3 威胁区的“洞”必须另想办法
所以，想在保持凸优化框架的同时处理威胁区，核心思路不是“用凸优化直接解非凸约束”，而是“把问题重新表述成凸问题”。常见的手段我后面会展开，这里先提三个最主流的方向：
- 把威胁区变成惩罚项塞进目标函数；
- 把自由空间切割成若干个凸胞，让路径在凸胞内走；
- 引入 0/1 整数变量，判断路径从威胁区的哪一侧绕过去。
我以前也幼稚地直接把非凸约束写进 CVXPY，结果求解器直接报错或者给出一堆没法看的轨迹。后来才明白，所谓“用凸优化做带威胁区的路径规划”，几乎都是用了某种改造术，而不是硬刚。
2. 我为什么没有直接从障碍物边缘做几何规划
2.1 几何规划看起来很直接，但坑不少
最早我拿到这个项目时，第一反应是：干嘛不用 A* 或者 RRT？A* 在栅格地图上搜一条避障路径，然后用 B 样条平滑，不是也能飞吗？确实能飞，但做了一段时间之后，我发现了几个很烦的问题。
第一，几何搜索只得到一条折线路径，完全不考虑无人机动力学。四旋翼有最大速度、最大加速度、最小转弯半径，一条在几何上很漂亮的路径，可能根本飞不出来。
第二，平滑阶段容易把路径拉回威胁区。很多平滑算法只关心曲线光顺，不感知障碍物。B 样条一拉，很容易把路径拉进禁飞区，你还得再去做碰撞检测，重新修。
第三，几何路径很难处理“威胁暴露最小”这种连续指标。如果目标是绕开某个圆形威胁区，你可以做，但如果目标是权衡路径长度和暴露风险，几何搜索就力不从心了。
第四，是扩展性问题。威胁区一多，栅格分辨率稍微提高一点，A* 的搜索空间膨胀非常快；RRT 虽能随机采样，但结果每次都不一样，而且不一定最优。
我后来才意识到，凸优化不是要去替代几何搜索，而是应该和几何搜索配合：先用几何方法找到一条粗略的拓扑路径，再用凸优化在满足动力学和安全约束的前提下把轨迹磨平。
2.2 三种把威胁区塞进凸优化框架的工程做法
在项目里，我实际对比了三种主流的改造方式，每种都有适用场景。
| 方法 | 核心思想 | 凸性 | 优点 | 问题 |
|---|---|---|---|---|
| 惩罚函数法 | 把威胁区作为目标函数中的惩罚项，离威胁越近代价越大 | 通常需要额外处理才能保持凸 | 实现简单，能表达风险权衡 | 容易陷入局部最优，权重难调 |
| 凸安全走廊法 | 将自由空间分解为凸多边形，把轨迹约束在走廊内 | 严格凸（走廊约束为线性） | 求解稳定，适合大场景 | 走廊生成质量直接影响结果 |
| 混合整数凸优化 | 用 0/1 变量表示从威胁区哪一侧通过，用大 M 法线性化 | 整数+凸，混合整数凸优化 | 表达能力强，模型精确 | 小规模可用，大规模组合爆炸 |
惩罚函数法最直接。比如你可以在目标函数里加一项：
J += w / (||p - c_j||^2 + epsilon)
但要注意，这个函数在威胁区边缘变化很剧烈，直接放进 QP 里大概率不是凸函数。项目里有一个版本就是这么干的，结果求解器经常报“problem is not DCP”，要不就是路径在威胁区边缘反复震荡。后来我把惩罚项改成线性近似，配合信赖域约束，才勉强能用。
凸安全走廊法是我个人认为最工程化的方案。它的思路分三步：
- 用 A* 或者 Dijkstra 在栅格地图上找一条粗路径；
- 沿着粗路径生成一串有重叠的凸多边形，这些多边形就是安全走廊；
- 把无人机运动学离散化，求解一个二次规划 QP，位置约束是“每个时间步落在对应的凸多边形内”。
这套流程的好处是，非凸的避障约束被走廊“消化”了。走廊本身是凸的，路径又被限制在走廊里，所以避障自然满足。剩下的全是线性约束和二次目标，QS 又快又稳。
混合整数凸优化则更适合小规模、强约束的场景。比如只有两三个威胁区，你希望精确建模“路径在威胁区左边还是右边、上边还是下边”。这时候引入 0/1 变量，用大 M 法把“不能在威胁区内部”转成一组互斥的线性约束。求解规模小的时候效果非常好，但威胁区一多，整数变量组合爆炸，求解时间指数上升。
2.3 我在这个 zip 项目里最终采用的流程
翻完压缩包里的代码，我发现作者并没有用惩罚函数的那种粗暴写法，而是走了一条“A* 粗路径 + 凸走廊生成 + QP 轨迹平滑”的路线。整套数据流是这样的：
- 地图上标出威胁区，栅格化，并对威胁区做膨胀处理；
- A* 搜索一条从起点到终点、完全不碰威胁区的粗路径；
- 沿粗路径按照固定步长生成一系列凸多边形，相邻多边形必须有重叠；
- 将时间轴分成多段，每一段对应一个凸多边形，把位置约束写成线性不等式；
- 用 CVXPY 建模，OSQP 求解，得到一条满足动力学约束的平滑轨迹。
这套流程整体上很稳，而且每个环节都能单独验证。我觉得这比强行在目标函数里加非凸惩罚要靠谱得多。
3. 把动力学和威胁规避写成凸优化：一个可跑的简化算例
3.1 先建立二阶积分运动模型
要真的跑通凸优化，第一步是把无人机模型简化成适合优化的形式。这里我用的是多旋翼水平面内的二阶积分模型：
状态量是位置和速度：
x_k = [px_k, py_k, vx_k, vy_k]^T
控制量是加速度：
u_k = [ax_k, ay_k]^T
用前向欧拉法离散化，时间步长 dt：
px_{k+1} = px_k + vx_k * dt + 0.5 * ax_k * dt^2 py_{k+1} = py_k + vy_k * dt + 0.5 * ay_k * dt^2 vx_{k+1} = vx_k + ax_k * dt vy_{k+1} = vy_k + ay_k * dt这个模型在低速无人机场景下够用。如果你要飞战斗机一样的机动，那得换成更复杂的状态方程，但优化形式不变，只是约束更多。
优化目标我设为最小化控制能量，也就是各个时间步加速度的平方和。这样轨迹不会出现不必要的急加减速，也利于飞控跟踪。
约束包括：
- 起点的位置、速度必须等于设定值；
- 终点的位置、速度必须等于设定值；
- 每个时间步的速度大小要小于最大值；
- 每个时间步的加速度大小要小于最大值；
- 每个时间步的位置必须落在对应的凸走廊内。
前面四条都好理解，最后一条是威胁规避的核心。因为威胁区已经在生成走廊时被排除了，所以只要位置落在走廊内，就一定不会进入威胁区。
3.2 CVXPY 实现一个走廊约束下的轨迹优化
这里我给出一个最简化的可运行示例。为了方便贴代码，我把走廊简化为两个有重叠的凸矩形：前一半时间位置落在左下矩形[0,6] x [0,6]，后一半时间位置落在右上矩形[4,10] x [4,10]。这两个矩形的重叠区域[4,6] x [4,6]就是路径过渡的地方。
import cvxpy as cp import numpy as np # 时间参数 N = 40 # 时间步数 dt = 0.15 # 时间步长 # 起点和终点（位置、速度） p0 = np.array([0.0, 0.0]) pT = np.array([10.0, 10.0]) v0 = np.array([0.0, 0.0]) vT = np.array([0.0, 0.0]) # 变量定义 px = cp.Variable(N) py = cp.Variable(N) vx = cp.Variable(N) vy = cp.Variable(N) ax = cp.Variable(N - 1) ay = cp.Variable(N - 1) constraints = [] # 初始和终端状态约束 constraints += [px[0] == p0[0], py[0] == p0[1]] constraints += [vx[0] == v0[0], vy[0] == v0[1]] constraints += [px[N-1] == pT[0], py[N-1] == pT[1]] constraints += [vx[N-1] == vT[0], vy[N-1] == vT[1]] # 动力学约束 for k in range(N - 1): constraints += [px[k+1] == px[k] + vx[k] * dt + 0.5 * ax[k] * dt**2] constraints += [py[k+1] == py[k] + vy[k] * dt + 0.5 * ay[k] * dt**2] constraints += [vx[k+1] == vx[k] + ax[k] * dt] constraints += [vy[k+1] == vy[k] + ay[k] * dt] # 走廊约束（这里其实是两个凸矩形，真实项目中来自上游算法） for k in range(N): if k < N // 2: constraints += [0 <= px[k], px[k] <= 6] constraints += [0 <= py[k], py[k] <= 6] else: constraints += [4 <= px[k], px[k] <= 10] constraints += [4 <= py[k], py[k] <= 10] # 速度和加速度边界 constraints += [cp.abs(vx) <= 2.5, cp.abs(vy) <= 2.5] constraints += [cp.abs(ax) <= 2.0, cp.abs(ay) <= 2.0] # 目标函数：最小化控制能量 objective = cp.Minimize(cp.sum_squares(ax) + cp.sum_squares(ay)) # 求解 prob = cp.Problem(objective, constraints) prob.solve(solver=cp.OSQP, verbose=False) # 输出结果 if prob.status == cp.OPTIMAL: print("求解成功，控制能量 =", prob.value) print("前 3 个位置：", np.vstack([px.value[:3], py.value[:3]]).T) else: print("求解失败，状态 =", prob.status)注意，真实项目中走廊约束不是这样手写的。这里的矩形只是为了让你直观看到“凸约束怎么加进去”。如果上游生成的走廊是一组凸多边形，你只需要把每个时刻的约束换成对应的多边形不等式即可。
3.3 求解结果和工程上的解读
这段代码用 OSQP 求解通常几毫秒就能出结果。你得到的是每个时间步的位置、速度、加速度，路径天然满足动力学约束和走廊约束。
我第一次跑通的时候异常兴奋，因为代码改一行就能让轨迹从直线变成绕过威胁区的曲线。但后来发现，真正的挑战从来不是求解器，而是你怎么把威胁区准确地变成约束。
还有一个细节：CVXPY 里面位置约束用的是“每个时间步必须在某个凸多边形内”，但如果两个相邻时间步被分配到不同的凸多边形，而两个多边形没有重叠，那么这一步到下一步可能是跳变，优化器会报不可行。所以生成走廊时，相邻凸多边形一定要有重叠区。
4. 仿真里最容易翻车的三个细节
4.1 走廊不连通或太窄，QP 直接报 infeasible
这是我仿真中遇过最频繁的问题。A* 粗路径往往贴着威胁区边缘走，如果你沿着粗路径用固定宽度生成凸多边形，在某些拐弯处两个多边形可能只连接在一个点上，QP 中就会出现“前一步还在这个区域，下一步必须跳到那个区域”的矛盾约束，最终求解器告诉你 infeasible。
调试方法很简单：把每个时刻的走廊约束单独打印出来，看看相邻时间点的位置可行域是否有交集。如果没有，说明走廊生成环节出问题了。
我的实践是，在生成走廊时给每个凸多边形加 20% 的膨胀余量，强制相邻多边形有足够重叠。膨胀多了会导致路径绕路，但至少求解稳定。之后再逐步缩小膨胀系数。
4.2 目标函数里乱加威胁暴露项，结果反而更危险
有人希望路径离威胁区中心越远越好，于是把威胁暴露代价直接写进 QP 目标，比如：
J += w / (||p - c_j||^2 + epsilon)
这个想法很自然，但问题在于这个函数在威胁区附近是高度非凸的。你把它加进 QP，目标函数变成非凸，求解器要么直接报错，要么给你一个局部最优解。更可怕的是，某些求解器强行求解，给出的结果看起来没问题，实际路径却可能在威胁区边缘画圈，比不加还危险。
稳妥的做法
本文还有配套的精品资源，点击获取
