1. 先搞清楚一个问题自由空间建模为什么是轨迹优化的关键做过无人机自主导航或者移动机器人规划的朋友应该都有这种感觉轨迹优化本身并不难难的是“把环境约束表达成优化器能理解的数学形式”。早年我刚开始接触运动规划算法时踩过一个大坑三维栅格地图直接拿来当碰撞约束结果优化器要么不收敛要么轨迹贴墙飞要么计算量大到飞控顶不住。后来认真研究了一圈发现业界主流方案里非常实用的一个思路就是“基于飞行走廊的轨迹优化”。飞行走廊这个概念本质上是在说——与其告诉优化器“整个地图哪些地方是障碍物”不如直接告诉它“哪一片区域是你随便飞的”。这个区域被构造成一连串凸多面体或者长方体串成一条通道无人机沿着这条通道飞任何落在通道里的轨迹点都是安全的。这样一来非凸的碰撞避免问题就被转化成了几个简单的不等式约束优化难度直接下降一个量级。它是怎么做到的呢其实思路很朴素。我们知道无人机轨迹通常做成分段多项式优化目标是让轨迹平滑、动力学可行、时间最优、同时不撞障碍物。“不撞障碍物”这个约束在栅格地图上是高度非凸的而现代优化库处理非凸约束的手段要么是随机采样、要么是凸近似效率都很低。飞行走廊的思路就是把可飞空间先凸分解让这段轨迹落在多个凸区域的并集里。每个凸区域能被写成一组线性不等式而线性不等式约束在许多优化框架里是可以高效求解的。有个形象的类比假设你在商场里要从A点走到B点最常见的办法是服务员告诉你“前方直行第三个路口右转再走五十米”。这就是走廊式导航路径是一条被限定的通道。另一种办法是甩给你一张CAD图纸让你自己算哪块地砖能踩、哪块不能。飞行走廊就是前一种思路——先把人能走的路画出来再让轨迹规划算法在这个清晰的通道里发挥优化能力。这篇文章适合正在做无人机/机器人运动规划相关课题的同学也适合那些对轨迹优化熟悉、但对“如何系统处理环境约束”感到困惑的从业者。我会从为什么需要飞行走廊、走廊是怎么生成的、路由约束如何写进优化器、到调参和实机踩坑完整拆一遍这条技术链路。2. 为什么ESDF不是万能的飞行走廊到底解决了什么2.1 从一张占据栅格地图说起常见的做法就是在占据栅格地图里搜索一条无碰撞路径然后在路径附近构建一个欧氏距离场ESDFEuclidean Signed Distance Field让优化器查询每个轨迹点到最近障碍物的距离并把这个距离作为约束或者惩罚项。这套方案在室内的结构环境下工作得很好。因为墙面规整、地形平坦ESDF的梯度方向稳定优化器比较容易收敛。但我自己在一栋旧厂房里试过一次里面堆满了形状不规则的料架、杂物还有悬在半空中的吊臂ESDF算出来的梯度方向极其复杂轨迹时常被“夹”在几根柱子之间出不来来调整权重比例也很难找到满意解。搞运动规划的人心里都有数ESDF方案有几个绕不开的痛点计算开销每次重规划都要做一遍距离场更新环境越大越慢。动态环境下距离场还要同步处理新障碍物频率稍快就容易丢帧。梯度陷阱优化器本质上在做梯度下降ESDF的梯度在狭窄通道和障碍物密集区域会变得很“尖”收敛结果经常卡在局部最优。阈值敏感约束距离设得太小轨迹贴着障碍物走设得太大狭窄区域直接被判为不可通行无人机只能绕远路。2.2 走廊把非凸问题变成凸问题的逻辑飞行走廊方案的底层逻辑是把“全局非凸的安全条件”转换为“局部凸的安全集合”。具体到数学上假设走廊由K个凸多面体组成每个凸多面体可写成P_i { p ∈ R³ | A_i p ≤ b_i }这里的A_i是一个m×3矩阵b_i是m维向量m表示这个凸多面体有多少个面。无人机轨迹上的任意一个采样点如果满足A_i p ≤ b_i这个不等式组就说明这个点在走廊的安全区域内。这时候轨迹优化的问题就变成了一条被分成M段的多项式轨迹每一段被分配到某个凸多面体内部然后要求整条轨迹的时间分配、平滑性、动力学可行性都在这个框架中同时优化。之所以这样做有效是因为凸约束的优化问题通常有全局最优解而且求解速度快。你不需要再担心“梯度被局部障碍物卡住”因为走廊已经帮你把可飞区域圈好了优化器只需要在这个圈内找到满足平滑和动力学要求的曲线。本质上这是一种“以保守性换鲁棒性”的思路——让出一部分机动空间换来更高的求解成功率。2.3 和硬约束罚函数方法放在一起看更清晰我不止一次被人问到直接加一个惩罚项比如对每个轨迹点到障碍物的距离求倒数不也能逼着轨迹远离障碍物吗为什么不直接用这个方法还要费力先构建一条走廊这个问题的答案我在一次仿真对比中看得特别清楚。用单纯的罚函数法规划的轨迹总是“试图从两个距离都很远的障碍物之间的中点穿过”因为这样惩罚项最小。但实际环境中障碍物的形状是不规则的中点可能只是局部最安全的点并不是真正可飞的通道。真机上飞这种轨迹轻则绕路绕到电池耗尽重则直接撞进一堆稀疏点云的间隙里。飞行走廊相当于给轨迹加了一条“通行证”式的限制——你不用再自己判断哪些地方能飞走廊已经把所有可飞空间从几何上确定下来了。罚函数法在凹形障碍物密度较高的场景中往往需要反复调节权重、不得已增加迭代次数才能跑出一条勉强能用的轨迹。而走廊方法因为约束形式简单优化的鲁棒性明显更好。3. 从路径到走廊完整的构建流水线3.1 先找一个粗糙路径作为种子飞行走廊的起点通常是一条无碰撞的粗糙路径。你可以用A*、JPS、RRT*这类搜索算法在占据栅格地图或者点云地图上找出一条从起点到终点的通路。这条路径不需要平滑甚至不需要动力学可行它的作用只是告诉走廊生成算法“大概往哪个方向走”。我自己在实践中最常用的组合是在二维占据栅格上做JPS搜索然后沿路径的每个关键点做垂直方向的膨胀形成一个“空中走廊”的初始候选区域。JPS相比A*在大地图上的节点数少很多而且搜索出的路径拐点少更容易生成形状规整的走廊块。3.2 区域生长算法让走廊块长出来拿到一条粗糙路径之后最直接的做法是沿路径点做区域生长。这个过程的伪代码可以写成这样输入路径点集合 P[0..N]地图 occupancy_map扩展步长 step 输出走廊块列表 corridors for each path point p_i: cube 以 p_i 为中心、初始边长 s 的立方体 while true: cube 在 x/y/z 正负方向尝试扩张一步 step 检查扩张后的立方体是否与障碍物相交 若不相交则更新 cube继续扩张 若任一方向相交则停止该方向的扩张 当六个方向都无法扩张时退出循环cube 加入 corridors这段逻辑其实很简单一个立方体像吹气球一样往外扩碰到障碍物就停下来直到六个方向都顶到障碍物为止。所有路径点都执行一遍之后你会得到一串相互嵌套或重叠的立方体它们的并集就是一条连通的可飞通道。写代码的时候有一个细节特别容易忽略——相邻走廊块的连通性检查。如果两个相邻立方体之间没有重叠区那么轨迹从上一段过渡到下一段时插值点很有可能跑出走廊。因此生成的每个新立方体都要和上一块做交集判断只要某个方向的扩张导致两块区域完全失去了重叠就该退一步保证至少有1~2个栅格宽度的交叠。3.3 凸分解的两种路线立方体链还是多面体上面描述的区域生长法生成的是一个个独立的立方体这在大地图中很常用因为立方体相交判断简单、内存占用小。但在一些弯曲复杂的通道中立方体链会有明显的缺点转弯处的覆盖冗余大可飞空间利用率低轨迹被限制在较死板的直角通道里。这时候多面体走廊的优势就出来了。常见做法是在离散的候选点周围做泰森图Voronoi分析提取广义泰森走廊GVD corridor或者用快速凸分解算法如HACD改进版把自由空间划分成若干个凸多面体。多面体走廊对弯曲通道的贴合度要好很多但构建算法复杂度也高不适合在地图上高频更新。我自己的经验是分场景选择场景类型推荐方案原因室内结构化环境、走廊多立方体链快速、稳定、内存小室外复杂地形、树林或废墟多面体走廊可飞空间利用率高轨迹更顺滑动态障碍物频繁出现立方体链局部刷新构建算法实时性好便于局部更新3.4 膨胀系数与安全裕量的博弈走廊构建中还有一个必须面对的权衡膨胀系数。膨胀得太狠走廊块可能会被挖穿障碍物稀疏区域的栅格造成安全判断失效膨胀得不够轨迹只能在狭窄通道里憋屈地飞速度提不起来。我在调试时习惯将膨胀系数设置为无人机半径的1.5倍起步后续根据实机表现逐步缩小到1.2倍左右。注意这只是一个起始参考真实项目中还要考虑定位误差和动态障碍物的位置噪声。比如在室外强风环境中飞行无人机实际位置与估计位置偏差可能达到0.3米那走廊的安全余量就必须留够0.5米以上否则高速飞行时很容易擦到墙边。这个安全裕量不仅影响走廊生成的几何尺寸还直接影响后续优化器对轨迹“贴近走廊边界”行为的容忍度。轨迹优化倾向于沿着走廊中心或者边界飞具体取决于约束权重。如果安全裕量太小优化器会认为贴着边界飞行也安全但实际上真机一个波动就出去了。所以我的建议是在内层走廊之外再自动向内收缩一层安全余量形成“优化走廊”而不是直接把生成走廊交给优化器。4. 把走廊写进轨迹优化从轨迹表示到约束建模4.1 轨迹表示选型分段多项式与MINCO走廊构建完成下面就是重头戏——轨迹优化。轨迹的表示方式直接决定约束和求解效率这一段可能是新手最容易困惑的地方。常用的轨迹表示有分段多项式Piecewise Polynomial和MINCOMinimum Control Effort轨迹类。分段多项式的思路是把整条轨迹按时间分成多段每段是一个多项式函数相邻段之间保证一定阶次的连续性通常是位置、速度、加速度连续。MINCO是在分段多项式基础上进一步优化了参数化方式直接把轨迹系数重参数化为“端点的位置和中间节点的位置 各段时间”。这种表示的好处是空间维度和时间维度可以联合优化求解时用无约束优化即可适合嵌入式平台实时运行。许多现代开源规划框架比如一些知名自主导航项目的局部规划模块都采用了类似MINCO的设计思路。我自己在实际项目里更倾向于用带贝塞尔曲线形式的分段多项式。贝塞尔曲线有一个非常漂亮的特性整条曲线完全落在其控制点的凸包内。这意味着只要控制点满足某组线性不等式那么曲线上任意一点都自动满足这组不等式。用这个性质来处理飞行走廊约束简直不要太爽——你只需要约束控制点落在走廊块内整条轨迹就绝对安全。4.2 距离约束如何变成可微的惩罚项现在关键来了怎么让优化器知道轨迹要待在走廊里理想情况下应该是硬约束所有控制点满足不等式 A_i p ≤ b_i。但在实际求解器里硬约束往往导致可行域退化或者初值敏感因此工程上更常用的是软约束设计——把碰出走廊的程度转化成一个可微的惩罚函数。一个经典的做法是用分段二次函数。对每一个控制点p_j定义它相对走廊不等式组A_i p ≤ b_i的“违反量”d_jd_j max(0, A_i p_j - b_i)然后把这个违反量积分到代价函数中。为了让梯度平滑实际的实现通常会用一个近似函数比如对每个不等式维度做软阈值的平方和L_collision Σ_j Σ_k w_k · smooth_max(0, (A_i p_j)_k - (b_i)_k)这里的smooth_max可以用一个小的ε参数来做平滑截断当输入小于0时输出一个极小值大于0时输出二次增长。这样整个代价函数处处可微优化器可以用梯度类方法直接求解。我当时第一次实现这个惩罚项时犯了一个很典型的错误只对轨迹采样点做约束没有对控制点做约束。结果就是两个采样点之间的轨迹段可以大幅度地穿出走廊优化器还认为它很安全。解决办法是把控制点也加入约束列表——这也是贝塞尔形式天生的优势直接用凸包特性把采样点约束扩展到控制点约束问题就解决了。4.3 时间分配与动力学可行性走廊约束只是轨迹优化的一半剩下的核心目标是让轨迹真正“飞得起来”。这里涉及三个子目标平滑性最小化加加速度或加加加速度jerk或snap的平方积分保证无人机飞行姿态变化不突兀。时间最优在安全的前提下总飞行时间尽量短但时间太短会导致速度和加速度超出电机能力。动力学可行性每段轨迹的速度、加速度幅值必须约束在无人机硬件允许范围内。这些约束同样可以转化为对多项式系数的约束。比如对贝塞尔形式轨迹控制点与速度、加速度之间有线性的映射关系所以给每个控制点的速度上限和加速度上限加上不等式约束即可。在时间分配上初始解法是均匀分配然后根据每段轨迹的长度和速度限制做一次迭代更新。这里有个人经验不要把时间分配完全交给优化器盲搜先用启发式初始化——每段时长和该段几何长度成正比再让优化器微调收敛速度会快很多。4.4 一个可参考的代价函数结构把上面所有项整合起来优化目标大致可以写成J_total λ_smooth · J_smooth λ_time · J_time λ_collision · L_collision λ_dyn · L_dynJ_smooth是jerk或snap积分项J_time是总时间或每段时间平方项L_collision是走廊违反惩罚L_dyn是速度/加速度超限惩罚权重λ的选择我的经验是先调λ_smooth和λ_time把轨迹形状调到基本合理再加上L_collision从小到大试最后再引入L_dyn用来限制速度波动。一口气把所有权重同时放开几乎是自找麻烦因为各惩罚项的量纲不同梯度尺度差异巨大优化器很难平衡。5. 真机调试心得一组经过验证的参数与常见问题5.1 一组可以当起点的参数以下是我在某型四旋翼上调试通过的参数范围可以作为初始参考具体还要结合飞机的质量和动力特性调整参数参考值说明走廊安全余量0.3 ~ 0.5 m无人机半径1.5~2倍视定位精度调整轨迹段数/时间3 ~ 6段段数越多越灵活但求解耗时增加jerk权重 λ_smooth10 ~ 30过大会导致轨迹绕大弯时间权重 λ_time1 ~ 5过大会导致速度剖面尖锐碰撞惩罚权重 λ_collision50 ~ 200从小往大调找到边界值优化最大迭代50 ~ 100次实机上建议限制在50次内表格里的权重和经济指标没有绝对关系关键是理解调整方向想让轨迹更安全就加大λ_collision并加厚安全余量想让飞行更顺畅、能耗更低就增大λ_smooth想让无人机“敢冲”就适度压缩时间分配同时保证动力学约束紧跟其后。5.2 我踩过的一个大坑轨迹贴走廊边界走有个现象非常典型优化出来的轨迹在走廊内部但几乎是贴着走廊壁走的虽然理论上安全真机上跑起来却心惊胆战。原因在于碰撞惩罚项是二次的靠近走廊边界时违反量很小优化器认为这个位置惩罚可接受于是轨迹被拉向边界以换取更短的路径和更少的平滑代价。解决这个问题有两个办法第一构建走廊时向内收缩安全余量让“优化走廊”比“真实可飞走廊”小一圈这样即使轨迹贴着优化走廊边界飞离真实障碍物也还有物理余量第二在碰撞惩罚项里引入一个二次下界只要控制点靠近走廊边界以内的一定距离就开始施加“预惩罚”。第二种办法更优雅但需要多一个阈值参数我最初的实现中因为不想引入额外调参量就选了第一种效果也很直接。5.3 奇异走廊当凸分解失效时怎么办区域生长算法有时会生成畸形走廊常见于局部环境异常拥挤的场景。比如两堵墙之间的缝只有1.2米宽但无人机直径就有0.8米走廊块生成到极限也只能刚好嵌入这条缝优化器被卡在一个极窄长度尺度上轨迹质量非常差。这种场景我总结了几种应对方式检查走廊块的最小尺寸是否大于安全阈值如果小于阈值则放弃该走廊块改用绕路路径。在廊道入口和出口之间插入中间过渡走廊块人为拆成长条形避免单个凸块横跨狭窄瓶颈。对走廊块做PCA主成分分析把长度方向自动识别出来沿该方向做扩张其余方向保持收缩。这能让走廊贴合通道走势给优化器留出更多裕量。5.4 仿真里跑通了真机却振荡怎么办仿真环境和真机之间永远存在一条鸿沟。我在这一两年里最常遇到的振荡现象不是因为算法本身出错而是因为优化出的轨迹对时间参数过于敏感——轨迹上某一段速度剖面出现高频抖动飞控跟踪时产生谐振。排查思路是这样的先可视化速度剖面和加速度剖面看是否有明显的高频波动如果有就调大λ_smooth尤其是加大jerk项的权重如果仍然振荡检查时间分配是否合理某一段时长短于1秒且路径弯曲剧烈几乎必然导致过大的加速度指令。另外还有一个容易被忽略的因素就是轨迹更新时间步长和优化耗时之间的关系。如果优化一次需要80毫秒而控制循环只有20毫秒等于每4个控制周期才更新一次轨迹这中间无人机已经飞出去一截轨迹参考和实际位置出现相位偏差。我的处理方式是严格控制优化迭代次数上限在50次以内并开启上一帧最优解作为当前帧初始值的热启动把单次优化耗时压到30毫秒以下振荡问题基本消失。6. 结个尾一些源于实践的经验之谈做基于飞行走廊的轨迹优化这几年我最深的一个体会是走廊不是银弹但它是工程上性价比极高的近似。它牺牲了一部分对自由空间的精确刻画换取了优化求解的速度和稳定性在真机场景里这恰恰是最宝贵的东西。相比于ESDF方案走廊构建代码量不大、依赖少适合快速在嵌入式平台上落地相比纯采样方法它又能给出确定性的、可重复的优化结果便于系统联调。如果你想在现有系统里加入这套方法我建议的路线是先离线生成走廊并可视化确认走廊对环境的贴合度再接一个最简单的QP优化器用贝塞尔曲线表示轨迹只加平滑性和走廊约束飞通之后再逐步加入时间分配、动力学约束和动态障碍物刷新。这样每一步都有清晰的验证节点出了问题也能快速定位。最后分享一个小技巧调试走廊参数时把走廊和优化后的轨迹叠加到同一张图里看重点关注轨迹是否总是贴着某个固定方向走。如果是大概率不是轨迹优化器的问题而是走廊的安全余量在这个方向上给得太小了。把余量方向改成各向异性的很多疑难杂症自然就消失了。