简介针对无人机飞行走廊规划场景这份压缩包提供了一套基于B样条曲线连接与优化的完整算法实现适合无人机算法研究者、开发者以及自主导航方向的学习者。资源包含B样条生成与调优、路径平滑、避障及实时优化等核心模块并配有状态估计与控制接口代码能够支撑从轨迹生成到执行的全流程验证。压缩包共133个文件以hpp/cpp头文件与源码为主承担核心算法实现另含动态库so、配置文件cfg、说明文档md及网格grid等辅助资源整体约9.7MB结构清晰便于按需查阅。目前已有213人浏览学习其中涵盖碰撞检测、轨迹节点处理等关键实现可帮助读者快速理解飞行走廊规划中B样条的实际应用并在此基础上进行二次开发或实验对比。1. 无人机飞行走廊规划为什么 B 样条连接轨迹值得拆开复现飞行走廊规划这个名字听起来偏学术但它解决的问题非常具体无人机要从 A 点飞到 B 点中间躲开障碍物还得让轨迹平滑到飞控不会在空中“画龙”。我见过不少新手直接用搜索算法得到的折线路径再用直线把拐角拉直结果无人机实际飞出很大的侧滑甚至撞上走廊边。这个方案先用几何方法给无人机圈出一条安全通道也就是飞行走廊再用 B 样条从起点到终点穿过走廊最后通过优化调整控制点让轨迹在满足安全性约束的同时保持平滑。它的核心价值就是解决“路径上没有障碍物但飞行器根本飞不出来”的问题。这个文件包里最常见的实现形态是先把环境拆成若干凸多边形走廊再用 B 样条生成参考轨迹。新手能从中学会走廊约束怎么转化为凸约束熟手则能提炼出一套可复用的轨迹生成与优化框架。2. 飞行走廊与 B 样条绑定的原因凸空间与局部支撑曲线的配合2.1 飞行走廊的本质把非凸自由空间拆成凸子空间飞行走廊并不是一条线而是一系列凸多边形的集合。假设地图上有若干障碍物无人机能飞的空域往往是任意形状的非凸区域直接在这个非凸空间上做优化很难。常见做法是先用栅格地图做障碍物膨胀再做连通域分析把自由空间分成若干块子区域每一块用凸多边形近似。这样整个可飞行区域就变成了一系列凸多面体的并集每个多边形内部完全无障碍物。把走廊表示为凸子空间有利于后续优化因为“点是否在多边形内”可以写成一组线性不等式。比如二维凸多边形每条边可以表示成向量参数形式向内法向量与点坐标的内积小于等于某阈值。对于轨迹优化来说线性约束比非线性距离约束稳定得多求解速度也更快。实际项目中走廊的生成不一定非要从栅格地图开始。如果已经有全局路径规划器给出的路径点比如从 A* 或者 RRT 得到的路径常见做法是沿着这些路径点按预设宽度生成相邻的矩形走廊段然后对相邻矩形求并集并做凸包化处理。这个做法简单直接也是很多教学型代码包里采用的方式。重要的是每个走廊段必须是凸的两个相邻段之间要有重叠区域否则后面 B 样条优化时控制点会掉入凹口轨迹就可能穿进障碍物。2.2 B 样条凭什么比贝塞尔曲线更合适B 样条是贝塞尔曲线的推广最大区别在于基函数只在一个局部区间上非零。以三次 B 样条为例每个控制点只影响曲线的一小段移动一个控制点不会让整条曲线都发生形变。贝塞尔曲线就不一样伯恩斯坦基函数在整段参数上都非零移动任意一个控制点整条曲线都会重新分布这对走廊分段的场景不利。走廊约束本质是分段表达的。第一段走廊对应一个凸多边形第二段对应另一个凸多边形。我们希望轨迹在每一段里都遵循相应的走廊约束这就需要一种能灵活修改局部区域的曲线表达方式。B 样条的局部支撑性正好匹配走廊分段结构可以只调整某一小段控制点而不会影响全局轨迹。另一个关键性质是凸包性。B 样条曲线始终落在控制点所张成的凸包内这是由基函数非负且归一化保证的。也就是说只要所有控制点都落在某个凸走廊多边形内部那么整条 B 样条曲线就一定在这个多边形内部。这个性质非常重要它把连续曲线的安全性检查转化成了离散的控制点安全性检查省去了在曲线上密集采样的成本。这也是把 B 样条用于飞行走廊时最核心的理论依据。2.3 从走廊边界到控制点约束一条保安全的推理链设某个走廊段是凸多边形 SS 由若干条边的半平面交定义。B 样条曲线的控制点是 P_0 到 P_n如果所有 P_i 都满足 S 的所有半平面约束那么由于曲线是控制点的凸组合曲线上的任意一点也必然满足这些半平面约束。于是“曲线不穿墙”这件事就变成了“控制点不穿墙”这件事。这一步推导是最容易出错的地方很多人误以为只要控制点大部分在走廊内曲线就安全。凸包性说的是所有控制点都在凸集内曲线才安全。所以控制点的初始化和优化过程中的约束都必须非常严格不能只放大多数点进入走廊留一两个点在走廊外“微调”。从实现角度看这个性质也决定了优化问题的写法凸走廊约束作为硬约束平滑度作为软目标。3. 用代码实现 B 样条连接轨迹从走廊到可飞参考线3.1 生成走廊多边形给定路径点与膨胀宽度先用最简单的方式构造走廊。根据全局路径产生的路径点沿路径方向生成一系列带一定宽度的矩形走廊段。我一般会把宽度参数单独提出来方便后续做无人机尺寸加安全余量的调整。import numpy as np from scipy.spatial import ConvexHull def create_corridor_segments(waypoints, width): 将一组带方向的关键点转换成若干个凸多边形包围的走廊分段。 waypoints: (N,2) 数组width: 走廊宽度。 返回 polys 列表每个元素是 (m,2) 的凸多边形顶点。 polys [] for i in range(len(waypoints) - 1): p1 waypoints[i] p2 waypoints[i 1] vec p2 - p1 length np.linalg.norm(vec) if length 1e-6: continue dir_vec vec / length normal_vec np.array([-dir_vec[1], dir_vec[0]]) half width * 0.5 left_p1 p1 normal_vec * half left_p2 p2 normal_vec * half right_p1 p1 - normal_vec * half right_p2 p2 - normal_vec * half quad np.array([left_p1, left_p2, right_p2, right_p1]) hull ConvexHull(quad) polys.append(quad[hull.vertices]) return polys这段代码先把两个路径点之间的方向向量算出来再取垂直方向作为矩形截面的法向。half 是走廊宽度的一半之所以不是直接用路径点做矩形中心是为了让走廊轴线与原始全局路径重合。最后用 ConvexHull 对四个角点求凸包是为了防止路径出现倒退弯曲时生成凹四边形。实际栅格地图环境下我会在生成矩形后把矩形与连续自由空间求交再做膨胀保证走廊段之间足够重叠。3.2 B 样条基函数与曲线求值Cox-de Boor 递推生成走廊后需要把 B 样条曲线放进去。这里不依赖现成的黑匣子样条库直接用 Cox-de Boor 递推公式实现基函数这样能精确控制节点向量和阶数。def bspline_basis(u, degree, i, knots): 用 Cox-de Boor 递推公式计算 B 样条基函数 N_{i,degree}(u)。 degree 是样条阶数i 是控制点索引knots 是节点向量。 if degree 0: if knots[i] u knots[i 1]: return 1.0 return 0.0 left 0.0 denom1 knots[i degree] - knots[i] if denom1 1e-12: left (u - knots[i]) / denom1 * bspline_basis(u, degree - 1, i, knots) right 0.0 denom2 knots[i degree 1] - knots[i 1] if denom2 1e-12: right (knots[i degree 1] - u) / denom2 * bspline_basis(u, degree - 1, i 1, knots) return left right def bspline_curve(control_points, degree, knots, num_points200): 沿节点向量采样生成整条曲线点列。 n len(control_points) - 1 u_min knots[degree] u_max knots[n 1] us np.linspace(u_min, u_max, num_points) curve [] for u in us: point np.zeros(control_points.shape[1]) for i in range(n 1): point bspline_basis(u, degree, i, knots) * control_points[i] curve.append(point) return np.array(curve)基函数递推里最关键的是两个分母判断。数值上节点向量可能出现重复值分母归零时如果不保护就会产生除零错误。这里用 1e-12 作为阈值是经验值太小容易在极端节点分布下溢出太大则会影响递推精度。bspline_curve 函数里用 u_min 和 u_max 确定采样区间是因为 clamped 节点向量首尾重复实际定义域从第 degree 个节点开始到第 n1 个节点结束。3.3 初始化控制点首尾夹紧与走廊中线对齐生成 B 样条曲线前先要确定控制点数量和节点向量。为了让曲线严格穿过起点和终点节点向量两端需要重复 degree 1 次同时控制点首尾也要对应重复若干次。def clamped_knots(num_ctrl, degree): 生成两端重复的节点向量曲线会穿过首尾控制点。 inner_segments num_ctrl - (degree 1) knots [0.0] * (degree 1) for i in range(1, inner_segments): knots.append(float(i)) knots [float(inner_segments)] * (degree 1) return knots def initial_control_points(waypoints, degree): 用路径点初始化控制点首尾重复夹紧中间取走廊中线的路径点。 ctrl [] for _ in range(degree): ctrl.append(waypoints[0]) for wp in waypoints[1:-1]: ctrl.append(wp) for _ in range(degree): ctrl.append(waypoints[-1]) return np.array(ctrl, dtypefloat)重复首尾控制点的次数等于样条阶数而不是随便重复两三次。如果 degree 是 3重复三次可以保证曲线在端点处至少有一阶连续性如果重复次数不足起始点处曲线会和第一条线段偏离。中间路径点直接作为初始控制点可能让曲线略微偏离走廊中轴线但后续优化会修正这里只需要一个落在走廊内的可行解。4. 轨迹优化平滑性、安全余量与动力学约束一起解4.1 为什么直接生成的 B 样条不能直接飞初始的那条 B 样条确实已经在走廊内但它只是几何上可通行飞行器动力学上未必可飞。路径点直接作为控制点时控制点间距不均匀会导致速度曲线出现明显波动。如果某一段走廊很窄控制点密集堆积曲线局部弯曲过度角速度很容易超过飞控限制。所以优化阶段的目标不是彻底改变曲线形状而是在不破坏走廊约束的前提下重新分配控制点位置。常见做法是最小化高阶导数的能量从加加速度层面让轨迹更平滑。另一个容易忽视的点是参考线偏离代价不加这个代价时优化器会为了平滑度把轨迹大幅拉离走廊中线导致姿态长时间倾斜不够安全。4.2 目标函数设计平滑项、偏离项与碰撞惩罚项的权值我一般会把目标函数拆成三项平滑项衡量控制点三阶差分的大小偏离项衡量控制点与走廊中轴线的偏移碰撞惩罚项在控制点距离走廊边界太近时产生惩罚。平滑项权重最大偏离项次之碰撞惩罚项只在接近边界时触发。def cost_function(ctrl_flat, ctrl_ref, w_smooth, w_dev): 轨迹优化目标函数平滑项 参考线偏离项。 ctrl ctrl_flat.reshape(-1, 2) # 三阶差分近似加加速度能量 jerk np.diff(ctrl, n3, axis0) smooth_cost np.sum(jerk ** 2) # 相对走廊中轴线的偏离代价 dev_cost np.sum((ctrl - ctrl_ref) ** 2) return w_smooth * smooth_cost w_dev * dev_cost三阶差分用 np.diff 的 n3 参数一次就能拿到离散加加速度的代理项。这里用的是离散差分不是连续导数但在控制点等距分布假设下已经足够。w_smooth 取 1.0 作为基准w_dev 通常取 0.1 到 0.5。权重差一个数量级是正常的因为平滑项的数值范围往往远大于偏离项。如果偏离项权重超过 0.5优化结果会偏向直线导致曲线挤压走廊边界。4.3 用 scipy.optimize 做约束优化的参数配置约束进入优化器时要把走廊的凸多边形边界写成线性不等式形式。每个凸多边形的每条边生成一个法向量控制点与该法向量的内积不超过一个常量就表示点在多边形内部。from scipy.optimize import minimize def polygon_constraint_matrix(poly): 凸多边形转成 A [x,y] b 的线性约束形式。 A [] b [] n_edges len(poly) for i in range(n_edges): p1 poly[i] p2 poly[(i 1) % n_edges] edge p2 - p1 inward np.array([-edge[1], edge[0]]) inward_norm inward / np.linalg.norm(inward) A.append(inward_norm) b.append(np.dot(inward_norm, p1)) return np.array(A), np.array(b) def optimize_bspline(ctrl0, corridor_polys, w_smooth, w_dev): 控制点作为优化变量走廊约束为不等式约束。 n_ctrl len(ctrl0) ctrl_ref ctrl0.copy() def cost(x): return cost_function(x, ctrl_ref, w_smooth, w_dev) def constraints_fun(x): ctrl x.reshape(-1, 2) vals [] for idx, poly in zip(range(len(corridor_polys)), corridor_polys): # 简化示例假设每个控制点都对应一个独立的走廊多边形 A, b polygon_constraint_matrix(poly) cp ctrl[idx % n_ctrl] vals.append(b - A cp) return np.concatenate(vals) cons {type: ineq, fun: constraints_fun} res minimize(cost, ctrl0.flatten(), methodSLSQP, constraintscons, options{maxiter: 300, ftol: 1e-6}) return res.x.reshape(-1, 2)这里为了演示给每个控制点分配了一个对应的走廊多边形。实际工程里走廊段数量往往少于控制点数量需要做一个“控制点分组”的操作判断每个控制点位于哪个走廊段内并只施加该走廊段的线性约束。SLSQP 在处理线性不等式约束时很稳定但要求初始解必须满足所有约束。初始控制点都在走廊内时优化才能顺利收敛。maxiter 不要给得太小300 次迭代在控制点少于 20 个时足够。5. 避坑与常见问题排查B 样条轨迹规划的 5 个高频翻车点5.1 现象优化后轨迹贴近走廊壁安全余量不够明明控制点都在走廊内部曲线也没有穿墙但飞行器飞起来时距离障碍物非常近稍微受到风扰就可能撞上。原因是目标函数里只设置了“不能出走廊”的硬约束没有鼓励轨迹远离边界。优化器发现平滑度提升的同时把轨迹推向边界是符合数学目标的只是不符合工程安全目标。解决方法是加软惩罚项控制点离边界小于预设安全距离时按距离的平方惩罚。安全距离一般取无人机半径加上 0.5 倍机体尺寸。5.2 现象速度曲线像锯齿忽快忽慢B 样条曲线本身平滑但它的参数是节点向量里的节点间距。节点间距均匀意味着每个控制点区间分配的弧长差不多但走廊弯曲处实际路径长度可能差异很大。节点间距不匹配就导致参数化速度畸变。现象是飞机在同一段轨迹中莫名加速再急减速。解决思路有两种一种是把节点向量改成非均匀分布让节点间距与实际路径长度成正比另一种更常用把时间间隔作为额外优化变量通过时间缩放把速度曲线拉直。5.3 现象起点和终点速度方向跑偏优化时如果把所有控制点都当成自由度首端和末端的控制点会被平滑项拉动导致曲线虽然经过起点终点但切线方向不是预期方向。比如无人机起点要求沿某个方向起飞优化后曲线一上来就转了弯。原因是只约束了位置没有约束导数方向。解决方法是把控制点分成两组前三个和后三个控制点保持位置固定只优化中间控制点这样端点速度方向由固定控制点的相对位置确定不会被优化器带偏。5.4 现象非线性求解器不收敛的玄学同一个优化问题换个地图就不收敛或者初值稍微改动就发散。这往往是初始解贴到约束边界导致的SLSQP 在线性不等式约束附近计算梯度时有限差分得到的步长跨越了约束边界雅可比矩阵失真。我的习惯是先检查初始控制点到各走廊边的距离如果存在距离很小甚至为负的控制点就先手动把控制点拉回走廊中轴线而不是直接交给求解器。另一个技巧是变量归一化把所有控制点坐标缩放到同一个量级能显著提升数值稳定性。5.5 现象极窄走廊转弯处轨迹穿了墙控制点明明都在各自的走廊段内但曲线在两个走廊段交界处跑出了多边形。原因多半是相邻走廊段没有重叠形成一个凹口。B 样条的凸包性只保证曲线落在所有控制点凸包内而两个凸走廊段的并集不一定是凸集控制点在各自段内混合后曲线落在联合凸包里就可能穿过凹口。解决方法是在生成走廊时让相邻段重叠至少一个控制点间距的长度或者把全局所有控制点约束在每段走廊的补集之外最简单实用的还是前者。6. 进阶验证技巧把 B 样条轨迹跑起来之前先量化评估6.1 曲率与角速度检查优化结束不代表轨迹可以直接下发。我习惯先对曲线做密集采样计算每个采样点的曲率半径再结合预设最大速度换算成最大角速度。飞控的姿态环通常对角速度有硬限制超了就会出现跟踪滞后。曲率计算很简单对曲线点列用中心差分求导再按曲率公式算。6.2 离散碰撞采样与安全余量复核凸包性质保证控制点安全但我仍会在曲线上再做一次密集碰撞采样原因有两个一是走廊段之间的重叠区域可能被重复计算二是数值求解器每次迭代结束后的约束满足度只是近似。采样间隔设置为无人机半径的一半检查每个采样点到所有障碍物的距离是否大于安全阈值。这一步不花多少时间却能把绝大多数穿墙问题挡在仿真阶段。6.3 基于走廊分段的局部重规划策略在线运行时不一定每次都要重新优化整条曲线。如果雷达检测到新障碍物先判断障碍物落在哪个走廊段内只把该段的控制点集合释放为优化变量其他段保持不动。这样重规划耗时从秒级降到几十毫秒级同时保持全局轨迹的连续性。实现要点是记录每个控制点从属于哪个走廊段的索引重规划时固定其他段的控制点并保持首尾交接处的控制点位置不变。有一次我在模拟项目 X 里直接跳过曲率验证把优化结果丢给无人机结果它在一个大角度转弯处出现明显的位置跟踪偏差。复盘才发现最大曲率在狭窄走廊处是预设限制的两倍。自那以后我养成了保存样条后先跑一段静态分析的习惯确认曲率和碰撞余量都通过再进仿真。这个习惯帮我省掉了大量的机上调试时间希望帮到你。本文还有配套的精品资源点击获取