RRT*与最小抖动轨迹：四轴飞行器三维路径规划实战解析

简介这是一套基于RRT算法与最小抖动轨迹生成的四轴飞行器路径规划C项目面向无人机/机器人方向的学生、研究人员及开发者可用于路径规划算法验证、课程设计或毕业设计实践。压缩包共10个文件包含6个C源文件涵盖旧路径规划、路径规划、轨迹生成、目标点变换、缩放等核心模块、1个头文件、CMake构建配置、ROS包描述及说明文档包体仅21KB结构精简便于阅读。目前已有49人学习下载代码经过测试运行成功功能可用代码注释与文档说明能辅助理解RRT扩展与轨迹平滑的衔接思路若运行遇到问题还支持私聊答疑和远程教学对初学者比较友好。整体是一份轻量但完整的无人机路径规划参考实现适合在此基础上扩展或迁移到实际项目中。1. RRT* 最小抖动四轴路径规划不是 A* 的二维翻版四轴飞行器的路径规划本质上是在三维空间里找一条从 A 到 B、不撞障碍、还能被飞控真正跟踪的曲线。很多从业者把二维小车上的 A* 或普通 RRT 直接搬到四轴上结果发现路径折角太大飞控一跟踪就掉高或者震荡。RRT* 的价值在于渐进最优——它通过重新布线让随机树不断逼近最优路径而最小抖动轨迹生成minimum snap则把 RRT* 输出的几何折线变成满足四轴动力学约束的光滑时间曲线。两者结合正是无人机自主导航和动态避障任务里最常见的工程组合。下面从三维 RRT* 的 C 实现讲起再落到最小抖动轨迹生成最后给出可运行的工程架构和参数调优经验。2. 把 RRT* 从二维扩展到三维采样、扩展、重连和渐进最优2.1 为什么选 RRT* 而不是普通 RRTRRT 在高维空间只有概率完备性它保证“只要有解就能找到解”但不保证解的代价接近最优。普通 RRT 的扩展方式是从随机点出发找最近树节点然后向随机点扩展一步。这个方式在障碍稀疏时很快但生成路径会非常曲折。RRT* 在扩展后增加重连rewire步骤会检查新节点附近树中已有的节点如果通过新节点的路径比原来的父节点更近就改父节点。这样随着采样点增加路径代价逐渐下降最终收敛到最优解。代价函数通常选路径长度但在三维空间中也可以加入障碍物距离、高度变化等项。对于四轴状态是 (x, y, z)比小车多了 z 轴的维度采样的随机数和障碍物模型也要跟着变。一个常见误区是把 2D 的 RRT* 库直接改个坐标就上结果碰撞检测没有处理机身的宽度路径贴着墙飞真机上桨叶直接削到墙面。2.2 三维空间采样和扩展的 C 代码骨架// rrt_star_3d.h #pragma once #include Eigen/Core #include vector #include memory struct Node { Eigen::Vector3d pos; // 三维位置状态 int parent -1; // 父节点索引 double cost 0.0; // 从起点到该节点的累计代 }; class RRTStar3D { public: RRTStar3D(const Eigen::Vector3d start, const Eigen::Vector3d goal, double step_size, double goal_threshold, double search_radius); void setCollisionMap(const std::shared_ptrclass CollisionMap map); bool plan(int max_iter, std::vectorEigen::Vector3d path); private: Eigen::Vector3d sample(); int nearest(const Eigen::Vector3d q_rand); bool steer(int near_idx, const Eigen::Vector3d q_rand, Eigen::Vector3d q_new); bool collisionFree(const Eigen::Vector3d from, const Eigen::Vector3d to); void rewire(int new_idx, const Eigen::Vector3d q_new); std::vectorNode nodes_; Eigen::Vector3d start_, goal_; double step_size_, goal_threshold_, search_radius_; std::mt19937 rng_; std::shared_ptrclass CollisionMap collision_map_; };// rrt_star_3d.cpp #include rrt_star_3d.h Eigen::Vector3d RRTStar3D::sample() { // 以 10% 概率直接采目标点避免随机树盲目扩散 if (std::uniform_real_distributiondouble(0.0, 1.0)(rng_) 0.1) { return goal_; } // 三维均匀采样限制在规划空间的边界内 std::uniform_real_distributiondouble dist_x(0.0, 20.0); std::uniform_real_distributiondouble dist_y(0.0, 20.0); std::uniform_real_distributiondouble dist_z(0.0, 8.0); return Eigen::Vector3d(dist_x(rng_), dist_y(rng_), dist_z(rng_)); } bool RRTStar3D::steer(int near_idx, const Eigen::Vector3d q_rand, Eigen::Vector3d q_new) { Eigen::Vector3d delta q_rand - nodes_[near_idx].pos; double dist delta.norm(); if (dist 1e-6) return false; if (dist step_size_) { q_new nodes_[near_idx].pos delta / dist * step_size_; } else { q_new q_rand; } return collisionFree(nodes_[near_idx].pos, q_new); }这段代码里sample()中 10% 的目标偏置是工程上很常用的设置能明显加快收敛。对障碍物极其密集的场景偏置太大会让扩展经常失败可以降低到 5%。steer()中的step_size_决定了单步扩展长度四轴在室内小空间我一般设 0.51.0 米室外可以到 2 米。步长过大会导致折线拐角更剧烈增加后续最小抖动轨迹的修剪压力步长过小则树节点爆炸重连时间变长。注意rng_的种子在测试时要固定比如rng_(42)否则每次规划结果不同没办法回归测试。真机运行时用std::random_device{}()或时间戳做种子即可。我一般把这段封装在构造函数里并通过setCollisionMap注入碰撞检测后端这样单元测试时可以用一个假地图。重连逻辑是 RRT* 区别于 RRT 的核心给出一个简化的rewire实现void RRTStar3D::rewire(int new_idx, const Eigen::Vector3d q_new) { // 遍历邻域内所有节点尝试把新节点作为它们的新父节点 // 如果 lowering cost 则更新父节点和代价 for (size_t i 0; i nodes_.size(); i) { if ((int)i new_idx) continue; double dist (nodes_[i].pos - q_new).norm(); if (dist search_radius_) continue; double potential nodes_[new_idx].cost dist; if (potential nodes_[i].cost) { // 这里还要做碰撞检测确保边不与障碍物相交 nodes_[i].parent new_idx; nodes_[i].cost potential; } } }注意这个简化版本没有在重连时调用collisionFree实际操作中必须在更新前检查nodes_[i].pos到q_new的连线是否无碰撞否则路径会斜穿障碍物。这也是很多 RRT* 实现里最隐蔽的 bug。2.3 核心参数表RRT* 必调的三组量参数默认值作用调参建议step_size_0.8 m单次扩展长度影响路径分辨率空间大取大障碍密集取小但不要小于飞行器半径的两倍goal_threshold_0.5 m认为到达目标的距离至少大于飞行器半径否则终点会跟障碍物相交search_radius_1.5 m重连时搜索邻域半径应大于 step_size_并根据节点数自适应衰减表中的search_radius_如果固定会导致两个问题树稀疏时重连效果差树密集时重连计算量爆炸。实践中可以用r min(search_radius_, gamma * sqrt(log(n)/n))让它随节点数 n 衰减gamma 是跟规划空间体积有关的常数通常取 510。当goal_threshold_过小时终点附近采样命中率太低树会在终点附近空转过大则导致最终路径离目标点较远后续飞控还要补一段直线。2.4 RRT* 与双向 RRT、混合 A* 的界限双向 RRT 从起点和目标同时扩展两棵树找到交点后合并能够显著提升搜索速度但它并不是最优的——没有重连机制两棵树的交点往往对应一条代价很高的折线。动态避障小车路径规划里双向 RRT 经常用来快速得出一条可行路径然后交给局部规划器做平滑。如果需要“快速找一条可用路径”双向 RRT 更合适如果更看重路径代价的下限RRT* 更稳妥。混合 A* 则完全不同它把连续状态空间离散到网格上同时用车辆运动学模型做前向仿真天然满足非完整约束但计算量比 RRT* 大一个量级。四轴飞行器是一个全向运动体x, y, z 三个方向都能平移所以 RRT* 的输出路径在几何上合法但在动力学上不合法——这就是下一章要引入最小抖动轨迹的原因。3. 从几何路径到最小抖动轨迹为什么折线不能直接给飞控3.1 四轴动力学与轨迹平滑的物理意义四轴飞行器的推力方向由姿态决定而姿态变化受电机响应和机体转动惯量限制。如果给飞控一条折线轨迹折点处速度方向突变四轴需要瞬间产生无限大的加速度这是不可能的。飞控能做的是用位置控制环去跟踪轨迹点但带宽有限跟踪折线的结果就是“切角”——路径从折点内侧绕过可能撞上障碍物。你仔细观察多旋翼的“点头”现象其实就是位置环在强行跟踪一个不可导的轨迹。最小抖动轨迹的“抖动”对应位置的四阶导数d^4 x / dt^4它在四轴飞行中表示推力变化率。最小化这个量相当于让电机推力的变化最平缓因此飞行器能更精确地跟踪。实际工程中也常用“最小加加速度”jerk三阶导但在高速竞赛中snap才是关键因为加加速度的突变同样会激发机体共振。3.2 快车道多项式轨迹优化原理常用做法是把每段轨迹表示为时间 t 的 7 阶多项式系数向量 c 有 8 个分量。起点和终点的位置、速度、加速度、加加速度各提供 2 个约束共 8 个所以 7 阶多项式恰好唯一确定一组系数。最小抖动优化可以写成一个二次规划QP问题min sum_{segment i} ∫ (p_i^(4)(t))^2 dt s.t. p_i(t_i) waypoint_i, p_i(t_i) v_i, p_i(t_i) a_i, p_i(t_i) j_i这里的p_i^(4)是四阶导。QP 的目标矩阵是 Hessian约束矩阵由多项式导数在时间点取值组成。在 C 里可以调用 OSQP 这类开源求解器也可以直接构造线性方程组求闭式解。闭式解法的代码简短适合嵌入式环境如果需要处理时间分配变化后的实时优化OSQP 更灵活。3.3 C 实现最小抖动轨迹生成7 阶多项式单段求解// min_snap_solver.cpp // 输入起点状态 p0, v0, a0, j0终点状态 p1, v1, a1, j1段时间 T // 输出7 阶多项式系数 c0..c7 Eigen::Matrixdouble, 8, 8 A; double T2 T * T, T3 T2 * T, T4 T2 * T2; double T5 T4 * T, T6 T5 * T, T7 T6 * T; A 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 0, 0, 1, T, T2, T3, T4, T5, T6, T7, 0, 1, 2*T, 3*T2, 4*T3, 5*T4, 6*T5, 7*T6, 0, 0, 2, 6*T, 12*T2, 20*T3, 30*T4, 42*T5, 0, 0, 0, 6, 24*T, 60*T2, 120*T3, 210*T4; Eigen::Matrixdouble, 8, 1 b; b p0, v0, a0, j0, p1, v1, a1, j1; Eigen::Matrixdouble, 8, 1 c A.colPivHouseholderQr().solve(b);这段代码里矩阵 A 的每一行分别对应一个约束等式前四行是起点状态后四行是终点状态。c中的元素按多项式升幂排列得到轨迹p(t) c0 c1 t ... c7 t^7。注意这里把幂运算展开成T2,T3等避免调用pow()在循环内执行时更高效也能减少浮点误差。colPivHouseholderQr().solve()比inverse()稳定7 阶矩阵的求解时间在微秒级。实际项目中如果路径有 N 个内部路径点需要把 N1 段多项式联立起来并加入内部点处速度、加速度、加加速度连续的约束形成带等式约束的 QP。为了快速落地我常用OSQP的稀疏矩阵形式建模约束行数等于 8*(N1) 3*N。在路径点少于 20 个时线性求解器的闭式解反而更快。3.4 时间分配最小抖动的隐藏魔数最小抖动轨迹的质量高度依赖时间分配。如果时间太短要求飞行器在极短时间走完长距离速度会非常大甚至超过电机限幅如果时间太长轨迹虽然平滑但毫无效率还可能被动态障碍物撞上。常见的做法是先用梯形速度规划估计每段最小时间然后乘一个安全系数。下面是我常用的参数表时间分配参数推荐值作用注意事项每段最小时间distance / v_max * 1.2保证速度不超限高动态场景可降到 1.1安全系数1.2~1.5补偿飞控延迟风大时取大值最大速度 v_max从飞控参数读取限制全段速度室内设 2 m/s室外可到 6 m/s最大加加速度连续可变影响路径点平滑度多段连接时必须一致另外时间分配中的绝对时间差会影响矩阵 A 的条件数。一段 0.5 s另一段 8 s条件数会飙升导致求解精度下降。解决办法是归一化每段先规划成 01 的相对时间求解后按真实时间缩放系数也就是做变量替换t T * tau再把多项式系数换算回来。4. 工程落地从算法原型到可运行的四轴规划节点4.1 模块划分与代码结构真正上机的四轴路径规划程序不会只有一个 RRT* 类。我一般会分成四层导航层接收目标点、管理全局路径、规划层RRT* 和轨迹生成、避障层局部重规划、接口层发布轨迹给下位机。在 C 项目里示例目录结构如下quad_planner/ ├── include/ │ ├── rrt_star_3d.h │ ├── minimum_snap.h │ ├── collision_map.h │ └── planner_manager.h ├── src/ │ ├── rrt_star_3d.cpp │ ├── minimum_snap.cpp │ ├── collision_map.cpp │ └── main_planner.cpp ├── CMakeLists.txt └── README.md这里collision_map是 RRT* 的碰撞检测后端。如果用 ROS2 环境直接接nav_msgs::msg::OccupancyGrid是最省事的方式如果做嵌入式可以用octomap的OcTree。注意在 CMake 里必须把 Eigen、nanoflann 和 OSQP 的 include 路径配置好建议用 CMake Tools 做跨平台编译。我在 VSCode 里会用cmake --build build触发构建launch.json里配一个cppdbg调试配置设置program参数到编译产物就能断点打断在 RRT* 和最小抖动求解函数里。项目源码里每个头文件我都会写 Doxygen 风格注释标明类的职责、关键方法的输入输出参数README里放参数说明和调参案例这样后续维护不需要看代码也能定位问题。4.2 碰撞检测与 KD 树加速别让 RRT* 慢成乌龟class CollisionMap { public: void setInflateRadius(double r) { inflate_radius_ r; } bool isFree(const Eigen::Vector3d p) const; bool isLineFree(const Eigen::Vector3d a, const Eigen::Vector3d b, int steps 10) const; private: std::vectorEigen::Vector3d occupied_centers_; double inflate_radius_ 0.3; };isLineFree中steps控制检测精度。太多会拖慢 RRT* 每步扩展太少可能漏检细窄障碍。我一般按距离自适应steps (b-a).norm() * 20。当障碍物数量超过 500 时线性扫描isFree会明显拖慢整体规划。可以用nanoflann构建 KD 树查询最近障碍物距离如果小于inflate_radius_就判定碰撞。下面是量级对比障碍物数量线性扫描耗时量级KD 树耗时量级10010^-5 s10^-6 s100010^-4 s10^-6 s1000010^-3 s10^-5 s静态地图里 KD 树只构建一次动态障碍环境则要支持插入和删除可以用nanoflann的KDTreeSingleIndexAdaptor配合std::vector做重建。注意在 RRT* 的循环里不要每次采样都调用 KD 树的重建函数否则反而更慢。4.3 与飞控的接口ROS2 与 MAVLink 的时间同步在 ROS2 环境下路径规划结果要发给飞控通常用OffboardControl模式。轨迹可以用px4_msgs::msg::TrajectorySetpoint发布但真正执行时飞控需要的是带时间戳的控制点。一个常见坑是只给飞控一条路径飞控内部按自己的时间线跟踪导致轨迹速度与规划阶段计算的速度不一致。解决办法是发布轨迹多项式本身而不是离散点。自定义消息可以这样填充auto msg quad_planner_msgs::msg::Trajectory(); msg.segment_count segs.size(); msg.start_time this-now(); msg.coefficients.resize(segs.size() * 8); for (size_t i 0; i segs.size(); i) { for (size_t j 0; j 8; j) { msg.coefficients[i * 8 j] segs[i].coeff(j); } } publisher_-publish(msg);接收端根据start_time和当前时间差求 t在所在段内用多项式求值。注意这里的时间戳应使用单调时钟不能用wall clock否则系统休眠或网络同步误差会直接转化为轨迹位偏差。MAVLink 的OFFBOARD模式也要确保飞控收到消息后立刻切换到轨迹跟踪模式不能先让飞控进入位置保持再切换那样会有 200 ms 左右的响应延迟。4.4 调试技巧在 RViz 中同时显示采样树和轨迹调试 RRT* 不用等真机RViz 里加两个 MarkerArray 就能看出问题。一个显示树的边颜色按节点代价从蓝到红渐变另一个显示最小抖动轨迹的密集采样点每 0.05 秒一个点。这样你可以直观看到轨迹是否穿越障碍物是否在折点附近有剧烈抖动。如果轨迹在路径点附近有波浪多半是时间分配不均匀或端点速度约束未设为零。如果树生长正常但终点区域扩展缓慢考虑把目标偏置概率提高到 20%或者把goal_threshold_增大到 0.8 米。还需要注意坐标系问题。RViz 中地图坐标系通常是map飞控坐标系是odom在显示树和轨迹之前必须把轨迹的坐标从规划坐标系转换到地图坐标系。我一般用tf2的TransformListener监听map到odom的变换否则轨迹看起来会在真实位置附近漂移干扰判断。5. 进阶抖动代价权重、时间分配和在线重规划5.1 让 RRT* 输出满足时间最优的布局普通 RRT* 的代价函数是路径长度但最小抖动轨迹的时间分配由路径形状决定。可以把 RRT* 的代价函数改为length lambda * turning_angle其中lambda取 0.51.5。这样树会倾向于平滑的路径后期最小抖动生成的压力小很多。在 C 里只需修改cost与rewire比较时的代价计算函数不影响整体结构。注意转角计算要基于父节点到当前点的方向避免每次重算整个路径。5.2 时间分配的 3 个必调参数每段最小时间t_min k * distance / v_maxk 通常取 1.1在狭窄通道中取 1.5。最大速度v_max从飞控参数里读不要硬编码。每个机架的最大速度差异很大室内小飞机和室外巡检机的v_max可能相差 3 倍。最大加速度a_max影响轨迹末端的跟踪精度在风力大的环境下往小调否则偏差积累会触发安全保护。5.3 验证方法仿真到真机的递进先在纯软件仿真中给地图加噪声观察轨迹是否始终避障。然后做硬件在环仿真HITL接入真实飞控参数对比轨迹指令和飞控反馈。真机初飞时把step_size_调到 0.5 米让路径更细密不要一开始就用大步长。如果飞控出现前后摇摆检查最小抖动轨迹的加加速度是否在约束范围内以及时间分配是否过紧。5.4 技巧固定周期触发在线重规划动态场景下每 0.5 秒重新执行一次 RRT*然后把当前飞控位置作为新起点保留原路径后半段作为目标引导。这样可以把计算量集中在局部避免重规划整个全局路径。注意重规划时必须清空旧树但保留终点偏置点否则节点越积越多单次规划时间会线性增长。还有一点重新规划后需要重新求解最小抖动轨迹时间分配应从当前时刻重新计算不能拿旧轨迹的剩余段直接拼接否则速度、加速度在拼接点会突变。这种情况下最小抖动轨迹生成的时间分配也需要按新起点重新计算而不是沿用旧轨迹的剩余段。本文还有配套的精品资源点击获取