简介这是一份面向无人机自主导航方向的飞行走廊规划源码包基于B样条曲线完成轨迹连接与优化适合研究路径平滑、避障策略及实时控制的开发者与学习者。压缩包共133个文件约9.7MB以hpp与cpp源码为主辅以头文件、配置文件、launch及rviz可视化文件另有少量so动态库、msg/srv接口定义和readme文档整体目录结构清晰便于按模块阅读和二次开发。当前已有213人浏览学习属于中等热度的小型算法参考项目。内容涵盖B样条控制点选取与曲线生成、碰撞检测地图构建、轨迹跟踪与状态估计接口并配有模拟环境配置可帮助使用者快速搭建走廊规划实验理解参数调整对飞行路径平滑性及避障效果的影响适合需要快速上手工程实现的无人机算法爱好者参考借鉴。1. 飞行走廊 B样条为什么这条技术路线值得投时间无人机在树林、城市峡谷或厂房内部做自主飞行时最怕的不是飞不过去而是算了半天发现路径穿过了一面看不见的墙。飞行走廊规划的思路是先把三维空间里的可通行区域切成一串凸多面体让无人机只在这些区域里活动再把轨迹的每一段用B样条曲线连接起来最后用非线性优化把曲线修到平滑、可飞、不碰墙。这套走廊约束 B样条参数化 轨迹优化的组合是当前做无人机航迹规划最实用的工程方案之一尤其适合需要实时重规划、对轨迹平滑性有硬性要求的场景。本文会把走廊怎么生成、B样条怎么初始化、优化目标怎么设、参数怎么调、坑在哪讲透直接落到能复现的程度。2. 飞行走廊生成从占据栅格到凸多面体的关键一步2.1 走廊的本质为什么必须把自由空间凸化飞行走廊规划的第一步不是画轨迹而是把环境里能飞的空间抽出来。在大多数实际工程里环境地图来自激光雷达或深度相机生成的占据栅格Occupancy Grid它的表达形式是某个格子有障碍物或某个格子是空闲的。但占据栅格有一个很麻烦的问题它是一堆离散的布尔值没法直接用来做几何约束。轨迹优化时需要判断轨迹上这个点是否在允许区域内如果每次都查栅格计算量大且不连续优化器很容易在离散边界上来回震荡。所以要把自由空间凸化。所谓凸化就是把一大片自由空间拆成若干个凸多面体Convex Polyhedron每个多面体内部任意两点的连线都在该区域内。这样轨迹上的任意一点只要落在某个凸多面体内就可以用一组线性不等式来描述约束Ax b。这组不等式在优化里是连续可导的约束条件直接交给求解器处理即可。走廊就是由这些凸多面体首尾相连组成的通道——轨迹不会飞出走廊也就不会撞上障碍物。2.2 主路径提取与走廊扩张常见做法与选型走廊生成的典型流程分三步。第一步用A*、JPS或者RRT*在占据栅格上找一条从起点到终点的粗路径这条路径可以贴着障碍物走甚至路径上的某些点离障碍物很近没关系它只用来指示方向。第二步沿着粗路径上的每个采样点朝周围做膨胀——不断向外扩张直到碰到占据栅格里的障碍物。第三步把膨胀出来的空间用凸多面体近似表达相邻多面体之间要有重叠区域保证轨迹连接点能平滑过渡。工程里最常见的做法是用立方体膨胀 凹坑填充先沿着路径点初始化一串立方体然后迭代地拉伸、扩张每个立方体的边长直到逼近障碍物边界。具体实现时要注意走廊的宽度不能只取决于地图分辨率还要考虑无人机尺寸和动力学约束。比如一架轴距 450mm 的穿越机走廊半径至少要留 0.8 米以上否则最后优化出的轨迹虽然几何上不碰墙但实际飞行时桨尖气流会扫到树枝或墙壁。2.3 走廊生成的可执行流程一个最小实现下面给一个走廊生成的伪代码实现思路基于占据栅格地图输出一串凸多面体列表。这个实现不依赖特定库核心逻辑就是沿主路径扩张并记录边界。import numpy as np from dataclasses import dataclass dataclass class ConvexCorridor: # 每个走廊段用一个中心点和半边长来近似凸多面体 center: np.ndarray # (3, ) 中心坐标 half_size: np.ndarray # (3, ) 各轴半径 # 实际上后续优化会转成 Ax b def to_inequality(self): A np.vstack([np.eye(3), -np.eye(3)]) b np.concatenate([self.half_size, self.half_size]) return A, b self.center def generate_corridor(path_points, grid_map, min_radius0.8): path_points: (N, 3) 粗路径点 grid_map: 占据栅格1表示占据0表示空闲 min_radius: 走廊最小半径米 corridors [] for p in path_points: half np.array([min_radius, min_radius, min_radius]) # 逐步扩张半边长直到碰到障碍物或达到上限 while True: # 采样当前立方体边界上的点查询栅格 corners sample_cube_boundary(p, half) occupied [grid_map.is_occupied(c) for c in corners] if any(occupied): break # 碰到障碍物停止扩张 half np.array([0.1, 0.1, 0.1]) # 步长10cm if np.max(half) 4.0: # 防止无限扩张 break corridors.append(ConvexCorridor(p, half)) return corridors这段代码的逻辑是沿着粗路径点逐个扩张正方体直到边界采样点碰到占据栅格或达到扩张上限。half_size逐渐增大走廊就膨胀开了。实际工程里有个细节需要注意这段代码里sample_cube_boundary的采样密度直接影响走廊质量——采样太稀会漏掉障碍物导致走廊内表面包含障碍。我一般会在每个面上采样 9 个点3x3 均匀分布同时在棱边上额外采样 4 个点来兜底。另外min_radius一定要显式设置不要依赖栅格分辨率。走廊生成完之后最好把邻近且方向一致的走廊段合并。因为如果每一米就生成一个凸多面体整条走廊可能有几十个段每个段在优化时都提供一组约束会显著增加求解规模。常见的合并策略是如果相邻两个走廊段的重叠体积占比超过 80%或者中心点连线方向与两段轴向夹角小于 15 度就把它们合并成一个更大的凸多面体。这一条在长距离巡检任务里特别管用能把走廊段数从 50 压缩到 10 个以内。3. 用均匀B样条把走廊串起来控制点设计与轨迹参数化3.1 为什么选B样条而不是多项式或minimum snap轨迹参数化有几种路线多项式比如 minimum snap/jerk、贝塞尔曲线、B样条。多项式轨迹在无约束环境下很棒闭式解求起来快但一旦要加穿过走廊这种几何约束每个多项式段的系数都要跟约束做耦合非常繁琐。贝塞尔曲线的凸包性质好但全局支撑性差拼接点处的高阶连续性需要额外处理。B样条恰好是兼顾两者的方案它有局部支撑性移动一个控制点只影响附近几段轨迹又有天然的连续性k 次 B 样条自动有 k-1 阶连续还满足凸包性质曲线一定落在控制点形成的凸包内。轨迹落在控制点凸包内这一条对走廊规划来说简直是量身定做的如果把控制点全部放进走廊的凸多面体里那么整条B样条曲线都在走廊内——因为曲线在控制点凸包内控制点在走廊内凸多面体的凸包性质保证曲线也在走廊内。这就在数学上保证了约束控制点 约束整条轨迹优化时只需要约束控制点不需要在曲线上密集采样验证计算量直接降了一个数量级。3.2 控制点放进走廊的初始化方法拿到走廊之后下一步是在走廊内初始化一串B样条控制点。最直接的做法是沿走廊中心线按固定间距采样控制点然后把每个控制点向内收缩到走廊的安全边界内比如收缩到走廊中心到边界距离的 80% 处。这样初始轨迹一定在走廊内且保留了足够的修正空间给优化器。这里要讲清楚B样条的两个核心参数次数 p 和节点向量。工程里我一般用三次 B 样条p3因为三次已经保证了加速度连续C2 连续更高次数的收益很小但计算量明显上升。节点向量如果做均匀分布则轨迹的时间分配就是均匀的这会导致一个常见问题走廊中某些地方转弯很急需要更长的时间来过弯均匀时间分配可能让速度规划出来的轨迹在过弯处速度过高。所以更讲究一点的做法是后面优化环节里把节点向量也作为变量来处理。初始阶段直接用均匀节点向量即可。初始化代码可以这样写def init_control_points(corridors, spacing1.2, p3): corridors: ConvexCorridor 列表 spacing: 控制点间距米 p: B样条阶数 返回: 控制点数组 (M, 3) centers np.array([c.center for c in corridors]) # (N, 3) # 沿中心线按固定弧长插值出初始控制点 dist np.linalg.norm(np.diff(centers, axis0), axis1) # 相邻走廊中心距离 cum_dist np.concatenate([[0], np.cumsum(dist)]) total_len cum_dist[-1] num_points int(np.ceil(total_len / spacing)) 1 # 插值出 num_points 个均匀分布的点 interp_pts np.interp( np.linspace(0, total_len, num_points), cum_dist, centers[:, 0] ) # 这里只展示x轴处理实际要逐轴处理 # 逐轴插值 pts np.zeros((num_points, 3)) for axis in range(3): pts[:, axis] np.interp( np.linspace(0, total_len, num_points), cum_dist, centers[:, axis] ) return pts原因在于插值后的控制点可能落在走廊边界附近或者落在多面体边缘给优化器留的余量不足。我一般会再做一步收缩处理以每段走廊中心为中心把控制点沿中心方向收缩 10%-20% 的走廊半径。这个操作很便宜但能明显减少优化迭代次数——优化器不需要花大量迭代先把点拉回走廊内可以直接去优化平滑性和时间。3.3 轨迹碰撞检测与修正初始化一个代码示例初始化之后必须检查轨迹是否真的落在走廊内。虽然理论上控制点在走廊内、曲线就在走廊内但这是有前提的B样条曲线的凸包是控制点全体的凸包而走廊可能是分段的多个凸多面体控制点分布在多个走廊段内。曲线在任意一段上只受局部几个控制点影响所以真正严格的做法是逐段验证。下面是一个快速检测与修正的方法。def check_and_fix_trajectory(control_points, corridors, p3): 逐点采样B样条曲线检查是否飞出走廊超出则把控制点向走廊中心拉 # 假设已有B样条求值函数 bspline_eval(control_points, p, u) # 在归一化参数域 [0,1] 内采样 100 个点验证 u_samples np.linspace(0, 0, 100) # 实际应使用 knot vector 归一化 # 下面是关键检查逻辑 for i in range(100): u i / 99.0 pt bspline_eval(control_points, p, u) # 找到距离最近的走廊段中心 centers np.array([c.center for c in corridors]) dist np.linalg.norm(centers - pt, axis1) nearest_id np.argmin(dist) # 检查点是否在最近的走廊段扩张范围内 A, b corridors[nearest_id].to_inequality() if np.any(A pt - b 0): # 如果飞出走廊把影响该点的控制点拉回来 # 定位到影响该曲线段的控制点索引 seg_id int(np.clip(u * (len(control_points) - p), 0, len(control_points) - p - 1)) # 把控制点朝走廊中心收缩 direction centers[nearest_id] - control_points[seg_id] control_points[seg_id] 0.3 * direction return control_points这段代码演示的是一个检测-拉回的修正循环。注意在均匀节点向量下参数 u 与控制点索引的换算关系是 seg_id u * (M - p)其中 M 是控制点数量。这个是 B 样条工程实现里最容易写错的地方因为 De Boor 算法里节点区间和控制点并不是一对一映射而是错位的需要仔细看定义。另外拉回系数 0.3 是经验值拉得太猛会让轨迹在后续迭代里出现抖动。反复修正 3-5 次之后轨迹基本就能完全收进走廊了。常用的半自动办法是把走廊和初始轨迹导出出来直接在三维可视化里叠加地图叠加层查看。这步建议自动化因为飞行走廊规划的调试过程中超过一半的玄学问题其实是可视化没做出来——你根本不知道轨迹是哪里飞出去的。我自己的习惯是花半天时间把可视化的脚本写好后续调试效率能提高一倍以上。4. 轨迹优化把能飞变成飞得稳、飞得省4.1 优化问题的构成目标函数与硬约束走廊只是把可通行空间框出来了B样条只是把轨迹表达出来了但初始轨迹通常是又绕又抖的。优化环节要做的就是在不飞出走廊的前提下让轨迹尽量平滑、尽量符合无人机动力学。这个优化问题写成一个带约束的非线性优化目标函数 平滑项 动力学项 时间正则项其中平滑项常用的是加速度/加加速度jerk的平方积分。物理含义很直观jerk 越小轨迹越柔顺电机和机架的负担越小。硬约束是控制点必须落在走廊的凸多面体内。不过这个硬约束在实际工程里一般不会直接写成不等式约束丢给求解器因为投影到凸多面体上的操作计算量不小。更常见的做法是惩罚法目标函数里加一项控制点穿出走廊的深度平方和用一个大权重把它压住。这种处理方式虽然没有硬约束那么严格但工程实现简单、求解稳定。用硬约束的求解器比如用序列二次规划在约束数量多时经常遇到不可行问题而惩罚法几乎不会。4.2 如何把时间分配落进优化里B样条轨迹的时间分配藏在节点向量里。均匀节点向量意味着所有段用时相同这往往不是最优的。比如轨迹要从一个狭窄的门洞穿过门洞附近必须走慢一点如果节点向量均匀分布轨迹在门洞附近的时间被平均分配速度可能还是偏快。所以优化的一个关键变量是节点向量间隔。具体做法是把相邻节点之间的时间间隔 dT 作为优化变量目标函数里加一个总时间尽量短的正则项。但 dT 不能太小否则轨迹退化成蠕动也不符合实际。一般会在目标函数里设置时间项权重让优化器在更快完成和加速度可控之间做权衡。工程实现上有一个非常实用的技巧先跑一次固定时间分配的优化拿到平滑轨迹后根据轨迹上每点的速度幅值重新分配时间——速度高的段说明这段路程短但控制点多适当增加时间速度低的段压缩时间然后重新用新的节点向量再优化一次。这个两步法比直接把时间变量丢给求解器要稳定得多尤其适合新手快速跑通流程。4.3 工程中最常改的三个参数第一个是惩罚权重。把飞行走廊约束作为惩罚项加进目标函数时权重设太小轨迹会穿出走廊设太大求解器会优先满足走廊约束而牺牲平滑性。我的经验是从 100 开始逐步调大。观察指标是求解完成后统计控制点穿出走廊的最大距离如果大于 5cm就把权重翻倍。第二个是平滑项的阶数。用 jerk 还是 snap对最终轨迹形态影响很大。多旋翼一般用 jerk 就够了snap 会让轨迹在起点和终点处看起来过于笔直反而失去自然感。如果做的是穿越机花飞那才需要 snap。第三个是控制点间距。间距太密轨迹自由度太高、优化容易抖动间距太疏轨迹又无法贴合走廊的弯折。经验值是 0.8-1.5 倍无人机轴距。下面是一段典型的优化目标函数构建代码使用惩罚法def cost_function(control_points, corridors, p3): 返回总代价: 平滑代价 走廊惩罚代价 时间正则 # 平滑代价: 用控制点的三阶差分近似 jerk 积分 # B样条的导数可以用控制点差分表示均匀节点向量下 diff1 np.diff(control_points, n1, axis0) diff2 np.diff(diff1, n1, axis0) diff3 np.diff(diff2, n1, axis0) jerk_cost np.sum(diff3 ** 2) # 走廊惩罚: 统计穿出走廊的控制点 pen_cost 0.0 weight_pen 100.0 # 惩罚权重经验值从100起步 for pt in control_points: for corr in corridors: A, b corr.to_inequality() violation A pt - b pen_cost np.sum(np.maximum(violation, 0) ** 2) * weight_pen # 时间正则: 控制点间距的总长鼓励轨迹不要绕远路 length_cost np.sum(np.linalg.norm(np.diff(control_points, axis0), axis1)) weight_length 1.0 total jerk_cost pen_cost weight_length * length_cost return total这段代码的要点diff3的三阶差分在均匀节点向量下恰好在常数尺度上对应 jerk 的离散近似虽然严格来说需要乘以与节点间距相关的系数但在实践中直接用它做相对比较完全可行。走廊惩罚用了np.maximum(violation, 0)只有穿出走廊的轴才产生代价且是平方惩罚——穿出越深代价增长越快优化器会被自然地推回走廊内。长度代价是防止轨迹为了减少 jerk 而绕远路。参数调整时要注意各代价的量纲匹配。jerk 代价的量级通常非常小均匀节点向量下差分值可能在 0.01 级别所以它会自动被走廊惩罚压制。如果发现优化结果出现明显穿墙第一件事不是去调权重而是去检查控制点初始化是不是离走廊边界太近或者在走廊生成阶段把min_radius调大一点。权重只是兜底不能替代合理的初始化。5. 避坑指南走廊规划与B样条优化的五个常见问题5.1 走廊生成成功但轨迹仍然穿墙碰撞检测的采样密度问题现象三维可视化里B样条轨迹明显地穿过障碍物但检查控制点又都在走廊内。原因走廊段生成时采样点太稀立方体扩张过程中绕过了细长障碍物比如一根电线杆或一根斜拉索导致走廊表面其实包含了一个视觉上不可见的凸出障碍。轨迹在凸包性质的保证下确实没飞出走廊但走廊本身就不是严格自由的。解决把走廊扩张时的边界采样密度提高并在走廊生成后做一次障碍物点云到走廊表面的距离检查——如果任何障碍物点到走廊边界的距离小于无人机安全半径就把该走廊段收缩或切开重生成。5.2 B样条轨迹在平滑性和动态可行性之间打架现象优化后的轨迹从几何角度看极其平滑但无人机实际飞的时候跟踪误差很大特别是急转弯处总跟不上指令。原因目标函数只优化了 jerk 或 snap没有显式约束速度的上限。B样条优化天然喜欢把轨迹变平滑但这意味着轨迹会尽量走直线、尽量在拐角处切弯导致局部速度非常高。解决在目标函数里加入速度约束即在轨迹上采样点处计算速度幅值超过阈值则加惩罚。更根本的解决办法是重新考虑时间分配——转弯处的节点间隔应该更小即留给转弯更多的时间。5.3 优化后的轨迹大幅偏离可见路径走廊白做了现象优化之前粗路径是直的优化之后轨迹绕了一个大弯看起来很蠢。原因走廊生成得过于宽松。走廊虽然限制住了控制点但控制点在走廊内的活动空间其实很大——如果走廊宽度达到三四米优化器会为了降低 jerk 而把轨迹甩到走廊的另一侧甚至在长走廊里做S 型游走。解决适当地收窄走廊或者在走廊内加一条中心线吸引项目标函数里加一点控制点到走廊中心线的距离平方和权重不需要大0.1 左右就够轨迹就不会飘逸了。这算是血泪经验我第一次做走廊规划时看到优化结果绕了远路还以为代码有 bug最后发现是走廊太宽了。5.4 优化迭代不收敛或收敛特别慢现象求解器迭代几百次后代价函数还在下降但轨迹形态已经稳定继续迭代没有实际意义。原因把一些本应在初始化阶段解决的问题留给了优化器。典型的就是控制点初始化太差有的控制点在走廊边界上惩罚项给出的梯度非常大求解器不得不先救人再优化。解决初始化阶段就把所有控制点收缩到走廊中心周围保证每个控制点距边界至少 20% 走廊半径。迭代慢还有一个常见原因是惩罚权重太大导致目标函数的 Hessian 矩阵病态。如果权重已经开到 10000 以上还没法收敛不如回头检查初始化而不是继续加权重。5.5 走廊段之间的过渡处轨迹出现折角现象两段走廊交叠区域附近轨迹有一个明显的折点平滑性在那里断掉了。原因两段走廊的凸多面体交叠区域太窄控制点被压在交叠区里没有空间做平滑过渡。凸多面体是凸的但两段走廊之间形成的走廊颈部可能很窄。解决在走廊生成时保证相邻凸多面体的交叠体积足够大做法是让每个走廊段的扩张范围多延伸 30%-50% 到相邻段中。如果交叠区域仍然窄就在该处插入额外的控制点给优化器更多自由度去过渡。6. 进阶技巧把走廊信息融进优化做实时局部重规划前面讲的都是全局规划。实际飞行中无人机不可能每飞一段都重新做全局走廊——环境在变计算时间也不够。更常见的架构是先跑一遍全局走廊规划得到一条参考轨迹然后无人机沿参考轨迹飞行时用一个滑动窗口做局部重规划。窗口内的控制点跟着无人机走每飞过一个节点就向前补一个新节点新节点的走廊约束只取当前位置附近的两三个走廊段。这个架构下走廊生成不需要每次全部重跑只需要在探测到新障碍物时沿着当前轨迹前方 10-20 米的范围重新生成局部走廊段。B样条的局部支撑性在这里派上大用场修改局部几个控制点只会影响附近几段轨迹远处的轨迹完全不受扰动——这正是实时重规划时最想要的性质。如果当初选了多项式轨迹这一步就得整条轨迹重新解算。我个人习惯在最终实现里加一个走廊安全边距的经验值默认设为无人机半径的 1.5 倍。走廊生成时按这个边距缩小走廊边界优化时留出的余量就能吸收控制误差和风扰。代价是走廊变窄、轨迹灵活性下降但换来的是实飞时的保险系数明显提升。有一次我在茂密的树丛里试飞传感器噪声导致地图上有几根幽灵树枝安全边距恰好把轨迹从这些假障碍旁边推开实飞没有蹭到任何东西。这个参数一旦确认就把它写死在配置里不要再动。如果你刚开始做这个方向我的建议是不要一上来就追求复杂的求解器配置先用惩罚法把最小闭环跑通占据栅格地图 → 粗路径 → 走廊 → B样条初始化 → 惩罚优化 → 可视化验证。整个链路跑通之后再逐步加时间分配优化、局部重规划、安全边距这些进阶手段。这套思路让我自己少走了很多弯路希望帮到你。本文还有配套的精品资源点击获取