简介面向自主四旋翼飞行器与无人机规划开发者此项目提供一套完整的在线轨迹生成框架能在先前未知环境中输出安全、动态可行的贝塞尔轨迹。系统分前端路径查找与后端轨迹优化前端支持FM与A两种搜索方式并构建基于路径的飞行走廊后端利用贝塞尔曲线约束将分段轨迹严格限制在走廊与动力学边界内兼顾实时性与可行性。包内共133个文件以C头文件50个hpp、源文件24个cpp及基础头文件13个h为主另含配置、消息定义、文档与可视化文件压缩包约9.71MB目录结构清晰便于阅读和二次开发。代码经测试可运行并附有说明文档与注释适合高校学生、科研人员用作课程设计、毕业设计或项目起步参考。已有146人下载学习遇到运行问题可联系作者远程指导。1. 在线无人机规划框架要解决的不只是避障一台四旋翼飞进一间从没到过的屋子视觉或激光每扫一圈地图就变一次。上一秒规划好的轨迹下一秒就可能被一堵新发现的墙拦腰截断。常见的做法是触发一次完整的全局重规划但旋翼机不是扫地机器人它不能停下来等计算完成——悬停本身就是一种风险。这个在线无人机规划框架的核心是把轨迹生成压缩成毫秒级的局部问题用贝塞尔曲线表达待飞路径一边感知一边重规划同时保证轨迹在几何上不撞障碍物、在动力学上不被电机出力上限打回原形。“安全”和“动态可行”这两件事是互相撕扯的安全要求轨迹远离障碍物往往意味着绕路和减速动态可行要求轨迹足够顺滑、加速度和加加速度不越界又往往逼着轨迹贴向障碍物边缘。这个框架的全部价值就是在未知环境中实时找到那个两边都留有余量的解。适合正在做自主四旋翼航迹规划或读了论文但没跑通代码的工程师看下面从原理到参数一层层拆开。2. 为什么贝塞尔轨迹能同时保证安全与动态可行性——理论基础2.1 贝塞尔轨迹的数学结构与导数闭环贝塞尔曲线用一组控制点定义一条参数曲线。对于由 n1 个控制点 C_0, ..., C_n 定义的 n 阶曲线任意参数 τ ∈ [0,1] 处的值是B(τ) Σ_{i0}^{n} C_i · n! / (i! · (n-i)!) · τ^i · (1-τ)^(n-i)伯恩斯坦基函数的巧妙之处在于所有系数非负且求和为 1所以整条曲线始终落在控制点的凸包内。更关键的是曲线的导数依然是贝塞尔曲线而且控制点可以直接用相邻控制点的差分表达。速度控制点为 V_i n · (C_{i1} - C_i)加速度控制点为 A_i n(n-1) · (C_{i2} - 2C_{i1} C_i)加加速度控制点同理。这意味着只要约束控制点的位置和差分关系就能同时约束位置、速度、加速度三条曲线形成一个封闭的约束链条。def evaluate_bezier(ctrl_pts, tau): 评估 n 阶贝塞尔曲线在参数 tau 处的位置、速度、加速度 n len(ctrl_pts) - 1 # de Casteljau 递推数值上比直接算伯恩斯坦系数更稳定 pos [p.copy() for p in ctrl_pts] for r in range(1, n 1): for i in range(n - r 1): pos[i] pos[i] * (1 - tau) pos[i 1] * tau result_pos pos[0] # 速度n 阶曲线的速度是 n-1 阶控制点为 n*(C[i1]-C[i]) n_vel n vel_ctrl [n_vel * (ctrl_pts[i 1] - ctrl_pts[i]) for i in range(n)] vel evaluate_bezier_derivative(vel_ctrl, tau, order1) # 加速度同理在 vel_ctrl 基础上再差分一次 acc_ctrl [n_vel * (vel_ctrl[i 1] - vel_ctrl[i]) for i in range(n - 1)] return result_pos, vel, evaluate_bezier_derivative(acc_ctrl, tau, order2)上面这段代码的评估路径很有讲究。de Casteljau 递推避免了直接计算伯恩斯坦系数时的阶乘溢出和浮点误差特别是 n 超过 10 时差距非常明显。速度控制点的差分公式看起来简单但它揭示了整个框架的核心统一性位置约束、速度约束、加速度约束本质上是同一组控制点在不同差分深度上的投影优化一个量会牵动其他两个量。实际工程中轨迹通常由多段贝塞尔曲线拼接而成而不是用一条高阶曲线跨越整个飞行路径。因为高阶贝塞尔曲线的控制点布局一旦被障碍物干扰局部修复会污染全局。常见的做法是每段取 6 到 8 个控制点段与段之间只共享端点位置不共享控制点内部结构。2.2 安全约束利用凸包性质做快速碰撞检测贝塞尔曲线的凸包性质在安全约束上的价值是可以把连续曲线的碰撞检测降维成有限个控制点的碰撞检测。由于整条曲线都落在控制点的凸包内只要所有控制点都在自由空间中就能推断整段曲线在自由空间中。反过来说如果某个控制点进入了膨胀障碍区域这段曲线就有碰撞风险需要在优化中把这个控制点推出去。实际框架中很少直接用控制点做分辨率级别的碰撞判定而是用欧几里得符号距离场。每个控制点查询一次 ESDF得到该点到最近障碍物的带符号距离。如果距离小于安全阈值 r_safe就在该点施加一个斥力性的惩罚。做法是距离越小惩罚越大惩罚函数通常取 (r_safe - d)^3 这样的形式因为在优化中对控制点位置的导数连续可求。# ESDF 字段中一个体素的查询结果示例 # 负值点在障碍物内部正值点到障碍物表面的最近距离0紧贴表面 voxel_query(x1.2, y3.4, z0.8) - distance0.35, is_occupiedfalse这里的参数 r_safe 不是机身半径而是机身半径加控制余量。四旋翼的机身半径通常在 0.2 米左右加上控制误差和轨迹跟踪延迟r_safe 至少取 0.4 到 0.5 米。很多框架的碰撞事故不是轨迹本身撞了而是 r_safe 取得太小轨迹贴着障碍物表面飞一个轻微侧风就把机身吹进障碍区。只靠凸包检查还不够因为控制点都在安全区不代表控制点之间的连线也在安全区。更稳妥的做法是把一条贝塞尔段细分成若干个采样点对每个采样点做一次 ESDF 查询控制点约束和采样点约束同时参与优化。采样点数量取曲线阶数的 3 到 5 倍再多收益不大反而拖累求解速度。2.3 动态可行性把速度/加速度上限转成控制点约束动态可行性约束的目标是保证生成的轨迹在当前电机推力、旋翼转速限制和机身姿态角限制下能够被执行而不只是在数学上连续可导。四旋翼的物理限制主要体现为三个边界最大速度推力与阻力的平衡、最大加速度电机输出力矩的上限、最大加加速度执行器响应速度的上限。贝塞尔曲线在这里再次展现出优势。上一节推导过速度控制点是位置控制点的差分乘以阶数 n。对于整段时间跨度为 T 的曲线实际速度为 dP/dt dP/dτ · (1/T)。如果设定最大速度 v_max那么约束就变为对任意 i|n · (C_{i1} - C_i) / T| ≤ v_max。这个约束是线性的标准二次规划求解器可以直接处理。但工程上有两个隐藏的坑。第一个坑是加速度的约束。电机推力不仅提供前向加速度还要平衡重力。水平面的最大加速度不是 a_max而是 sqrt(a_max^2 g^2)姿态角倾斜时还要再打折扣。第二个坑是加加速度约束的采样方式。贝塞尔曲线的加加速度控制点可以在优化中约束但求解后需要做一次离散采样检查因为连续曲线的极值可能出现在两个控制点之间。# 动态可行性检查伪代码先检查控制点再密集采样兜底 for each segment: for i in range(n - 2): jerk_i n*(n-1)*(n-2) * (C[i3] - 3*C[i2] 3*C[i1] - C[i]) / T^3 assert |jerk_i| jerk_max vel_i n * (C[i1] - C[i]) / T assert |vel_i| vel_max acc_i n*(n-1) * (C[i2] - 2*C[i1] C[i]) / T^2 assert |acc_i| acc_max # 控制点约束通过后再按 dt0.05 对轨迹密集采样检查一次 for tau in arange(0, 1, 0.05): _, v, a evaluate_bezier(ctrl_pts, tau) assert norm(v) vel_max and norm(a) acc_max如果密集采样时某处越界就直接拉长时间段 T。这是在线框架中保持可行性的最后一道保险弦。时间参数 T 会在下一章详细展开。3. 在线重规划闭环从感知到贝塞尔轨迹生成的最小实现3.1 前端路径搜索与后端轨迹生成为什么要拆开在线规划框架里通常同时运行两个规划器前端的路径搜索器和后端的轨迹优化器。前端在局部 ESDF 里用 A* 或 RRT* 找一条只考虑几何避障的粗路径后端接收粗路径的关键点作为引导用贝塞尔优化生成动态可行的平滑轨迹。这个两段式架构不是过度设计而是出于一个实际限制纯搜索方法生成的折线路径加加速度无穷大纯优化方法在复杂迷宫中容易陷入局部最小值找不到通路。前端的粗路径只要求分辨率级别的安全性搜索空间是离散的体素步长通常设成机身直径的 0.5 倍。后端负责把折线路径的尖角磨平。所以前后端的接口不是完整路径只是路径上的若干关键点一般每隔 1 到 1.5 米取一个取多了后端没有自由度做平滑取少了后端会为了穿过狭小通道而把轨迹拱成大弧。前后端共同的假想敌是运动障碍物。ESDF 在两次完整更新之间保持静态如果障碍物移动了ESDF 中的占用信息已经过时。在线框架一般用前端搜索的时间预算来控制重规划频率比如每 0.2 到 0.5 秒触发一次快速搜索如果搜索发现当前轨迹的控制点已经落入障碍物就立即强制重规划一次完整流程。def replan_if_needed(esdf_map, current_traj, world_state): 检测当前轨迹是否与最新地图冲突必要时触发重规划 # 对当前轨迹按固定分辨率采样逐个查询欧几里得符号距离场 for s in np.arange(0.0, 1.0, 0.05): pos, _, _ current_traj.eval(s) dist esdf_map.query(pos) if dist SAFETY_RADIUS: # 触发重规划先用 A* 在局部地图上搜路径 raw_path a_star_search(esdf_map, current_traj.start, current_traj.goal) # 再以后端贝塞尔优化平滑 return generate_bezier_traj(raw_path, esdf_map, current_traj.start_state) return current_traj上面这段代码对应的逻辑是任何时刻新地图与旧轨迹冲突都用“搜索优化”的完整链路重新生成轨迹。其中 current_traj.start_state 是关键输入它保存了当前飞行器的位置、速度、加速度作为新轨迹的第 0 个控制点约束保证切换时不会突兀地顿一下或抖一下。3.2 轨迹段的时间分配与可行性检查时间分配是贝塞尔轨迹生成中最容易被低估的环节。同样的控制点位置把 T 从 2 秒缩到 1 秒速度直接翻倍加速度变四倍先前满足动态可行性的轨迹瞬间不可行。常见的做法是用前端路径的总长度除以期望巡航速度得到初始总时间再按各段的长度比例分配到每一段上。在线场景中期望巡航速度本身是动态调整的。飞行器接近障碍物时速度上限自动下调同时时间参数需要相应放大。这里门槛在于调整时间参数会改变轨迹形状进而影响安全性所以严格的做法是“时间分配-轨迹优化”迭代两到三轮先分配时间优化轨迹再检查可行性不满足就放大时间重新优化。# 时间分配参数表常见取值 segment_length1.5m, v_cruise1.0m/s - T_init1.5s segment_length1.5m, v_cruise0.5m/s - T_init3.0s segment_length0.8m, v_cruise1.0m/s - T_init0.8s一个工程技巧是在时间放大时对整条轨迹的所有段等比缩放而不是只放大违反可行性那一段。只放大一段会造成段与段之间的速度剖面前快后慢控制器在段边界处会感受到一个速度跃迁即使数学上速度是连续的加速度剖面也会出现一个不该存在的突变。3.3 重规划触发条件与轨迹切换的时间一致性重规划触发分为被动和主动两类。被动触发是当前轨迹采样点 ESDF 距离低于安全阈值的瞬间立刻响应这一条永不妥协。主动触发则有两类常见做法一是每隔固定周期检查一次比如 0.3 秒即使没有碰撞风险也重新优化一次目的是持续提升轨迹质量或响应地图的微小变化二是当飞行器实际位置与轨迹参考位置偏离超过某个阈值时触发例如偏离 0.3 米这说明控制器已经跟不住了继续走老轨迹没有意义。轨迹切换时最重要的一件事是时间一致性。新轨迹的第 0 段第 0 个控制点要锁定当前飞行器的位置速度控制点要锁定当前速度加速度控制点要尽可能接近当前实际加速度值。这几个锁定条件把新旧轨迹在切换点焊接在一起。如果只锁定位置不锁定速度切换瞬间控制器会收到一个速度阶跃信号飞机会明显前窜或后仰一下。现实中锁定到加速度已经很难因为 IMU 的加速度读数噪声较大常见做法是用滤波后的加速度做软约束即加一个较大的权重而不是硬约束。时刻 t_switch 的轨迹切换约束硬约束与权重 位置: B(t_switch) p_current # 硬约束 速度: d/dt B(t_switch) v_current # 硬约束 加速度: d2/dt2 B(t_switch) a_filtered # 权重 w50软约束3.4 一段可运行的最小优化回路综合上面所有机制一个最小可运行的回路可以收敛到约 10 行核心逻辑。这里的代价函数同时考量了平滑性、障碍距离和时间最优三条通道各自通过权重调节。实际操作中权重对数值非常敏感后面第 4 章会给出推荐调参顺序。def optimize_bezier(raw_path, esdf_map, start_state, dt0.1): 把前端路径转换为平滑的动态可行贝塞尔轨迹 ctrl_pts initialize_ctrl_from_path(raw_path, M8) # 均匀放置初始控制点 for iter in range(MAX_ITER): # 1. 将控制点映射到 ESDF 距离场计算安全惩罚 d [esdf_map.query(p) for p in ctrl_pts] safety_cost sum(max(0, r_safe - di) ** 3 for di in d) # 2. 用控制点差分计算 jerk 代价衡量轨迹平滑性 jerk np.diff(ctrl_pts, n2, axis0) jerk_cost np.mean(jerk ** 2) # 3. 时间代价所有段的总时间平方 time_cost total_time ** 2 # 4. 加权求和后用梯度下降更新控制点 total 0.9 * safety_cost 5.0 * jerk_cost 0.1 * time_cost grad compute_gradient(total, ctrl_pts) ctrl_pts - learning_rate * grad # 5. 每迭代 10 次做一次动态可行性检查 if iter % 10 0 and is_inside_limits(ctrl_pts, total_time): break return BezierSpline(ctrl_pts, total_time)这段代码的骨架映射了前面讲的所有约束。safety_cost 惩罚进入安全距离的控制点jerk_cost 抑制控制点之间的高阶抖动time_cost 防止轨迹为了躲障碍物而无限绕路。gradient 通过反向传播到控制点位置。实际在线系统中梯度下降会被更精细的求解器替代比如用 L-BFGS 或基于黎曼流形的优化器加速收敛但核心逻辑不改变。4. 项目源码阅读路径与关键参数调整4.1 从源码中定位核心模块的顺序拿到这套框架的源码如果从 main 函数开始读会迷失在传感器回调、状态估计和消息队列里。推荐按“入口-地图-轨迹-控制器”这条链路去读。先找到轨迹生成模块通常是轨迹类相关的目录里面应该有贝塞尔基函数实现、时间分配、碰撞检查、优化求解器四个文件。这四个文件的耦合关系非常清晰基函数提供数学工具时间分配决定时间参数碰撞检查计算安全代价优化器把三者攒在一起。地图模块的核心是 ESDF 的增量更新逻辑不是建图逻辑。前者决定重规划频率的天花板后者通常复用现成的 SLAM 或重建库。看看地图模块的接口里有没有提供“查询某一条轨迹是否有碰撞风险”的批量查询接口有这个接口说明框架设计时考虑了在线规划的实时性要求。控制模块需要关注的是轨迹跟踪方式。常见的是用位置环加姿态环的串级 PID轨迹模块输出的加速度被送入姿态解算模块转换成期望姿态角和油门指令。读源码时在轨迹模块的 update 函数里搜索 publish 或者 callback能看到轨迹下发到控制器的频率和话题名这是后面调参数时验证重规划是否生效的实际抓手。4.2 影响安全与动态可行性的核心参数参数调整是这套框架落地时工作量最大的部分。很多问题不是代码写错了而是参数本身矛盾。下面表格列出四个对轨迹行为影响最明显的参数按调整优先级排列。参数含义典型初始值调大效果调小效果单段控制点数 M每段贝塞尔曲线的控制点数6-8曲线柔性更好但优化难度上升曲线僵硬动态可行性更容易违反安全余量 r_safe机身到障碍物的最近允许距离0.4-0.5 m飞行更保守绕路增多飞行更激进碰撞概率上升时间分配基准 v_cruise期望巡航速度0.8-1.2 m/s到达更快但加速度约束更难满足飞行更平稳总耗时增加平滑权重 w_jerk加加速度惩罚系数3.0-8.0轨迹更柔顺但可能严重绕路轨迹更短但抖动明显调参顺序有讲究。先固定安全余量 r_safe因为它是硬底线必须体现机身物理尺寸和控制精度的实际边界。然后调时间分配基准 v_cruise注意观察悬停或低速犰行时加速度是否达到上限。最后才动平滑权重和优化权重。平滑权重是感知最明显的参数权重太大轨迹像隔着一层厚玻璃看障碍物明明有直线通道也要绕个大弧权重太小则轨迹在障碍物边缘抖成一团麻花。4.3 一个值得优先尝试的调参验证场景建议先用一个单一简单障碍物场景做闭环验证飞行器从起点直线飞向目标路径中间放置一个静态障碍物。这个场景能快速暴露安全约束和动态可行性之间的矛盾是否被参数失衡触发。观察飞行轨迹的记录和日志输出如果飞行器在障碍物前减速过猛原因是时间分配基准设定过快如果轨迹绕过障碍物后在后面出现 S 形摆动则说明平滑权重太低、控制点数量偏少。遇到参数调整失效时先回到源码看 ESDF 的更新逻辑是否正常。很多框架参数调整失败都不是参数问题而是 ESDF 更新延迟地图没有变化轨迹自然不变。在日志中搜索“esdf updated”和“replan triggered”两类消息用时间戳对比它们的间隔。如果 ESDF 更新后 1 秒内没有触发重规划说明碰撞检测的阈值判定逻辑出了问题回到 3.1 节那个 replan_if_needed 函数的条件去检查。5. 排错技巧与边界验证方法安全性和动态可行性的矛盾点5.1 用三段式验证判断框架是否真正在线把框架部署到实物或仿真环境后用固定的验证流程检查它是否满足在未知环境中生成安全、动态可行轨迹的目标。第一步是“地图突变测试”在飞行过程中突然在地图前方添加一堵虚拟墙观察从地图更新到新轨迹发布的时间间隔。这个间隔如果在 50 毫秒以下说明在线性能达标100 毫秒以上就有碰撞真实威胁需要检查重规划回调是否有阻塞操作。第二步是“跟踪误差对比”让飞机沿同一条贝塞尔轨迹飞行两次一次在空旷环境一次在近距离布满障碍物的走廊。对比两次的跟踪误差曲线如果走廊场景误差明显放大说明轨迹的动态可行性余量不足。曲线形状本身也很有信息量误差在段边界处出现尖峰说明段间连续约束没锁紧误差在加加速度大的地方持续放大说明加速度上限设定超过了电机的实际能力。# 离线回放验证命令对比轨迹命令与实际姿态估计 rosbag play flight_test.bag roslaunch bezier_check check_tracking.launch output:error_analysis.csv # 观察 csv 中第 3 列超过 0.3m 即视为跟踪失败点5.2 三个行为异常的现象学诊断现象一是轨迹静态轨迹贴合障碍物边缘飞行。此刻安全约束的惩罚函数可能梯度消失特别是惩罚函数在距离接近 0 时导数趋近 0优化器推不动控制点。解决思路是把惩罚函数改为线性增加而不是三次方增加让罚函数在近场有足够的推力。现象二是控制点约束全部满足但离散采样后发现加速度超限。上一章解释过贝塞尔曲线的极值可以发生在控制点之间检查没有用稀疏的控制点约束代替密集采样。工程修复是实行双重检查先约束控制点再按 0.02 秒间隔采样做第二轮检查两轮都通过才发布轨迹。现象三是轨迹生成耗时高且重规划频率低。耗时往往发生在 ESDF 查询上而不是优化求解。此时可以做两件事为轨迹生成模块单独建一个 ESDF 的无锁只读副本将控制点数量从 8 降到 6。这两步通常能把单次重规划耗时压到 30 毫秒以内在线框架才算名副其实。验证的最后一步一定是实物悬停测试。将飞行器固定在一个水平转台上执行重规划打断流程观察姿态响应是否超过可控范围。动态可行的含义不只是数学上约束满足还包括卡尔曼滤波延迟、执行器饱和、旋翼气流扰动这些现实因素叠加后依然可控。本文还有配套的精品资源点击获取
无人机在线轨迹规划：基于贝塞尔曲线的安全与动态可行方法
简介面向自主四旋翼飞行器与无人机规划开发者此项目提供一套完整的在线轨迹生成框架能在先前未知环境中输出安全、动态可行的贝塞尔轨迹。系统分前端路径查找与后端轨迹优化前端支持FM与A两种搜索方式并构建基于路径的飞行走廊后端利用贝塞尔曲线约束将分段轨迹严格限制在走廊与动力学边界内兼顾实时性与可行性。包内共133个文件以C头文件50个hpp、源文件24个cpp及基础头文件13个h为主另含配置、消息定义、文档与可视化文件压缩包约9.71MB目录结构清晰便于阅读和二次开发。代码经测试可运行并附有说明文档与注释适合高校学生、科研人员用作课程设计、毕业设计或项目起步参考。已有146人下载学习遇到运行问题可联系作者远程指导。1. 在线无人机规划框架要解决的不只是避障一台四旋翼飞进一间从没到过的屋子视觉或激光每扫一圈地图就变一次。上一秒规划好的轨迹下一秒就可能被一堵新发现的墙拦腰截断。常见的做法是触发一次完整的全局重规划但旋翼机不是扫地机器人它不能停下来等计算完成——悬停本身就是一种风险。这个在线无人机规划框架的核心是把轨迹生成压缩成毫秒级的局部问题用贝塞尔曲线表达待飞路径一边感知一边重规划同时保证轨迹在几何上不撞障碍物、在动力学上不被电机出力上限打回原形。“安全”和“动态可行”这两件事是互相撕扯的安全要求轨迹远离障碍物往往意味着绕路和减速动态可行要求轨迹足够顺滑、加速度和加加速度不越界又往往逼着轨迹贴向障碍物边缘。这个框架的全部价值就是在未知环境中实时找到那个两边都留有余量的解。适合正在做自主四旋翼航迹规划或读了论文但没跑通代码的工程师看下面从原理到参数一层层拆开。2. 为什么贝塞尔轨迹能同时保证安全与动态可行性——理论基础2.1 贝塞尔轨迹的数学结构与导数闭环贝塞尔曲线用一组控制点定义一条参数曲线。对于由 n1 个控制点 C_0, ..., C_n 定义的 n 阶曲线任意参数 τ ∈ [0,1] 处的值是B(τ) Σ_{i0}^{n} C_i · n! / (i! · (n-i)!) · τ^i · (1-τ)^(n-i)伯恩斯坦基函数的巧妙之处在于所有系数非负且求和为 1所以整条曲线始终落在控制点的凸包内。更关键的是曲线的导数依然是贝塞尔曲线而且控制点可以直接用相邻控制点的差分表达。速度控制点为 V_i n · (C_{i1} - C_i)加速度控制点为 A_i n(n-1) · (C_{i2} - 2C_{i1} C_i)加加速度控制点同理。这意味着只要约束控制点的位置和差分关系就能同时约束位置、速度、加速度三条曲线形成一个封闭的约束链条。def evaluate_bezier(ctrl_pts, tau): 评估 n 阶贝塞尔曲线在参数 tau 处的位置、速度、加速度 n len(ctrl_pts) - 1 # de Casteljau 递推数值上比直接算伯恩斯坦系数更稳定 pos [p.copy() for p in ctrl_pts] for r in range(1, n 1): for i in range(n - r 1): pos[i] pos[i] * (1 - tau) pos[i 1] * tau result_pos pos[0] # 速度n 阶曲线的速度是 n-1 阶控制点为 n*(C[i1]-C[i]) n_vel n vel_ctrl [n_vel * (ctrl_pts[i 1] - ctrl_pts[i]) for i in range(n)] vel evaluate_bezier_derivative(vel_ctrl, tau, order1) # 加速度同理在 vel_ctrl 基础上再差分一次 acc_ctrl [n_vel * (vel_ctrl[i 1] - vel_ctrl[i]) for i in range(n - 1)] return result_pos, vel, evaluate_bezier_derivative(acc_ctrl, tau, order2)上面这段代码的评估路径很有讲究。de Casteljau 递推避免了直接计算伯恩斯坦系数时的阶乘溢出和浮点误差特别是 n 超过 10 时差距非常明显。速度控制点的差分公式看起来简单但它揭示了整个框架的核心统一性位置约束、速度约束、加速度约束本质上是同一组控制点在不同差分深度上的投影优化一个量会牵动其他两个量。实际工程中轨迹通常由多段贝塞尔曲线拼接而成而不是用一条高阶曲线跨越整个飞行路径。因为高阶贝塞尔曲线的控制点布局一旦被障碍物干扰局部修复会污染全局。常见的做法是每段取 6 到 8 个控制点段与段之间只共享端点位置不共享控制点内部结构。2.2 安全约束利用凸包性质做快速碰撞检测贝塞尔曲线的凸包性质在安全约束上的价值是可以把连续曲线的碰撞检测降维成有限个控制点的碰撞检测。由于整条曲线都落在控制点的凸包内只要所有控制点都在自由空间中就能推断整段曲线在自由空间中。反过来说如果某个控制点进入了膨胀障碍区域这段曲线就有碰撞风险需要在优化中把这个控制点推出去。实际框架中很少直接用控制点做分辨率级别的碰撞判定而是用欧几里得符号距离场。每个控制点查询一次 ESDF得到该点到最近障碍物的带符号距离。如果距离小于安全阈值 r_safe就在该点施加一个斥力性的惩罚。做法是距离越小惩罚越大惩罚函数通常取 (r_safe - d)^3 这样的形式因为在优化中对控制点位置的导数连续可求。# ESDF 字段中一个体素的查询结果示例 # 负值点在障碍物内部正值点到障碍物表面的最近距离0紧贴表面 voxel_query(x1.2, y3.4, z0.8) - distance0.35, is_occupiedfalse这里的参数 r_safe 不是机身半径而是机身半径加控制余量。四旋翼的机身半径通常在 0.2 米左右加上控制误差和轨迹跟踪延迟r_safe 至少取 0.4 到 0.5 米。很多框架的碰撞事故不是轨迹本身撞了而是 r_safe 取得太小轨迹贴着障碍物表面飞一个轻微侧风就把机身吹进障碍区。只靠凸包检查还不够因为控制点都在安全区不代表控制点之间的连线也在安全区。更稳妥的做法是把一条贝塞尔段细分成若干个采样点对每个采样点做一次 ESDF 查询控制点约束和采样点约束同时参与优化。采样点数量取曲线阶数的 3 到 5 倍再多收益不大反而拖累求解速度。2.3 动态可行性把速度/加速度上限转成控制点约束动态可行性约束的目标是保证生成的轨迹在当前电机推力、旋翼转速限制和机身姿态角限制下能够被执行而不只是在数学上连续可导。四旋翼的物理限制主要体现为三个边界最大速度推力与阻力的平衡、最大加速度电机输出力矩的上限、最大加加速度执行器响应速度的上限。贝塞尔曲线在这里再次展现出优势。上一节推导过速度控制点是位置控制点的差分乘以阶数 n。对于整段时间跨度为 T 的曲线实际速度为 dP/dt dP/dτ · (1/T)。如果设定最大速度 v_max那么约束就变为对任意 i|n · (C_{i1} - C_i) / T| ≤ v_max。这个约束是线性的标准二次规划求解器可以直接处理。但工程上有两个隐藏的坑。第一个坑是加速度的约束。电机推力不仅提供前向加速度还要平衡重力。水平面的最大加速度不是 a_max而是 sqrt(a_max^2 g^2)姿态角倾斜时还要再打折扣。第二个坑是加加速度约束的采样方式。贝塞尔曲线的加加速度控制点可以在优化中约束但求解后需要做一次离散采样检查因为连续曲线的极值可能出现在两个控制点之间。# 动态可行性检查伪代码先检查控制点再密集采样兜底 for each segment: for i in range(n - 2): jerk_i n*(n-1)*(n-2) * (C[i3] - 3*C[i2] 3*C[i1] - C[i]) / T^3 assert |jerk_i| jerk_max vel_i n * (C[i1] - C[i]) / T assert |vel_i| vel_max acc_i n*(n-1) * (C[i2] - 2*C[i1] C[i]) / T^2 assert |acc_i| acc_max # 控制点约束通过后再按 dt0.05 对轨迹密集采样检查一次 for tau in arange(0, 1, 0.05): _, v, a evaluate_bezier(ctrl_pts, tau) assert norm(v) vel_max and norm(a) acc_max如果密集采样时某处越界就直接拉长时间段 T。这是在线框架中保持可行性的最后一道保险弦。时间参数 T 会在下一章详细展开。3. 在线重规划闭环从感知到贝塞尔轨迹生成的最小实现3.1 前端路径搜索与后端轨迹生成为什么要拆开在线规划框架里通常同时运行两个规划器前端的路径搜索器和后端的轨迹优化器。前端在局部 ESDF 里用 A* 或 RRT* 找一条只考虑几何避障的粗路径后端接收粗路径的关键点作为引导用贝塞尔优化生成动态可行的平滑轨迹。这个两段式架构不是过度设计而是出于一个实际限制纯搜索方法生成的折线路径加加速度无穷大纯优化方法在复杂迷宫中容易陷入局部最小值找不到通路。前端的粗路径只要求分辨率级别的安全性搜索空间是离散的体素步长通常设成机身直径的 0.5 倍。后端负责把折线路径的尖角磨平。所以前后端的接口不是完整路径只是路径上的若干关键点一般每隔 1 到 1.5 米取一个取多了后端没有自由度做平滑取少了后端会为了穿过狭小通道而把轨迹拱成大弧。前后端共同的假想敌是运动障碍物。ESDF 在两次完整更新之间保持静态如果障碍物移动了ESDF 中的占用信息已经过时。在线框架一般用前端搜索的时间预算来控制重规划频率比如每 0.2 到 0.5 秒触发一次快速搜索如果搜索发现当前轨迹的控制点已经落入障碍物就立即强制重规划一次完整流程。def replan_if_needed(esdf_map, current_traj, world_state): 检测当前轨迹是否与最新地图冲突必要时触发重规划 # 对当前轨迹按固定分辨率采样逐个查询欧几里得符号距离场 for s in np.arange(0.0, 1.0, 0.05): pos, _, _ current_traj.eval(s) dist esdf_map.query(pos) if dist SAFETY_RADIUS: # 触发重规划先用 A* 在局部地图上搜路径 raw_path a_star_search(esdf_map, current_traj.start, current_traj.goal) # 再以后端贝塞尔优化平滑 return generate_bezier_traj(raw_path, esdf_map, current_traj.start_state) return current_traj上面这段代码对应的逻辑是任何时刻新地图与旧轨迹冲突都用“搜索优化”的完整链路重新生成轨迹。其中 current_traj.start_state 是关键输入它保存了当前飞行器的位置、速度、加速度作为新轨迹的第 0 个控制点约束保证切换时不会突兀地顿一下或抖一下。3.2 轨迹段的时间分配与可行性检查时间分配是贝塞尔轨迹生成中最容易被低估的环节。同样的控制点位置把 T 从 2 秒缩到 1 秒速度直接翻倍加速度变四倍先前满足动态可行性的轨迹瞬间不可行。常见的做法是用前端路径的总长度除以期望巡航速度得到初始总时间再按各段的长度比例分配到每一段上。在线场景中期望巡航速度本身是动态调整的。飞行器接近障碍物时速度上限自动下调同时时间参数需要相应放大。这里门槛在于调整时间参数会改变轨迹形状进而影响安全性所以严格的做法是“时间分配-轨迹优化”迭代两到三轮先分配时间优化轨迹再检查可行性不满足就放大时间重新优化。# 时间分配参数表常见取值 segment_length1.5m, v_cruise1.0m/s - T_init1.5s segment_length1.5m, v_cruise0.5m/s - T_init3.0s segment_length0.8m, v_cruise1.0m/s - T_init0.8s一个工程技巧是在时间放大时对整条轨迹的所有段等比缩放而不是只放大违反可行性那一段。只放大一段会造成段与段之间的速度剖面前快后慢控制器在段边界处会感受到一个速度跃迁即使数学上速度是连续的加速度剖面也会出现一个不该存在的突变。3.3 重规划触发条件与轨迹切换的时间一致性重规划触发分为被动和主动两类。被动触发是当前轨迹采样点 ESDF 距离低于安全阈值的瞬间立刻响应这一条永不妥协。主动触发则有两类常见做法一是每隔固定周期检查一次比如 0.3 秒即使没有碰撞风险也重新优化一次目的是持续提升轨迹质量或响应地图的微小变化二是当飞行器实际位置与轨迹参考位置偏离超过某个阈值时触发例如偏离 0.3 米这说明控制器已经跟不住了继续走老轨迹没有意义。轨迹切换时最重要的一件事是时间一致性。新轨迹的第 0 段第 0 个控制点要锁定当前飞行器的位置速度控制点要锁定当前速度加速度控制点要尽可能接近当前实际加速度值。这几个锁定条件把新旧轨迹在切换点焊接在一起。如果只锁定位置不锁定速度切换瞬间控制器会收到一个速度阶跃信号飞机会明显前窜或后仰一下。现实中锁定到加速度已经很难因为 IMU 的加速度读数噪声较大常见做法是用滤波后的加速度做软约束即加一个较大的权重而不是硬约束。时刻 t_switch 的轨迹切换约束硬约束与权重 位置: B(t_switch) p_current # 硬约束 速度: d/dt B(t_switch) v_current # 硬约束 加速度: d2/dt2 B(t_switch) a_filtered # 权重 w50软约束3.4 一段可运行的最小优化回路综合上面所有机制一个最小可运行的回路可以收敛到约 10 行核心逻辑。这里的代价函数同时考量了平滑性、障碍距离和时间最优三条通道各自通过权重调节。实际操作中权重对数值非常敏感后面第 4 章会给出推荐调参顺序。def optimize_bezier(raw_path, esdf_map, start_state, dt0.1): 把前端路径转换为平滑的动态可行贝塞尔轨迹 ctrl_pts initialize_ctrl_from_path(raw_path, M8) # 均匀放置初始控制点 for iter in range(MAX_ITER): # 1. 将控制点映射到 ESDF 距离场计算安全惩罚 d [esdf_map.query(p) for p in ctrl_pts] safety_cost sum(max(0, r_safe - di) ** 3 for di in d) # 2. 用控制点差分计算 jerk 代价衡量轨迹平滑性 jerk np.diff(ctrl_pts, n2, axis0) jerk_cost np.mean(jerk ** 2) # 3. 时间代价所有段的总时间平方 time_cost total_time ** 2 # 4. 加权求和后用梯度下降更新控制点 total 0.9 * safety_cost 5.0 * jerk_cost 0.1 * time_cost grad compute_gradient(total, ctrl_pts) ctrl_pts - learning_rate * grad # 5. 每迭代 10 次做一次动态可行性检查 if iter % 10 0 and is_inside_limits(ctrl_pts, total_time): break return BezierSpline(ctrl_pts, total_time)这段代码的骨架映射了前面讲的所有约束。safety_cost 惩罚进入安全距离的控制点jerk_cost 抑制控制点之间的高阶抖动time_cost 防止轨迹为了躲障碍物而无限绕路。gradient 通过反向传播到控制点位置。实际在线系统中梯度下降会被更精细的求解器替代比如用 L-BFGS 或基于黎曼流形的优化器加速收敛但核心逻辑不改变。4. 项目源码阅读路径与关键参数调整4.1 从源码中定位核心模块的顺序拿到这套框架的源码如果从 main 函数开始读会迷失在传感器回调、状态估计和消息队列里。推荐按“入口-地图-轨迹-控制器”这条链路去读。先找到轨迹生成模块通常是轨迹类相关的目录里面应该有贝塞尔基函数实现、时间分配、碰撞检查、优化求解器四个文件。这四个文件的耦合关系非常清晰基函数提供数学工具时间分配决定时间参数碰撞检查计算安全代价优化器把三者攒在一起。地图模块的核心是 ESDF 的增量更新逻辑不是建图逻辑。前者决定重规划频率的天花板后者通常复用现成的 SLAM 或重建库。看看地图模块的接口里有没有提供“查询某一条轨迹是否有碰撞风险”的批量查询接口有这个接口说明框架设计时考虑了在线规划的实时性要求。控制模块需要关注的是轨迹跟踪方式。常见的是用位置环加姿态环的串级 PID轨迹模块输出的加速度被送入姿态解算模块转换成期望姿态角和油门指令。读源码时在轨迹模块的 update 函数里搜索 publish 或者 callback能看到轨迹下发到控制器的频率和话题名这是后面调参数时验证重规划是否生效的实际抓手。4.2 影响安全与动态可行性的核心参数参数调整是这套框架落地时工作量最大的部分。很多问题不是代码写错了而是参数本身矛盾。下面表格列出四个对轨迹行为影响最明显的参数按调整优先级排列。参数含义典型初始值调大效果调小效果单段控制点数 M每段贝塞尔曲线的控制点数6-8曲线柔性更好但优化难度上升曲线僵硬动态可行性更容易违反安全余量 r_safe机身到障碍物的最近允许距离0.4-0.5 m飞行更保守绕路增多飞行更激进碰撞概率上升时间分配基准 v_cruise期望巡航速度0.8-1.2 m/s到达更快但加速度约束更难满足飞行更平稳总耗时增加平滑权重 w_jerk加加速度惩罚系数3.0-8.0轨迹更柔顺但可能严重绕路轨迹更短但抖动明显调参顺序有讲究。先固定安全余量 r_safe因为它是硬底线必须体现机身物理尺寸和控制精度的实际边界。然后调时间分配基准 v_cruise注意观察悬停或低速犰行时加速度是否达到上限。最后才动平滑权重和优化权重。平滑权重是感知最明显的参数权重太大轨迹像隔着一层厚玻璃看障碍物明明有直线通道也要绕个大弧权重太小则轨迹在障碍物边缘抖成一团麻花。4.3 一个值得优先尝试的调参验证场景建议先用一个单一简单障碍物场景做闭环验证飞行器从起点直线飞向目标路径中间放置一个静态障碍物。这个场景能快速暴露安全约束和动态可行性之间的矛盾是否被参数失衡触发。观察飞行轨迹的记录和日志输出如果飞行器在障碍物前减速过猛原因是时间分配基准设定过快如果轨迹绕过障碍物后在后面出现 S 形摆动则说明平滑权重太低、控制点数量偏少。遇到参数调整失效时先回到源码看 ESDF 的更新逻辑是否正常。很多框架参数调整失败都不是参数问题而是 ESDF 更新延迟地图没有变化轨迹自然不变。在日志中搜索“esdf updated”和“replan triggered”两类消息用时间戳对比它们的间隔。如果 ESDF 更新后 1 秒内没有触发重规划说明碰撞检测的阈值判定逻辑出了问题回到 3.1 节那个 replan_if_needed 函数的条件去检查。5. 排错技巧与边界验证方法安全性和动态可行性的矛盾点5.1 用三段式验证判断框架是否真正在线把框架部署到实物或仿真环境后用固定的验证流程检查它是否满足在未知环境中生成安全、动态可行轨迹的目标。第一步是“地图突变测试”在飞行过程中突然在地图前方添加一堵虚拟墙观察从地图更新到新轨迹发布的时间间隔。这个间隔如果在 50 毫秒以下说明在线性能达标100 毫秒以上就有碰撞真实威胁需要检查重规划回调是否有阻塞操作。第二步是“跟踪误差对比”让飞机沿同一条贝塞尔轨迹飞行两次一次在空旷环境一次在近距离布满障碍物的走廊。对比两次的跟踪误差曲线如果走廊场景误差明显放大说明轨迹的动态可行性余量不足。曲线形状本身也很有信息量误差在段边界处出现尖峰说明段间连续约束没锁紧误差在加加速度大的地方持续放大说明加速度上限设定超过了电机的实际能力。# 离线回放验证命令对比轨迹命令与实际姿态估计 rosbag play flight_test.bag roslaunch bezier_check check_tracking.launch output:error_analysis.csv # 观察 csv 中第 3 列超过 0.3m 即视为跟踪失败点5.2 三个行为异常的现象学诊断现象一是轨迹静态轨迹贴合障碍物边缘飞行。此刻安全约束的惩罚函数可能梯度消失特别是惩罚函数在距离接近 0 时导数趋近 0优化器推不动控制点。解决思路是把惩罚函数改为线性增加而不是三次方增加让罚函数在近场有足够的推力。现象二是控制点约束全部满足但离散采样后发现加速度超限。上一章解释过贝塞尔曲线的极值可以发生在控制点之间检查没有用稀疏的控制点约束代替密集采样。工程修复是实行双重检查先约束控制点再按 0.02 秒间隔采样做第二轮检查两轮都通过才发布轨迹。现象三是轨迹生成耗时高且重规划频率低。耗时往往发生在 ESDF 查询上而不是优化求解。此时可以做两件事为轨迹生成模块单独建一个 ESDF 的无锁只读副本将控制点数量从 8 降到 6。这两步通常能把单次重规划耗时压到 30 毫秒以内在线框架才算名副其实。验证的最后一步一定是实物悬停测试。将飞行器固定在一个水平转台上执行重规划打断流程观察姿态响应是否超过可控范围。动态可行的含义不只是数学上约束满足还包括卡尔曼滤波延迟、执行器饱和、旋翼气流扰动这些现实因素叠加后依然可控。本文还有配套的精品资源点击获取