简介无人机飞行走廊规划与B样条轨迹优化代码包面向无人机算法研究人员、机器人开发者及自动化专业学生解决复杂环境下安全飞行路径的平滑生成与实时优化难题。包体共133个文件、9.7MB以C源文件cpp和头文件hpp/h为核心配合动态库so、配置参数cfg、仿真地图grid及说明文档md/pdf覆盖B样条曲线构造与控制点调整、碰撞地图构建、实时避障、路径跟踪及控制指令转换等完整流程便于按模块阅读和部署。目前已有212人学习下载。资源提供了可直接运行的无人机轨迹规划算法实现可结合传感器数据动态优化B样条曲线适应地形限制与避障需求计算效率较高文档和配置示例还能帮助快速搭建实验环境是学习无人机自主导航和开展相关课题的重要参考资料。1. 无人机飞行走廊规划里的B样条连接轨迹到底解决什么问题把无人机轨迹优化的求解维度压到10个控制点比在栅格地图上插1000个路径点更容易躲开障碍。这个反直觉的结论来自标题里两个东西的配合飞行走廊规划把带障碍的三维空间切成一组互相重叠的凸多面体B样条再用少量控制点把穿越走廊的路径连成一条有界、连续、可求导的轨迹后面的轨迹优化只做一件事让这条轨迹同时满足平滑性、安全性和动力学限制。这条技术链在无人机避障、无人机路径规划算法和无人机仿真里都是常见主干。与其直接在栅格上做轨迹精度控制不如先找一条粗路径再用B样条把问题降成低维参数空间最后用梯度法调参数。下面默认你已经跑过至少一种路径搜索算法但不要求用过B样条我把B样条的数学、走廊生成、优化目标函数和参数排查串成一条可以复现的线。2. B样条函数的数学底子与无人机运动学映射B样条在这里不是拿来画曲线的而是拿来当轨迹表达式的。对比一下插值多项式和贝塞尔插值多项式全局支撑动一个点整条曲线都变贝塞尔曲线虽然控制点直观但控制点数量一多每个点仍然影响整段曲线。对无人机避障来说每次重规划只希望改局部轨迹B样条的局部支撑特性决定了改某个控制点只影响邻近几个参数区间这是它成为轨迹优化主流表达的第一个原因。2.1 de Boor递推与控制点数量一条完整轨迹只需要十几个参数B样条曲线定义为基函数与控制点的加权和C(u) Σ N(i, p, u) * P(i)其中N(i, p, u)是第i个p次基函数P(i)是控制点。基函数用de Boor递推计算下面这个小函数可以直接抄走import numpy as np def bspline_basis(i, p, u, knots): # de Boor 递推计算第 i 个 p 次基函数在参数 u 处的值 if p 0: return 1.0 if knots[i] u knots[i 1] else 0.0 d1 knots[i p] - knots[i] d2 knots[i p 1] - knots[i 1] c1 (u - knots[i]) / d1 if d1 1e-9 else 0.0 c2 (knots[i p 1] - u) / d2 if d2 1e-9 else 0.0 return c1 * bspline_basis(i, p - 1, u, knots) c2 * bspline_basis(i 1, p - 1, u, knots) def sample_bspline(ctrl_pts, p3, n_samples200): ctrl_pts np.asarray(ctrl_pts, dtypefloat) n len(ctrl_pts) # clamped 均匀节点向量首尾各 p1 个重复节点 knots np.concatenate(( np.zeros(p 1), np.arange(1, n - p), np.full(p 1, n - p) )) u_start, u_end knots[p], knots[n] us np.linspace(u_start, u_end, n_samples) curve np.zeros((n_samples, ctrl_pts.shape[1])) for k, u_val in enumerate(us): for i in range(n): curve[k] bspline_basis(i, p, u_val, knots) * ctrl_pts[i] return curve, us, knots这段代码里clamped节点向量让曲线一定经过第一个和最后一个控制点中间控制点不经过曲线只对曲线产生“拉扯”。递归写法适合对照递推公式理解如果放进实时优化循环建议把基函数预计算成矩阵n个点采样一次就是一次矩阵乘法。控制点数量少的时候递归开销可以忽略20个控制点以上再考虑优化。控制点数量怎么定我一般的做法是走廊有几段控制点就取“走廊段数2到4”。比如三段走廊用6到8个控制点。控制点太少轨迹拐不过弯控制点太多曲线自由度上去以后容易在走廊里扭来扭去优化也更慢。这个数量关系后面调参时还会再碰到。2.2 凸包性质与导数控制点控制点约束等价于轨迹约束B样条有一个对飞行走廊规划极其重要的性质整条曲线一定落在所有控制点形成的凸包内。换句话说如果所有控制点都被约束在一个凸多面体里那整条曲线就不可能跑出这个凸多面体。这个性质把“约束一条连续曲线”变成“约束离散控制点”问题难度一下子降了一个维度。再看导数。B样条的导数仍然是B样条导数控制点可以由位置控制点差分得到。均匀节点间距为1时速度控制点就是相邻位置控制点的差Q(i) P(i1) - P(i)。加速度控制点就是二阶差分。这意味着速度、加速度的上下限也可以换算成控制点差分的上下限不需要在每个采样点上都加约束。对无人机来说轨迹至少需要C2连续也就是位置、速度、加速度连续。用三次B样条p3就满足C2这也是最常见的选择。如果后续要做加加速度优化或要求更顺滑的油门变化就升到p4或p5。我习惯从三次开始调参数少、收敛快真机试飞发现油门抖动再往上加阶数而不是一开始就选高次。3. 从栅格地图到飞行走廊凸多面体生成的最小可行实现飞行走廊不是靠B样条自己长出来的它来自环境地图的预处理。常见做法是先做路径搜索再把粗路径膨胀成走廊。不需要一次性生成全局精确的自由空间只需要保证走廊覆盖粗路径、走廊段之间相互重叠、每个走廊段是凸的。3.1 粗路径骨架A*、RRT或均匀采样都行先别急着优化第一步是拿一条几何可行的折线。A在栅格地图上最直观RRT在高维或连续空间里更通用但这步出来的路径不需要动力学可行只需要大致穿过障碍区之间。路径点通常比较密直接拿去做走廊会导致走廊段数量爆炸所以要重采样def resample_path(path, step1.0): pts [] for a, b in zip(path[:-1], path[1:]): a np.asarray(a, dtypefloat) b np.asarray(b, dtypefloat) dist np.linalg.norm(b - a) n max(int(dist / step), 1) for k in range(n): pts.append(a (b - a) * k / n) pts.append(np.asarray(path[-1], dtypefloat)) return np.array(pts)重采样步长step取无人机机身直径的1到2倍比较合理。步长太大走廊段与段之间重叠面积小B样条在连接处被挤得很紧步长太小控制点数量跟着涨优化变慢。重点是让相邻路径点之间的距离略小于期望走廊段尺寸这样膨胀后能自然形成重叠区。3.2 逐段膨胀生成走廊段检查、收缩、重叠有了稀疏路径点后逐段生成轴对齐包围盒AABB。以每个路径点为中心从一个小立方体开始向外扩张直到碰到障碍物为止。可以先用一个最简单的碰撞检查函数def box_free(occupancy, center, half_size, check_res2): # occupancy(x, y, z) - True 表示被占用check_res 是每个轴采样数 for dx in np.linspace(-half_size, half_size, check_res): for dy in np.linspace(-half_size, half_size, check_res): for dz in np.linspace(-half_size, half_size, check_res): if occupancy(center[0] dx, center[1] dy, center[2] dz): return False return True def generate_corridor(occupancy, path, max_half_size2.0, expand_step0.2): corridors [] for p in path: h expand_step while h max_half_size: if not box_free(occupancy, p, h): break h expand_step h max(h - expand_step, expand_step) # 合并重叠且中心距近的走廊段 if corridors: last_c, last_h corridors[-1] dist np.linalg.norm(np.asarray(p) - np.asarray(last_c)) if dist 2.0 * min(h, last_h): new_c (np.asarray(p) np.asarray(last_c)) / 2.0 corridors[-1] (new_c, min(h, last_h)) continue corridors.append((p, h)) return corridors这段代码的思路很直白先找到一个不碰障碍的最大盒子再看它和上一个盒子是否重叠重叠就合并。所有走廊段都输出成“(中心点, 半尺寸)”的元组列表之后转成凸约束时每个盒子就是一组形如 center - half x center half 的不等式。这里有两个注意点。第一check_res2只检查角点小障碍物可能藏在盒子内部但没碰到角点工程上要改用ESDF欧几里得符号距离场直接查最近障碍距离或者把check_res提到每轴4到6个点代价是走廊生成变慢。第二AABB在斜向走廊里浪费空间严重如果发现走廊体积利用率太低可以把盒子换成带旋转的OBB或者用凸分解把自由空间切成多个凸多面体。我更推荐先跑通AABB版本等轨迹优化和参数整定都稳定了再换凸分解减少变量同时引入。4. 走廊约束转控制点不等式B样条轨迹优化的目标函数设计走廊生成后问题变成“找一组控制点让B样条尽量贴合走廊中线同时满足平滑和动力学限制”。理论上有硬约束写法每个控制点都必须落在对应走廊段的凸多面体里靠凸包性质保证整条轨迹安全。但实际工程里走廊段的划分经常不精确硬约束一旦无解整个优化直接失败。所以我习惯用软约束惩罚轨迹采样点穿出走廊的深度权重调大后效果接近硬约束但优化过程稳定得多。4.1 走廊约束与控制点的映射关系走廊段是一个凸多面体记为 P_k {x | A_k * x b_k}。如果用硬约束就是对控制点 P_i 加一组线性不等式。因为B样条的凸包性质所有控制点满足约束就等价于整条曲线满足约束逻辑上很干净。软约束的做法是采样曲线上的点计算它到最近走廊段的“穿透深度”然后惩罚。穿透深度d(p)定义成负数表示点在走廊内正数表示穿出多远。用盒子走廊的话计算方式很简单def signed_dist_to_corridor(p, corridors): # corridors: [(center, half_size), ...] best np.inf for c, h in corridors: d np.max(np.abs(p - np.asarray(c)) - h) best min(best, d) return best注意这里用的是切比雪夫距离也就是点在轴对齐盒子外部的最大越界量。对AABB走廊来说这个距离够用换成凸多面体走廊时要改成真正的点到凸集距离一般用active set方法或QP子问题求解。4.2 三项目标函数光滑、碰撞、动力学优化目标我分成三项。第一项是光滑性用加速度平方积分J_smooth ∫ || a(t) ||² dt。加速度对应推力加速度变化剧烈意味着油门变化剧烈所以这项同时对电池、电机和执行器友好。二阶积分是最低要求四旋翼如果想更稳可以把被积函数换成加加速度。第二项是碰撞惩罚J_collision Σ max(d(p_j), 0)²对轨迹采样点逐点计算。平方是为了让梯度在穿透深度为0时连续优化器不会在走廊边界来回震荡。第三项是动力学约束J_dyn Σ max(||v_j||² - v_max², 0)² Σ max(||a_j||² - a_max², 0)²。同样用平方松弛处理。速度上限和加速度上限由飞控和电机选型决定比如室内小无人机常用 v_max3m/sa_max4m/s²室外高速机可以放宽到 10m/s 和 8m/s²。4.3 用scipy做数值优化的最小代码把三项拼成一个无约束优化问题控制点作为优化变量。控制点数量少直接用L-BFGS-B加上有限差分梯度就够跑from scipy.optimize import minimize def trajectory_cost(flat_ctrl, ctrl_shape, corridors, knots, degree, w_smooth1.0, w_collision200.0, w_dyn10.0, v_max3.0, a_max4.0, n_samples100): P flat_ctrl.reshape(ctrl_shape) curve, us, _ sample_bspline(P, degree, n_samples) dt us[-1] - us[0] dt_sample dt / n_samples vel np.diff(curve, axis0) / dt_sample acc np.diff(vel, axis0) / dt_sample J_smooth np.mean(np.sum(acc ** 2, axis1)) J_collision 0.0 for pt in curve: d signed_dist_to_corridor(pt, corridors) if d 0: J_collision d ** 2 J_dyn 0.0 J_dyn np.mean(np.maximum(np.sum(vel ** 2, axis1) - v_max ** 2, 0) ** 2) J_dyn np.mean(np.maximum(np.sum(acc ** 2, axis1) - a_max ** 2, 0) ** 2) return w_smooth * J_smooth w_collision * J_collision w_dyn * J_dyn # 初始化把走廊中心线等间隔取成控制点 centers np.array([c for c, h in corridors]) ctrl_init np.linspace(centers[0], centers[-1], len(centers) 2) res minimize( trajectory_cost, ctrl_init.flatten(), args(ctrl_init.shape, corridors, knots, 3), methodL-BFGS-B, options{maxiter: 500} ) ctrl_opt res.x.reshape(ctrl_init.shape)这段代码里速度、加速度用有限差分近似。控制点少于20个时这种近似完全够用如果控制点超过100个或要跑实时重规划建议换成解析B样条导数或者用自动微分库否则数值梯度会明显拖慢收敛。注意到碰撞项的权重初始给了200光滑项是1这是为了克服米制单位的量纲差异加速度平方的量级和小数穿透深度的量级差很多权重不动的话轨迹会优先保光滑穿墙穿得心安理得。5. B样条轨迹优化必调参数与失败排查这个优化问题最麻烦的不是写代码而是权重和初值。初值用走廊中心线等间隔取控制点已经是比较接近最优解的位置接下来主要调四类参数。5.1 必调参数表参数推荐起点调节方向失败表现B样条阶数p3抖动就升到4或5轨迹过度振荡且收敛变慢控制点数量走廊段数3拐弯不够就增加抖动就减少曲线过直或过弯光滑权重w_smooth1.0曲线绕远就增大穿墙就减小轨迹离走廊边界太近碰撞权重w_collision200穿墙就调大到500甚至1000曲线穿出走廊动力学权重w_dyn10速度超限就调大收敛慢就调小速度加速度超限总时间T距离/平均速度速度超限就增大T末端切线与走廊中心线偏移参数调节的顺序比参数值更重要。我一般先固定p3和控制点数量把w_collision拉大让轨迹先安全穿过走廊再逐步加w_smooth让轨迹变直最后加w_dyn把速度压下来。如果一开始就让三项权重互相竞争你很难判断是哪一项出了问题。调参就像给无人机做串级PID外环内环整定先保证基本稳定再一层层加约束。5.2 时间分配公式与速度上界B样条本身是参数曲线参数u不等价于时间必须做一次映射。常见做法是把总时间T均匀分配到参数区间上。给定B样条阶数p、控制点数量nclamped均匀节点向量参数区间长度是n-p速度上界和T的关系很直接T ≥ (n - p) * max‖P(i1) - P(i)‖ / v_max这个式子来自速度控制点的凸包性质均匀节点间距为1时速度控制点近似等于位置控制点差分乘以时间缩放系数后整条速度曲线都被这些差分控制点的凸包罩住。所以只要T满足这个不等式理论速度上界就不会超过v_max。实际使用中我会把T再放大10%到20%给加速度限制留余量因为速度满足不代表加速度满足加速度还受二阶差分影响。5.3 三种失败模式分别怎么查穿墙是撞权重。先画轨迹和走廊的三维图看穿墙位置如果整段均匀穿过说明w_collision太小如果只在走廊连接处穿出说明走廊段重叠不够要么调大走廊生成时的expand_step要么把控制点数量减一点让曲线在连接处别扭得那么厉害。抖动是过拟合。控制点太多或T太小都会让轨迹出现高频摆动。先把控制点数量降到走廊段数2试试再把p从3升到4。抖动经常伴随速度曲线毛刺把速度画出来看一眼就清楚。不收敛是初值问题或量纲问题。初值如果偏离走廊太远碰撞惩罚的梯度极大L-BFGS-B会直接乱跑。做法是先只优化碰撞项w_collision1w_smooth0w_dyn0把轨迹拉进走廊再逐步恢复其他项权重。这个过程类似课程学习稳妥且不费调试时间。6. 动态环境下的B样条重规划热启动与时间重分配技巧室内低速飞行和室外高速飞行的最大区别是环境刷新频率不同。动态环境下走廊每隔几百毫秒就更新一次轨迹优化必须能快速跟上。这里有两个工程技巧值得单独拿出来讲。6.1 热启动用上一帧控制点做初值静态环境里初值取走廊中心线即可动态环境里浪费不起这个收敛时间。上一帧已经有一组接近最优的控制点新走廊生成后直接把这组控制点作为下一轮优化的初值比从中心线重新开始快得多。实践中L-BFGS-B在靠近最优解附近的收敛速度远快于冷启动往往几十次迭代就能稳定。需要注意走廊变化的大区域对应的控制点会偏离新走廊很远碰撞惩罚梯度会对这部分控制点施加很大的修正力。为了防止修正过头可以把上一帧的速度信息也保留下来用相邻两帧控制点差分作为下一帧初值的速度项这样就相当于告诉优化器“轨迹原本的速度方向是什么不要一下子拧断”。6.2 走廊刷新后的时间重分配新走廊的路径长度往往和旧走廊不一样T还沿用上一帧的值会导致速度上界变化。每次走廊刷新后重新算一次TT_new (n - p) * max_norm(ctrl_opt[:, 1:] - ctrl_opt[:, :-1]) / v_max * 1.15这个公式和5.2节同源只是把优化后的控制点差分代进去。T_new算完要重新跑优化因为参数映射变了目标函数也会变。注意T变大意味着速度上限放宽曲线可以更慢但更平滑T变小会让优化变得更难收敛所以重规划场景里我宁可把T算得偏大。6.3 一个用于验证的最小仿真场景最后给一个可以十分钟跑完的验证流程在20m见方的地图里摆三个箱子障碍手动指定一条三段折线作为粗路径生成三段走廊取6个控制点p3w_collision500。优化完检查三个量最大速度是否超过1.2倍v_max轨迹采样点到走廊的最近距离是否大于-0.05m最大加速度是否超过1.3倍a_max。如果三个检查都过说明这套B样条飞行走廊优化流程的基本参数已经立住了再放到无人机仿真环境里挂载传感器数据做闭环重规划。本文还有配套的精品资源点击获取