激光雷达里程计是什么？Point-LIO 在足式机器人上的适配

假设你已经能用 SDK 让一台四足机器人往前走了。下一个需求几乎必然是这个：让它走到走廊尽头再拐弯，或者回到出发点。

于是你会问一个看起来很朴素的问题——它自己知道走到哪了吗？

答案是：不问就不知道。你发给它的是速度指令和步态指令，不是坐标。机器人执行完这条指令后到底位移了多少、朝向转了多少度，需要有一个独立的模块持续地估计出来。这个模块叫里程计（odometry）。

这篇文章我们围绕宇树的 point_lio_unilidar 这个仓库来讲。先把话说在前面：本文基于公开仓库的文档与代码结构写成，我手上没有实机、也没有跑过这套算法，所有涉及运行效果的表述都会标明是仓库文档里的说法，不是我的实测。

轮式机器人的答案，足式用不了

轮式机器人的里程计有个非常朴素的解法：在电机轴上装编码器，数轮子转了多少圈。轮子周长是已知的，转过的角度乘以周长就是走过的距离；左右轮转速有差，就能算出转向角。这套方法在差速驱动那篇里展开过，实现成本极低，只要有编码器就能做。

它成立的前提是一个很强的假设：轮子和地面之间是纯滚动，不打滑。

足式机器人身上，这个假设从三个方向同时崩掉：

第一，足端会打滑。 光滑地砖、碎石、草地，落脚点和地面之间随时可能发生相对滑动。你从关节角度反解出「脚在机身坐标系下移动了多少」，但脚同时在地上蹭了一段，这段位移就凭空丢了。

第二，存在腾空相。 四足机器人小跑起来会有四条腿全部离地的瞬间，人形走路时也有单脚支撑与双脚支撑的交替。腿不着地的时候，关节角度告诉你的是腿在空中怎么摆，跟机身相对地面走了多远没有直接关系。

第三，落脚冲击带来剧烈振动。 每一步落地都是一次撞击，机身会有一个明显的加速度尖峰。这类高频冲击对任何依赖平滑运动假设的估计方法都是噪声源。

你当然可以做腿式里程计（leg odometry），用接触检测判断哪条腿在支撑相，再用运动学反推机身位移——这在足式状态估计里是一个真实存在的方案，短时间内也确实好用。但它对打滑没有任何抵抗力，误差只会单调累积，走上一段就飘到不知道哪去了。

所以足式机器人需要一个不依赖足地接触假设的位姿来源。目前工程上的主流答案，是激光雷达惯性里程计。

LIO：为什么必须是「激光 + IMU」

LIO 是 Lidar-Inertial Odometry 的缩写，激光雷达惯性里程计。名字里两个传感器并列，这不是堆料，是因为任何一个单独用都不成立。

只用激光会怎样

激光雷达的工作方式是扫描：发射器一边旋转一边测距，攒够一圈才凑成一帧完整点云。问题在于，扫这一圈是要花时间的，而这段时间里载体一直在动。

结果就是同一帧点云里，前面的点和后面的点是在不同位姿下测出来的。你把它们当成同一时刻的快照去做配准，几何形状本身就是歪的。载体动得越快、姿态变化越剧烈，这个畸变越严重。对足式机器人来说，机身在步态周期里本来就在持续俯仰和侧摆，这个问题被放大了。

第二个问题是环境退化。激光配准的本质是拿这一帧点云去和已有的地图对齐，靠的是几何特征。如果你把机器人放进一条笔直的长走廊，两侧墙面平行、地面天花板平整——沿走廊方向平移一段，点云看起来几乎一模一样。算法没有任何依据判断你到底往前走了多少。空旷的大平地、大面积的白墙，都会出现类似的退化。

只用 IMU 会怎样

IMU 的性格正好相反。它输出角速度和加速度，频率高、延迟低，而且完全不看环境——走廊也好、旷野也好，它照样输出。

但它有个致命伤：它测的是导数，用起来必须积分。加速度积一次得速度，再积一次得位置；角速度积一次得姿态。每一次积分都会把传感器的零偏和噪声一起累积进去，而且是随时间发散的。零偏本身还会随温度慢慢漂移。纯靠 IMU 推算位置，撑不了多久就跑飞了。

两者的短板恰好互补

把这两张牌摆在一起看，互补关系非常清楚：

	激光雷达	IMU
输出频率	低（按帧）	高
是否随时间漂移	不漂（有环境约束）	漂
受环境影响	大（几何退化）	无
剧烈运动下	点云畸变	反而是它最擅长的

融合的逻辑因此变得很自然：

• IMU 提供高频的短时运动先验。两帧点云之间那段时间里载体怎么动的，由 IMU 说了算。这个先验还有个直接用途——点云运动补偿：知道扫描过程中每一时刻的位姿，就能把每个点都投影回同一个参考时刻，把畸变掰回来。
• 激光提供低频但不漂移的绝对约束。每来一帧点云，就用它和地图的配准结果去修正 IMU 积分出来的位姿，顺便把 IMU 的零偏也一起估出来。

一个负责「短时间内准」，一个负责「长时间不飘」。这就是 LIO 这三个字母的全部含义。

point_lio_unilidar 这个仓库在做什么

搞清楚原理，再回头看仓库就容易多了。

按 README 的说法，这个仓库做的事情很明确：把 Point-LIO 这个激光惯性里程计算法，适配到宇树自家的激光雷达产品上，具体点名了 Unitree LiDAR L1 和 Unitree LiDAR L2 两款。README 把这两款雷达的共同特点概括为大视场角、非重复式扫描、成本较低、适合低速移动机器人这几条。

对 Point-LIO 本身，README 的描述是：一套鲁棒、高带宽的激光惯性里程计，能在剧烈振动和激进运动下提供准确的高频里程计与可靠的建图，并给出了原始算法的项目地址与论文出处——Point-LIO 本身是港大 MaRS 实验室的开源工作，宇树这个仓库是在它之上做适配。

「剧烈振动和激进运动」这几个字，正好对上了我们前面说的足式机器人痛点。宇树把这个算法拿过来适配，选型逻辑是自洽的。

需要说清楚的是：这个仓库是适配，不是重新发明。核心算法来自上游，宇树做的是让它认识自家雷达的数据格式、坐标系和参数。理解这一点，你在读目录的时候就不会找错重点。

目录里能读出的分层

把仓库的实质性目录摊开：

config/      —— 各型号雷达的参数配置
launch/      —— 各型号雷达的 ROS 启动文件
src/         —— 算法主体
include/     —— 依赖的算法组件
rviz_cfg/    —— 可视化配置
PCD/         —— 点云地图输出目录
Log/         —— 日志与绘图脚本

先看 config/ 和 launch/，这两个目录是完全对称的：

config/                      launch/
  avia.yaml                    mapping_avia.launch
  horizon.yaml                 mapping_horizon.launch
  ouster64.yaml                mapping_ouster64.launch
  velody16.yaml                mapping_velody16.launch
  unilidar_l1.yaml             mapping_unilidar_l1.launch
  unilidar_l2.yaml             mapping_unilidar_l2.launch

一个雷达型号一个 yaml，一个雷达型号一个 launch，命名一一对应。这个结构本身就说明了适配工作是怎么做的：算法主体不动，每接一款新雷达就加一组「配置 + 启动文件」。前四个是上游 Point-LIO 就支持的其他厂商雷达，后两个 unilidar_l1 / unilidar_l2 是宇树加的。

如果哪天你手上有一款仓库里没列的雷达想接进来，路径也就清楚了：照着 unilidar_l2.yaml 抄一份改参数，再配一个 launch 文件。这比读算法源码现实得多。

再看 src/：

src/preprocess.cpp / .h      —— 点云预处理
src/IMU_Processing.hpp       —— IMU 处理
src/Estimator.cpp / .h       —— 状态估计
src/laserMapping.cpp         —— 主节点
src/parameters.cpp / .h      —— 参数读取

这五组文件几乎是把前面讲的原理原样映射成了代码结构：雷达数据进来先过 preprocess，IMU 数据进来先过 IMU_Processing，两路汇进 Estimator 做融合，laserMapping 是把这一切串起来的 ROS 节点，parameters 负责把 yaml 里的配置读进来。读一个陌生算法仓库时，先把 src/ 下的文件名和你理解的数据流对一遍，对不上的地方就是你理解有偏差的地方。

include/ 下面则是三个独立的算法组件，各自带着自己的 README 和 LICENSE：

include/IKFoM/          —— 流形上的迭代卡尔曼滤波工具箱
include/ikd-Tree/       —— 增量式 kd 树
include/FOV_Checker/    —— 视场检查
include/common_lib.h
include/so3_math.h

这几个是上游 Point-LIO 系列一直在用的基础件，从名字和它们各自携带的独立 LICENSE 就能看出是被整体引入的第三方实现。它们的内部机制不是这篇的重点，但有一点值得记住：这类仓库的「算法」二字，实际上分散在好几个独立组件里，主仓库更像是把它们组装起来的胶水。so3_math.h 这个文件名也透露了一件事——姿态在这里是按李群/李代数处理的，不是用欧拉角硬算。

顺带一提，Log/ 目录下除了几个 txt，还躺着 plot.py、plot_imu.py、plot_out.py 三个绘图脚本。一个算法仓库愿意把调试用的绘图脚本一起提交上来，通常意味着作者调参时是真的天天在看这些曲线。你自己调不出效果的时候，这几个脚本是现成的入口。

编译与运行：README 里的步骤链

README 给出的前置依赖是这么一条链：

• Ubuntu 与 ROS：文档里写的是在 Ubuntu 20.04 + ROS noetic 上测试过，并明确提醒 Ubuntu 18.04 及更低版本存在环境问题、尽量不要用。另外需要装 pcl-conversions 这个 ROS 包。（版本要求可能随仓库更新变化，以官方仓库当前文档为准。）
• Eigen：线性代数库，libeigen3-dev。
• 雷达 SDK：用 L1 要先编 unilidar_sdk，用 L2 要先编 unilidar_sdk2。

最后这条依赖关系值得停一秒。它意味着这套东西是两个进程：雷达 SDK 那边跑一个 ROS 节点，负责把雷达的原始数据解析成 ROS 话题发出来；Point-LIO 这边跑另一个节点，订阅这些话题做里程计。两个仓库分别编译、分别启动。

这也解释了 README 的运行章节为什么每次都要开两个终端：

# 终端 1：先把雷达数据发出来
cd unilidar_sdk/unitree_lidar_ros
source devel/setup.bash
roslaunch unitree_lidar_ros run_without_rviz.launch

# 终端 2：再启动 Point-LIO
source devel/setup.bash
roslaunch point_lio_unilidar mapping_unilidar_l2.launch

注意第一个用的是 run_without_rviz.launch 而不是普通的 run.launch——可视化交给 Point-LIO 那边统一做，雷达节点这边就不必再开一个 rviz 了。仓库里的 rviz_cfg/ 下面备了 loam_unilidar_default.rviz 和 loam_unilidar_display.rviz 两套配置，也印证了这个分工。

跑完之后，缓存的点云地图会存成一个 pcd 文件，README 给的路径在 PCD/scans.pcd，用 pcl_viewer scans.pcd 就能看。仓库里 PCD/ 目录下放着一个 do_not_delete_this_file.txt——这是个很常见的小技巧，git 不跟踪空目录，塞个占位文件进去才能保证你 clone 下来就有这个输出目录，否则程序写文件时会因为路径不存在而失败。

没有雷达也能先跑起来

README 里还有一件对学习者很友好的事：它提供了官方录制的 rosbag 数据集下载地址，L1 一份、L2 两份（室内与公园）。没有硬件的时候，用 rosbag play 回放数据，Point-LIO 那边照跑不误——它不关心话题是雷达实时发的还是 bag 里放出来的。

这是学习这类算法最省钱的路径：先用官方数据集把整条链路跑通、把 rviz 里的效果看明白，再考虑买硬件。同样的道理在宇树雷达产品线那篇里也提过，先想清楚要拿它做什么，再决定买哪一款。

阅读 README 时的两个小坑

读文档要带点怀疑。这份 README 里有几处前后不一致，照抄会卡住：

一是 3.4 节讲 L2 的 SDK 编译时，git clone 的是 unilidar_sdk2，但下一行 cd 进的却是 unilidar_sdk/unitree_lidar_ros，目录名少了个 2。

二是运行章节里，工作空间一会儿写 catkin_point_lio_unilidar，一会儿写 catkin_unilidar_point_lio，两个名字顺序是反的。而 5.2、5.4 节给出的 pcd 输出路径写成了 point_lio_unilidarPCD/scans.pcd，中间少了一个斜杠，跟 5.1、5.3 节给的路径对不上。

这些都是笔误，不影响算法本身，但如果你是照着命令一行行粘贴的新手，很容易在这里怀疑人生。遇到路径类的错误，先对一眼 README 内部有没有自相矛盾，比闷头 debug 快得多。

两个必须做对的准备动作

外参标定：雷达和 IMU 之间的相对位姿

LIO 要把两个传感器的观测放到同一个坐标系里算，前提是知道它们之间的相对位姿——这叫外参（extrinsic）。

宇树这两款雷达有个便利之处：IMU 是集成在雷达内部的，所以外参是出厂就固定的常数，不需要你自己标。unilidar_sdk2 的 README 直接把这个值写出来了——点云坐标系与 IMU 坐标系的三根轴相互平行、只差一个原点平移，并给出了完整的变换矩阵（README 里记作 T_LI，是一个只有平移、没有旋转的齐次变换）。README 同时说明了点云坐标系的定义：原点在雷达底部安装面中心，+X 轴与底部出线方向相反，+Y 轴由 +X 轴逆时针旋转九十度得到，+Z 轴垂直于底面。（以官方仓库与用户手册当前文档为准。）

那为什么还要专门讲外参？因为外参错了，症状很隐蔽。它不会让程序崩溃，也不会报错，只会让位姿估计持续地偏一点点，而这个偏差会随着走的距离越来越大。等你发现地图歪了，往往已经排查过一圈别的地方了。

更要紧的是，一旦你不是用雷达内置的 IMU，而是想用机器人机身上那个 IMU，外参就得自己确定——那是另一个坐标系，安装位置和朝向都要重新量。这时候「+X 轴与出线方向相反」这类定义就不是文档里的废话了，它是你算安装矩阵的唯一基准。

静止初始化：README 反复强调的那几秒

README 在讲 L1 和 L2 的运行方式时，各强调了一遍同一句话：为保证 IMU 正确初始化，算法开始运行的最初几秒最好让雷达保持静止。

这句话很容易被当成客套话跳过去，但它对应的是一个实打实的物理需求。静止的那几秒，算法在做两件事：

一是估计 IMU 零偏。 陀螺仪在完全静止时理论上应该输出零角速度，实际输出的那个非零值就是零偏。加速度计同理。取一段静止数据求个平均，就能把这个常值偏差量出来，后面积分时扣掉。如果这段数据里混进了运动，你量出来的「零偏」就把真实运动也算进去了，从第一帧起就带着错。

二是确定重力方向。 加速度计测的是比力，静止时它感受到的就是重力反作用。这个矢量在 IMU 坐标系下的方向，直接告诉你哪边是「下」，也就定出了初始的横滚角和俯仰角。没有这个基准，后续所有姿态估计都缺一个锚。

所以那几秒不是「等程序热身」，是在采标定数据。放好、松手、别碰，数几秒再动——这是所有 LIO 系统上手时最容易被忽略、又最容易翻车的一步。

从工程角度看，足式平台还有哪些额外的麻烦

下面这段我不去猜宇树或 Point-LIO 作者的具体考虑，只从工程角度说说足式平台相比轮式、手持设备多出来的几个变量。

机身姿态变化剧烈。 轮式底盘在平地上跑，姿态基本恒定；足式机器人在一个步态周期里就会经历明显的俯仰和侧摆，上下坡、跨障时幅度更大。装在机身上的雷达跟着一起晃，同一帧点云的扫描过程中姿态可能已经转过去不少了。这正是运动补偿要处理的场景，也是 README 里「激进运动」这个词的现实来源。

落脚冲击是高频振动源。 每一步触地都是一次撞击，加速度计上会出现尖峰。IMU 采样率高，这些尖峰会被完整地采进来；如果处理不当，积分时它们就变成了虚假的速度增量。README 明确把「严重振动」列为 Point-LIO 声称能应对的工况，这是选它的一个理由，但不等于随便怎么装都没事。

雷达的安装位置会影响一切。 装得越靠外、离机身旋转中心越远，同样的姿态变化在雷达处产生的线速度就越大；装得低了容易被自己的腿扫进视野，形成一片随步态周期规律变化的「动态障碍」；装在有共振的支架上，机械振动会直接叠加到 IMU 读数里。这些都是装配阶段就该想好的事，出了问题在软件里很难补救。

还有一个容易被忽略的：时间戳。 雷达节点和 Point-LIO 节点是两个进程，中间隔着 ROS 话题；点云、IMU 两路数据的时间戳必须在同一个时基上，融合才有意义。unilidar_sdk2 的示例输出里同时打印了 system stamp 和数据自带的 stamp，就是在提醒你这两者未必是一回事。

这些都不是「装上就能用」的东西。别人的机器人上调好的参数，换个安装位置就未必合适。

里程计和 SLAM 是什么关系

最后把概念理清楚，这两个词经常被混着用。

里程计只回答一个问题：我相对起点移动了多少。 它是一个纯粹的递推过程——用上一时刻的位姿加上这一段的运动增量，得到当前位姿。每一步都有误差，误差会一直累积下去，没有任何机制把它拉回来。走个大圈回到出发点，它算出来的位置和真正的起点大概率对不上，差多少取决于走了多远、环境好不好。

SLAM 多做了两件事：建图，以及回环检测。 同步定位与建图，一边估计自己在哪、一边把环境地图攒出来。关键在于，当你走回一个来过的地方，回环检测能认出「这里我来过」，然后触发一次全局优化，把整条轨迹的累积误差摊回去——这个动作里程计做不到，因为它压根不记得历史。

所以标准的说法是：里程计是 SLAM 的前端。 前端负责高频、实时地吐出位姿，后端负责回环与全局一致性。Point-LIO 这个仓库的定位就在前端——虽然它也把点云攒成了 pcd 地图，宇树的演示视频也用了 SLAM 这个词，但从算法名字（Odometry）到输出形式，它的核心职责是里程计。

这两者的分工也解释了它们各自的用武之地：需要实时反应的场合（避障、局部路径规划）吃的是前端的高频输出，这一点在机器人避障那篇里已经能感觉到；而需要长时间在一个空间里反复来回的场合（巡检、送物），就必须有后端兜住累积误差。

关于回环检测怎么做、点云地图怎么维护和复用、以及宇树体系里还有哪些相关工具，我们放到宇树 SLAM 工具链那篇里接着展开。想先把传感器这一层的基础补齐的，可以从传感器卷入手。

读完这个仓库，值得记住的几件事

1. 足式机器人的位姿不能靠数轮子。 打滑、腾空、冲击这三件事同时存在，任何基于足地接触假设的方法都只能作为短时补充。
2. LIO 里的两个传感器谁也离不开谁。 IMU 补运动畸变、扛几何退化，激光压漂移、给绝对约束。理解了这层互补，你看任何一套融合方案都不会晕。
3. 适配仓库的价值在 config 和 launch，不在算法源码。 一个型号一套 yaml + 一个 launch 这种对称结构，就是它告诉你「怎么接入新硬件」的方式。
4. 外参和静止初始化是两个沉默的坑。 都不报错，都只让结果慢慢偏掉，而且都发生在你还没开始调参之前。
5. 没有硬件也能学。 官方 rosbag 数据集在那儿放着，先把链路跑通再谈买设备。

宇树 SDK 那一侧的通信机制、以及这些数据最终怎么和控制层接起来，可以对照unitree_sdk2 架构拆解一起看——一边是感知怎么进来，一边是指令怎么出去，凑齐了才是一个完整的机器人系统。