机器人怎么建图并自己走？从示例清单读懂拓扑图导航

你大概见过这样的场面：一台机器人在厂区里自己走，走到配电柜前停一下，拍张照，再走到下一个点。看着挺神奇，但如果让你从零把这套东西做出来，第一个卡住你的问题很可能不是「怎么走」，而是——它脑子里那张地图，到底长什么样？

这个问题的答案，藏在一个你可能不会想到的地方：示例程序的文件名。

unitree_slam 这个仓库的 README 短得离谱，去掉标题就剩五六行 cmake 和 make。但它的示例目录里平铺着 23 个 .cpp 文件，每个文件名都是一个动词短语。把这 23 个名字列出来排一排，这套系统能干什么、它怎么理解世界，基本就清楚了。

先说清楚边界：这篇文章基于公开仓库的文档与代码结构，我没有真机，也没有跑过其中任何一个示例。 下面所有结论都来自文件树、消息定义和 README 里那几行命令，涉及运行效果的地方我一律不写。

先看这个仓库到底装了什么

顶层只有四个实质性的目录，外加一个 CMakeLists.txt 和 README.md：

ros2_msg/              —— 两个消息包的 .msg 定义
rviz2/                 —— 两份 RViz2 可视化配置
unitree_robotics/      —— 头文件 + 预编译库
unitree_slam_example/  —— 23 个示例程序

README 给的全部操作是这样：

mkdir build
cd build
cmake ..
make
export LD_LIBRARY_PATH=$PWD/../unitree_robotics/lib/$(uname -m):$LD_LIBRARY_PATH
./demo_h1 eth0   # eth0 是子网 123 所在的网卡名

这段短命令里有三个细节值得停一下。

第一，LD_LIBRARY_PATH 指向 lib/$(uname -m)。 说明库是按 CPU 架构分目录放的，uname -m 在开发机上是 x86_64，在机器人的计算板上是 aarch64——目录里确实这两份都有。这跟 unitree_sdk2 的组织方式是一脉相承的。

第二，程序的唯一参数是网卡名。 不是 IP，不是配置文件，就是一个 eth0。这是 DDS 的典型用法：你告诉它从哪块网卡去发现同一网段上的其他参与者，剩下的事情它自己处理。

第三，也是最关键的一条——这个仓库里没有 SLAM 算法。 unitree_robotics/lib/ 下面只有 libunitree_sdk2.a 和 libunitree_ros2_idl_cpp.a，加上 CycloneDDS 的几个 .so。没有任何一个库文件的名字里带 slam，也没有任何一行位姿图优化或者点云配准的源码。

那这 23 个示例在干什么？看消息定义就明白了。unitree_robotics/include/unitree/ros2_idl/ 下面躺着一对文件：RequestSlam_.hpp 和 ResponseSlam_.hpp。请求-响应，成对出现。这些示例是客户端，它们的活儿是把指令发出去、把结果收回来；真正跑建图和定位的那个东西在机器人那一侧，你调用它，但你看不到它。

23 个示例，摊开就是一份能力清单

把 unitree_slam_example/ 下的文件名全部列出来，按语义归类，会分得非常干净：

【建图生命周期】
start_mapping.cpp        end_mapping.cpp

【地图的图结构编辑】
add_node.cpp             add_edge.cpp
delete_node.cpp          delete_edge.cpp
query_node.cpp           query_edge.cpp
close_all_node.cpp

【定位与重定位】
pose_init.cpp            start_relocation.cpp

【导航执行与过程控制】
start_nav.cpp            single_nav.cpp
multiple_nav_default.cpp multiple_nav_set.cpp
pause_nav.cpp            recover_nav.cpp
return_origin.cpp

【雷达与机型适配】
demo_h1.cpp              demo_b2.cpp
demo_mid360.cpp          demo_xt16.cpp
rslidar_dds_test.cpp

这份清单本身就在讲一个完整的故事：先建图（start/end mapping），然后编辑这张图（node/edge 的增删查），机器人开机后先搞清楚自己在哪（pose_init / relocation），最后按图导航（nav 系列），过程中还能暂停和恢复。

而这里面信息量最大的，是中间那一组。

核心：为什么会有 node 和 edge

一个 SLAM 系统给你 add_node、delete_edge 这种接口，这件事很不寻常。

如果地图只是一张「哪里能走、哪里不能走」的图片，那你需要的接口应该是「设置目标点坐标 (x, y)」，而不是「增加一个节点」「删除一条边」。节点和边是图论的词汇。 它们出现在这里，说明这套系统对空间的表示，除了图片，还有一张图（graph）。

这不是我的推断，仓库里有硬证据。ros2_msg/ 下有个包就叫 graph_msg，里面只有两个消息：

ros2_msg/graph_msg/Node.msg
ros2_msg/graph_msg/Edge.msg

对应到 IDL 层，unitree_robotics/include/unitree/ros2_idl/ 里也有 Node_.hpp 和 Edge_.hpp。

栅格地图：把空间切成格子

先说大家更熟悉的那一种。同样在 ros2_idl/ 目录里，你能找到这两个：

OccupancyGrid_.hpp
MapMetaData_.hpp

OccupancyGrid——占据栅格。这是移动机器人领域最经典的地图表示：把地面切成一格一格，每格记一个值，表示这里被占据、空闲，还是没探测过。MapMetaData 配套记录这张栅格图的元信息，比如分辨率和原点在哪。

栅格地图的好处是任意两点之间都能规划路径。因为每一格都有明确状态，路径规划算法可以在格子之间搜索，绕过障碍，找出一条通路。你在 rviz2/ 里看到的 costmap.rviz，配的就是这一层——代价地图（costmap）是在占据栅格之上再叠一层「代价」，离墙越近代价越高，让规划出来的路不至于贴着柱子擦过去。

同一个目录里还有 build_map.rviz，一份配置对应建图阶段，一份对应导航阶段。两份可视化配置的存在，本身就说明这是两个不同的工作模式，看的数据不一样。

拓扑地图：只记关键点和它们怎么连

拓扑地图完全是另一种思路。它不关心每一平方厘米，只记住若干个关键点位（节点），以及哪两个点之间可以直接走过去（边）。

最好的类比是地铁线路图。真实的地铁线路是弯弯曲曲的，但线路图上只有站点和连线。你要从 A 站到 D 站，看图就知道 A→B→C→D，不需要知道隧道具体怎么拐弯。

对机器人来说，一张拓扑图长这样：

节点：门口、走廊拐角、配电柜1、配电柜2、充电桩
边：  门口 ── 走廊拐角 ── 配电柜1
                  │
                  └──── 配电柜2 ── 充电桩

导航任务就变成了在这张图上找一条路径，然后逐段执行。

两者不是二选一，是分工

这是最容易误解的地方。看到「拓扑地图」，很多人第一反应是「那不是比栅格图信息少很多吗」。是少，但它们干的不是同一件事。

拓扑图管全局：我要去哪，按什么顺序去。 栅格图管局部：脚下这个箱子怎么绕。

机器人执行「从配电柜1 走到 配电柜2」这一段边的时候，靠的是局部的栅格图和代价地图，实时避开临时出现的东西——纸箱、人、被挪动的推车。这跟我们在机器人避障那篇里聊的局部规划是同一件事，只是传感器从超声波换成了激光雷达。

而「先去1号柜再去2号柜」这个决策，是在拓扑图上做的，跟栅格无关。

这也解释了 multiple_nav_default.cpp 和 multiple_nav_set.cpp 为什么会成对出现：多点导航有一个「默认」的走法，也有一个可以由你「设定」的走法。在拓扑图上做多点巡航，本来就是一件很自然的事——点位是现成的，你只需要给出顺序。

巡检类应用为什么偏爱拓扑图

想清楚这一点，一大批工业机器人的工作方式你就懂了。

栅格图上的自由导航听起来更聪明：给个坐标，它自己找路。但在真实的厂区、变电站、机房里，「聪明」经常不是优点。

• 路线可控。工厂里有些通道不希望机器人走——可能太窄，可能有叉车，可能是无关区域。栅格图上的规划器会觉得那条路更短，于是就走了。拓扑图不会：没连边，就没这条路。
• 可复现。今天走的路和昨天一模一样，出问题的时候你能复盘。自由规划的路径受当时环境影响，每次都不同。
• 可编辑。这一条后面单独说。
• 便于挂任务。每个节点是一个有名字的实体，你可以往上面挂东西：到了这个点拍张照、到了那个点读一次仪表。栅格图上的一个坐标 (12.4, 3.7) 没法挂任务，节点可以。

最后这一点在 Python_unitree_demos 那个仓库里体现得非常直白。那个仓库的描述写的是「宇树 SLAM 工业应用场景 Python 示例」，里面 16 个文件全都以 Action 结尾：

MoveToAction.py              MoveByAction.py
RotateAction.py              RotateToAction.py
SeriesMoveToAction.py        FollowPathPointsAction.py
GoHomeAction.py              ReturnToParkingAction.py
EnterElevatorAction.py       LeaveElevatorAction.py
MultiFloorMoveAction.py      MultiFloorBackHomeAction.py
MoveToTagAction.py           BackOffFromTagAction.py
ManualRelocalizationAction.py RecoverLocalizationAction.py

EnterElevatorAction / LeaveElevatorAction / MultiFloorMoveAction——进电梯、出电梯、跨楼层移动。这三个名字放在一起，把「节点是可以挂语义的」这件事说透了。跨楼层导航在纯栅格图的世界里是个尴尬问题（不同楼层是两张互不相干的图），但在拓扑图里它就是几个特殊节点加几条特殊的边。

GoHomeAction 和 ReturnToParkingAction 也一样，「家」和「停靠点」都是被命名的节点。

add_node 意味着什么：地图是可以被人改的

这是我读这个仓库时最想聊的一条。

add_node / add_edge / delete_node / delete_edge / query_node / query_edge——增、删、查，六个接口配得整整齐齐。这在数据库里叫 CRUD，在这儿意味着：这张拓扑图不是建图算法一次性生成完就锁死的产物，它是一份可以被人打开、修改、再保存的数据。

为什么这很重要？因为纯自动建图的结果，几乎总是需要人工修正。

举几个实际会遇到的情况：机器人建图时正好有扇门是开着的，图上就多出了一条通往隔壁房间的边，而那扇门平时是锁的；或者某段走廊反光严重，自动生成的节点密度乱七八糟；又或者你希望它在某个位置多停一下，而那个位置压根没被识别成关键点。

有了 add_node 和 delete_edge，这些都是几行代码的事。没有的话，你只能重新建一遍图，祈祷这次的结果好一点。

还有一个 close_all_node.cpp。「关闭所有节点」——这个名字我一开始没看懂，因为它跟前面六个 CRUD 接口的动词不是一路的。我不去猜作者具体的意图，但从工程角度看，能对全部节点做一次批量状态操作，本身就说明节点是有状态的实体，而不只是几个坐标。

顺带说一句，ros2_msg/ 下除了 graph_msg，还有第二个包 unitree_interfaces，里面是三个 Qt 开头的消息：

ros2_msg/unitree_interfaces/QtCommand.msg
ros2_msg/unitree_interfaces/QtEdge.msg
ros2_msg/unitree_interfaces/QtNode.msg

QtEdge / QtNode 和 graph_msg 里的 Edge / Node 显然是同一组概念的两套表示，QtCommand 则多出一个「命令」。同样的类型在 ros2_idl/ 里也各有一份 QtEdge_.hpp / QtNode_.hpp / QtCommand_.hpp。

同一个概念在系统里存在两套消息定义，通常说明它有两类消费者。 一套供程序内部流转，另一套供某个外部工具使用——而那个工具需要额外的「命令」通道。这跟「地图可以被编辑」这条线索是能对上的：编辑地图这件事，最终总要落到某个人面前的界面上。我只能说到这里，仓库里没有更多材料了。

pose_init：机器人开机时并不知道自己在哪

导航控制那一组里，最值得单独讲的是 pose_init.cpp 和 start_relocation.cpp。

这里面藏着一个新手常常想不到的问题：机器人重新开机时，它并不知道自己站在已有地图的哪个位置。

建图的时候不存在这个问题——建图是从零开始的，机器人把出发点定义为原点，之后一路累积。但地图存下来之后关机、搬动、再开机，情况就变了。它手上有一张完整的地图，也能看到周围的环境，但这两者对不上号：眼前这个拐角，是地图上的哪个拐角？

这就是重定位（relocalization）问题。解决它无非两条路：

一是给一个初始猜测，然后让算法在这个猜测附近做匹配、收敛到准确位姿。这对应 pose_init——初始位姿。人告诉机器人「你大概在这儿，朝这个方向」，剩下的交给算法。

二是全局搜索，不给提示，让算法在整张地图上找最像的地方。这对应 start_relocation 这种独立的重定位流程。

两个示例同时存在，说明这两条路在这套系统里都有对应的接口。

Python_unitree_demos 里对应的两个动作把这层关系摆得更明白：ManualRelocalizationAction（手动重定位）和 RecoverLocalizationAction（恢复定位）。前者是人介入，后者听名字是定位丢了之后的自动恢复。定位会丢——这是移动机器人工程里必须提前接受的事实，长走廊、大空旷区域、环境被大幅改动，都可能让匹配失效。系统里有没有一条「丢了怎么办」的路径，是能不能长期无人值守运行的关键差别之一。

顺便看一眼 pause_nav.cpp / recover_nav.cpp / return_origin.cpp 这三个。暂停、恢复、回原点，加上前面的 start_nav，构成了一个最小的导航状态机。这套状态设计不复杂，但每一个都对应着现场的真实需求：有人挡路要暂停，任务中断要能回到起点。

雷达适配这一组：只说事实

最后一组是五个 demo：

demo_h1.cpp        demo_b2.cpp
demo_mid360.cpp    demo_xt16.cpp
rslidar_dds_test.cpp

前两个按机器人型号命名，后三个按雷达命名——Livox Mid-360、禾赛 XT16，以及 rslidar（速腾）。这里我只陈述一个事实：示例目录里出现了针对这几款雷达的程序。 它们支持到什么程度、有没有覆盖同厂商的其他型号、别的雷达能不能接，仓库里没写，我就不写。这类兼容性问题从文件名推不出来，推出来也不可信。

想了解宇树自家雷达产品线的组织方式，可以看宇树雷达家族那篇；而激光雷达的点云是怎么变成里程计和地图的，属于 SLAM 前端的话题，Point-LIO 那篇讲得更细。

还有一个细节值得记：unitree_robotics/include/unitree/robot/ 下面有 b2、g1、go2、h1 四个型号目录，但示例里只有 demo_h1 和 demo_b2。这只能说明示例没有覆盖到其他型号，不能推断出任何别的结论。仓库没写的事，就是没写。

闭源这件事，中性地说

unitree_robotics/ 这个目录，把这个仓库的性质交代得很清楚：里面是几百个头文件，加上两个架构目录下的预编译库。CycloneDDS 的 C 和 C++ 绑定（dds/、ddscxx/）、sdk2 自己的通用层（unitree/common/：线程池、周期线程、日志、JSON、DDS 封装）、通信抽象层（robot/channel/、robot/client/、robot/server/），以及各型号的能力客户端。

ros2_idl/ 那一整个目录尤其能说明问题。除了前面提过的 Node_、Edge_、OccupancyGrid_，还有一批标准的 ROS 消息类型：Odometry_、PointCloud2_、PoseWithCovarianceStamped_、Imu_、Image_、Twist_、Header_……这些名字对写过 ROS 的人来说都是熟面孔。它把 ROS 常用的消息类型在 DDS 层面重新声明了一遍，这样不装 ROS 也能用同一套数据结构收发。

PoseWithCovarianceStamped 这个类型名本身就在讲一件事：位姿不是一个确定的坐标，而是一个带不确定度的估计值，协方差描述的就是「我有多确定」。SLAM 系统里几乎所有的位姿都是这个性质。我不去断言它和 pose_init 之间的调用关系（仓库里没写），但这个类型出现在一个 SLAM 仓库里，是完全合理的。

清单里还有几个我没能解读出来的：Star_.hpp、ToControl_.hpp、UnitreeCommond_.hpp。名字给的信息不够，硬猜没意义，就放在这儿。

至于「闭源」本身——你能调用，但看不到内部实现。这没什么好评价的，工业产品这么做很常见。对学习者来说，重点是这套东西的学习价值主要落在接口设计上：一个 SLAM 系统应该向外暴露哪些能力、怎么组织、哪些必须留给人操作，这些通过读接口是能学到的。至于算法本身怎么实现，得去别的地方学。

读完这个仓库，记住三件事

第一，示例文件名是最诚实的能力文档。 README 可以写得潦草，但示例程序是要能编译运行的，每一个都对应一个真实存在的接口。下次遇到文档稀薄的仓库，先把示例目录列出来按动词归类，比读三遍 README 有用。

第二，地图不是一张图片，是两张图的叠加。 栅格图（OccupancyGrid + costmap）负责脚下几米，拓扑图（Node + Edge）负责全局路线。理解了这个分工，你就明白为什么工业巡检机器人总是沿着固定路线走，而不是每次都自己找路——那不是技术不够，是刻意的设计选择。

第三，凡是需要人介入的地方，都会在接口上留下痕迹。 add_node 说明地图要人改，pose_init 说明开机要人指位置，ManualRelocalizationAction 说明定位丢了要人捞。一套系统里这类接口的完备程度，比它宣传的自动化水平更能说明它有没有真的在现场跑过。

下一步往哪走？如果你想理解拓扑图上那些边具体是怎么被走完的，得回到运动控制层；如果你更关心点云到底怎么变成地图，那是 SLAM 前端的事。这两条线在卷 U 的其他文章里都有对应的篇目。