做过几年无人机自主导航也调过不少运动规划算法。要说最让人头疼的环节不是感知也不是控制而是“明明路径出来了飞起来却像坐过山车”——要么急刹猛拐要么贴障碍物太近让人手心冒汗。后来把方案切换到基于飞行走廊的轨迹优化之后问题一下子清晰了很多。这篇文章就把这套方法的思路、数学模型、工程实现细节和踩坑经验一次说清楚给正在折腾运动规划算法的朋友一份能直接参考的笔记。飞行走廊Flight Corridor本质上是把障碍物之间的自由空间抽象成一连串凸多面体让轨迹优化只有一个明确任务在这些凸边界内生成一条平滑、动态可行、时间合理的轨迹。相比直接在栅格地图上做轨迹规划飞行走廊剥离了大量离散化和分辨率带来的干扰把“避障”和“平滑”两个问题解耦是工程上非常实用的中间表达。这篇文章适合三类人看一是刚接触无人机轨迹优化、想知道怎么把“路径”变成“轨迹”的初学者二是已经在跑A*、RRT*等前端规划但后端轨迹总是抖或者撞障碍物的开发者三是做机器人运动规划的工程师想了解如何用凸优化工具约束轨迹空间。我会从走廊生成讲到轨迹求解再到实际调参和故障排查尽量把每一个坑都标出来。1. 飞行走廊是怎么来的为什么它能让轨迹优化变简单1.1 先理清路径和轨迹的区别很多人在这一步就拧巴了。路径是一串离散的几何点比如从起点到终点经过的10个坐标点轨迹则是带时间戳的连续函数要回答“t1.2秒的时候飞机在哪个位置速度是多少加速度是多少”。运动规划算法里常用两段式前端用图搜索或采样方法找一条安全路径后端再做轨迹优化。如果没有飞行走廊后端通常会把路径点当作硬约束或者用势场把轨迹推向路径。但路径点一旦落在障碍物附近或者路径本身有锯齿轨迹优化就会非常痛苦——为了穿过一个狭窄的路径点轨迹可能需要在很小的空间里剧烈转弯产生的加速度和jerk会严重超标飞控根本跟不住。飞行走廊解决的就是这个“中间表达”问题。它不要求轨迹精确经过路径点而是给轨迹划出一片连续的、凸的、安全的“通道”。轨迹优化只负责在这个通道里找最平滑、最符合动力学约束的曲线。路径点的精细位置不再重要重要的是这条轨迹不能跑出通道。1.2 凸多面体为什么是天然的“安全信封”飞行走廊最常见的表示是凸多面体convex polytope每个面用一组线性不等式表示a_i^T x b_i, i 1, 2, ..., m比如一个立方体走廊实际上就是上下左右前后六个半空间的交集。轨迹上的任意一点都满足所有这些不等式就能保证这个点一定在走廊内部也就一定安全。凸多面体有三个好处工程上特别关键。第一凸性保证了优化问题的良态。一个点如果始终满足所有线性不等式那么这段轨迹上的任意线性组合也在走廊内。这样轨迹优化可以放心地用凸优化求解器比如OSQP、MOSEK、CVXPY后端不需要处理非凸的避障约束也不用反复做碰撞检测。第二距离度量直观。点到凸多面体边界的距离可以通过求解一个线性规划或者用半空间的几何关系快速估算。做安全裕度检查、走廊收缩、轨迹膨胀检测时这个能力非常有用。第三数据量小。几十个面就能表达很大一片安全区域。相比在高分辨率栅格地图上逐点检查碰撞走廊的表达效率高出好几个数量级。1.3 为什么不是直接B样条或者人工势场有人会问既然B样条也可以平滑势场也能避障为什么非要飞行走廊B样条的问题在于约束是局部的调控制点才能影响轨迹但控制点多了之后碰撞约束还是需要逐点检查而且怎么把“不能进障碍物”这个二值约束连续化很麻烦。势场最大的坑是局部极小值无人机可能在两个障碍物之间来回震荡或者停在“看起来安全”但实际死胡同的地方。飞行走廊最大的价值是“把几何问题代数化”。障碍物变成线性不等式轨迹优化变成一个带约束的二次规划QP或更一般的凸优化问题。这样做的结果就是理论上一定能找到局部最优解工程上可以稳定复现不像启发式方法那样每次跑结果都不一样。2. 从地图到走廊前端几何生成的完整流程2.1 先建图再膨胀把自由空间提取出来飞行走廊的前端一般是占据栅格地图或者八叉树地图。对于无人机我常用的是OctoMap它能够增量更新内存占用也远小于密集栅格。拿到地图之后第一步不是直接生成走廊而是对障碍物做膨胀处理把无人机的半径、动力学限制和控制器跟踪误差都留出余量。膨胀系数怎么定我一般按这个经验公式膨胀半径 无人机半宽 安全裕量 控制误差补偿 感知误差补偿比如一架轴距300mm的小型无人机半宽约0.15m安全裕量0.2m控制误差0.1m感知误差0.1m那么膨胀半径就是0.55m。这个参数直接决定了走廊的宽度。膨胀太小轨迹优化出来的结果在真实飞行中很容易“擦墙”膨胀太大狭窄空间可能直接被堵死规划直接失败。膨胀之后地图里的自由空间就变成了一个个连通区域。接下来可以从种子点开始做区域生长或者用形态学方法提取自由空间骨架。比较经典的做法是先跑一个A*或JPSJump Point Search找到一条参考路径然后沿着路径生成一串立方体或凸多面体走廊。2.2 用PCA做方向感知的走廊生长生成立方体走廊最朴素的做法是沿着参考路径每隔一段距离放一个固定大小的正方体然后剔除和障碍物相交的方块。但这样生成的小方块之间可能错位轨迹优化时容易在方块交界处产生不必要的拐弯。更优雅的做法是PCA主成分分析走廊生长。原理是先取一个包含参考路径局部片段的点集对这些点做主成分分析得到主方向然后沿着主方向生成一个“椭球形”的候选走廊再在最大内部椭球的基础上把椭球切成长方体。这个方法生成的长方体会自然贴合路径走向走廊数量少且边界方向合理。不过PCA法也有个前提点集要相对规整如果参考路径在狭窄空间里绕来绕去PCA的主方向可能失真。工程上我是这样处理的先把参考路径做一次低通滤波或B样条拟合让方向变化平滑一些再做PCA。2.3 从“最大内接椭球”到凸多面体一个高质量的走廊最好在满足安全性的前提下尽量宽。怎么找“尽量宽”的凸区域经典算法是“内接最大椭球迭代扩展”。大致步骤是在一个种子点附近用线性规划找一个最大内接椭球用椭球表面积或体积做目标。椭球确定后计算椭球各个方向的支撑平面把这些支撑平面作为候选面。用候选面切割冗余空间逐步把椭球扩展成多面体。重复迭代直到多面体碰到障碍物或达到体积上限。这个过程的数学实现有些繁琐但有现成的库可以借用比如Debra Devaurs等人提出的“Inscribed Ellipsoid”方法在很多开源运动规划库中都能找到对应实现。关键是理解思路用椭球做近似用线性不等式做安全包络。实际在快速原型阶段我不会一上来就上最大内接椭球。先用分段立方体走通全流程后面再替换成多面体细化这样调试难度低很多。2.4 走廊生成的质量标准走廊好不好直接决定了轨迹优化的上限。我总结了一条检查清单连贯性相邻走廊必须有足够大的重叠区域不能出现仅有一个点相接的“咽喉”否则轨迹很难自然通过。覆盖性整套走廊要覆盖参考路径的绝大部分不要让轨迹优化有“无路可走”的感觉。边界方向合理性长走廊的长轴方向应顺着路径走向避免出现斜切路径的边界否则轨迹为了躲边界会产生额外拐弯。数量适中走廊段数太多分段多项式的段数也会多计算量增加且段间连续性约束变多段数太少每段走廊体积变小轨迹活动空间不足。一般10米长的路径走廊段数在5~10段比较合适。走廊生成在工程里其实占了规划的很大一部分工作量。很多轨迹优化做不好问题不在后端而在前端把走廊搞得太碎或者太窄。3. 轨迹优化在走廊里求一条平滑轨迹3.1 目标函数为什么大家都爱最小化snap拿到走廊下一步就是求轨迹。目前主流做法是把轨迹表示成分段多项式比如每段是一个5阶或7阶多项式p(t) c0 c1 t c2 t^2 c3 t^3 c4 t^4 c5 t^5 c6 t^6 c7 t^7然后最小化加加速度jerk三阶导或 snap四阶导。对无人机来说最小化jerk会让加速度变化平缓适合常规飞行最小化snap则更注重螺旋桨推力变化率适合高速或者视觉避障场景因为snap小意味着机载相机的角速度和加速度变化小图像不会因为剧烈抖动而模糊。目标函数一般写成J ∫ (p^(3)(t))^2 dt 最小化jerk 或 J ∫ (p^(4)(t))^2 dt 最小化snap加上权重之后可以用余弦变换或直接构造二次型J c^T Q c其中c是多项式系数向量Q是根据时间段积分预计算出来的Hessian矩阵。3.2 分段多项式的连续性和时间分配分段多项式的优点是简单但麻烦在于段与段之间的连续性。一般要求相邻多项式在连接点处位置、速度、加速度连续即p_i(T_i) p_{i1}(0) pi(T_i) p{i1}(0) pi(T_i) p{i1}(0)这些连续性约束都是线性等式约束可以统一写成A_eq c b_eq扔进二次规划求解器即可。时间分配也是个关键。每段路径分配多少时间直接影响速度和加速度大小。最简单的做法是按每段路径长度等比例分配然后乘一个平均速度系数。但这样容易出现“长段时间很多短段时间很少”的不均匀现象导致轨迹速度忽快忽慢。更合理的做法是从一个初始时间分配开始先求解轨迹再根据轨迹峰值速度/加速度调整时间迭代几次。我曾经用过一个很实用的方法第一次规划用偏大的时间保证平滑然后用轨迹的实际速度分布反推最短可行时间再调低循环三次左右就能得到比较激进而又可行的轨迹。3.3 走廊约束如何变成线性不等式走廊约束是这次优化的核心。如果轨迹在t时刻的位置为p(t)而p(t)是多项式那么约束p(t)在走廊内本来是非凸的——因为p(t)是非线性的t是变量。工程上常用的招是“离散时间检查”把每一段轨迹按时间均匀采样十几个点在每个采样点处要求a_i^T p(t_k) b_i其中a_i、b_i是当前走廊边界对应的半空间参数。由于p(t_k)关于多项式系数是线性的所以这些约束仍然是线性不等式。采样点越密约束越严格但计算量也越大。我一般每一段取8~12个采样点既能保证不穿墙又不至于让QP规模爆炸。还有一个细节同一时刻的采样点要对应到正确的走廊段。因为走廊是按空间分段生成的轨迹段和走廊段不一定一一对应需要根据参考路径的索引做映射。最好是在生成走廊时就约定好“第几段路径对应第几段走廊”这样轨迹分段和走廊分段天然对齐。3.4 一个最小QP的完整数学形式把上面这些拼在一起一个最简单的基于飞行走廊的轨迹优化问题长这样minimize J c^T Q c w1 * 软约束惩罚项 subject to A_eq c b_eq 起点终点和连续性约束 A_ineq c b_ineq 走廊采样点约束 |v_max|、|a_max|、|j_max| 等可选约束这里的软约束惩罚项可以用来处理“尽量贴近路径”的期望或者是对时间、能量做加权。硬约束是必须满足的比如起点终点的位置、速度、加速度走廊约束可以做成硬约束也可以做成软约束。实操中我倾向于把走廊约束做硬约束把“贴近参考路径”做软约束。因为走廊已经保证了安全路径贴近度只是让轨迹看起来自然没必要用硬约束把轨迹绑死在参考路径上。3.5 求解器选择与代码框架这个问题最终落到二次规划求解上。对于几百维的小规模QPOSQP非常快而且支持热启动适合在规划频率10Hz~50Hz下持续求解。MOSEK也很稳但商业授权不是所有人都方便用。如果后面要加整数变量比如时间最优分配与拓扑选择联合优化那就得上Gurobi或者SCIP这类MILP求解器不过这是另一个话题了。代码框架上我习惯把流程拆成三个模块地图与走廊模块负责膨胀、路径搜索、走廊生成输出走廊参数。轨迹优化模块输入走廊和端点约束构造QP调用OSQP求解。检查与输出模块对求解结果做插值、碰撞复查、动力学可行性审查输出给控制器。这个框架的好处是模块之间只通过标准数据接口通信换地图表示、换求解器、换轨迹阶数都不影响其他部分。4. 从仿真到真机绕不开的工程细节和坑4.1 数值稳定性多项式基函数的选择分段多项式最容易被忽视的问题就是数值稳定性。直接用幂基c0 c1 t ... c7 t^7当时间t比较大比如一段轨迹飞5秒t^7会非常巨大和系数相乘之后数值范围可能跨越十几个数量级。QP求解器内部做矩阵分解时很容易出现数值病态导致解出来的系数看起来合理但轨迹点验算时严重超差。我踩过最大的坑就是这里仿真能飞真机一飞就偏检查半天发现是多项式数值条件数太差。解决办法有两个。一是做时间缩放把每段时间归一化到[0,1]这样t的幂次不会爆炸但要注意缩放后的连续性约束需要修正。二是改用B样条或者最小二乘拟合的系数空间比如用Bernstein基数值稳定性会好很多。轻量级方案里我推荐做时间归一化再构造目标函数改动最小见效最快。4.2 走廊边缘的“贴地飞行”问题轨迹优化很容易出现一种现象轨迹在走廊里大面积贴着边界走因为贴着边界的路径可能看起来更“平滑”弯曲更少或者目标函数更小。但真实飞行中边界附近气压测量和视觉特征往往不稳定稍微有一点干扰就可能撞进障碍物。对策是生成走廊时再做一次“内缩”。比如走廊每个面的约束从a_i^T x b_i缩成a_i^T x b_i - margin_imargin_i 一般取0.1~0.2m或者按走廊窄边的比例动态设置。内缩之后轨迹即使在边界附近也还有一层物理安全兜底。我见过有人贪图效率取消这层内缩结果真机在走廊边上触发了一次紧急避障。那次的教训是安全裕量永远不要省在最后一层。4.3 时间分配对轨迹可行性的影响刚刚说过时间分配需要迭代。但迭代时间过长会影响实时性所以还要设置上限比如最多迭代三次超出上限就采用当前最优可行解。另外时间分配和走廊约束有耦合。如果某段时间过于紧张轨迹为了满足走廊约束可能不得不剧烈加速导致峰值速度极高。这种情况下即使QP有解物理上也可能飞不出来。我的做法是求解后检查峰值速度和峰值加速度是否在飞行器极限的80%以内如果超了就把对应段的时间乘以1.3重新求解。检查公式很简单v_peak max |p(t)| 0.8 * v_limit a_peak max |p(t)| 0.8 * a_limit注意这里的0.8不是死规矩。如果是竞速无人机可以放到0.95如果云台相机需要稳定画面可能压到0.5。4.4 失败后的Fallback策略没有哪个系统能保证100%规划成功。走廊生成失败、QP无解、轨迹碰撞检查不过这些都要有应对方案。我常用的fallback策略分成三个层次第一层重新生成走廊。可能是参考路径穿过了过于狭窄的区域调整膨胀半径或者改变路径搜索的启发式权重再试一次。第二层放宽走廊约束。把软约束权重加大或者把硬约束走廊适当“放松”一些但要重新检查碰撞。第三层切换到安全悬停或保守路径。让无人机沿原路径减速停住再重新做全局规划。现实中第三层其实用到不多但必须存在否则系统一遇到复杂环境就失去保护。5. 常见问题速查你大概率会遇到这几个5.1 优化结果轨迹直接穿墙先检查走廊约束是否真的加进QP了。很多人写完目标函数忘了把走廊不等式拼接进约束矩阵结果轨迹只满足起点终点连续性自然随便穿。其次检查走廊和轨迹段的对应关系轨迹在t时刻应该满足的是当前所在空间位置的走廊约束而不是索引固定的走廊约束。如果轨迹段和走廊段错位也会穿墙。5.2 QP无解或者求解很慢无解往往是因为约束太紧。尝试增加每段时间扩大走廊内缩量或者把部分硬约束改成软约束比如把“速度必须小于某值”改成“速度超过阈值的部分加惩罚”。求解慢则检查约束矩阵是否稀疏如果用稠密矩阵存稀疏约束OSQP也救不了你。另外热启动估计在连续规划周期之间复用上一次的解通常能将每次求解时间降低一半以上。5.3 走廊太碎或者前后不连贯多半是膨胀半径太大导致窄通道被截断或者参考路径在窄通道里弯曲太多导致PCA生成长走廊失败。可以试试降低膨胀半径里的“控制误差补偿”项或者改用沿路径法线方向扩展的非轴对齐长方体。窄通道里做非轴对齐走廊效果立竿见影。5.4 轨迹看起来平滑但飞起来抖地面站显示的轨迹曲线平滑不代表实际的微分信号平滑。检查一下是不是jerk或者snap目标函数的权重太小或者说你只用了位置约束但速度和加速度连续性没加全。还有一个容易忽略的点轨迹跟踪控制器有延迟如果轨迹加速度过大控制器滞后会导致实际飞行轨迹超调。这时候适当降低目标函数对“快”的权重或者对轨迹做低通滤波都能缓解。5.5 参数速查表参数项建议初值调节方向膨胀半径0.5m小型无人机太小容易碰撞太大窄通道直接失败走廊内缩margin0.1~0.2m视觉传感器噪声大时增大每段采样点数8~12太少穿墙太多计算量大时间分配迭代次数3实在紧迫可以1~2次速度安全系数0.8竞速可放宽云台稳定需收紧分段多项式阶数7或5阶数高更平滑但数值风险更大6. 我个人的一些体会做了这么久运动规划我觉得飞行走廊这套体系最大的价值不是某个算法多炫酷而是它把无人机的运动规划拆成了一条清晰的流水线建图、搜索、走廊生成、轨迹优化、检查输出。每个环节能单独调试也能单独替换。你不需要一次性面对一个巨大的非线性优化问题而是用多个简单问题的组合去逼近一个复杂问题。真机飞行和仿真最大的不同是你会发现地图、感知、控制每一环都有误差。飞行走廊的安全裕量本质上是为这些误差买的保险。我习惯在仿真里把裕量压到很小来测试算法极限但一旦上真机我会立刻把裕量恢复到保守值。先安全再激进这个顺序不要搞反。最后分享一个小的调试技巧在可视化工具里把走廊的半透明体、参考路径、优化轨迹同时显示出来运行一遍完整的规划循环。如果发现轨迹明显偏向走廊一侧去看看那一侧对应的感知数据往往能找到传感器盲区或者地图更新延迟的问题。这比盯着数字调参直观得多。飞行走廊和轨迹优化是运动规划算法里一个很值得深耕的方向希望这篇笔记能帮你少走几步弯路。