做无人机的运动规划最绕不开的其实是两件事路径怎么找轨迹怎么走。路径好办A*、RRT、JPS随便选一个都能给你一条不撞墙的折线但麻烦的是飞机不能真的飞折线你总得把这条折线变成一条平滑、连续可飞、而且全程保证安全的轨迹。这时候“飞行走廊”这套思路就派上用场了——先把你搜到的那条路“撑”成一条由凸空间串起来的通道再在通道里做轨迹优化。这篇文章就围绕“基于飞行走廊的轨迹优化”展开聊聊我是怎么理解这件事的以及在工程项目里踩过哪些坑。如果你正在做无人机、移动机器人或者机械臂的路径规划或者正在读相关论文想找个能落地的方案这篇东西应该能帮上忙。我不会只贴公式会把每一步背后的“为什么”也讲清楚比如为什么走廊一定得是凸的、时间分配到底影响什么、轨迹优化时为什么要反复调采样点密度。这些细节论文里一般不写但实际跑起来全是坑。1. 飞行走廊把路径从“一条线”升级成“一块空间”1.1 为什么不能用路径点直接做平滑很多人第一次做轨迹优化想法很直接A找到一条路径我把它丢给样条插值或者B样条平滑一下不就行了理论上能跑但实际上你很快会发现一个致命问题——A只保证路径点本身不在障碍物里路径点之间的连线完全可能穿墙而过。尤其在栅格地图里路径点是离散的两个相邻栅格中心之间那条线穿过的邻居栅格有时候是障碍物但你根本不知道。飞行走廊想解决的就是这个事。它不只是拿一条路径而是把路径周围的自由空间“挖”出来变成一个由凸多面体组成的连续通道。只要轨迹全程落在这个通道里那它必然不会碰撞任何障碍物。本质上是把“离散的安全点”升级成了“连续的安全区域”。有了走廊之后你的轨迹优化就不再需要对障碍物做逐点判断了只需要把轨迹约束在凸空间内部就行这个性质后面会反复用到。1.2 走廊的数学表达凸多面体为什么是首选飞行走廊的数学形式通常是若干个凸多面体Convex Polyhedron用线性不等式表示A_i x ≤ b_i其中 A_i 是矩阵b_i 是向量整组不等式描述的是三维空间里的一个有界凸区域。走廊就是若干个这样的多面体首尾相接形成一个空间管道。为什么要强调“凸”因为凸空间有一个极其友好的性质在凸集内部任意两点连线仍然在这个集合内。换句话说只要轨迹的每段曲线都落在对应的凸多面体里那么这段轨迹的任何一个中间点都不可能撞到障碍物。这个性质在做优化时特别重要。如果走廊是非凸的轨迹优化问题就变成了非凸问题求解器很容易陷入局部最优而且你要额外处理一堆数值稳定性问题。凸多面体配合多项式轨迹很多条件可以直接转换成线性约束或者用梯度算出来跑起来又快又稳。我自己的体会是轨迹优化本身不复杂真正决定成败的往往是前面的“空间构造”——走廊建得好不好。1.3 走廊是怎么生成的从A*路径到凸空间走廊的生成流程一般分三步。第一步是前端搜索用A*之类的方法得到一条可行路径第二步是沿着路径点做空间膨胀得到一个个凸多面体第三步是处理相邻凸多面体之间的重叠关系保证整条走廊连续无断裂。具体的膨胀算法很多最常用的是基于八叉树地图的迭代膨胀法。大致思路是从路径节点对应的那个栅格出发向六个方向尝试扩张只要扩张后仍然是自由空间就继续扩碰到障碍物就停。这样做出来的东西其实是一个个长方体虽然简单但已经能用。你还可以做得更讲究一点比如对每个节点做“最大内接凸多面体”的搜索让走廊形状更贴合真实空间。不过这里要注意越复杂的生成算法计算代价越高前端一个节点可能要算几十毫秒全局耗时不一定划算。还有一个工程上的细节相邻走廊块之间必须有足够的重叠区。因为轨迹是分段的一段轨迹落在走廊 A下一段轨迹落在走廊 B如果两个走廊之间没有交集那轨迹在切换段的时候就必须插进一个“缝隙”这很容易导致穿出走廊。我见过很多新手忽略了这个问题生成了好几块互不相交的凸包优化完了轨迹在中间凹进去撞到障碍物都查不出原因。2. 两种主流走廊生成策略代价与效果怎么平衡2.1 立方体膨胀几十行代码就能跑通立方体膨胀是最容易实现的方案我最初就是拿它练手的。思路是把路径经过的每个节点在地图上做一个“向四周长胖”的操作。具体到八叉树地图里你只需要不断尝试把周围的 free 节点并入这个立方体直到碰到 occupied 或者 unknown 节点停止。优点非常明显实现简单、鲁棒性高、几何判定快。因为每个走廊段都是轴对齐的长方体后面做轨迹优化时约束可以拆成三个坐标轴独立处理各种计算都省事。缺点是空间利用率不高尤其在地形狭窄的地方一个长方体可能装不下太多自由空间这就导致需要很多个长方体段才能覆盖整条路径走廊段数上去了轨迹优化的变量也变多了。我自己在室内环境跑的时候立方体膨胀表现挺好因为房间结构规整自由空间本来就可以用方块近似。但在仿真的树林环境里就暴露问题了树木之间的空隙是倾斜的轴对齐的长方体根本没法高效覆盖一段路径可能要生成十几个小方块优化效果也一般。2.2 凸多面体裁剪更贴合空间但更复杂如果你想追求更紧凑的走廊形态可以用增量式裁剪法。它的做法是先假定一个足够大的种子多面体然后不断拿障碍物信息去“裁剪”它最后剩下的就是一块贴合实际自由空间的凸多面体。这个过程有点像雕刻先给你一块大石头然后根据障碍物的位置把多余的部分凿掉最后得到一块不规则的、但与障碍物严格分离的凸块。这种方法的优势是段数少、贴合度高尤其适用于点云或mesh地图这种障碍物形态不规则的场景。但代价也不小你要实现一个可靠的多面体裁剪算法还要处理各种退化情况比如种子多面体被裁剪到零体积、或者几个障碍物夹出来的凸包非常“瘦”导致后续优化困难。更重要的是多面体的表达不再轴对齐了轨迹优化时约束判断要处理 A_i x ≤ b_i 这种一般形式代码复杂度明显上升。我的建议是地图结构规整、项目周期紧的时候直接用立方体膨胀地形复杂、希望轨迹质量更高的时候再上凸多面体裁剪。工程上没必要一开始就追求花哨先把整个闭环跑通再慢慢升级某一个模块。2.3 走廊连续性检查很多人忽略的一个环节生成完走廊序列之后我一定会做一次连续性检测。方法是两两检查相邻凸多面体是否有交集。如果是长方体这个检查就很直观三个坐标方向上只要每个方向上两个区间都有重叠那这两个长方体就有交集。如果是凸多面体一般形式可以用线性规划或者采样方法来检测。这一步特别值得做。因为很多时候你生成走廊的时候是“各自膨胀”的两个相邻走廊块之间可能存在一个小缺口虽然视觉上看不出来但优化器会感知到——轨迹如果想从一块穿到另一块就必须经过没被覆盖的区域。查出来之后怎么修最简单的办法是稍微放大两个多面体让它们强制重叠复杂一点可以在两个中心点之间补一个中间多面体。实际操作中我更喜欢在生成阶段就要求“必须在原路径节点之间至少保持一段重叠”从源头掐掉缺口。3. 轨迹优化怎么把“安全”写进目标函数3.1 轨迹表示多项式拼接与MINCO的思想有了走廊接下来就是轨迹优化。这一步要做的是生成一条平滑、动态可行、并且全程落在走廊内部的轨迹。工程上最常用的轨迹表示是分段多项式把整个轨迹按时间切成若干段每段都是一个多项式函数段与段之间保证位置、速度、加速度连续。为什么要分段因为如果只用一条多项式从头到尾一方面表达自由度不够复杂路径拟合不了另一方面约束条件耦合在一起解起来也慢。MINCOMinimum Control Effort这类方法的厉害之处在于它用一组边界条件把多项式系数和每段时间映射成控制点优化变量可以直接变成“中间点的位置”和“每段时间”。这样一来段间连续性约束自动满足优化器的自由度大大降低。我试过自己手工构造边界约束来解多项式系数说实话很繁琐后来切到MINCO的思路上整个人都轻松了。你可以这样理解你只需要告诉优化器“轨迹经过哪些点、每段大概耗时多少”它就能帮你算出一条光滑的多项式轨迹而你只需要负责调整这些中间点和时间。3.2 走廊约束的两种加法硬约束和惩罚项走廊约束怎么进优化器是新手最容易迷糊的地方。最简单的做法是把“轨迹必须落在凸多面体内部”写成一组线性不等式约束直接丢给求解器这就是硬约束。但这里有个理论上的坑轨迹是一条连续曲线不是离散的几个点你不可能把曲线上的每个点都变成约束。实际操作中有两种解法。一种是采样法把每段轨迹分成很多小时间区间在每个采样点处检查是否满足 A_i x ≤ b_i。这种方法实现简单但需要采得足够密才能保证安全采得越多优化规模越大。另一种是极值法算多项式在某段区间上的最大值只要最大值仍满足不等式那整段轨迹必然满足。极值法理论更完备但计算复杂度高在三维空间里处理起来麻烦得多。如果你不想做硬约束也可以把走廊约束转成目标函数里的惩罚项——计算轨迹点到多面体边界的距离超出边界就施加一个大的惩罚。惩罚项的好处是优化问题变成了无约束问题可以用更简单的优化器坏处是权重不好调太小会穿墙太大会让轨迹缩在里面看起来“窝囊”。我在实际工程里通常先试硬约束遇到求解器数值困难的时候再退回到惩罚项方案。3.3 时间分配常被低估的关键因素很多人在轨迹优化时只关注平滑性、避障却忽略了时间分配导致最终的轨迹要么飞得太快、要么优化失败。时间分配的目标是给每一段轨迹一个合理的飞行时长。如果时间太短速度、加速度会爆炸如果时间太长轨迹会拖拖拉拉效率低下。工程上常用的初值估计方法是把每段的路径长度除以期望巡航速度得到每段的基础时间然后乘以一个系数留出余量。比如期望速度是 3m/s某段路径长度为 1.2m那初始时间可以设为 0.4 秒乘以 1.1 的裕量系数。后续在优化中时间本身也可以作为变量去优化但要注意这是一个高度非凸的问题很容易收敛到不合理的解。我个人的做法是分两步先固定时间做位置优化再用一维搜索去调整每段时间往复几次。这样做虽然不会得到全局最优但胜在稳定可靠。3.4 目标函数怎么设计平滑性、动态可行性与安全目标函数的设计直接决定轨迹的质量。我一般至少会加三项平滑性惩罚、动态可行性惩罚、走廊安全惩罚。平滑性惩罚最常用的是最小化加速度或加加速度jerk的平方积分这能让轨迹不抖动动态可行性惩罚是让轨迹的速度和加速度保持在执行器的物理极限内比如无人机最大速度、最大推力走廊安全惩罚则是前面说的硬约束或惩罚项的来源。这三项放在一起权重怎么定也是有讲究的。平滑性权重太高轨迹会“绕远路”明明可以穿过去的空间结果拐了一个大弯动态可行性权重太高轨迹会过度保守飞起来像老太太散步安全惩罚权重太高则会让轨迹尽量缩在走廊中心虽然安全但不够灵活。我习惯用一套自动调权重的脚本——扫描几个典型场景看轨迹的最大速度和最大加速度再回推权重是否合适。你要是嫌麻烦可以从单位量级入手让各项的数值量级接近效果通常会好很多。4. 实操过程与调参实录4.1 一个最小化实现环境、代码与数据流我拿一个具体例子来说说完整流程。假设我们在一个 2D 简化环境里做实验栅格地图障碍物已经标好。第一步用 A* 从起点搜到终点得到一串路径点。第二步把每个路径点周围膨胀成长方形走廊块生成一个 corridor 列表。第三步用 python 的 scipy.optimize.minimize 做轨迹优化用分段多项式表示轨迹走廊约束直接用采样点方式加进约束。核心伪代码大概是这样的def objective(traj_coeffs): # 计算平滑性惩罚 # 计算动态可行性惩罚 return cost def constraint_in_corridor(traj_coeffs): # 在每段轨迹里采样 N 个点 # 检查每个点是否在对应走廊块内 return violation_value # 用 SLSQP 求解带约束的优化问题 result minimize(objective, init_coeffs, constraints{type: ineq, fun: constraint_in_corridor}, methodSLSQP)代码里最需要留意的是 constraint_in_corridor 的返回值。我一开始犯的错误是把“是否在走廊内”写成 0/1 的判断函数结果梯度完全失效优化器根本没法迭代。后来改成返回“到走廊边界的带符号距离”比如在走廊内部返回正值越靠近边界值越小出了边界变成负值。这样优化器才能正常用梯度信息。4.2 调参的一些经验膨胀尺寸、采样密度、时间初值在跑通第一个版本之后调参就成了主要工作。首先是走廊的膨胀尺寸。膨胀太大走廊可能覆盖到障碍物边缘虽然优化不一定会撞上但安全裕度明显变小膨胀太小走廊太窄轨迹优化器没有发挥空间轨迹会变得很僵硬。我一般把膨胀系数设为机器人半径的 1.2 到 1.5 倍既保留安全余量又不至于太过保守。然后是采样密度。采样点太少轨迹中间可能偷偷穿出走廊采样点太多优化器会很慢。我试过 2D 环境下每段采样 10 个点已经够了但换到 3D、或者走廊形状特别怪异的时候需要采 20~30 个点。你可以在优化前方块内做一次快速预检如果轨迹离边界已经比较近了就加密采样否则保持默认值。时间初值也很重要。我经常看到有人拿单位时间 1 秒分配给所有段结果优化出来的轨迹要么一会快一会慢要么速度上限被击穿。一个好做法是先计算整条路径总长度除以期望速度得到总时间再按各段长度比例分配。这样每段时间的初始值大致合理优化更容易收敛。4.3 从折线到平滑轨迹优化前后的对比优化之前轨迹是一条条折线转折处速度方向突变无人机根本没法跟踪。优化之后轨迹变成了平滑曲线中间点被“柔化”速度和加速度都连续。我印象最深的一次对比是刚开始用纯 A* 路径直接飞飞机在拐角处疯狂抖动PID 控制器都跟不住后来用了走廊约束的轨迹优化同样一段路最大加速度降了 40%跟踪误差明显减小飞起来像换了一台机器。这个对比说明一个道理路径规划解决的是“有没有路”的问题轨迹优化解决的是“走不走得稳”的问题。飞行走廊的价值就在于把两个问题清晰地分开——你不需要在前端搜索时就考虑平滑性只需要在后端优化时确保轨迹约束在安全空间里。思路清晰了代码结构也好设计。4.4 最容易踩的坑轨迹穿出走廊我排过的 bug 里最多的就是“明明加了走廊约束轨迹还是跑出去了”。排查下来原因往往不是走廊生成错了而是约束加得不够。第一种情况是采样间隔太大轨迹在两个采样点之间跑出去了。解决办法是加密采样或者换成极值约束。第二种情况是走廊块之间衔接处有缝隙前面已经提到过需要检查相邻凸包的 overlap。第三种情况是约束写错了符号——把 A_i x ≤ b_i 写成了 A_i x ≥ b_i那优化器自然会把轨迹往走廊外推。这种错误很隐蔽因为代码不报错只有可视化轨迹时才能发现。我的建议是无论如何都要加一个可视化步骤把走廊、路径、最终轨迹画在同一张图里眼睛扫一眼往往比代码调试快得多。5. 常见问题与排查技巧实录5.1 常见问题速查表为了让你省事我把实际项目中遇到频率最高的问题整理成一张表遇到故障可以直接对照排查故障现象可能原因排查方向优化后轨迹穿出走廊采样点太少或约束符号写反加密采样点检查约束不等式方向优化器不收敛cost 震荡时间初值不合理惩罚权重过大调整时间分配降低惩罚项权重轨迹过于保守绕远路平滑性权重过高走廊太窄降低平滑权重增大走廊膨胀系数相邻走廊块有缝隙轨迹在块间凹进去走廊生成时没有检查重叠区检查相邻凸包交集补建中间走廊块优化结果时快时慢速度曲线不连续没有加动态可行性约束在目标函数中加入速度/加速度限制项5.2 调试技巧可视化 分模块验证遇到问题不要一上来就怀疑优化器。我的经验是先把各模块拆开验证。第一步单独跑走廊生成输出走廊可视化确认走廊本身没有缝、没有穿墙第二步不接走廊约束先只做平滑性优化看轨迹是否光滑这一步基本能暴露参数初值的问题第三步再加上走廊约束如果此时轨迹出问题那就是约束相关的 bug。可视化工具方面2D 可以画 matplotlib3D 可以用 RViz 或者 open3d。我特别建议在图上同时画出 A* 路径点、走廊边界、优化后轨迹、障碍物边界四层叠在一起看。多数问题一眼就能定位——是走廊破了还是轨迹飞了还是约束方向反了。这个方法帮我省了大量时间。5.3 一个容易被忽视的细节坐标系与地图分辨率最后说一个隐蔽的坑坐标系和地图分辨率的匹配。有的项目地图坐标系是左上角原点有的可能是中心点八叉树地图的分辨率也各不相同。如果你把路径点塞进走廊生成器时没有做坐标变换或者在约束函数里用错了缩放系数那整个流程的行为都会很诡异——比如走廊永远生成不对、轨迹明明在走廊内却说穿墙了。我吃过一次亏把一份从仿真器导出的点云地图直接拿来生成走廊结果点云的单位是毫米路径规划用的单位是米膨胀算法跑出来的走廊看起来巨大无比。调了半天才发现是单位问题。所以动手之前先检查一遍各模块的数据单位、坐标系定义统一好再往下走。6. 个人体会与下一步扩展做完整套流程之后我最大的感受是路径搜索其实已经不是瓶颈真正花时间的是“空间表示”和“约束建模”。飞行走廊的价值在于它把复杂的碰撞规避问题转化成了相对简单的凸约束问题让轨迹优化既快又稳。当然飞行走廊也不是万能的——如果环境中有动态障碍物走廊没法预先建好需要引入额外的局部规划如果走廊生成得太碎优化规模也会剧增这时候你可能要考虑用平面分割或者其他更高级的凸分解方法。最后再分享一个小技巧在你做走廊生成的时候不要把每个路径点孤立地膨胀可以尝试“窗口式”地一次处理三四个点把它们的自由空间合并成一个大块。这样做出来的走廊段数更少轨迹优化的负担也更轻。我在几个测试场景里试过走廊数量能少 30% 左右优化耗时明显下降。你可以先跑通基础版本再试试这个优化应该会有惊喜。
