简介自主导航技术是移动机器人和无人机系统的核心能力其关键在于可靠的状态估计、环境建模与轨迹规划。在无GPS环境下传感器融合与实时避障成为工程难点。本文将围绕基于ROS2的机器人运动规划与避障系统深入解析视觉惯性里程计如何提供定位基准ESDF距离场如何加速碰撞检测以及A*全局路径搜索与Ego-Planner局部轨迹优化如何协同工作。同时讨论YOLOv4-tiny目标检测在动态降落平台追踪中的应用梳理从硬件选型、代码架构到实机调试的完整链路。该方法适用于无人机自主导航、地面机器人避障及ROS1向ROS2迁移等典型场景为构建高可靠性的自主移动系统提供工程参考。 前阵子正好做了一套基于ROS2的机器人运动规划与避障系统前前后后折腾了两个月最后整理成一个zip工程。有人问我是怎么把全局规划、局部避障、动态目标检测和自主降落串起来的今天干脆把这套系统的设计和实现思路完整梳理一遍。重点讲我为什么这么选型、代码结构怎么组织、每一步怎么调试以及那些文档里根本不会写的坑。如果你正准备做无人机自主导航、移动机器人局部避障或者想把手头的ROS1项目迁到ROS2这篇应该对你有用。1. 项目到底在做什么需求拆解与方案选型1.1 一个典型的自主导航任务这个项目的核心场景是无GPS环境下机器人或者无人机需要靠机载传感器独立完成几件事实时确定自己在哪里、在未知环境里构建地图、规划出一条能到目标点的路径同时躲避动态障碍物最终在一个移动平台上完成降落。听起来不算复杂但把每个词拆开就很要命。首先是“无GPS”意味着所有定位和状态估计都只能依赖机载视觉、惯性测量单元这类传感器其次是“动态避障”障碍物不是静止的墙和柱子可能有移动的车辆、人员或者其他飞行器最后是“自主降落”降落目标本身还在移动需要一边追踪一边生成下降轨迹。整个任务链是感知、建图、规划、控制的持续闭环缺一环都跑不通。这里我明确一下系统边界全局路径负责在已知或逐步探索的地图上给出一个稀疏的导航路线局部规划负责在传感器能感知到的范围里生成一条平滑、安全、动力学可行的轨迹目标检测模块负责识别降落平台并持续跟踪最后把生成好的轨迹交给底层飞控或者运动控制器去执行。1.2 为什么选ROS2而不是继续在ROS1里凑合我在项目早期确实纠结过这个问题。很多经典算法包比如Ego-Planner、A*路径规划、体素地图构建最早都是基于ROS1写的社区里ROS1的资料也明显更多。但最后我还是选了ROS2原因是这个项目对多机协同、节点状态管理和通信实时性有比较明确的需求。ROS2的核心通信基于DDS天然支持分布式部署。机载电脑Jetson TX2、地面站、可能还有第二台计算节点之间可以直接通过话题和服务通信不需要像ROS1那样额外搭一套master。此外ROS2引入节点生命周期管理、参数动态配置、launch文件统一启动这些机制在实际调试复杂系统时帮助很大。当然迁移成本真实存在。很多工程里你看到的是ROS1的launch文件、rospy或者roscpp接口迁到ROS2之后话题名、CLI命令、参数系统、tf2用法都有变化。不过现在ROS2的生态已经非常完整humble版本基本覆盖了导航、建图、机械臂控制这些主流方向新版项目直接基于ROS2做没有太大问题。2. 整体架构怎么搭从感知到执行的模块划分2.1 系统分层与数据流整套系统在逻辑上分四层感知层、建图与定位层、规划决策层、控制输出层。感知层的输入是机载相机和IMU。这里我用的是RGB-D相机也就是同时输出彩色图和深度图深度图用来构建占据地图和ESDF距离场彩色图用来跑目标检测。IMU数据用于视觉里程计的矫正和短时间状态预测。建图与定位层把感知数据融合成两个关键产物一个是机器人当前在世界坐标系下的位姿一个是描述周围环境障碍物分布的ESDF地图。ESDF全称是Euclidean Signed Distance Field即欧氏符号距离场它把环境表示成一张栅格场每个栅格存的是它到最近障碍物的距离。这个表示对轨迹优化特别友好因为代价函数可以直接查询距离场得到一个梯度方向。规划决策层拿到当前位姿、目标点、ESDF地图、障碍物动态信息后先做一次全局路径搜索A*再在局部用Ego-Planner做轨迹优化同时根据目标检测结果判断是否进入降落决策状态。控制输出层在无人机场景就是PX4飞控。这里我们不再直接输出电机PWM而是通过MAVROS或者ROS2的PX4接口把轨迹生成的位置、速度期望值发给飞控由飞控底层姿态控制器去执行。2.2 核心组件选型与理由模块选型方案选型理由机载平台Jetson TX2功耗低自带GPU可以加速神经网络和距离场计算接口全飞控PX4开源、稳定支持对外部位置和速度指令的接管自定位RGB-D相机 IMU 视觉里程计无GPS环境下的主流方案RGB-D直接给深度省去双目标定地图表示体素地图 ESDF体素地图适合增量更新ESDF直接服务轨迹优化全局规划A*算法实现简单、稳定在栅格地图上很容易得到全局可行路径局部规划Ego-Planner能够生成光滑、动力学可行的轨迹支持动态环境避障目标检测YOLOv4-tiny轻量级网络Jetson TX2上能跑实时检测降落平台足够可视化RViz2ROS2官方工具方便实时看轨迹、地图和检测框很多初学者会问为什么全局和局部不直接用同一个算法。我解释一下全局路径只需要拓扑上的可行性不要求平滑A*在一张栅格图上的搜索效率和实现成本是最划算的。局部规划由于要实时避开动态障碍物而且必须考虑机器人/无人机的动力学约束所以需要一个能输出平滑轨迹的优化器Ego-Planner正好解决这个问题。两者配合既不会让规划器陷入局部极小又能在局部做高频避障调整。3. 核心模块实现细节地图、规划和避障3.1 无GPS定位与ESDF地图构建先讲定位。整套系统在无GPS环境里的基础是视觉惯性里程计。RGB-D相机给出深度图像后我们通过特征点匹配估算帧间位姿变化然后用IMU做帧间插值和短期预测。视觉里程计在快速旋转或者纹理稀疏的环境里容易漂移IMU虽然长期有零偏但短期内的角速度和加速度测量非常可靠所以两者做紧耦合或者松耦合是当前工程上最稳妥的做法。这里有一个绕不开的关键点TF坐标树要维护正确。整个系统里有多个坐标系相机坐标系、IMU坐标系、机体坐标系、世界坐标系、目标检测框坐标系任何一个TF关系写错后面的地图和规划全部是乱的。调试的时候我习惯先用静态TF把相机到机体的外参标定好再用rviz2里的TF显示插件逐帧检查坐标系方向。ESDF的地图构建流程大致是先根据深度图把空间离散成体素有障碍物占据的体素标为occupied空区域标为free然后对整张图做距离传播计算。距离传播的思路类似于把障碍物表面当作波源向外扩散每个体素记录到最近表面的距离。Jetson TX2上我们实现了并行版本用GPU加速距离场更新这样地图可以保持较快频率刷新让局部规划器始终基于最新的环境信息做避障。如果硬件上不想依赖GPU也可以纯CPU跑ESDF但分辨率不能拉太高。实测经验是0.2米分辨率的地图在CPU上更新一次需要几十毫秒勉强能接受但低于0.1米就会明显拖慢整条链路所以是否上GPU取决于你的实时性要求。3.2 全局路径与局部轨迹规划如何配合整个规划模块的运行流程可以总结成一个循环先由A*在地图上搜出一条从当前位置到目标点的栅格路径再把这条路径的关键点作为局部规划的参考导向Ego-Planner以这些导向点和ESDF地图为输入生成一条满足动力学约束的平滑轨迹。A的实现本身不复杂但有几个细节直接影响效果。启发式函数我选的是欧氏距离比曼哈顿距离在八连通栅格上更准确。地图的代价要考虑障碍物距离离障碍物太近的栅格要增大代价这样搜索出来的路径不会贴墙走。另外A搜索出的原始路径会有大量折点直接丢给局部规划器会导致轨迹不自然我一般会做一个简单的路径简化处理把共线的中间点去掉保留关键转弯点。Ego-Planner生成轨迹的核心是一个优化问题。它的目标函数通常包含三个主要项轨迹平滑性代价、碰撞代价、动力学可行性代价。平滑性代价让轨迹尽量少抖动碰撞代价通过ESDF距离场查询得到一个“离障碍物越近代价越高”的惩罚动力学可行性代价限制位置、速度、加速度甚至加加速度的极值。优化器需要权衡这几项最终生成一条既舒适又安全的轨迹。这个优化过程在Jetson TX2上能做到几十赫兹基本满足避障需求。全局和局部的配合上我犯过一次错误一开始把全局路径频率调得很高每秒钟都重新A*搜索结果CPU被大量占用局部规划的实时性反而下降。后来改成只有当目标点变化或者地图更新范围超出一定阈值时才重新搜索全局路径其他时间只做局部避障整条链路的压力立刻降了下来。3.3 动态目标检测与降落决策避障系统不只是绕开障碍物最终任务是降落到一个移动平台上。目标检测模块我用的YOLOv4-tiny在Jetson TX2上FPS可以跑到30帧左右识别降落平台的矩形框。RGB-D相机的好处是检测框内的深度信息可以直接换算目标在相机坐标系下的三维位置再通过TF变换到世界坐标系得到降落平台的实时坐标。降落决策的逻辑是这样的系统先判断目标是否被稳定检测到连续若干帧都有检测结果且置信度超过阈值才认为目标可信。然后根据目标当前速度和位置预测一个合适的前置接地点让飞行器生成一条平滑的下降轨迹在接近目标的过程中不断修正接地点。这个思路和“追着目标跑”不太一样更接近“到未来某个时刻目标刚好移动到的位置去等它”。这里面有一个比较隐蔽的坑深度图中的检测框边缘经常出现深度跳变或者空洞直接用框内像素的平均深度很容易估偏。我的做法是取检测框中心区域的一小块像素做一个深度中值滤波剔除掉明显异常的值再把中值深度投影到三维空间。这个细节对最终降落精度影响很大。4. 从源码到实机编译、启动与调参记录4.1 环境准备与编译顺序如果你要复现这套系统环境建议直接用Ubuntu 22.04加ROS2 Humble这是目前最稳的组合。Jetson TX2如果刷的是Ubuntu 18.04那可能需要用ROS2 Foxy或者把系统刷到JetPack 5以后的版本再装Ubuntu 20.04/22.04然后再装对应ROS2版本。依赖项主要包括Eigen3用于矩阵运算PCL点云库用于处理深度图像点云OpenCV用于图像处理和目标检测推理CUDA/cuDNN用于GPU加速MAVSDK或者MAVROS用于和PX4通信。编译顺序建议先编译基础依赖再编译消息定义包最后编译核心算法包。如果一次全部编译经常会出现因为消息类型还没生成导致别的包找不到头文件的错误。工程建议用colcon构建。单独编译某一个包用colcon build --packages-select 包名避免每次改动都要全量编译。我习惯把所有自定义消息放在一个单独的msg包里面这样算法包之间不直接依赖对方的头文件只依赖消息定义解耦比较干净。4.2 启动流程与核心命令系统运行起来后我会开几个终端分别启动模块方便分开看日志。第一个终端启动视觉里程计节点第二个启动ESDF建图节点第三个启动全局和局部规划节点第四个启动目标检测节点第五个启动和PX4通信的节点。如果你已经把所有节点封装成launch文件可以直接用ros2 launch命令一键启动。我这里的launch文件里包含了参数加载、节点启动、tf静态变换发布等逻辑。参数包括传感器内外参、地图分辨率、规划频率、速度限制、避障安全距离、目标检测置信度阈值等。在实际飞到无人机之前我会先把整个系统接到Gazebo仿真里跑一遍。PX4官方支持Gazebo和ROS2的仿真环境把仿真飞机的状态通过MAVROS/px4_msgs发布到ROS2再用RViz2可视化地图、轨迹和检测框。这样很多问题可以在没有炸机风险的情况下暴露出来。4.3 调参记录调参是整个项目里最花时间的部分。我把几个关键的参数和调参经验列一下。ESDF地图分辨率0.1米精度高但计算量大0.3米计算快但避障效果粗糙。我用的是0.15米算是一个折中。安全距离这个参数直接影响轨迹离障碍物多远。太小了飞控一震荡就会蹭到障碍物太大了在狭窄环境下根本找不到路径。我通常先设成机器人半径的1.5倍然后根据实际测试逐步收紧。规划频率局部规划30Hz全局规划1Hz。局部规划频率太低了动态障碍物避不开太高了CPU占用飙升。目标检测置信度阈值0.5比较合理。阈值太高容易漏检导致降落目标直接消失阈值太低会出现大量误检尤其容易把地面图案当成平台。平滑性代价权重和碰撞代价权重的比例也需要反复调。碰撞权重太高轨迹会过度保守绕路严重平滑权重太高轨迹容易“抄近路”从障碍物边缘切过去。我习惯先给碰撞代价一个比较高的初值再逐步降低找到一个“刚好看不到轨迹切障碍物”的临界值然后留20%的余量。5. 问题排查与避坑实录5.1 常见问题速查表现象可能原因解决方案两个ROS2节点互相看不到话题DDS发现机制问题多机部署时尤其常见检查是否在同一个域配置相同的ROS_DOMAIN_ID多机场景需要配置共享发现服务器建图错乱地图中出现大量错误障碍物深度图和彩色图时间戳不同步检查传感器消息时间戳必要时用时间同步器做近似同步轨迹规划卡顿CPU占用接近100%全局规划频率设置过高降低全局规划频率只在目标点变化或地图大范围更新时触发A*降落平台位置估计有偏差深度图边缘跳变导致测距不准检测框中心区域做深度中值滤波无人机飞行过程中剧烈抖动平滑性代价过低轨迹太僵硬提高平滑性权重检查速度/加速度限制是否合理目标检测漏检严重置信度阈值过高或者模型对特定角度泛化差降低阈值补充不同角度的训练数据5.2 几个印象深刻的坑第一是DDS发现问题。在实验室局域网里调试时机载电脑和地面站经常出现一方收不到另一方话题的情况。查了很久才发现是两个节点的ROS_DOMAIN_ID不一致一个用了默认的0另一个手工设成了别的值。这个在ROS1时代根本不存在因为ROS1所有节点都依赖同一个roscore。ROS2里一定要统一检查域ID。第二是TF树错误。有一次飞行测试无人机一直在原地打转看日志发现视觉里程计明明给出了位移但控制回路接收到的参考位置一直是原点。最后定位到问题是某个节点发布的子坐标系相对父坐标系的平移量写反了。这种问题在rviz2里如果只看整体画面很难发现一定要逐级检查TF树。第三是Jetson TX2上的显存限制。YOLOv4-tiny本身不占太多显存但如果同时开GPU加速的ESDF地图更新和深度图像处理显存就捉襟见肘。解决方法是给每个进程显式限制TensorRT的显存使用量并且关掉不需要的GUI工具把资源尽量留给算法。第四是仿真和实机的差距。在Gazebo里一切完美一上实机就频繁触发避障急停。原因是仿真里的障碍物模型非常干净深度图质量很高而真实环境中相机噪声、光照变化让ESDF地图出现了很多细碎的假障碍物。后来我加了一个地图置信度机制只有连续多帧被观测为占据的体素才标记成障碍物瞬间出现的孤立占据点忽略掉。这样实机稳定性提升非常明显。6. 一些心得和后续扩展思路6.1 如果重新做一次我会在哪些地方改进首先是建图这块我可能不会再手写完整的体素地图管线而是优先评估Cartographer或者基于OctoMap的方案。当然手写的好处是对每个细节都有完全掌控比如ESDF的更新策略、体素的置信度设置这让我在排查问题时心里有底。但如果你追求快速出成果站在成熟方案之上做集成会省很多时间。其次是目标检测模型的训练数据。YOLOv4-tiny虽然轻量但在特定环境下的误检率取决于训练数据覆盖度。我一开始用的预训练模型基本只能检测常见的行人、车辆落到“识别降落平台”这个任务上效果一般。后来补充了几百张不同光照、不同角度的平台图像做微调漏检率明显下降。模型小型化之后如果用TensorRT做推理加速帧率还能再提升。最后是代码架构。第一次做的系统在包与包之间直接调用对方头文件导致后面想替换某个算法模块非常痛苦。如果重新来我会把所有跨模块交互都改成自定义消息和服务把核心算法包做成纯算法库外部包只启动节点和消息转换。这样的好处是单个模块可以独立测试后续换算法不影响其他部分。6.2 后续可以怎么扩展这套系统的架构不只适用于无人机地面移动机器人、双轮平衡车、机械臂避障都可以复用同样的感知—规划—控制框架。如果用ROS2的Nav2可以把全局规划换成 Nav2 内置的规划器局部规划换成DWA或者TEB这些模块在ROS2生态里都是开箱即用的非常适合做地面机器人的导航应用。如果是机械臂场景可以考虑引入MoveIt。MoveIt的规划器处理的是机械臂关节空间和笛卡尔空间核心的碰撞检测和避障思路和这套系统是相通的。把ESDF地图换成机械臂工作空间的碰撞表示规划问题就是从当前关节角到目标位姿的无碰撞轨迹搜索。另外多机协同也是一个自然扩展方向。ROS2的DDS天生支持多机通信如果用共享发现服务器把多台机器人的状态同步到同一个网络就可以做协同建图、分布式的编队规划和互相避让这也是我从一开始就用ROS2的重要原因之一。从我个人经验来说这类系统的调试永远不会一帆风顺。视觉偶尔丢帧、飞控响应延迟、地图更新不及时任何一个环节抖动都会体现在最终轨迹质量上。但正因如此把每个模块拆开单独验证、把每次实机测试的日志保留下来、把参数影响记录下来这套工程才真正变成你自己的东西。希望这篇分享能帮你少走一些弯路也欢迎有类似项目经验的同行多交流。本文还有配套的精品资源点击获取 基于ROS2的机器人运动规划与避障系统设计与实现
简介自主导航技术是移动机器人和无人机系统的核心能力其关键在于可靠的状态估计、环境建模与轨迹规划。在无GPS环境下传感器融合与实时避障成为工程难点。本文将围绕基于ROS2的机器人运动规划与避障系统深入解析视觉惯性里程计如何提供定位基准ESDF距离场如何加速碰撞检测以及A*全局路径搜索与Ego-Planner局部轨迹优化如何协同工作。同时讨论YOLOv4-tiny目标检测在动态降落平台追踪中的应用梳理从硬件选型、代码架构到实机调试的完整链路。该方法适用于无人机自主导航、地面机器人避障及ROS1向ROS2迁移等典型场景为构建高可靠性的自主移动系统提供工程参考。 前阵子正好做了一套基于ROS2的机器人运动规划与避障系统前前后后折腾了两个月最后整理成一个zip工程。有人问我是怎么把全局规划、局部避障、动态目标检测和自主降落串起来的今天干脆把这套系统的设计和实现思路完整梳理一遍。重点讲我为什么这么选型、代码结构怎么组织、每一步怎么调试以及那些文档里根本不会写的坑。如果你正准备做无人机自主导航、移动机器人局部避障或者想把手头的ROS1项目迁到ROS2这篇应该对你有用。1. 项目到底在做什么需求拆解与方案选型1.1 一个典型的自主导航任务这个项目的核心场景是无GPS环境下机器人或者无人机需要靠机载传感器独立完成几件事实时确定自己在哪里、在未知环境里构建地图、规划出一条能到目标点的路径同时躲避动态障碍物最终在一个移动平台上完成降落。听起来不算复杂但把每个词拆开就很要命。首先是“无GPS”意味着所有定位和状态估计都只能依赖机载视觉、惯性测量单元这类传感器其次是“动态避障”障碍物不是静止的墙和柱子可能有移动的车辆、人员或者其他飞行器最后是“自主降落”降落目标本身还在移动需要一边追踪一边生成下降轨迹。整个任务链是感知、建图、规划、控制的持续闭环缺一环都跑不通。这里我明确一下系统边界全局路径负责在已知或逐步探索的地图上给出一个稀疏的导航路线局部规划负责在传感器能感知到的范围里生成一条平滑、安全、动力学可行的轨迹目标检测模块负责识别降落平台并持续跟踪最后把生成好的轨迹交给底层飞控或者运动控制器去执行。1.2 为什么选ROS2而不是继续在ROS1里凑合我在项目早期确实纠结过这个问题。很多经典算法包比如Ego-Planner、A*路径规划、体素地图构建最早都是基于ROS1写的社区里ROS1的资料也明显更多。但最后我还是选了ROS2原因是这个项目对多机协同、节点状态管理和通信实时性有比较明确的需求。ROS2的核心通信基于DDS天然支持分布式部署。机载电脑Jetson TX2、地面站、可能还有第二台计算节点之间可以直接通过话题和服务通信不需要像ROS1那样额外搭一套master。此外ROS2引入节点生命周期管理、参数动态配置、launch文件统一启动这些机制在实际调试复杂系统时帮助很大。当然迁移成本真实存在。很多工程里你看到的是ROS1的launch文件、rospy或者roscpp接口迁到ROS2之后话题名、CLI命令、参数系统、tf2用法都有变化。不过现在ROS2的生态已经非常完整humble版本基本覆盖了导航、建图、机械臂控制这些主流方向新版项目直接基于ROS2做没有太大问题。2. 整体架构怎么搭从感知到执行的模块划分2.1 系统分层与数据流整套系统在逻辑上分四层感知层、建图与定位层、规划决策层、控制输出层。感知层的输入是机载相机和IMU。这里我用的是RGB-D相机也就是同时输出彩色图和深度图深度图用来构建占据地图和ESDF距离场彩色图用来跑目标检测。IMU数据用于视觉里程计的矫正和短时间状态预测。建图与定位层把感知数据融合成两个关键产物一个是机器人当前在世界坐标系下的位姿一个是描述周围环境障碍物分布的ESDF地图。ESDF全称是Euclidean Signed Distance Field即欧氏符号距离场它把环境表示成一张栅格场每个栅格存的是它到最近障碍物的距离。这个表示对轨迹优化特别友好因为代价函数可以直接查询距离场得到一个梯度方向。规划决策层拿到当前位姿、目标点、ESDF地图、障碍物动态信息后先做一次全局路径搜索A*再在局部用Ego-Planner做轨迹优化同时根据目标检测结果判断是否进入降落决策状态。控制输出层在无人机场景就是PX4飞控。这里我们不再直接输出电机PWM而是通过MAVROS或者ROS2的PX4接口把轨迹生成的位置、速度期望值发给飞控由飞控底层姿态控制器去执行。2.2 核心组件选型与理由模块选型方案选型理由机载平台Jetson TX2功耗低自带GPU可以加速神经网络和距离场计算接口全飞控PX4开源、稳定支持对外部位置和速度指令的接管自定位RGB-D相机 IMU 视觉里程计无GPS环境下的主流方案RGB-D直接给深度省去双目标定地图表示体素地图 ESDF体素地图适合增量更新ESDF直接服务轨迹优化全局规划A*算法实现简单、稳定在栅格地图上很容易得到全局可行路径局部规划Ego-Planner能够生成光滑、动力学可行的轨迹支持动态环境避障目标检测YOLOv4-tiny轻量级网络Jetson TX2上能跑实时检测降落平台足够可视化RViz2ROS2官方工具方便实时看轨迹、地图和检测框很多初学者会问为什么全局和局部不直接用同一个算法。我解释一下全局路径只需要拓扑上的可行性不要求平滑A*在一张栅格图上的搜索效率和实现成本是最划算的。局部规划由于要实时避开动态障碍物而且必须考虑机器人/无人机的动力学约束所以需要一个能输出平滑轨迹的优化器Ego-Planner正好解决这个问题。两者配合既不会让规划器陷入局部极小又能在局部做高频避障调整。3. 核心模块实现细节地图、规划和避障3.1 无GPS定位与ESDF地图构建先讲定位。整套系统在无GPS环境里的基础是视觉惯性里程计。RGB-D相机给出深度图像后我们通过特征点匹配估算帧间位姿变化然后用IMU做帧间插值和短期预测。视觉里程计在快速旋转或者纹理稀疏的环境里容易漂移IMU虽然长期有零偏但短期内的角速度和加速度测量非常可靠所以两者做紧耦合或者松耦合是当前工程上最稳妥的做法。这里有一个绕不开的关键点TF坐标树要维护正确。整个系统里有多个坐标系相机坐标系、IMU坐标系、机体坐标系、世界坐标系、目标检测框坐标系任何一个TF关系写错后面的地图和规划全部是乱的。调试的时候我习惯先用静态TF把相机到机体的外参标定好再用rviz2里的TF显示插件逐帧检查坐标系方向。ESDF的地图构建流程大致是先根据深度图把空间离散成体素有障碍物占据的体素标为occupied空区域标为free然后对整张图做距离传播计算。距离传播的思路类似于把障碍物表面当作波源向外扩散每个体素记录到最近表面的距离。Jetson TX2上我们实现了并行版本用GPU加速距离场更新这样地图可以保持较快频率刷新让局部规划器始终基于最新的环境信息做避障。如果硬件上不想依赖GPU也可以纯CPU跑ESDF但分辨率不能拉太高。实测经验是0.2米分辨率的地图在CPU上更新一次需要几十毫秒勉强能接受但低于0.1米就会明显拖慢整条链路所以是否上GPU取决于你的实时性要求。3.2 全局路径与局部轨迹规划如何配合整个规划模块的运行流程可以总结成一个循环先由A*在地图上搜出一条从当前位置到目标点的栅格路径再把这条路径的关键点作为局部规划的参考导向Ego-Planner以这些导向点和ESDF地图为输入生成一条满足动力学约束的平滑轨迹。A的实现本身不复杂但有几个细节直接影响效果。启发式函数我选的是欧氏距离比曼哈顿距离在八连通栅格上更准确。地图的代价要考虑障碍物距离离障碍物太近的栅格要增大代价这样搜索出来的路径不会贴墙走。另外A搜索出的原始路径会有大量折点直接丢给局部规划器会导致轨迹不自然我一般会做一个简单的路径简化处理把共线的中间点去掉保留关键转弯点。Ego-Planner生成轨迹的核心是一个优化问题。它的目标函数通常包含三个主要项轨迹平滑性代价、碰撞代价、动力学可行性代价。平滑性代价让轨迹尽量少抖动碰撞代价通过ESDF距离场查询得到一个“离障碍物越近代价越高”的惩罚动力学可行性代价限制位置、速度、加速度甚至加加速度的极值。优化器需要权衡这几项最终生成一条既舒适又安全的轨迹。这个优化过程在Jetson TX2上能做到几十赫兹基本满足避障需求。全局和局部的配合上我犯过一次错误一开始把全局路径频率调得很高每秒钟都重新A*搜索结果CPU被大量占用局部规划的实时性反而下降。后来改成只有当目标点变化或者地图更新范围超出一定阈值时才重新搜索全局路径其他时间只做局部避障整条链路的压力立刻降了下来。3.3 动态目标检测与降落决策避障系统不只是绕开障碍物最终任务是降落到一个移动平台上。目标检测模块我用的YOLOv4-tiny在Jetson TX2上FPS可以跑到30帧左右识别降落平台的矩形框。RGB-D相机的好处是检测框内的深度信息可以直接换算目标在相机坐标系下的三维位置再通过TF变换到世界坐标系得到降落平台的实时坐标。降落决策的逻辑是这样的系统先判断目标是否被稳定检测到连续若干帧都有检测结果且置信度超过阈值才认为目标可信。然后根据目标当前速度和位置预测一个合适的前置接地点让飞行器生成一条平滑的下降轨迹在接近目标的过程中不断修正接地点。这个思路和“追着目标跑”不太一样更接近“到未来某个时刻目标刚好移动到的位置去等它”。这里面有一个比较隐蔽的坑深度图中的检测框边缘经常出现深度跳变或者空洞直接用框内像素的平均深度很容易估偏。我的做法是取检测框中心区域的一小块像素做一个深度中值滤波剔除掉明显异常的值再把中值深度投影到三维空间。这个细节对最终降落精度影响很大。4. 从源码到实机编译、启动与调参记录4.1 环境准备与编译顺序如果你要复现这套系统环境建议直接用Ubuntu 22.04加ROS2 Humble这是目前最稳的组合。Jetson TX2如果刷的是Ubuntu 18.04那可能需要用ROS2 Foxy或者把系统刷到JetPack 5以后的版本再装Ubuntu 20.04/22.04然后再装对应ROS2版本。依赖项主要包括Eigen3用于矩阵运算PCL点云库用于处理深度图像点云OpenCV用于图像处理和目标检测推理CUDA/cuDNN用于GPU加速MAVSDK或者MAVROS用于和PX4通信。编译顺序建议先编译基础依赖再编译消息定义包最后编译核心算法包。如果一次全部编译经常会出现因为消息类型还没生成导致别的包找不到头文件的错误。工程建议用colcon构建。单独编译某一个包用colcon build --packages-select 包名避免每次改动都要全量编译。我习惯把所有自定义消息放在一个单独的msg包里面这样算法包之间不直接依赖对方的头文件只依赖消息定义解耦比较干净。4.2 启动流程与核心命令系统运行起来后我会开几个终端分别启动模块方便分开看日志。第一个终端启动视觉里程计节点第二个启动ESDF建图节点第三个启动全局和局部规划节点第四个启动目标检测节点第五个启动和PX4通信的节点。如果你已经把所有节点封装成launch文件可以直接用ros2 launch命令一键启动。我这里的launch文件里包含了参数加载、节点启动、tf静态变换发布等逻辑。参数包括传感器内外参、地图分辨率、规划频率、速度限制、避障安全距离、目标检测置信度阈值等。在实际飞到无人机之前我会先把整个系统接到Gazebo仿真里跑一遍。PX4官方支持Gazebo和ROS2的仿真环境把仿真飞机的状态通过MAVROS/px4_msgs发布到ROS2再用RViz2可视化地图、轨迹和检测框。这样很多问题可以在没有炸机风险的情况下暴露出来。4.3 调参记录调参是整个项目里最花时间的部分。我把几个关键的参数和调参经验列一下。ESDF地图分辨率0.1米精度高但计算量大0.3米计算快但避障效果粗糙。我用的是0.15米算是一个折中。安全距离这个参数直接影响轨迹离障碍物多远。太小了飞控一震荡就会蹭到障碍物太大了在狭窄环境下根本找不到路径。我通常先设成机器人半径的1.5倍然后根据实际测试逐步收紧。规划频率局部规划30Hz全局规划1Hz。局部规划频率太低了动态障碍物避不开太高了CPU占用飙升。目标检测置信度阈值0.5比较合理。阈值太高容易漏检导致降落目标直接消失阈值太低会出现大量误检尤其容易把地面图案当成平台。平滑性代价权重和碰撞代价权重的比例也需要反复调。碰撞权重太高轨迹会过度保守绕路严重平滑权重太高轨迹容易“抄近路”从障碍物边缘切过去。我习惯先给碰撞代价一个比较高的初值再逐步降低找到一个“刚好看不到轨迹切障碍物”的临界值然后留20%的余量。5. 问题排查与避坑实录5.1 常见问题速查表现象可能原因解决方案两个ROS2节点互相看不到话题DDS发现机制问题多机部署时尤其常见检查是否在同一个域配置相同的ROS_DOMAIN_ID多机场景需要配置共享发现服务器建图错乱地图中出现大量错误障碍物深度图和彩色图时间戳不同步检查传感器消息时间戳必要时用时间同步器做近似同步轨迹规划卡顿CPU占用接近100%全局规划频率设置过高降低全局规划频率只在目标点变化或地图大范围更新时触发A*降落平台位置估计有偏差深度图边缘跳变导致测距不准检测框中心区域做深度中值滤波无人机飞行过程中剧烈抖动平滑性代价过低轨迹太僵硬提高平滑性权重检查速度/加速度限制是否合理目标检测漏检严重置信度阈值过高或者模型对特定角度泛化差降低阈值补充不同角度的训练数据5.2 几个印象深刻的坑第一是DDS发现问题。在实验室局域网里调试时机载电脑和地面站经常出现一方收不到另一方话题的情况。查了很久才发现是两个节点的ROS_DOMAIN_ID不一致一个用了默认的0另一个手工设成了别的值。这个在ROS1时代根本不存在因为ROS1所有节点都依赖同一个roscore。ROS2里一定要统一检查域ID。第二是TF树错误。有一次飞行测试无人机一直在原地打转看日志发现视觉里程计明明给出了位移但控制回路接收到的参考位置一直是原点。最后定位到问题是某个节点发布的子坐标系相对父坐标系的平移量写反了。这种问题在rviz2里如果只看整体画面很难发现一定要逐级检查TF树。第三是Jetson TX2上的显存限制。YOLOv4-tiny本身不占太多显存但如果同时开GPU加速的ESDF地图更新和深度图像处理显存就捉襟见肘。解决方法是给每个进程显式限制TensorRT的显存使用量并且关掉不需要的GUI工具把资源尽量留给算法。第四是仿真和实机的差距。在Gazebo里一切完美一上实机就频繁触发避障急停。原因是仿真里的障碍物模型非常干净深度图质量很高而真实环境中相机噪声、光照变化让ESDF地图出现了很多细碎的假障碍物。后来我加了一个地图置信度机制只有连续多帧被观测为占据的体素才标记成障碍物瞬间出现的孤立占据点忽略掉。这样实机稳定性提升非常明显。6. 一些心得和后续扩展思路6.1 如果重新做一次我会在哪些地方改进首先是建图这块我可能不会再手写完整的体素地图管线而是优先评估Cartographer或者基于OctoMap的方案。当然手写的好处是对每个细节都有完全掌控比如ESDF的更新策略、体素的置信度设置这让我在排查问题时心里有底。但如果你追求快速出成果站在成熟方案之上做集成会省很多时间。其次是目标检测模型的训练数据。YOLOv4-tiny虽然轻量但在特定环境下的误检率取决于训练数据覆盖度。我一开始用的预训练模型基本只能检测常见的行人、车辆落到“识别降落平台”这个任务上效果一般。后来补充了几百张不同光照、不同角度的平台图像做微调漏检率明显下降。模型小型化之后如果用TensorRT做推理加速帧率还能再提升。最后是代码架构。第一次做的系统在包与包之间直接调用对方头文件导致后面想替换某个算法模块非常痛苦。如果重新来我会把所有跨模块交互都改成自定义消息和服务把核心算法包做成纯算法库外部包只启动节点和消息转换。这样的好处是单个模块可以独立测试后续换算法不影响其他部分。6.2 后续可以怎么扩展这套系统的架构不只适用于无人机地面移动机器人、双轮平衡车、机械臂避障都可以复用同样的感知—规划—控制框架。如果用ROS2的Nav2可以把全局规划换成 Nav2 内置的规划器局部规划换成DWA或者TEB这些模块在ROS2生态里都是开箱即用的非常适合做地面机器人的导航应用。如果是机械臂场景可以考虑引入MoveIt。MoveIt的规划器处理的是机械臂关节空间和笛卡尔空间核心的碰撞检测和避障思路和这套系统是相通的。把ESDF地图换成机械臂工作空间的碰撞表示规划问题就是从当前关节角到目标位姿的无碰撞轨迹搜索。另外多机协同也是一个自然扩展方向。ROS2的DDS天生支持多机通信如果用共享发现服务器把多台机器人的状态同步到同一个网络就可以做协同建图、分布式的编队规划和互相避让这也是我从一开始就用ROS2的重要原因之一。从我个人经验来说这类系统的调试永远不会一帆风顺。视觉偶尔丢帧、飞控响应延迟、地图更新不及时任何一个环节抖动都会体现在最终轨迹质量上。但正因如此把每个模块拆开单独验证、把每次实机测试的日志保留下来、把参数影响记录下来这套工程才真正变成你自己的东西。希望这篇分享能帮你少走一些弯路也欢迎有类似项目经验的同行多交流。本文还有配套的精品资源点击获取