基于 Fast_LIO 架构将局部规划从 Pure Pursuit 迁移至 TEB 算法的实践

工程算法修改方法论：以 Fast_LIO 导航系统为例

在工程实践中修改现有代码算法时，核心原则是'黑盒替换（接口对齐）'。以下以从基于 Fast_LIO 框架的导航代码修改为采用 TEB 算法的全过程为例。

第一部分：拆解原系统的框架和数据流

原系统的数据流向图（核心主线）

感知端：3D 雷达 -> 障碍物提取
- Livox 雷达输出点云 -> dual_radar_extractor（利用 Patchwork++ 算法把地面、楼梯滤除） -> 输出纯障碍物点云（估计话题名叫 /obstacle_cloud 或类似名称）。
- 知识点：行业经验储备：当 SLAM 工程师看到 ground_seg (Ground Segmentation，地面分割) 和 patchworkpp 这两个词放在一起时，容易联想到其功能。Patchwork++ 是一篇著名的开源论文算法，专门解决复杂 3D 地形（包括斜坡、楼梯、坑洼）的地面分割问题。它输出两个结果：
  1. ground_cloud（可通行的地面/楼梯点云）
  2. non_ground_cloud / obstacle_cloud（墙壁、行人等纯障碍物点云）
局部规划端：调整局部规划参数的 pure_pursuit 节点（集成方案）
- 它接收 /global_path 和 /scan（或局部代价地图）。
- 工程师在 pure_pursuit 里面塞入了一个小型的 A* 和 Dijkstra 算法（从 yaml 的 enable_local_replanning: true 可以看出）。
- 当遇到障碍物时，它先用小 A* 绕开，然后再用自带的 path_smoother_node 把这段绕开的路径变平滑，最后再用纯追踪算法（Pure Pursuit）去算左右轮该转多快。
- 最终输出速度指令（话题：/cmd_vel）。
- 打开 pure_pursuit_params.yaml：
```
cmd_vel_topic: "/cmd_vel"  # 控制指令话题
```
- 打开 pure_pursuit_local_planner.launch：
```

<arg name="cmd_vel_topic" default="/cmd_vel" />

<remap from="/cmd_vel" to="$(arg cmd_vel_topic)" />
```
- 知识点：ROS Remap 机制：在 ROS C++ 代码里，开发者通常会写死一个内部名字（比如 from="/cmd_vel"）。但为了让这个节点能适配不同的机器人，会在 launch 文件里用 <remap> 把它映射到系统真实的话题名（to="$(arg cmd_vel_topic)"）。因为这里的 default 是 /cmd_vel，所以可以确信，底层底盘（go2_ros_control）接收速度的话题就是 /cmd_vel。
全局规划端：全局地图 -> 3D 全局路径
- global_path_planner 在你构建好的拓扑地图和空心 3D PCD 中寻路 -> 输出带有高度的 3D 全局路径（话题：/global_path）。
- 确认接口话题：打开 pure_pursuit_params.yaml：
```
global_path_topic: "/global_path"  # 全局路径话题
```
- 知识点：发布/订阅契约：局部规划器（Pure Pursuit）是'消费者'，它订阅了 /global_path。在 ROS 的世界里，有消费者就必然有'生产者'。谁是生产者？必然是全局规划器（Global Planner）。所以，全局规划器一定在往 /global_path 这个话题发数据。
- 确认全局规划的逻辑（拓扑+PCD）：回头看目录树：
```
./global_planner/topology_rviz_plugin/map: connected_topology_map.txt key_nodes_map.txt
./global_planner/pcd_map/pcd: aft_pgo_free_space.pcd
```
  我看到了拓扑节点文本（key_nodes_map.txt），看到了 PGO（位姿图优化）之后提取的空心自由空间点云（aft_pgo_free_space.pcd）。这就证明了：你的全局规划器在工作时，是先读取这些预先建好的 3D 地图，用图搜索算法（A* 或 Dijkstra）在拓扑点之间连线，最终生成一条具有 (x, y, z) 三维坐标的路径，发到了 /global_path 上。

原系统痛点

为什么觉得导航效果不佳？因为上一个工程师用传统 A* 去做局部动态避障，A* 是没有'时间'和'机器人运动学（加速度、最大角速度）'概念的，算出来的路机器人走起来很生硬；另外他还加了人工势场法（Potential Field），这很容易让四足机器人在狭窄走廊里陷入'死锁（卡在原地左右乱晃）'。

enable_local_replanning: true       # 启用了局部重规划
dijkstra_obstacle_threshold: 10     # 竟然在纯追踪里套了一个 Dijkstra 算法！
potential_field:                    # 甚至还加了'人工势场法'！
attractive_gain: 8.0                # 吸引力
repulsive_gain: 1.0                 # 排斥力

第二部分：讲解工程改造的逻辑方法论

当我们要将原先的 pure_pursuit 替换为 TEB 时，标准的工作流是这样的：

步骤 1：找到要替换的'黑盒'

在这个项目里，'黑盒'就是 pure_pursuit_local_planner_node。

步骤 2：明确'黑盒'的输入和输出接口（Interface）

需要什么（输入）：
1. 机器人当前位置（Odometry / TF 树）
2. 全局路径 (nav_msgs/Path，话题 /global_path)
3. 障碍物信息（局部代价地图 Local Costmap）
产出什么（输出）：
- 底盘速度指令 (geometry_msgs/Twist，话题 /cmd_vel)

第三部分：给出 TEB 最终版代码

步骤 1：创建 TEB 功能包

在你的终端里运行（如果已经建了就跳过）：

cd ~/ws_nav/src/go2_real/navigation_3d/local_planner
catkin_create_pkg teb_local_tracker roscpp tf2 tf2_ros costmap_2d teb_local_planner nav_msgs geometry_msgs
cd teb_local_tracker
mkdir config launch

步骤 2：写入最终版 YAML 配置文件

新建 config/teb_costmap_params.yaml，填入以下内容（注意看填入的雷达话题名）：

# ================= 局部代价地图配置 =================
local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5
  rolling_window: true
  width: 5.0
  height: 5.0
  resolution: 0.05
  plugins:
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
  obstacle_layer:
    observation_sources: point_cloud_sensor
    point_cloud_sensor:
      sensor_frame: mid360_link
      data_type: PointCloud2
      topic: /dual_radar_traversable_extractor/obstacle_cloud
      marking: true
      clearing: true
      min_obstacle_height: 0.1
      max_obstacle_height: 1.5
  inflation_layer:
    inflation_radius: 0.35

步骤 3：写入核心 C++ 节点

在 src/teb_tracker_node.cpp 中填入这个极简的包装器（已优化过）：

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <teb_local_planner/teb_local_planner_ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>

class TebTracker {
public:
  TebTracker(tf2_ros::Buffer& tf) : costmap_ros_("local_costmap", tf) {
    teb_planner_.initialize("TebLocalPlannerROS", &tf, &costmap_ros_);
    path_sub_ = nh_.subscribe("/global_path", 1, &TebTracker::pathCallback, this);
    vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    timer_ = nh_.createTimer(ros::Duration(0.1), &TebTracker::controlLoop, this);
    ROS_INFO("TEB Tracker Initialized! Ready to dodge obstacles and climb stairs!");
  }
private:
  void pathCallback(const nav_msgs::Path::ConstPtr& msg) {
    current_path_ = *msg;
    std::vector<geometry_msgs::PoseStamped> plan = msg->poses;
    teb_planner_.setPlan(plan);
  }
  void controlLoop {
     (current_path_.poses.()) ;
    geometry_msgs::Twist cmd_vel;
     (teb_planner_.(cmd_vel)) {
      vel_pub_.(cmd_vel);
    }  {
      (, );
      cmd_vel.linear.x = ;
      cmd_vel.angular.z = ;
      vel_pub_.(cmd_vel);
    }
  }
  ros::NodeHandle nh_;
  ros::Subscriber path_sub_;
  ros::Publisher vel_pub_;
  ros::Timer timer_;
  costmap_2d::Costmap2DROS costmap_ros_;
  teb_local_planner::TebLocalPlannerROS teb_planner_;
  nav_msgs::Path current_path_;
};

{
  ros::(argc, argv, );
  ;
  ;
  ;
  ros::();
   ;
}

步骤 4：写入 Launch 文件

新建 launch/teb_tracker.launch：

<?xml version="1.0"?>
<launch>
  <node pkg="teb_local_tracker" type="teb_tracker_node" name="teb_tracker_node" output="screen">
    <rosparam file="$(find teb_local_tracker)/config/teb_costmap_params.yaml" command="load" />
  </node>
</launch>

步骤 5：修改 CMakeLists.txt

打开 CMakeLists.txt，在最底部加上这两行，告诉系统去编译你的 C++ 文件：

add_executable(teb_tracker_node src/teb_tracker_node.cpp)
target_link_libraries(teb_tracker_node ${catkin_LIBRARIES})

编译与测试

终极验证：跑起 3_nav_start.sh！如果在终端里看到了绿色字体：TEB Tracker Initialized! Ready to dodge obstacles and climb stairs! 那就大功告成了！你可以给四足机器人发一个包含楼梯的导航目标点，看看它是不是能平滑地避开障碍物，并且勇敢地踏上楼梯。
替换启动脚本：打开最开始的那个大总管脚本 3_nav_start.sh，把原本启动 pure_pursuit 和 path_smoother 的行注释掉，加上这行：

# ----------------- 注释掉原有的 pure pursuit -----------------
# launch_bg "pure pursuit" roslaunch pure_pursuit_local_planner pure_pursuit_local_planner.launch
# sleep 1
# ----------------- 保留并确保启动雷达可通行区域提取 -----------------
# 这个必须启动，因为它的 patchworkpp 会把楼梯识别为地面，把人识别为障碍物
launch_bg "dual radar extractor" roslaunch dual_radar_traversable_extractor dual_radar_extractor.launch
sleep 3
# ----------------- 新增启动 TEB Tracker -----------------
launch_bg "TEB local tracker" roslaunch teb_local_tracker teb_tracker.launch
sleep 1

编译代码：回到工作空间根目录，执行编译：

cd ~/ws_nav
catkin_make
source devel/setup.bash

注意：如果提示找不到 teb_local_planner，记得先运行 sudo apt-get install ros-noetic-teb-local-planner 安装。

总结：工程修改的方法论

修改一个大型的机器人工程，绝对不是拿到代码就开始改 C++。标准流程是：

看文件结构和命名（找轮子）：看到 patchworkpp 就知道地面分割不用愁了；看到 topology 就知道全局规划是 3D 图搜索。
看 Launch 和 Yaml（摸清数据流）：从 remap 找话题名字，从 param 找上一个工程师的算法逻辑（比如发现了坑人的势场法）。
确定边界，黑盒替换：确认老规划器吃的是 Path，吐的是 Twist(cmd_vel)。我们写个新的 TEB 节点，同样吃 Path 吐 Twist。
最后才是敲代码：把新的 C++ 节点写出来，放到系统里。

基于 Fast_LIO 架构将局部规划从 Pure Pursuit 迁移至 TEB 算法的实践

工程算法修改方法论：以 Fast_LIO 导航系统为例

第一部分：拆解原系统的框架和数据流

原系统的数据流向图（核心主线）

原系统痛点

第二部分：讲解工程改造的逻辑方法论

步骤 1：找到要替换的'黑盒'

步骤 2：明确'黑盒'的输入和输出接口（Interface）

第三部分：给出 TEB 最终版代码

步骤 1：创建 TEB 功能包

步骤 2：写入最终版 YAML 配置文件

步骤 3：写入核心 C++ 节点

步骤 4：写入 Launch 文件

步骤 5：修改 CMakeLists.txt

编译与测试

总结：工程修改的方法论

更多推荐文章

相关免费在线工具

基于 Fast_LIO 架构将局部规划从 Pure Pursuit 迁移至 TEB 算法的实践

工程算法修改方法论：以 Fast_LIO 导航系统为例

第一部分：拆解原系统的框架和数据流

原系统的数据流向图（核心主线）

原系统痛点

第二部分：讲解工程改造的逻辑方法论

步骤 1：找到要替换的'黑盒'

步骤 2：明确'黑盒'的输入和输出接口（Interface）

第三部分：给出 TEB 最终版代码

步骤 1：创建 TEB 功能包

步骤 2：写入最终版 YAML 配置文件

步骤 3：写入核心 C++ 节点

步骤 4：写入 Launch 文件

步骤 5：修改 CMakeLists.txt

编译与测试

总结：工程修改的方法论

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具