3、坐标系与TF变换：人形机器人常用坐标系、TF树设计、坐标变换常见问题
坐标系和TF变换，说白了就是让机器人知道「我在哪，要去哪，周围的东西在哪」。人形机器人比轮式机器人复杂得多——它有两条腿、两条胳膊、一个会晃动的躯干，每个关节都在动。如果你TF树没搭好，导航就是一场灾难。
我记得刚入行那会儿，调一个人形机器人的导航，结果机器人原地转圈，死活不走直线。查了两天，最后发现是base_link和odom之间的TF发布频率不对。嗯，这种坑我踩过不少，今天咱们一次性说清楚。
3.1 人形机器人常用坐标系
人形机器人导航涉及的坐标系，比AGV多出一倍不止。我习惯把它们分成三类：全局坐标系、本体坐标系、感知坐标系。
| 坐标系 | 命名惯例 | 作用 | 谁发布的 |
|---|---|---|---|
| 地图坐标系 | map | 全局静态参考，SLAM建图基准 | map_server / SLAM节点 |
| 里程计坐标系 | odom | 局部连续位姿估计，漂移可接受 | 轮式里程计 / 腿式里程计 |
| 本体基座坐标系 | base_link | 机器人中心，所有子坐标系的父级 | robot_state_publisher |
| 躯干坐标系 | torso_link | 上半身参考，IMU通常装在这里 | URDF + robot_state_publisher |
| 头部/相机坐标系 | head_link / camera_link | 视觉感知原点 | URDF + robot_state_publisher |
| 足部坐标系 | left_foot_link / right_foot_link | 支撑脚检测、步态规划参考 | URDF + robot_state_publisher |
| 激光雷达坐标系 | laser_link | 2D/3D激光扫描原点 | URDF + robot_state_publisher |
这里有个关键点：map和odom之间不能有闭环修正。map→odom的变换只能由定位模块（比如AMCL）发布，而且只能单向修正。我在项目中见过有人把odom→base_link的变换也塞进定位模块，结果TF树直接炸了——两个父节点同时发布同一个子节点的变换，ROS会报「Transform conflict」错误。
3.2 TF树设计：人形机器人的特殊之处
TF树设计，我总结了一个原则：一个父节点，一条路径，没有环路。人形机器人因为关节多，很容易搞出环路来。你想想看，左腿和右腿最终都连到躯干，但如果躯干又通过某个传感器连回左腿，环路就出现了。
下面这张图是我个人习惯的TF树结构，适合双足人形机器人导航场景：
这张图里，base_stabilized是关键。人形机器人走路时躯干会上下颠簸、左右摇摆，如果激光雷达直接绑在base_link上，scan数据会跟着晃。我在base_stabilized里用IMU的roll/pitch做补偿，把激光数据投影到一个虚拟的水平面上。效果很明显——建图质量提升了一个档次。
3.3 坐标变换常见问题与避坑
做导航调参，80%的诡异问题都出在TF上。我列几个高频坑，你对照着排查：
3.3.1 时间戳不同步
ROS的TF系统依赖时间戳。如果两个坐标系的时间戳差太多，lookupTransform()会报ExtrapolationException。人形机器人关节多，每个关节的TF发布时间可能不一样。
我曾经遇到一个情况：左腿的TF延迟了20ms，右腿正常，结果机器人走路时左脚的位置总是「飘」的。排查到最后，发现是左腿关节的电机驱动板时钟不准。
tf2::TimePoint统一取ros::Time::now()，不要用传感器自带的时间戳。
3.3.2 坐标系命名不一致
URDF里叫left_foot_link，里程计节点里叫l_foot_link，导航栈里又写foot_left。这种问题我见得太多了。ROS不会帮你检查命名一致性，它只认字符串匹配。
我建议你做一个TF命名规范表，贴在工位上：
| 物理部件 | 标准命名 | 禁止命名 |
|---|---|---|
| 左足 | left_foot_link | l_foot, foot_left, left_ankle |
| 右足 | right_foot_link | r_foot, foot_right, right_ankle |
| 激光雷达 | laser_link | lidar, laser, hokuyo_link |
| 深度相机 | camera_depth_link | depth_camera, realsense_link |
3.3.3 静态TF发布时机不对
人形机器人有很多固定关节（比如躯干到头部），这些用static_transform_publisher发布。但有个坑：静态TF只在启动时发布一次。如果你在机器人运行中修改了URDF，静态TF不会自动更新。
我记得有一次调试，换了一个更重的激光雷达支架，重新生成了URDF，但忘了重启静态TF发布节点。结果导航一直报错，说激光雷达的位置不对。折腾了半小时才发现是静态TF没刷新。
tf2_ros::StaticTransformBroadcaster在代码里动态发布，每次更新URDF时重新广播一次。
3.3.4 腿式里程计的TF漂移
人形机器人没有轮子，里程计靠腿式里程计（footstep odometry）推算。但足部打滑、地面不平都会导致漂移。我见过最夸张的一次，机器人走了10步，odom漂了30厘米。
怎么处理？我的做法是：
- 不要直接依赖腿式里程计做导航定位。用它做短时间（1-2秒）的局部位姿估计就够了。
- 融合IMU和视觉。用
robot_localization包做EKF融合，把腿式里程计、IMU、视觉里程计的数据揉在一起。 - 定期重置odom。每次检测到双脚着地且稳定时，把odom的累积误差清零。
3.4 调试TF的实用工具
最后分享几个我常用的TF调试命令，你记一下：
tf2_monitor— 查看所有TF的发布频率、延迟、最大延迟。我每次启动机器人第一件事就是跑这个。tf2_echo map base_link— 实时查看两个坐标系之间的变换。检查odom漂移时特别好用。view_frames— 生成TF树的PDF图。如果TF树有环路，这张图一眼就能看出来。rviz的TF显示插件 — 在3D场景里看坐标系的位置和朝向。我习惯把base_link和laser_link的坐标轴显示出来，确认它们之间的相对关系对不对。
嗯，坐标系和TF变换这部分，说白了就是「地基」。地基没打好，后面所有导航算法都是空中楼阁。你花一天时间把TF树理清楚，后面能省下一周的调参时间。这个账，你自己算算。