简介针对四轴飞行器在复杂环境中的航迹规划问题这份C项目源码集成了RRT算法与最小抖动轨迹生成方法提供了从路径搜索到平滑轨迹输出的完整实现。适合机器人、自动化、电子信息等方向的在校学生和工程师作为课程设计、毕业设计或算法对比实验的参考资料。压缩包内共10个文件以6个C源文件为主配合头文件、CMake构建配置以及XML包配置可方便地编译与扩展压缩包整体仅21KB代码精简、逻辑集中另含README说明文档便于快速上手。目前已有49人学习代码经过运行验证对反馈的细节问题作者提供远程教学支持。下载后可基于现有框架替换地图输入、调整采样策略或修改代价函数快速验证RRT与最小抖动轨迹结合在不同场景下的性能表现。1. 四轴飞行器路径规划为什么要先采样再优化——RRT* C 的最小抖动组合四轴飞行器路径规划里RRT* 和最小抖动轨迹生成是最常见的组合之一尤其当你要在 C 里同时处理碰撞避让和轨迹平滑时。这套工程把路径搜索、坐标系变换、最小 SNAP 优化拆成独立模块适合做课设的计算机或自动化学生也适合想在一台 Linux 机器上跑通四轴规划管线的开发者。它解决的是一个很具体的问题先用 RRT* 找一条有障碍物语义的粗路径再用多项式轨迹让期望位置、速度、加速度连续可导。老手关心的是 RRT* 重连半径和 snap 代价权重怎么选新手最需要的是先搞清楚为什么不能直接把采样点连起来发给飞控。带着这两个问题去读源码会比直接跑 demo 更有收获。2. RRT* 搜索与最小抖动轨迹生成从采样到多项式优化的原理2.1 RRT* 的渐进最优性在哪里RRT* 和普通 RRT 的差别不在树的扩展方式而在加了一个 rewire 过程。普通 RRT 每轮只做采样、找最近节点、扩展、连接得到的路径是可行路径但边的选择完全依赖采样顺序路径代价往往会差一截。RRT* 在新节点加入后会以新节点为圆心以一定半径扫描已有节点如果通过新节点到达这些邻居的总代价比它们当前记录更低就改父节点这叫做 rewire。正是这个动作让 RRT* 在迭代次数趋于无穷时以概率 1 收敛到最优路径给后面的轨迹生成提供一个更好的初值。// RRT* 重连半径 near_radius 内的邻居都要检查 for (Node* neighbor : nearSet) { double newCost x_new-cost distance(x_new, neighbor); if (newCost neighbor-cost collisionFree(x_new, neighbor)) { neighbor-parent x_new; neighbor-cost newCost; } }上面代码里neighbor-cost存储从起点到该节点的累计代价x_new-cost是新节点自己的累计代价。这里用欧氏距离作为转入代价如果你换用 JPS 或 A* 的代价定义也需要同步调整。nearSet最好通过 KD 树查询得到工程里的DenseInput.h实际上是把地图/点云封装成这种查询的输入这样每次重连不用遍历整棵树上万个节点。对比项RRTRRT*渐进最优性不保证概率完备 渐进最优主要开销采样 碰撞检测碰撞检测 KD树近邻查询 rewiringC 实现复杂度低中需要维护 cost 和 parent对轨迹质量影响差路径较多锯齿好路径代价更接近最短这张表是选型时的第一层判断。实际工程里 RRT* 的收敛速度受搜索半径影响很大后面第 4 章会讲怎么把它设成跟step_size同一个量级。2.2 最小抖动轨迹最小化四阶导数的平方积分最小抖动minimum snap指的是让位置轨迹的四阶导数在整个时间段内尽可能小。四轴飞行器的电机转速变化很难瞬时完成snap 过大会造成期望加速度突变飞控跟踪时就会出现明显的超调。对每一维位置x(t)用 N 次多项式表示优化目标写成位置对时间求四阶导数后取平方在整个时间段内积分。x、y、z 三个轴分别求解最后合并为三维轨迹。由于每轴独立代码里往往只写一套函数再复用三次。// 计算一段轨迹中第 i 项与第 j 项系数对应的 cost 矩阵元素 double snapCostElement(int i, int j, double T) { if (i 4 || j 4) return 0.0; double d_i i * (i - 1) * (i - 2) * (i - 3); double d_j j * (j - 1) * (j - 2) * (j - 3); return d_i * d_j * std::pow(T, i j - 7) / (i j - 7); }i和j是多项式次数T为该段执行时间。四次求导后次数小于 4 的项全部为 0所以函数开头直接返回。计算公式的分子是两项的阶乘因子分母pow(T, ij-7) / (ij-7)来自对时间的积分。最后把每个分段的 cost 矩阵叠成一个大块对角矩阵交给二次规划求解器。注意这个公式只对一维有效三维需要重复生成三份再按 x、y、z 顺序排列。2.3 从折线路径到多项式轨迹为什么要加连续性约束RRT* 返回的粗路径是一系列折点直接把折点连起来速度在拐角处为 0加速度跳跃飞控没法跟。最小 SNAP 把每个折点当作分段边界条件设置当前段终点和下段起点位置连续同时让速度、加速度、加加速度甚至 snap 在交界处连续。这样整条轨迹不再有明显突变的加速度进入悬停时也不会出现末端过冲。求解时常见做法是把每段时间固定形成线性等式约束A_eq * coeff b_eq位置约束和连续性约束都塞进这一组等式里。但把 RRT* 的每一个折点都放进约束轨迹会被点绑死中间段仍然可能出现大角度转弯。一般我会先对粗路径做下采样按拐角大小筛选关键点只保留必要的边界条件。时间分配同样是基本面我建议先让各段时间和路径长度成正比再按最大速度/加速度做缩放。这个工程里的scaling.cpp就是干这件事的。3. 拆解 C 源码结构path_planning.cpp 与 trajectory_generator.cpp 如何配合3.1 源码目录与每个 .cpp 的职责这个工程的源码粒度拆得很科学。下载后可以先看CMakeLists.txt和package.xml然后按path_planning.cpp、trajectory_generator.cpp、scaling.cpp、goalpoint_transformer.cpp、transform.cpp的顺序读。old_path_planning.cpp是旧版实现先不用急着看等遇到解不了的问题再对比它和新版的差异。文件职责path_planning.cppROS 节点入口负责 RRT* 搜索并生成航点trajectory_generator.cpp接收航点生成最小 SNAP 分段多项式轨迹scaling.cpp时间缩放调整每段运行时间goalpoint_transformer.cpp把传入目标点转换成规划坐标系下的目标transform.cpp机体坐标系与世界坐标系的常用变换old_path_planning.cpp旧版路径规划实现留着做回归对比goalpoint_transformer我一般会理解成“目标点预处理”。四轴需要把世界坐标转到地图对齐的 frame还有可能做障碍物膨胀偏移。transform.cpp里最好只放线性变换和四元数转换不要放代价计算。DenseInput.h在include目录下给碰撞检测提供密集地图查询接口RRT* 采样时的collisionFree调用它。3.2 path_planning.cpp 里的 RRT* 主循环path_planning.cpp主循环逻辑和标准 RRT* 实现很接近但增加了工程化细节采样点可能来自用户指定目标或随机采样重连前必须先做碰撞检测避免把树连进障碍物。下面是一段贴合这个工程的伪代码级示意// 路径规划主循环采样、最近点、扩展、碰撞检测、重连 while (iter max_iter !reachedGoal) { Node* sample sampleR3(goal, goal_sample_rate); Node* nearest kdTree.findNearest(sample); Node* extended steer(nearest, sample, step_size); if (collisionFree(nearest, extended, dense_flags)) { extended-cost nearest-cost dist(nearest, extended); nearSet kdTree.radiuSearch(extended, rewire_radius); rewire(extended, nearSet); // 第 2.1 节里的关键步骤 tree.add(extended); if (dist(extended, goal) goal_threshold) reachedGoal true; } }sampleR3里的goal_sample_rate以一定概率直接采样目标点避免搜索目标附近不够。steer把新节点限制在step_size的步长内防止一次扩展跨越障碍物。collisionFree收到DenseInput.h承载的密集地图数据把扩展边离散成多个点逐点查。kdTree.radiuSearch是重连的性能瓶颈如果地图范围大而rewire_radius设得过大单次循环耗时可能增长 10 倍以上。3.3 trajectory_generator.cpp 的最小 SNAP 代价矩阵与求解trajectory_generator.cpp是这套代码的核心。它的输入是一组带时间的路径点输出是分段多项式系数。我一般会先在纸上把段数和多项式阶数算清楚每段 7 阶多项式意味着 8 个系数段间连续性约束会吃掉一部分自由度剩余自由度由目标函数决定。下面这段代码是复现最小 snap 代价矩阵时的常见写法跟这个工程的函数结构基本一致。// 将各段的 snap cost 矩阵拼接成整条轨迹的块对角 Q Eigen::MatrixXd Q Eigen::MatrixXd::Zero(coeffNum, coeffNum); for (int seg 0; seg segmentNum; seg) { int idx seg * segPolyOrder; for (int i 0; i segPolyOrder; i) for (int j 0; j segPolyOrder; j) Q(idx i, idx j) snapCostElement(i, j, segTime[seg]); } // 约束起点位置/速度/加速度终点位置以及段间位置速度连续性 Aeq * x beq; // 用稀疏矩阵拼装coeffNum等于segmentNum * segPolyOrderAeq每一行对应一个等式约束比如第 k 段终点位置等于第 k1 段起点位置。不能直接把 RRT* 的所有折点都塞进约束那样轨迹会被点绑死中间段仍然可能出现大角度转弯。常见做法是先对粗路径做下采样或按拐角大小筛选关键点只保留必要约束。如果工程里没有外部位姿求解器就用 Eigen 里现成的最小二乘或稀疏求解器严格保证连续性时也可以用闭式消元把约束代入目标函数只求未固定系数。3.4 goalpoint_transformer.cpp 与 scaling.cpp 的作用goalpoint_transformer.cpp看起来是软性的一环但它决定了 RRT* 的搜索空间。地图坐标系原点、起飞点和目标点通常不在一个 frame 里goalpoint_transformer负责把目标从用户输入的 frame 转到 map 或 odom 下并在转换后检查目标点是否被占据栅格覆盖。scaling.cpp则是修正每段时间路径短但中间低速度点少时间可以压缩路径长或分段较多时间必须放宽。一个常用做法是按路径段长度估计时间再按最大加速度修正// 根据路径段长度和期望最大速度估计时间 for (int i 0; i segments; i) { rawTime[i] segmentLength[i] / (0.8 * v_max); } // 再用最大加速度检验若加速度超限按 sqrt(2 * L / a_max) 方向调整分母 0.8 是为速度曲线留出的余量v_max取飞控允许最大速度的 80%。注意不能直接用L / v_max因为多项式轨迹的速度不会是恒定最大速度峰值会明显高于平均值。4. CMake 编译与参数调试让 RRT* 输出变成可执行轨迹4.1 编译环境准备与 CMakeLists 配置这套工程的 CMakeLists.txt 是 ROS/C 混合写法但核心算法不依赖 ROS只需要 Eigen3。在纯 Linux 环境下先把依赖装上再走 CMake 流程sudo apt install libeigen3-dev cmake build-essential mkdir -p build cd build cmake .. -DCMAKE_BUILD_TYPERelease make -j4代码主要使用Eigen::MatrixXd和std::pow如果编译时找不到 Eigen可以显示指定CMAKE_PREFIX_PATH。下面是一份与这个项目匹配的最小 CMakeLists 片段适合先不带 ROS 编译核心逻辑cmake_minimum_required(VERSION 3.10) project(path_planning) find_package(Eigen3 REQUIRED) add_executable(path_planning_node src/path_planning.cpp src/trajectory_generator.cpp src/transform.cpp src/goalpoint_transformer.cpp src/scaling.cpp ) target_include_directories(path_planning_node PRIVATE include) target_link_libraries(path_planning_node PRIVATE Eigen3::Eigen)如果项目同时是 ROS 包package.xml中一般声明 roscpp 和 tf2 依赖。把这条 CMake 路径走通后再用rosrun或 IDE 调试都方便。编译错误最多的地方在 Eigen 和std::pow重载ij-7为负数时pow会返回 1所以上一章的if (i 4 || j 4)判断不能省。4.2 核心参数与调参方向RRT* 部分的参数通常在path_planning.cpp顶部或 launch 文件里轨迹生成器的参数在trajectory_generator.cpp里。这个工程没有把所有参数都暴露成 launch直接改宏或构造函数入参是常见姿势。参数推荐范围影响max_iter3000 ~ 10000越大搜索越充分耗时线性增长rewire_radius1.0 ~ 3.0 m太小重连效果弱太大退化成 RRTstep_size0.3 ~ 1.0 m越小越容易贴近障碍物边界越慢goal_sample_rate0.05 ~ 0.2目标点采样概率太低收敛慢太高提早卡在局部segPolyOrder7 或 87 阶能保证到 snap 连续8 阶留余量max_v3 ~ 6 m/s期望最大速度不能超过飞控限制max_a2 ~ 5 m/s²影响时间分配的约束rewire_radius如果小于step_size新节点周围经常没有可重连节点一般取step_size的 2 倍以上。segPolyOrder取 7 是因为 7 阶多项式到四阶导数仍有自由度能同时满足位置到 snap 的边界条件。goal_sample_rate太高会让树过早集中在目标附近遇到障碍物时长时间绕不出去。4.3 把轨迹发布给控制器话题与期望状态输出trajectory_generator生成的结果在 ROS 里一般以期望位置、速度、加速度和 yaw 的数组发布常见是用nav_msgs/Odometry或自定义TrajectoryPointmessage。下面是一段在path_planning.cpp中发布轨迹的常见写法// 发布每一时刻的期望状态控制器内部做插值 nav_msgs::Odometry msg; msg.header.frame_id odom; msg.pose.pose traj.poseAt(t); msg.twist.twist.linear traj.velocityAt(t); msg.twist.twist.angular.z traj.yawAt(t); pub_.publish(msg);发送时用当前累计的t不是系统时间避免消息间隔不均匀导致速度计算偏差。如果你接 PX4/APM 的 MAVROS 接口需要把这种自定义话题做一层桥接。老一点的做法是把期望状态塞进视觉定位回调里一般不推荐回调频率抖动会直接影响轨迹平滑度。5. 最后一步的验证技巧时间缩放和轨迹平滑度怎么看5.1 scaling 先于调 Q 矩阵最小 SNAP 的优化结果受时间分配影响非常大。你常常会发现同样一组航点时间给 8 秒算出来的轨迹能飞给 4 秒就出现巨大的速度和加速度甚至直接违反约束。这时第一件事不是去改多项式阶数而是重算时间。实际项目里我会按路径长度估计时间再按最大加速度修正每段时间然后用修正后的时间重新生成 Q 矩阵。scaling.cpp在代码里做的就是这件事。常见错误是只对每段时间乘一个相同的缩放系数这会改变整条轨迹的速度分布导致某一段仍然炸掉。应该按每段长度和该段的最大弯道加速度独立调整。5.2 用可视化曲线验收轨迹轨迹是否平滑直接看加速度曲线。先发布期望状态话题再用rqt_plot把三个方向的期望加速度画出来比只看位置更直接。加速度曲线如果有尖角或跳出max_a范围就说明时间分配不合理。命令是rosrun rqt_plot rqt_plot /your_topic/desired_acceleration把话题名换成你自己发布的那个。如果不想开 GUI可以在scaling.cpp里加一段统计代码输出整条轨迹的最大速度、最大加速度和最大 snap 值。以四轴为例悬停状态下最大加速度超过 5 m/s² 往往已经接近飞控响应极限此时优先返回scaling.cpp放大时间段而不是去调 Q 矩阵里的权重。把不同max_a下的加速度曲线同时画在一起能直观看出时间缩放到底在发挥什么作用。本文还有配套的精品资源点击获取
RRT*与最小SNAP轨迹生成：C++四轴飞行器路径规划实现
简介针对四轴飞行器在复杂环境中的航迹规划问题这份C项目源码集成了RRT算法与最小抖动轨迹生成方法提供了从路径搜索到平滑轨迹输出的完整实现。适合机器人、自动化、电子信息等方向的在校学生和工程师作为课程设计、毕业设计或算法对比实验的参考资料。压缩包内共10个文件以6个C源文件为主配合头文件、CMake构建配置以及XML包配置可方便地编译与扩展压缩包整体仅21KB代码精简、逻辑集中另含README说明文档便于快速上手。目前已有49人学习代码经过运行验证对反馈的细节问题作者提供远程教学支持。下载后可基于现有框架替换地图输入、调整采样策略或修改代价函数快速验证RRT与最小抖动轨迹结合在不同场景下的性能表现。1. 四轴飞行器路径规划为什么要先采样再优化——RRT* C 的最小抖动组合四轴飞行器路径规划里RRT* 和最小抖动轨迹生成是最常见的组合之一尤其当你要在 C 里同时处理碰撞避让和轨迹平滑时。这套工程把路径搜索、坐标系变换、最小 SNAP 优化拆成独立模块适合做课设的计算机或自动化学生也适合想在一台 Linux 机器上跑通四轴规划管线的开发者。它解决的是一个很具体的问题先用 RRT* 找一条有障碍物语义的粗路径再用多项式轨迹让期望位置、速度、加速度连续可导。老手关心的是 RRT* 重连半径和 snap 代价权重怎么选新手最需要的是先搞清楚为什么不能直接把采样点连起来发给飞控。带着这两个问题去读源码会比直接跑 demo 更有收获。2. RRT* 搜索与最小抖动轨迹生成从采样到多项式优化的原理2.1 RRT* 的渐进最优性在哪里RRT* 和普通 RRT 的差别不在树的扩展方式而在加了一个 rewire 过程。普通 RRT 每轮只做采样、找最近节点、扩展、连接得到的路径是可行路径但边的选择完全依赖采样顺序路径代价往往会差一截。RRT* 在新节点加入后会以新节点为圆心以一定半径扫描已有节点如果通过新节点到达这些邻居的总代价比它们当前记录更低就改父节点这叫做 rewire。正是这个动作让 RRT* 在迭代次数趋于无穷时以概率 1 收敛到最优路径给后面的轨迹生成提供一个更好的初值。// RRT* 重连半径 near_radius 内的邻居都要检查 for (Node* neighbor : nearSet) { double newCost x_new-cost distance(x_new, neighbor); if (newCost neighbor-cost collisionFree(x_new, neighbor)) { neighbor-parent x_new; neighbor-cost newCost; } }上面代码里neighbor-cost存储从起点到该节点的累计代价x_new-cost是新节点自己的累计代价。这里用欧氏距离作为转入代价如果你换用 JPS 或 A* 的代价定义也需要同步调整。nearSet最好通过 KD 树查询得到工程里的DenseInput.h实际上是把地图/点云封装成这种查询的输入这样每次重连不用遍历整棵树上万个节点。对比项RRTRRT*渐进最优性不保证概率完备 渐进最优主要开销采样 碰撞检测碰撞检测 KD树近邻查询 rewiringC 实现复杂度低中需要维护 cost 和 parent对轨迹质量影响差路径较多锯齿好路径代价更接近最短这张表是选型时的第一层判断。实际工程里 RRT* 的收敛速度受搜索半径影响很大后面第 4 章会讲怎么把它设成跟step_size同一个量级。2.2 最小抖动轨迹最小化四阶导数的平方积分最小抖动minimum snap指的是让位置轨迹的四阶导数在整个时间段内尽可能小。四轴飞行器的电机转速变化很难瞬时完成snap 过大会造成期望加速度突变飞控跟踪时就会出现明显的超调。对每一维位置x(t)用 N 次多项式表示优化目标写成位置对时间求四阶导数后取平方在整个时间段内积分。x、y、z 三个轴分别求解最后合并为三维轨迹。由于每轴独立代码里往往只写一套函数再复用三次。// 计算一段轨迹中第 i 项与第 j 项系数对应的 cost 矩阵元素 double snapCostElement(int i, int j, double T) { if (i 4 || j 4) return 0.0; double d_i i * (i - 1) * (i - 2) * (i - 3); double d_j j * (j - 1) * (j - 2) * (j - 3); return d_i * d_j * std::pow(T, i j - 7) / (i j - 7); }i和j是多项式次数T为该段执行时间。四次求导后次数小于 4 的项全部为 0所以函数开头直接返回。计算公式的分子是两项的阶乘因子分母pow(T, ij-7) / (ij-7)来自对时间的积分。最后把每个分段的 cost 矩阵叠成一个大块对角矩阵交给二次规划求解器。注意这个公式只对一维有效三维需要重复生成三份再按 x、y、z 顺序排列。2.3 从折线路径到多项式轨迹为什么要加连续性约束RRT* 返回的粗路径是一系列折点直接把折点连起来速度在拐角处为 0加速度跳跃飞控没法跟。最小 SNAP 把每个折点当作分段边界条件设置当前段终点和下段起点位置连续同时让速度、加速度、加加速度甚至 snap 在交界处连续。这样整条轨迹不再有明显突变的加速度进入悬停时也不会出现末端过冲。求解时常见做法是把每段时间固定形成线性等式约束A_eq * coeff b_eq位置约束和连续性约束都塞进这一组等式里。但把 RRT* 的每一个折点都放进约束轨迹会被点绑死中间段仍然可能出现大角度转弯。一般我会先对粗路径做下采样按拐角大小筛选关键点只保留必要的边界条件。时间分配同样是基本面我建议先让各段时间和路径长度成正比再按最大速度/加速度做缩放。这个工程里的scaling.cpp就是干这件事的。3. 拆解 C 源码结构path_planning.cpp 与 trajectory_generator.cpp 如何配合3.1 源码目录与每个 .cpp 的职责这个工程的源码粒度拆得很科学。下载后可以先看CMakeLists.txt和package.xml然后按path_planning.cpp、trajectory_generator.cpp、scaling.cpp、goalpoint_transformer.cpp、transform.cpp的顺序读。old_path_planning.cpp是旧版实现先不用急着看等遇到解不了的问题再对比它和新版的差异。文件职责path_planning.cppROS 节点入口负责 RRT* 搜索并生成航点trajectory_generator.cpp接收航点生成最小 SNAP 分段多项式轨迹scaling.cpp时间缩放调整每段运行时间goalpoint_transformer.cpp把传入目标点转换成规划坐标系下的目标transform.cpp机体坐标系与世界坐标系的常用变换old_path_planning.cpp旧版路径规划实现留着做回归对比goalpoint_transformer我一般会理解成“目标点预处理”。四轴需要把世界坐标转到地图对齐的 frame还有可能做障碍物膨胀偏移。transform.cpp里最好只放线性变换和四元数转换不要放代价计算。DenseInput.h在include目录下给碰撞检测提供密集地图查询接口RRT* 采样时的collisionFree调用它。3.2 path_planning.cpp 里的 RRT* 主循环path_planning.cpp主循环逻辑和标准 RRT* 实现很接近但增加了工程化细节采样点可能来自用户指定目标或随机采样重连前必须先做碰撞检测避免把树连进障碍物。下面是一段贴合这个工程的伪代码级示意// 路径规划主循环采样、最近点、扩展、碰撞检测、重连 while (iter max_iter !reachedGoal) { Node* sample sampleR3(goal, goal_sample_rate); Node* nearest kdTree.findNearest(sample); Node* extended steer(nearest, sample, step_size); if (collisionFree(nearest, extended, dense_flags)) { extended-cost nearest-cost dist(nearest, extended); nearSet kdTree.radiuSearch(extended, rewire_radius); rewire(extended, nearSet); // 第 2.1 节里的关键步骤 tree.add(extended); if (dist(extended, goal) goal_threshold) reachedGoal true; } }sampleR3里的goal_sample_rate以一定概率直接采样目标点避免搜索目标附近不够。steer把新节点限制在step_size的步长内防止一次扩展跨越障碍物。collisionFree收到DenseInput.h承载的密集地图数据把扩展边离散成多个点逐点查。kdTree.radiuSearch是重连的性能瓶颈如果地图范围大而rewire_radius设得过大单次循环耗时可能增长 10 倍以上。3.3 trajectory_generator.cpp 的最小 SNAP 代价矩阵与求解trajectory_generator.cpp是这套代码的核心。它的输入是一组带时间的路径点输出是分段多项式系数。我一般会先在纸上把段数和多项式阶数算清楚每段 7 阶多项式意味着 8 个系数段间连续性约束会吃掉一部分自由度剩余自由度由目标函数决定。下面这段代码是复现最小 snap 代价矩阵时的常见写法跟这个工程的函数结构基本一致。// 将各段的 snap cost 矩阵拼接成整条轨迹的块对角 Q Eigen::MatrixXd Q Eigen::MatrixXd::Zero(coeffNum, coeffNum); for (int seg 0; seg segmentNum; seg) { int idx seg * segPolyOrder; for (int i 0; i segPolyOrder; i) for (int j 0; j segPolyOrder; j) Q(idx i, idx j) snapCostElement(i, j, segTime[seg]); } // 约束起点位置/速度/加速度终点位置以及段间位置速度连续性 Aeq * x beq; // 用稀疏矩阵拼装coeffNum等于segmentNum * segPolyOrderAeq每一行对应一个等式约束比如第 k 段终点位置等于第 k1 段起点位置。不能直接把 RRT* 的所有折点都塞进约束那样轨迹会被点绑死中间段仍然可能出现大角度转弯。常见做法是先对粗路径做下采样或按拐角大小筛选关键点只保留必要约束。如果工程里没有外部位姿求解器就用 Eigen 里现成的最小二乘或稀疏求解器严格保证连续性时也可以用闭式消元把约束代入目标函数只求未固定系数。3.4 goalpoint_transformer.cpp 与 scaling.cpp 的作用goalpoint_transformer.cpp看起来是软性的一环但它决定了 RRT* 的搜索空间。地图坐标系原点、起飞点和目标点通常不在一个 frame 里goalpoint_transformer负责把目标从用户输入的 frame 转到 map 或 odom 下并在转换后检查目标点是否被占据栅格覆盖。scaling.cpp则是修正每段时间路径短但中间低速度点少时间可以压缩路径长或分段较多时间必须放宽。一个常用做法是按路径段长度估计时间再按最大加速度修正// 根据路径段长度和期望最大速度估计时间 for (int i 0; i segments; i) { rawTime[i] segmentLength[i] / (0.8 * v_max); } // 再用最大加速度检验若加速度超限按 sqrt(2 * L / a_max) 方向调整分母 0.8 是为速度曲线留出的余量v_max取飞控允许最大速度的 80%。注意不能直接用L / v_max因为多项式轨迹的速度不会是恒定最大速度峰值会明显高于平均值。4. CMake 编译与参数调试让 RRT* 输出变成可执行轨迹4.1 编译环境准备与 CMakeLists 配置这套工程的 CMakeLists.txt 是 ROS/C 混合写法但核心算法不依赖 ROS只需要 Eigen3。在纯 Linux 环境下先把依赖装上再走 CMake 流程sudo apt install libeigen3-dev cmake build-essential mkdir -p build cd build cmake .. -DCMAKE_BUILD_TYPERelease make -j4代码主要使用Eigen::MatrixXd和std::pow如果编译时找不到 Eigen可以显示指定CMAKE_PREFIX_PATH。下面是一份与这个项目匹配的最小 CMakeLists 片段适合先不带 ROS 编译核心逻辑cmake_minimum_required(VERSION 3.10) project(path_planning) find_package(Eigen3 REQUIRED) add_executable(path_planning_node src/path_planning.cpp src/trajectory_generator.cpp src/transform.cpp src/goalpoint_transformer.cpp src/scaling.cpp ) target_include_directories(path_planning_node PRIVATE include) target_link_libraries(path_planning_node PRIVATE Eigen3::Eigen)如果项目同时是 ROS 包package.xml中一般声明 roscpp 和 tf2 依赖。把这条 CMake 路径走通后再用rosrun或 IDE 调试都方便。编译错误最多的地方在 Eigen 和std::pow重载ij-7为负数时pow会返回 1所以上一章的if (i 4 || j 4)判断不能省。4.2 核心参数与调参方向RRT* 部分的参数通常在path_planning.cpp顶部或 launch 文件里轨迹生成器的参数在trajectory_generator.cpp里。这个工程没有把所有参数都暴露成 launch直接改宏或构造函数入参是常见姿势。参数推荐范围影响max_iter3000 ~ 10000越大搜索越充分耗时线性增长rewire_radius1.0 ~ 3.0 m太小重连效果弱太大退化成 RRTstep_size0.3 ~ 1.0 m越小越容易贴近障碍物边界越慢goal_sample_rate0.05 ~ 0.2目标点采样概率太低收敛慢太高提早卡在局部segPolyOrder7 或 87 阶能保证到 snap 连续8 阶留余量max_v3 ~ 6 m/s期望最大速度不能超过飞控限制max_a2 ~ 5 m/s²影响时间分配的约束rewire_radius如果小于step_size新节点周围经常没有可重连节点一般取step_size的 2 倍以上。segPolyOrder取 7 是因为 7 阶多项式到四阶导数仍有自由度能同时满足位置到 snap 的边界条件。goal_sample_rate太高会让树过早集中在目标附近遇到障碍物时长时间绕不出去。4.3 把轨迹发布给控制器话题与期望状态输出trajectory_generator生成的结果在 ROS 里一般以期望位置、速度、加速度和 yaw 的数组发布常见是用nav_msgs/Odometry或自定义TrajectoryPointmessage。下面是一段在path_planning.cpp中发布轨迹的常见写法// 发布每一时刻的期望状态控制器内部做插值 nav_msgs::Odometry msg; msg.header.frame_id odom; msg.pose.pose traj.poseAt(t); msg.twist.twist.linear traj.velocityAt(t); msg.twist.twist.angular.z traj.yawAt(t); pub_.publish(msg);发送时用当前累计的t不是系统时间避免消息间隔不均匀导致速度计算偏差。如果你接 PX4/APM 的 MAVROS 接口需要把这种自定义话题做一层桥接。老一点的做法是把期望状态塞进视觉定位回调里一般不推荐回调频率抖动会直接影响轨迹平滑度。5. 最后一步的验证技巧时间缩放和轨迹平滑度怎么看5.1 scaling 先于调 Q 矩阵最小 SNAP 的优化结果受时间分配影响非常大。你常常会发现同样一组航点时间给 8 秒算出来的轨迹能飞给 4 秒就出现巨大的速度和加速度甚至直接违反约束。这时第一件事不是去改多项式阶数而是重算时间。实际项目里我会按路径长度估计时间再按最大加速度修正每段时间然后用修正后的时间重新生成 Q 矩阵。scaling.cpp在代码里做的就是这件事。常见错误是只对每段时间乘一个相同的缩放系数这会改变整条轨迹的速度分布导致某一段仍然炸掉。应该按每段长度和该段的最大弯道加速度独立调整。5.2 用可视化曲线验收轨迹轨迹是否平滑直接看加速度曲线。先发布期望状态话题再用rqt_plot把三个方向的期望加速度画出来比只看位置更直接。加速度曲线如果有尖角或跳出max_a范围就说明时间分配不合理。命令是rosrun rqt_plot rqt_plot /your_topic/desired_acceleration把话题名换成你自己发布的那个。如果不想开 GUI可以在scaling.cpp里加一段统计代码输出整条轨迹的最大速度、最大加速度和最大 snap 值。以四轴为例悬停状态下最大加速度超过 5 m/s² 往往已经接近飞控响应极限此时优先返回scaling.cpp放大时间段而不是去调 Q 矩阵里的权重。把不同max_a下的加速度曲线同时画在一起能直观看出时间缩放到底在发挥什么作用。本文还有配套的精品资源点击获取
