第三章：坐标系与刚体运动

说实话，坐标系这个问题，是SLAM里最容易让人栽跟头的地方。我见过太多新手，代码写得飞起，结果一跑起来地图全歪了——最后发现，就是坐标系搞混了。今天咱们就把这事彻底捋清楚。

3.1 世界坐标系、机器人坐标系、传感器坐标系

先说说这三个坐标系的关系。你想想看，一个机器人在地图里移动，它得知道自己在哪里吧？这就需要一个固定的参考系——世界坐标系。机器人本身也有自己的朝向和位置，这就是机器人坐标系。而传感器（比如激光雷达、相机）装在机器人身上，它们又有自己的坐标系。

核心概念：

  • 世界坐标系（World Frame）：固定不动，通常以地图原点为基准。单位是米，方向是右手系。
  • 机器人坐标系（Robot/Base Frame）：随机器人移动，原点一般在机器人中心。x轴朝前，y轴朝左，z轴朝上。
  • 传感器坐标系（Sensor Frame）：装在机器人上的每个传感器都有自己的坐标系。比如激光雷达的坐标系，原点在雷达中心。

我在项目里遇到过这么个事：有个同学把激光雷达的数据直接当成机器人坐标系下的数据用，结果机器人转了个身，地图就全乱了。其实很简单——你得先把传感器数据变换到机器人坐标系，再变换到世界坐标系。

3.2 欧拉角与四元数

说到旋转表示，欧拉角和四元数是绕不开的两个东西。我个人习惯是：人看用欧拉角，机器算用四元数。

欧拉角

欧拉角就是绕三个轴转的角度：绕x轴转叫roll（横滚），绕y轴转叫pitch（俯仰），绕z轴转叫yaw（偏航）。听起来很直观对吧？但有个大坑——万向锁。

避坑指南：我曾经在调试一个无人机SLAM时，发现俯仰角接近90度时，偏航和横滚突然就分不清了。这就是万向锁。所以，如果你要做全向旋转（比如无人机、机械臂），千万别只用欧拉角。

四元数

四元数就优雅多了。它用四个数表示旋转：q = [w, x, y, z]，其中w是实部，x、y、z是虚部。没有万向锁，插值也平滑。嗯，这里要注意：四元数必须归一化，不然旋转会变形。

举个例子，绕z轴转90度，四元数就是：

q = [cos(45°), 0, 0, sin(45°)] = [0.707, 0, 0, 0.707]

3.3 坐标变换实战（TF树）

好了，理论说完了，咱们来点实战。ROS里的TF树就是专门干这个的。说白了，TF树就是维护所有坐标系之间变换关系的一个树状结构。

我的经验：写TF树时，记住一个原则——谁是谁的父坐标系，谁是谁的子坐标系。比如机器人是父，激光雷达是子。这样变换关系就清晰了。

下面是一个典型的TF树结构图，我用SVG画出来，你看看就明白了：

world TF odom TF base_link TF laser_frame TF camera_frame 图例：world（固定）→ odom（里程计）→ base_link（机器人）→ laser_frame / camera_frame（传感器）

你看这个树，从world到odom，再到base_link，最后到各个传感器。每个箭头都代表一个变换关系，也就是一个TF。实际写代码时，你只需要发布这些变换，ROS会自动帮你维护整个树。

实战代码：发布TF变换

下面是一个简单的C++代码，发布从base_link到laser_frame的变换：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
  ros::init(argc, argv, "tf_broadcaster");
  ros::NodeHandle nh;
  
  tf::TransformBroadcaster br;
  tf::Transform transform;
  
  // 设置激光雷达相对于机器人中心的位置：x=0.2m, y=0, z=0.1m
  transform.setOrigin(tf::Vector3(0.2, 0.0, 0.1));
  
  // 设置旋转：绕z轴转0度（即无旋转）
  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  transform.setRotation(q);
  
  ros::Rate rate(10.0);
  while (nh.ok()) {
    // 发布变换：父坐标系base_link，子坐标系laser_frame
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), 
                                          "base_link", "laser_frame"));
    rate.sleep();
  }
  return 0;
}

小技巧：调试TF树时，用tf_echo命令可以实时查看任意两个坐标系之间的变换。比如：rosrun tf tf_echo base_link laser_frame。我调试时几乎离不开这个命令。

3.4 常见问题与避坑

最后，我总结几个实际项目中容易踩的坑：

  • 坐标系搞反：发布TF时，父坐标系和子坐标系别写反了。我曾经把parent和child写反，结果机器人在地图里乱窜。
  • 时间戳不对：TF变换一定要带正确的时间戳。如果时间戳是0，ROS会认为这是历史数据，可能不处理。
  • 四元数未归一化：手动构造四元数时，记得归一化。不然旋转矩阵会变形，地图会扭曲。
  • 欧拉角顺序：不同库的欧拉角顺序可能不同。ROS里是RPY（roll-pitch-yaw），但有些库是YPR。一定要确认清楚。

好了，坐标系和刚体运动这块，说白了就是搞清楚「谁在哪儿，怎么转」。你只要把TF树画清楚，代码写对，SLAM建图就成功了一半。剩下的，就是让算法去跑了。

本章小结：

  • 世界坐标系、机器人坐标系、传感器坐标系，三者通过TF树关联
  • 欧拉角直观但有万向锁，四元数适合计算
  • TF树是坐标变换的核心，发布时注意父子关系和时间戳

可可精品资料库

← 上一章 📖 返回目录 下一章 →