简介路径规划是无人机自主飞行的核心问题之一尤其在存在禁飞区、信号干扰源和建筑障碍等威胁区域的复杂环境中规划一条安全、平滑且实时可用的轨迹尤为关键。传统A*与RRT算法在连续高维空间中面临搜索爆炸或路径次优等瓶颈。凸优化通过将路径规划建模为带约束的数学优化问题利用内点法等求解器在毫秒级获得全局最优解成为解决此类问题的理想工具。针对威胁区域带来的非凸约束工程上常用顺序凸优化SCP将非凸问题迭代凸化结合信任域约束稳定逼近可行解配合CVXPY等建模工具可快速实现从算法到代码的落地。该方法已在农业植保、城市巡检、电力巡检等场景中得到验证为复杂约束下的无人机航迹规划提供了高效可靠的解决思路。 解压这个压缩包之前我以为又是一份“先跑起来再说”的仿真Demo。但实际把代码过了一遍之后发现这个项目解决的是无人机路径规划里一个很务实的问题当地图上存在禁飞区、信号干扰源、楼宇障碍这类“威胁区域”时怎么规划出一条既安全、又平滑、还能保证实时性的飞行路径。项目核心是凸优化思路是把路径规划问题转换成带约束的数学优化问题用求解器在毫秒级时间内拿到最优解。这类需求放到真实场景里非常普遍。农业植保无人机在果园作业时要避开边界树木城市巡检无人机要在楼宇间穿行并绕开信号塔电力巡检要在导线和杆塔之间规划航迹本质上都是“带威胁区域的路径规划”。而这个项目给出的思路是把威胁区域用数学约束来描述再通过凸优化算法求解而不是像传统图搜索那样在离散栅格上暴搜也不像RRT那样靠随机采样碰运气。文章会从问题建模、威胁区域数学化、非凸转凸的核心技巧、代码实现到常见坑位完整走一遍凸优化路径规划的落地流程。无论你之前用的是A*、RRT还是强化学习这篇内容应该都能给你提供一种新的解题视角。1. 先搞清楚问题再选算法威胁区域下路径规划的数学本质1.1 为什么不是A*、RRT而是凸优化很多做无人机路径规划的同学第一反应是A*加栅格地图或者RRT加碰撞检测。这两个方案本身没有错在二维平面、静态环境、地图规模可控的情况下都很成熟。但仔细推敲一下它们都有各自的硬伤。A的核心是图搜索需要把连续空间离散成栅格。栅格粒度决定了计算量和内存开销地图一大、维度一高状态空间指数膨胀。三维场景下如果还要考虑无人机航向角、俯仰角状态维度会进一步增加A的搜索空间很快就失控了。RRT走的是采样路线不需要显式建模障碍物适合高维空间而且成功率很高。但它的致命弱点是——RRT求出来的只是“一条可行路径”不是“最优路径”。路径可能会非常曲折、有很多无意义的绕行后续要加平滑优化否则无人机飞起来会非常不舒服转弯时还容易超出飞行器的动力学极限。凸优化解决的是另一个层面的问题它把路径规划当成一个严谨的数学优化问题来处理——决策变量是路径点的位置目标函数是路径长度、平滑度、能量的加权组合约束条件包括了威胁区域规避和动力学限制。这种建模方式有两个天然优势一是解有全局最优性保证不会给你一条“凑合能飞”的路径二是求解速度非常快现代内点法求解器在少量路径点的场景下几十毫秒就能出结果完全具备嵌入到实时规划模块的潜力。1.2 路径规划问题的“通用公式”任何路径规划问题其实都可以写成一个统一的数学框架[ \min_{x_0, x_1, ..., x_N} \quad J(x_0, x_1, ..., x_N) ] [ \text{s.t.} \quad x_0 x_{start}, \quad x_N x_{goal} ] [ \quad x_i \in \mathcal{X}_{free} ]这里 (x_i) 是路径点可以是二维坐标也可以是三维坐标加上速度、加速度等状态(J) 是目标函数决定我们要“优化什么”(\mathcal{X}_{free}) 是自由空间也就是不碰威胁区域、不违反动力学约束的可飞区域。凸优化要求目标函数是凸函数可行域是凸集。这个约束看起来很严格但实际工程中很多问题都能通过合理的建模技巧“凑”成凸问题。这也是这个项目最核心的看点怎么把一个带威胁区域的路径规划问题从“看起来不凸”变成“一个标准凸优化问题”然后交给求解器去算。1.3 凸优化解决问题的边界客观地说凸优化不是万能的。如果威胁区域形状极度不规则或者环境中有大量动态障碍物需要实时避让纯凸优化的表达能力会受限。对付这种情况通常的做法是凸优化和其他方法结合用RRT做全局粗规划再用凸优化对局部轨迹做平滑和优化或者用滚动时域的思路每个控制周期用凸优化在局部窗口内重新规划一小段路径。这个项目的价值不在于证明凸优化可以替代所有路径规划算法而在于提供了一套可复用的数学建模方法和代码框架。你把威胁区域换成高楼、禁飞区、信号干扰源把二维换成三维把静态障碍换成动态障碍这套框架依然能撑起来只需要在约束表达上做相应修改。2. 威胁区域建模从地图上的圈圈到矩阵里的约束2.1 威胁区域的几种典型数学表达威胁区域到底是什么取决于你的实际场景。在不同语境下它的几何形状和数学表达差别很大。我梳理了三种最常见的建模方式每种都对应不同的约束形式。圆形威胁区域容易理解也最常见。假设第 (j) 个威胁区域的圆心是 (c_j)半径是 (r_j)那么路径点 (x_i) 不能进入这个圆内部约束可以写成[ |x_i - c_j|^2 \geq r_j^2 ]注意这个约束本身是非凸的因为 (|x_i - c_j|^2) 是一个凸函数但要求“凸函数大于等于常数”定义的其实是补集整个可行域不是一个凸集。后面我会讲怎么处理这个问题。多边形威胁区域有棱有角比如建筑物轮廓。一个凸多边形可以用一组半平面约束表示。如果第 (j) 个威胁区域被表达成 (A_j x \leq b_j)内部那要求路径点在第 (j) 个区域外部就变成对每一个半平面都满足“至少有一个半平面被违反”的逻辑这个逻辑约束也是非凸的。但在特定情况下比如你只需要从一个固定的方向绕过这个多边形那么只需要选择其中某一两个半平面约束做反向即可这取决于绕行的先验方向。椭圆威胁区域风力影响区、信号干扰范围。椭圆约束可以用二次型表示((x_i - c_j)^T Q_j (x_i - c_j) \geq 1)其中 (Q_j) 是描述椭圆形状和朝向的正定矩阵。同一个道理这里的“大于等于”也让约束变成非凸。2.2 为什么非凸约束让求解器“头疼”可能你会觉得非凸就非凸直接用非线性求解器硬解不就行了理论上确实可以蒙特卡洛、遗传算法、粒子群什么都能试。但实际用起来非线性求解器或者启发式算法有几个麻烦一是求解时间不可控可能要跑几秒钟甚至几分钟二是无法保证全局最优经常陷入局部最优路径绕了很大的弯自己还不知道三是对初始值敏感换个起点路径就完全不一样了。凸优化求解器比如ECOS、OSQP、MOSEK之所以快是因为它们有成熟的底层算法理论支撑。这些算法利用了凸问题的优秀性质——局部最优就是全局最优而且有严格的对偶理论保证收敛性。问题在于你得先把问题“改造”成凸问题这也是项目代码里最见功夫的地方。2.3 如何处理“起点或终点在威胁区域内”的特殊情况还有一个容易被忽略的坑如果起点或者终点本身就在威胁区域内部那任何路径规划都不可能无碰撞地连接这两个点。代码里一般要加一个预处理计算起点和终点到所有威胁区域中心的距离如果发现距离小于安全半径直接报错返回提示用户调整起终点位置或者扩大安全半径。我在实际调试中遇到过几次这种情况一开始以为是约束写错了排查了很久才发现原来是起点坐标和威胁区域中心的距离只有0.3米而威胁半径是1.5米这显然不可能规划成功。所以这类问题在建模阶段就要提前检查与其让求解器返回不可行不如在输入阶段就给出明确提示。3. 非凸约束处理凸优化的三个实战套路3.1 套路一惩罚函数法把约束塞进目标函数最简单直观的处理方式是把威胁区域约束“软化”成一个惩罚项加到目标函数里。什么意思呢原来我们要求路径点必须距离威胁区域圆心超过 (r_j)现在把这个要求改成一个奖励或惩罚如果路径点太靠近威胁区域中心目标函数就增加一个很大的惩罚值迫使优化结果自动避开。用数学式子表达可以定义惩罚函数[ p_j(x_i) \max(0, \ r_j^2 - |x_i - c_j|^2)^2 ]当 (|x_i - c_j|^2 \geq r_j^2) 时惩罚为0一旦进入威胁区域内部惩罚值为正且越靠近圆心惩罚越大。然后目标函数变成[ J \sum_{i1}^{N-1} |x_i - x_{i-1}|^2 \lambda \sum_j \sum_i p_j(x_i) ](\lambda) 是惩罚权重通常设成一个较大的数。这种方法的好处是问题始终是凸的求解稳定坏处是“软约束”不能严格保证路径不碰威胁区域只能让路径“尽量远离”。如果 (\lambda) 不够大求解器可能会用轻微的越界换取更短的路径长度这在安全关键场景下是不可接受的。实战中我一般把惩罚函数法用于初值生成或者对安全性要求不高的快速路径参考而最终交给飞控执行的路径一定用下面两种硬约束处理方法之一。3.2 套路二顺序凸优化SCP用迭代逼近非凸约束这是本项目的核心亮点。思路是在当前参考轨迹附近把非凸约束线性化把每一次迭代变成求解一个凸优化子问题然后不断更新参考轨迹迭代收敛到原问题的一个可行解。具体到圆形威胁区域的约束 (|x_i - c_j|^2 \geq r_j^2)在参考轨迹点 (x_i^{ref}) 附近做一阶泰勒展开[ |x_i - c_j|^2 \approx |x_i^{ref} - c_j|^2 2(x_i^{ref} - c_j)^T (x_i - x_i^{ref}) ]令 (d x_i^{ref} - c_j)约束可以写成[ 2d^T (x_i - x_i^{ref}) |d|^2 \geq r_j^2 ]这是一个关于 (x_i) 的线性不等式完美地变成了一个凸约束。但问题是线性化只在参考点附近有效如果某个路径点离参考点很远线性化误差会很大。这时候需要引入信任域约束[ |x_i - x_i^{ref}| \leq \delta ](\delta) 是信任域半径控制每一次迭代允许路径点偏离参考轨迹的最大距离。整个SCP流程是用直线插值生成一条初始参考轨迹从起点到终点的直线碰威胁区域也没关系因为后面会迭代修正→ 在参考轨迹处构建所有威胁区域的线性化约束 → 加上信任域约束 → 求解凸优化子问题 → 把解更新为新的参考轨迹 → 重复直到路径变化小于阈值或者达到最大迭代次数。SCP不是严格意义上的凸优化但它充分“利用”了凸优化计算快的优势通过多次求解凸子问题来逼近原非凸问题效果在实操中非常可靠。这个项目用的就是这个方案每次迭代的QP问题求解通常在几十毫秒以内迭代10次也就是半秒左右的量级完全可以满足离线规划需求。3.3 套路三凸松弛把困难问题放宽第三种思路是凸松弛更适合处理组合决策类的约束。比如无人机在威胁区域之间穿行时可以选择从上方绕过也可以从左边绕过这本质上是离散选择。你可以引入二值变量 (z_{i,j}) 表示“路径点 (i) 是否从某个方向避开威胁区域 (j)”但这会引入整数规划求解难度大增。凸松弛的做法是把 (z_{i,j} \in {0,1}) 放宽成 (z_{i,j} \in [0,1])把问题转化为一个凸优化问题求解。如果松弛后的解恰好是0或1那太好不过了如果得到小数还需要通过分支定界或者舍入策略来恢复整数解。这个方法在这个项目里用得不多因为SCP已经能很好解决路径规划问题但如果你遇到的是“多个不可通行区域之间的组合选择”问题比如飞行器要在复杂工业厂房里穿行或者无人机需要在多个建筑栋之间选择绕行通道凸松弛会是很有价值的工具。3.4 三种套路如何选型一张表说清楚方法凸性是否保证硬避碰计算速度适用场景惩罚函数法始终凸否软约束最快快速初值生成不严格的安全场景顺序凸优化SCP迭代凸化是收敛后快静态威胁区域下的精确路径规划凸松弛凸/整数混合视恢复策略而定中组合绕行决策、通道选择问题真实项目里最推荐的组合拳是先用直线插值生成初始轨迹再用SCP迭代求解得到一条硬约束安全的平滑路径。如果时间非常紧张可以先用惩罚函数法跑一版快速参考路径再在这个路径的基础上只做几次SCP精修这样能大幅减少迭代次数。4. 从建模到代码CVXPY实现一个完整案例4.1 环境与依赖准备这个项目的代码是基于Python写的核心求解器是CVXPY加上ECOS或者OSQP后端。在动手之前先把环境装好pip install cvxpy numpy matplotlibCVXPY是一个非常好用的凸优化建模库你可以像写数学公式一样把优化问题写出来底层自动帮你选择求解器。前端建模后端求解这正是做路径规划验证最理想的组合。4.2 定义问题参数和解算框架假设我们的场景是二维地图起点 ((0, 0))终点 ((10, 10))三个圆形威胁区域均匀分布在中间区域路径点数量取40个。这里我把路径点数量定为 (N40)这样的粒度既不会让轨迹太粗导致切弯明显也不会让优化变量规模过大拖慢求解。核心代码框架如下import cvxpy as cp import numpy as np N 40 start np.array([0.0, 0.0]) goal np.array([10.0, 10.0]) obs [{center: np.array([5.0, 5.0]), radius: 1.2}, {center: np.array([3.0, 7.0]), radius: 1.0}, {center: np.array([7.0, 3.0]), radius: 0.8}] # 决策变量N个路径点每个点有x和y坐标 X cp.Variable((N, 2))4.3 目标函数与约束条件目标函数用相邻路径点之间的距离平方和之所以用距离平方而不是距离本身是因为平方项是凸二次函数在QP问题里非常好解而且加平方还能让路径点分布更均匀不会出现某一段特别密、某一段特别疏的情况。# 目标函数最小化路径总长度的平方等价于让路径尽量短 objective cp.Minimize(cp.sum(cp.sum((X[1:] - X[:-1])**2, axis1))) constraints [] # 固定起点和终点 constraints.append(X[0] start) constraints.append(X[N-1] goal)然后是威胁区域约束。这里使用SCP所以约束不是一次性写死的是在每次迭代里动态更新的。我先把参考轨迹初始化为一条直线# 初始化参考轨迹从起点到终点的直线插值 ref start (goal - start) * np.linspace(0, 1, N).reshape(-1, 1)接着进入迭代循环每次迭代重新构建线性化后的威胁约束并加入信任域约束max_iter 20 delta 0.5 # 信任域半径控制每次迭代允许轨迹偏离参考的最大距离 for it in range(max_iter): cons constraints.copy() # 信任域约束限制每个路径点不能偏离参考点太远 cons.append(cp.norm(X - ref, axis1) delta) # 线性化威胁区域约束 for obs_i in obs: c obs_i[center] r obs_i[radius] for i in range(N): d ref[i] - c # 在ref[i]处对 ||x - c||^2 r^2 做一阶泰勒展开 cons.append(2 * d (X[i] - ref[i]) cp.norm(d)**2 r**2) prob cp.Problem(objective, cons) prob.solve(solvercp.ECOS) if prob.status ! optimal: print(f迭代 {it}: 求解失败状态 {prob.status}) break # 更新参考轨迹 new_ref X.value change np.max(np.abs(new_ref - ref)) ref new_ref print(f迭代 {it}: 最大变化 {change:.4f}) if change 1e-4: print(迭代收敛) break4.4 结果分析和实际效果跑完这个代码你会看到迭代过程里“最大变化”逐渐减小一般5到10轮迭代就会收敛到一条稳定路径。由于信任域半径初始设成0.5每次迭代路径点不会大幅跳动路径会一步一步从直线“撑开”绕过障碍物非常直观。结束后用matplotlib把路径、威胁区域、参考轨迹画出来你就能看到一条平滑的、从起点出发绕开三个圆最终到达终点的轨迹。相比A*在栅格上走出来的带锯齿的路径凸优化结果要好看非常多基本上可以直接作为无人机飞控的参考轨迹输入。4.5 扩展到三维和动力学约束二维场景验证通过后扩展成三维只改四处地方决策变量维度从2改成3起终点坐标增加z分量威胁区域中心增加z分量威胁区域从球体替代圆。球体威胁区域的约束写法和圆形几乎一样[ |x_i - c_j|^2 \geq r_j^2 ]同样用SCP线性化完全不需要改算法框架。如果还要考虑无人机动力学约束最大速度、最大加速度、最小转弯半径就需要把路径点之间的差分成速度项和加速度项再加约束。比如速度约束 (v_i (x_{i1} - x_i) / \Delta t)要求 (|v_i|2 \leq v{max})这是一个凸二次锥约束CVXPY可以直接表达为cp.norm(X[i1] - X[i]) v_max * dt底层OSQP处理这种constraint非常熟练。5. 常见问题与排查技巧实录5.1 求解器报“INFEASIBLE”不可行这是凸优化路径规划里最常遇到的错误几乎每个跑过SCP的人都栽过。遇到不可行先别急着怀疑算法按这个顺序排查。第一检查起点终点是否在威胁区域内部。我前面提到过起点或终点进入威胁区域约束永远无法满足求解器必然报不可行。第二检查信任域半径是否太小。如果参考轨迹要绕开一个很大的威胁区域信任域半径却被设成0.1那每次迭代路径点只能挪动很小距离而当前路径还在威胁区域内部线性化约束本身就不满足子问题自然无解。这时候需要把信任域半径调大到0.5甚至1.0。第三检查线性化约束是否真的在参考点处构成可行方向。如果参考轨迹刚好穿过威胁区域的正中央而线性化只在圆的切线方向排斥路径点有时候路径点会被挤到两个威胁区域的夹缝里动弹不得需要给信任域更大的空间。调试技巧在抛出INFEASIBLE时把当前迭代的参考轨迹画出来叠加威胁区域和信任域范围肉眼看一眼就知道约束卡在哪个路径点上了。5.2 路径抖动/锯齿状严重如果跑出来的路径不是平滑的弧线而是锯齿状多半是目标函数的问题。目标函数如果只包含相邻路径点的一阶差分速度项路径允许有突然的拐弯反映在轨迹上就是折线很重。解决办法是在目标函数里加入二阶差分项也就是加速度项objective cp.Minimize( cp.sum(cp.sum((X[1:] - X[:-1])**2, axis1)) 10.0 * cp.sum(cp.sum((X[2:] - 2*X[1:-1] X[:-2])**2, axis1)) )二阶差分项会惩罚路径点之间的加加速度让曲线变得更加平滑。权重10.0可以调但别太大太大了路径会过度平滑导致切弯半径变大反而离威胁区域更近。5.3 实时性不够如何优化如果要把这套算法嵌入到无人机的机载电脑里做实时重规划40个路径点、20次迭代是绝对不可接受的单次求解时长会高出一个量级。我的优化经验有三个方向。第一减少决策变量维度。40个路径点可以降到15到20个路径的细节靠后面加上样条插值来补齐路径点少一个数量级求解时间能减少一个数量级。第二换求解器。CVXPY默认的ECOS是内点法精度高但速度慢换成OSQP或者专门为嵌入式设备设计的求解器QP问题求解时间能压到几毫秒。第三热启动。每次迭代把上一次的解法作为初始猜测传给求解器能明显减少内部迭代次数。CVXPY里可以用problem.solve(warm_startTrue)开启热启动。5.4 动态威胁区域或者完全非凸的威胁区域怎么处理这个项目本身处理的是静态威胁区域但真实飞行中威胁区域往往是动态的比如另一架无人机、飞鸟、突然出现的航拍飞行器。做法是把SCP和滚动时域控制结合起来每个控制周期只优化未来一小段时域比如5秒内的路径当检测到新障碍时在当前最优解的基础上重新运行SCP。因为SCP每次迭代只需要几十毫秒完全能满足动态避障的实时性要求。至于完全非凸的威胁区域比如U形障碍物、两个圆并联形成的哑铃形区域直接凸化会失真。实用做法是先对威胁区域做凸分解把它拆成多个凸子区域再用逻辑约束穿过的子区域数量、方向组织起来配合3.3节说的凸松弛技巧求解。这个方向展开讲又是一个长文但在工程上核心思想依然是尽量把问题拆成一个一个可以凸化的小块再用迭代和松弛把它们缝合起来。5.5 权重参数怎么调一个实际经验值目标函数里通常有路径长度权重、平滑度权重约束里有惩罚权重信任域半径也属于广义的“权重”。我的经验是先固定长度权重为1然后从小到大调平滑权重每次翻倍观察路径形状。惩罚权重(\lambda)如果用于软约束场景一般从100起步太大会导致目标函数数值量级差距过大影响求解器数值稳定性太小则约束形同虚设。信任域半径(\delta)的调节逻辑是先设大一点比如0.5到1.0让路径有足够空间大幅变形逃离威胁区域当路径变形的幅度变小之后把信任域缩小到0.1到0.2做精细化收敛。这个“先大后小”的动态信任域策略比固定信任域收敛速度更快路径质量也更好。在真实无人机项目里跑过这套方案的很多同事最后都会得出一个共同的感受凸优化路径规划的核心不是“用求解器算”而是“怎么把问题建好”。威胁区域怎么表达、约束怎么线性化、信任域怎么调、目标函数怎么组合这些每一步都直接影响最终路径的可用性。当前这个项目用SCP解决的静态威胁区域问题是一个基础但非常实用的样本把代码跑通、把参数调明白之后向三维扩展、向动态场景扩展路径就顺了。本文还有配套的精品资源点击获取 凸优化实战：无人机威胁区域路径规划与SCP求解
简介路径规划是无人机自主飞行的核心问题之一尤其在存在禁飞区、信号干扰源和建筑障碍等威胁区域的复杂环境中规划一条安全、平滑且实时可用的轨迹尤为关键。传统A*与RRT算法在连续高维空间中面临搜索爆炸或路径次优等瓶颈。凸优化通过将路径规划建模为带约束的数学优化问题利用内点法等求解器在毫秒级获得全局最优解成为解决此类问题的理想工具。针对威胁区域带来的非凸约束工程上常用顺序凸优化SCP将非凸问题迭代凸化结合信任域约束稳定逼近可行解配合CVXPY等建模工具可快速实现从算法到代码的落地。该方法已在农业植保、城市巡检、电力巡检等场景中得到验证为复杂约束下的无人机航迹规划提供了高效可靠的解决思路。 解压这个压缩包之前我以为又是一份“先跑起来再说”的仿真Demo。但实际把代码过了一遍之后发现这个项目解决的是无人机路径规划里一个很务实的问题当地图上存在禁飞区、信号干扰源、楼宇障碍这类“威胁区域”时怎么规划出一条既安全、又平滑、还能保证实时性的飞行路径。项目核心是凸优化思路是把路径规划问题转换成带约束的数学优化问题用求解器在毫秒级时间内拿到最优解。这类需求放到真实场景里非常普遍。农业植保无人机在果园作业时要避开边界树木城市巡检无人机要在楼宇间穿行并绕开信号塔电力巡检要在导线和杆塔之间规划航迹本质上都是“带威胁区域的路径规划”。而这个项目给出的思路是把威胁区域用数学约束来描述再通过凸优化算法求解而不是像传统图搜索那样在离散栅格上暴搜也不像RRT那样靠随机采样碰运气。文章会从问题建模、威胁区域数学化、非凸转凸的核心技巧、代码实现到常见坑位完整走一遍凸优化路径规划的落地流程。无论你之前用的是A*、RRT还是强化学习这篇内容应该都能给你提供一种新的解题视角。1. 先搞清楚问题再选算法威胁区域下路径规划的数学本质1.1 为什么不是A*、RRT而是凸优化很多做无人机路径规划的同学第一反应是A*加栅格地图或者RRT加碰撞检测。这两个方案本身没有错在二维平面、静态环境、地图规模可控的情况下都很成熟。但仔细推敲一下它们都有各自的硬伤。A的核心是图搜索需要把连续空间离散成栅格。栅格粒度决定了计算量和内存开销地图一大、维度一高状态空间指数膨胀。三维场景下如果还要考虑无人机航向角、俯仰角状态维度会进一步增加A的搜索空间很快就失控了。RRT走的是采样路线不需要显式建模障碍物适合高维空间而且成功率很高。但它的致命弱点是——RRT求出来的只是“一条可行路径”不是“最优路径”。路径可能会非常曲折、有很多无意义的绕行后续要加平滑优化否则无人机飞起来会非常不舒服转弯时还容易超出飞行器的动力学极限。凸优化解决的是另一个层面的问题它把路径规划当成一个严谨的数学优化问题来处理——决策变量是路径点的位置目标函数是路径长度、平滑度、能量的加权组合约束条件包括了威胁区域规避和动力学限制。这种建模方式有两个天然优势一是解有全局最优性保证不会给你一条“凑合能飞”的路径二是求解速度非常快现代内点法求解器在少量路径点的场景下几十毫秒就能出结果完全具备嵌入到实时规划模块的潜力。1.2 路径规划问题的“通用公式”任何路径规划问题其实都可以写成一个统一的数学框架[ \min_{x_0, x_1, ..., x_N} \quad J(x_0, x_1, ..., x_N) ] [ \text{s.t.} \quad x_0 x_{start}, \quad x_N x_{goal} ] [ \quad x_i \in \mathcal{X}_{free} ]这里 (x_i) 是路径点可以是二维坐标也可以是三维坐标加上速度、加速度等状态(J) 是目标函数决定我们要“优化什么”(\mathcal{X}_{free}) 是自由空间也就是不碰威胁区域、不违反动力学约束的可飞区域。凸优化要求目标函数是凸函数可行域是凸集。这个约束看起来很严格但实际工程中很多问题都能通过合理的建模技巧“凑”成凸问题。这也是这个项目最核心的看点怎么把一个带威胁区域的路径规划问题从“看起来不凸”变成“一个标准凸优化问题”然后交给求解器去算。1.3 凸优化解决问题的边界客观地说凸优化不是万能的。如果威胁区域形状极度不规则或者环境中有大量动态障碍物需要实时避让纯凸优化的表达能力会受限。对付这种情况通常的做法是凸优化和其他方法结合用RRT做全局粗规划再用凸优化对局部轨迹做平滑和优化或者用滚动时域的思路每个控制周期用凸优化在局部窗口内重新规划一小段路径。这个项目的价值不在于证明凸优化可以替代所有路径规划算法而在于提供了一套可复用的数学建模方法和代码框架。你把威胁区域换成高楼、禁飞区、信号干扰源把二维换成三维把静态障碍换成动态障碍这套框架依然能撑起来只需要在约束表达上做相应修改。2. 威胁区域建模从地图上的圈圈到矩阵里的约束2.1 威胁区域的几种典型数学表达威胁区域到底是什么取决于你的实际场景。在不同语境下它的几何形状和数学表达差别很大。我梳理了三种最常见的建模方式每种都对应不同的约束形式。圆形威胁区域容易理解也最常见。假设第 (j) 个威胁区域的圆心是 (c_j)半径是 (r_j)那么路径点 (x_i) 不能进入这个圆内部约束可以写成[ |x_i - c_j|^2 \geq r_j^2 ]注意这个约束本身是非凸的因为 (|x_i - c_j|^2) 是一个凸函数但要求“凸函数大于等于常数”定义的其实是补集整个可行域不是一个凸集。后面我会讲怎么处理这个问题。多边形威胁区域有棱有角比如建筑物轮廓。一个凸多边形可以用一组半平面约束表示。如果第 (j) 个威胁区域被表达成 (A_j x \leq b_j)内部那要求路径点在第 (j) 个区域外部就变成对每一个半平面都满足“至少有一个半平面被违反”的逻辑这个逻辑约束也是非凸的。但在特定情况下比如你只需要从一个固定的方向绕过这个多边形那么只需要选择其中某一两个半平面约束做反向即可这取决于绕行的先验方向。椭圆威胁区域风力影响区、信号干扰范围。椭圆约束可以用二次型表示((x_i - c_j)^T Q_j (x_i - c_j) \geq 1)其中 (Q_j) 是描述椭圆形状和朝向的正定矩阵。同一个道理这里的“大于等于”也让约束变成非凸。2.2 为什么非凸约束让求解器“头疼”可能你会觉得非凸就非凸直接用非线性求解器硬解不就行了理论上确实可以蒙特卡洛、遗传算法、粒子群什么都能试。但实际用起来非线性求解器或者启发式算法有几个麻烦一是求解时间不可控可能要跑几秒钟甚至几分钟二是无法保证全局最优经常陷入局部最优路径绕了很大的弯自己还不知道三是对初始值敏感换个起点路径就完全不一样了。凸优化求解器比如ECOS、OSQP、MOSEK之所以快是因为它们有成熟的底层算法理论支撑。这些算法利用了凸问题的优秀性质——局部最优就是全局最优而且有严格的对偶理论保证收敛性。问题在于你得先把问题“改造”成凸问题这也是项目代码里最见功夫的地方。2.3 如何处理“起点或终点在威胁区域内”的特殊情况还有一个容易被忽略的坑如果起点或者终点本身就在威胁区域内部那任何路径规划都不可能无碰撞地连接这两个点。代码里一般要加一个预处理计算起点和终点到所有威胁区域中心的距离如果发现距离小于安全半径直接报错返回提示用户调整起终点位置或者扩大安全半径。我在实际调试中遇到过几次这种情况一开始以为是约束写错了排查了很久才发现原来是起点坐标和威胁区域中心的距离只有0.3米而威胁半径是1.5米这显然不可能规划成功。所以这类问题在建模阶段就要提前检查与其让求解器返回不可行不如在输入阶段就给出明确提示。3. 非凸约束处理凸优化的三个实战套路3.1 套路一惩罚函数法把约束塞进目标函数最简单直观的处理方式是把威胁区域约束“软化”成一个惩罚项加到目标函数里。什么意思呢原来我们要求路径点必须距离威胁区域圆心超过 (r_j)现在把这个要求改成一个奖励或惩罚如果路径点太靠近威胁区域中心目标函数就增加一个很大的惩罚值迫使优化结果自动避开。用数学式子表达可以定义惩罚函数[ p_j(x_i) \max(0, \ r_j^2 - |x_i - c_j|^2)^2 ]当 (|x_i - c_j|^2 \geq r_j^2) 时惩罚为0一旦进入威胁区域内部惩罚值为正且越靠近圆心惩罚越大。然后目标函数变成[ J \sum_{i1}^{N-1} |x_i - x_{i-1}|^2 \lambda \sum_j \sum_i p_j(x_i) ](\lambda) 是惩罚权重通常设成一个较大的数。这种方法的好处是问题始终是凸的求解稳定坏处是“软约束”不能严格保证路径不碰威胁区域只能让路径“尽量远离”。如果 (\lambda) 不够大求解器可能会用轻微的越界换取更短的路径长度这在安全关键场景下是不可接受的。实战中我一般把惩罚函数法用于初值生成或者对安全性要求不高的快速路径参考而最终交给飞控执行的路径一定用下面两种硬约束处理方法之一。3.2 套路二顺序凸优化SCP用迭代逼近非凸约束这是本项目的核心亮点。思路是在当前参考轨迹附近把非凸约束线性化把每一次迭代变成求解一个凸优化子问题然后不断更新参考轨迹迭代收敛到原问题的一个可行解。具体到圆形威胁区域的约束 (|x_i - c_j|^2 \geq r_j^2)在参考轨迹点 (x_i^{ref}) 附近做一阶泰勒展开[ |x_i - c_j|^2 \approx |x_i^{ref} - c_j|^2 2(x_i^{ref} - c_j)^T (x_i - x_i^{ref}) ]令 (d x_i^{ref} - c_j)约束可以写成[ 2d^T (x_i - x_i^{ref}) |d|^2 \geq r_j^2 ]这是一个关于 (x_i) 的线性不等式完美地变成了一个凸约束。但问题是线性化只在参考点附近有效如果某个路径点离参考点很远线性化误差会很大。这时候需要引入信任域约束[ |x_i - x_i^{ref}| \leq \delta ](\delta) 是信任域半径控制每一次迭代允许路径点偏离参考轨迹的最大距离。整个SCP流程是用直线插值生成一条初始参考轨迹从起点到终点的直线碰威胁区域也没关系因为后面会迭代修正→ 在参考轨迹处构建所有威胁区域的线性化约束 → 加上信任域约束 → 求解凸优化子问题 → 把解更新为新的参考轨迹 → 重复直到路径变化小于阈值或者达到最大迭代次数。SCP不是严格意义上的凸优化但它充分“利用”了凸优化计算快的优势通过多次求解凸子问题来逼近原非凸问题效果在实操中非常可靠。这个项目用的就是这个方案每次迭代的QP问题求解通常在几十毫秒以内迭代10次也就是半秒左右的量级完全可以满足离线规划需求。3.3 套路三凸松弛把困难问题放宽第三种思路是凸松弛更适合处理组合决策类的约束。比如无人机在威胁区域之间穿行时可以选择从上方绕过也可以从左边绕过这本质上是离散选择。你可以引入二值变量 (z_{i,j}) 表示“路径点 (i) 是否从某个方向避开威胁区域 (j)”但这会引入整数规划求解难度大增。凸松弛的做法是把 (z_{i,j} \in {0,1}) 放宽成 (z_{i,j} \in [0,1])把问题转化为一个凸优化问题求解。如果松弛后的解恰好是0或1那太好不过了如果得到小数还需要通过分支定界或者舍入策略来恢复整数解。这个方法在这个项目里用得不多因为SCP已经能很好解决路径规划问题但如果你遇到的是“多个不可通行区域之间的组合选择”问题比如飞行器要在复杂工业厂房里穿行或者无人机需要在多个建筑栋之间选择绕行通道凸松弛会是很有价值的工具。3.4 三种套路如何选型一张表说清楚方法凸性是否保证硬避碰计算速度适用场景惩罚函数法始终凸否软约束最快快速初值生成不严格的安全场景顺序凸优化SCP迭代凸化是收敛后快静态威胁区域下的精确路径规划凸松弛凸/整数混合视恢复策略而定中组合绕行决策、通道选择问题真实项目里最推荐的组合拳是先用直线插值生成初始轨迹再用SCP迭代求解得到一条硬约束安全的平滑路径。如果时间非常紧张可以先用惩罚函数法跑一版快速参考路径再在这个路径的基础上只做几次SCP精修这样能大幅减少迭代次数。4. 从建模到代码CVXPY实现一个完整案例4.1 环境与依赖准备这个项目的代码是基于Python写的核心求解器是CVXPY加上ECOS或者OSQP后端。在动手之前先把环境装好pip install cvxpy numpy matplotlibCVXPY是一个非常好用的凸优化建模库你可以像写数学公式一样把优化问题写出来底层自动帮你选择求解器。前端建模后端求解这正是做路径规划验证最理想的组合。4.2 定义问题参数和解算框架假设我们的场景是二维地图起点 ((0, 0))终点 ((10, 10))三个圆形威胁区域均匀分布在中间区域路径点数量取40个。这里我把路径点数量定为 (N40)这样的粒度既不会让轨迹太粗导致切弯明显也不会让优化变量规模过大拖慢求解。核心代码框架如下import cvxpy as cp import numpy as np N 40 start np.array([0.0, 0.0]) goal np.array([10.0, 10.0]) obs [{center: np.array([5.0, 5.0]), radius: 1.2}, {center: np.array([3.0, 7.0]), radius: 1.0}, {center: np.array([7.0, 3.0]), radius: 0.8}] # 决策变量N个路径点每个点有x和y坐标 X cp.Variable((N, 2))4.3 目标函数与约束条件目标函数用相邻路径点之间的距离平方和之所以用距离平方而不是距离本身是因为平方项是凸二次函数在QP问题里非常好解而且加平方还能让路径点分布更均匀不会出现某一段特别密、某一段特别疏的情况。# 目标函数最小化路径总长度的平方等价于让路径尽量短 objective cp.Minimize(cp.sum(cp.sum((X[1:] - X[:-1])**2, axis1))) constraints [] # 固定起点和终点 constraints.append(X[0] start) constraints.append(X[N-1] goal)然后是威胁区域约束。这里使用SCP所以约束不是一次性写死的是在每次迭代里动态更新的。我先把参考轨迹初始化为一条直线# 初始化参考轨迹从起点到终点的直线插值 ref start (goal - start) * np.linspace(0, 1, N).reshape(-1, 1)接着进入迭代循环每次迭代重新构建线性化后的威胁约束并加入信任域约束max_iter 20 delta 0.5 # 信任域半径控制每次迭代允许轨迹偏离参考的最大距离 for it in range(max_iter): cons constraints.copy() # 信任域约束限制每个路径点不能偏离参考点太远 cons.append(cp.norm(X - ref, axis1) delta) # 线性化威胁区域约束 for obs_i in obs: c obs_i[center] r obs_i[radius] for i in range(N): d ref[i] - c # 在ref[i]处对 ||x - c||^2 r^2 做一阶泰勒展开 cons.append(2 * d (X[i] - ref[i]) cp.norm(d)**2 r**2) prob cp.Problem(objective, cons) prob.solve(solvercp.ECOS) if prob.status ! optimal: print(f迭代 {it}: 求解失败状态 {prob.status}) break # 更新参考轨迹 new_ref X.value change np.max(np.abs(new_ref - ref)) ref new_ref print(f迭代 {it}: 最大变化 {change:.4f}) if change 1e-4: print(迭代收敛) break4.4 结果分析和实际效果跑完这个代码你会看到迭代过程里“最大变化”逐渐减小一般5到10轮迭代就会收敛到一条稳定路径。由于信任域半径初始设成0.5每次迭代路径点不会大幅跳动路径会一步一步从直线“撑开”绕过障碍物非常直观。结束后用matplotlib把路径、威胁区域、参考轨迹画出来你就能看到一条平滑的、从起点出发绕开三个圆最终到达终点的轨迹。相比A*在栅格上走出来的带锯齿的路径凸优化结果要好看非常多基本上可以直接作为无人机飞控的参考轨迹输入。4.5 扩展到三维和动力学约束二维场景验证通过后扩展成三维只改四处地方决策变量维度从2改成3起终点坐标增加z分量威胁区域中心增加z分量威胁区域从球体替代圆。球体威胁区域的约束写法和圆形几乎一样[ |x_i - c_j|^2 \geq r_j^2 ]同样用SCP线性化完全不需要改算法框架。如果还要考虑无人机动力学约束最大速度、最大加速度、最小转弯半径就需要把路径点之间的差分成速度项和加速度项再加约束。比如速度约束 (v_i (x_{i1} - x_i) / \Delta t)要求 (|v_i|2 \leq v{max})这是一个凸二次锥约束CVXPY可以直接表达为cp.norm(X[i1] - X[i]) v_max * dt底层OSQP处理这种constraint非常熟练。5. 常见问题与排查技巧实录5.1 求解器报“INFEASIBLE”不可行这是凸优化路径规划里最常遇到的错误几乎每个跑过SCP的人都栽过。遇到不可行先别急着怀疑算法按这个顺序排查。第一检查起点终点是否在威胁区域内部。我前面提到过起点或终点进入威胁区域约束永远无法满足求解器必然报不可行。第二检查信任域半径是否太小。如果参考轨迹要绕开一个很大的威胁区域信任域半径却被设成0.1那每次迭代路径点只能挪动很小距离而当前路径还在威胁区域内部线性化约束本身就不满足子问题自然无解。这时候需要把信任域半径调大到0.5甚至1.0。第三检查线性化约束是否真的在参考点处构成可行方向。如果参考轨迹刚好穿过威胁区域的正中央而线性化只在圆的切线方向排斥路径点有时候路径点会被挤到两个威胁区域的夹缝里动弹不得需要给信任域更大的空间。调试技巧在抛出INFEASIBLE时把当前迭代的参考轨迹画出来叠加威胁区域和信任域范围肉眼看一眼就知道约束卡在哪个路径点上了。5.2 路径抖动/锯齿状严重如果跑出来的路径不是平滑的弧线而是锯齿状多半是目标函数的问题。目标函数如果只包含相邻路径点的一阶差分速度项路径允许有突然的拐弯反映在轨迹上就是折线很重。解决办法是在目标函数里加入二阶差分项也就是加速度项objective cp.Minimize( cp.sum(cp.sum((X[1:] - X[:-1])**2, axis1)) 10.0 * cp.sum(cp.sum((X[2:] - 2*X[1:-1] X[:-2])**2, axis1)) )二阶差分项会惩罚路径点之间的加加速度让曲线变得更加平滑。权重10.0可以调但别太大太大了路径会过度平滑导致切弯半径变大反而离威胁区域更近。5.3 实时性不够如何优化如果要把这套算法嵌入到无人机的机载电脑里做实时重规划40个路径点、20次迭代是绝对不可接受的单次求解时长会高出一个量级。我的优化经验有三个方向。第一减少决策变量维度。40个路径点可以降到15到20个路径的细节靠后面加上样条插值来补齐路径点少一个数量级求解时间能减少一个数量级。第二换求解器。CVXPY默认的ECOS是内点法精度高但速度慢换成OSQP或者专门为嵌入式设备设计的求解器QP问题求解时间能压到几毫秒。第三热启动。每次迭代把上一次的解法作为初始猜测传给求解器能明显减少内部迭代次数。CVXPY里可以用problem.solve(warm_startTrue)开启热启动。5.4 动态威胁区域或者完全非凸的威胁区域怎么处理这个项目本身处理的是静态威胁区域但真实飞行中威胁区域往往是动态的比如另一架无人机、飞鸟、突然出现的航拍飞行器。做法是把SCP和滚动时域控制结合起来每个控制周期只优化未来一小段时域比如5秒内的路径当检测到新障碍时在当前最优解的基础上重新运行SCP。因为SCP每次迭代只需要几十毫秒完全能满足动态避障的实时性要求。至于完全非凸的威胁区域比如U形障碍物、两个圆并联形成的哑铃形区域直接凸化会失真。实用做法是先对威胁区域做凸分解把它拆成多个凸子区域再用逻辑约束穿过的子区域数量、方向组织起来配合3.3节说的凸松弛技巧求解。这个方向展开讲又是一个长文但在工程上核心思想依然是尽量把问题拆成一个一个可以凸化的小块再用迭代和松弛把它们缝合起来。5.5 权重参数怎么调一个实际经验值目标函数里通常有路径长度权重、平滑度权重约束里有惩罚权重信任域半径也属于广义的“权重”。我的经验是先固定长度权重为1然后从小到大调平滑权重每次翻倍观察路径形状。惩罚权重(\lambda)如果用于软约束场景一般从100起步太大会导致目标函数数值量级差距过大影响求解器数值稳定性太小则约束形同虚设。信任域半径(\delta)的调节逻辑是先设大一点比如0.5到1.0让路径有足够空间大幅变形逃离威胁区域当路径变形的幅度变小之后把信任域缩小到0.1到0.2做精细化收敛。这个“先大后小”的动态信任域策略比固定信任域收敛速度更快路径质量也更好。在真实无人机项目里跑过这套方案的很多同事最后都会得出一个共同的感受凸优化路径规划的核心不是“用求解器算”而是“怎么把问题建好”。威胁区域怎么表达、约束怎么线性化、信任域怎么调、目标函数怎么组合这些每一步都直接影响最终路径的可用性。当前这个项目用SCP解决的静态威胁区域问题是一个基础但非常实用的样本把代码跑通、把参数调明白之后向三维扩展、向动态场景扩展路径就顺了。本文还有配套的精品资源点击获取