FAR Planner实战：从仿真到真机部署的避坑指南

1. FAR Planner核心原理与仿真环境搭建

FAR Planner作为基于动态可见度图的路径规划算法，其核心在于实时构建和更新环境的多边形表示。我在实际部署中发现，这种表示方法比传统栅格地图更节省内存资源，特别是在处理大范围场景时优势明显。规划器采用双线程架构：主线程负责路径搜索，辅助线程以20%的CPU占用率持续更新可见度图，这种设计让我的树莓派4B也能流畅运行。

搭建仿真环境时，推荐使用官方提供的Autonomous Exploration Development Environment。最近在Ubuntu 22.04上测试时，需要特别注意ROS Noetic的依赖项变化。除了基础的libusb-dev，还需要额外安装：

bash复制sudo apt install libpcl-dev ros-noetic-navigation ros-noetic-tf2-sensor-msgs

编译过程中最常见的坑是mesh文件下载失败。实测发现可以通过修改download_environments.sh脚本中的下载源来解决：

bash复制# 替换原下载链接为国内镜像
wget https://ghproxy.com/https://github.com/HongbiaoZ/autonomous_exploration_development_environment/raw/main/src/vehicle_simulator/meshes/environments.zip

2. 真机部署的五大关键适配

2.1 传感器数据接口改造

真实机器人通常使用Velodyne或Livox雷达，与仿真环境的虚拟传感器输出存在差异。需要修改vehicle_simulator/launch/system_real_robot.launch文件中的点云话题映射。最近帮客户部署时，发现Livox雷达需要特别添加以下转换：

xml复制<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pc2scan">
    <remap from="cloud_in" to="/livox/lidar"/>
    <param name="target_frame" value="base_link"/>
    <param name="range_min" value="0.5"/>
</node>

2.2 TF树动态校准方案

真实环境中TF树断裂是高频问题。我的经验是增加动态TF广播节点，在far_planner_node.cpp中插入：

cpp复制static tf2_ros::StaticTransformBroadcaster static_broadcaster;
geometry_msgs::TransformStamped static_transform;
static_transform.header.stamp = ros::Time::now();
static_transform.transform.translation.z = 0.2;  // 雷达安装高度补偿
static_broadcaster.sendTransform(static_transform);

2.3 控制器接口兼容性处理

不同机器人的控制接口差异很大。对于常见的CAN总线协议，需要修改src/far_planner/src/waypoint_example.cpp中的控制指令生成逻辑。最近适配AGV小车时，我增加了速度平滑处理：

cpp复制// 添加低通滤波器
filtered_velocity = 0.8 * filtered_velocity + 0.2 * current_velocity;

3. 真实场景特殊问题解决方案

3.1 动态障碍物误判处理

在工厂环境中，金属反光经常被误判为障碍物。可以通过调整src/local_planner/src/utility.cpp中的点云过滤参数：

cpp复制pcl::PassThrough<pcl::PointXYZ> pass;
pass.setFilterLimits(-5.0, 5.0);  // 限制有效高度范围
pass.setFilterFieldName("z");

3.2 时间戳同步终极方案

多传感器时间不同步会导致TF变换失败。除了设置use_sim_time参数，还需要在启动时强制时间同步：

bash复制rosparam set /use_sim_time true
while [ "$(rosparam get /use_sim_time)" != "true" ]; do sleep 0.1; done

4. 性能优化实战技巧

4.1 可见度图更新策略

在仓库场景中，可以通过修改far_planner/src/visibility_graph.cpp来优化性能：

cpp复制void VisibilityGraph::updateGraph() {
    if(obstacle_changes_ < 5) return;  // 变化较小时跳过更新
    // ...原有逻辑
}

4.2 内存占用优化

对于资源受限的嵌入式设备，调整CMakeLists.txt中的编译选项：

cmake复制add_definitions(-DUSE_ARM_NEON)  # ARM平台启用NEON指令集
set(CMAKE_CXX_FLAGS "-O3 -mfpu=neon-vfpv4")

5. 典型故障排查手册

最近三个月收集的常见错误及解决方案：

1. 点云丢失问题

   • 现象：RVIZ中看不到点云
   • 检查：rostopic hz /registered_scan
   • 解决：确认雷达驱动是否正确加载，检查tf树是否完整

2. 规划路径抖动

   • 现象：机器人运动轨迹不平滑
   • 调试：rosrun rqt_plot rqt_plot /cmd_vel/linear/x
   • 优化：调整local_planner.yaml中的smoothing_factor参数

3. CPU占用率过高

   • 现象：树莓派温度报警
   • 监控：htop -p $(pgrep far_planner)
   • 限制：使用cgroups限制CPU使用量

在真实机器人上部署时，建议先用低成本开发平台（如TurtleBot3）验证，再迁移到工业级设备。最近用Jetson Xavier NX部署时，发现需要特别关注散热问题，加装散热片后稳定性提升明显。