2. 坐标系与刚体运动：世界坐标系、机器人坐标系、旋转矩阵、四元数、欧拉角、齐次变换矩阵

做SLAM这些年，我最大的感触就是：坐标系搞不清楚，后面全白搭。你想想看，机器人明明在房间里转了一圈，结果轨迹画出来却飞到了天花板——八成是坐标系定义出了问题。

这一节，我们就来聊聊坐标系和刚体运动。说白了，就是回答两个问题：“我在哪？” 和 “我怎么动？”

2.1 世界坐标系 vs 机器人坐标系

先讲个我踩过的坑。有一次做AGV导航，我直接把激光雷达的数据当成全局坐标来用，结果机器人一转弯，地图就跟着转。嗯，后来才意识到——传感器数据是在机器人坐标系下的，必须转换到世界坐标系才能建图。

这两个坐标系的关系，我习惯这样理解：

  • 世界坐标系（World Frame）：固定不动的大地坐标系。通常以机器人启动位置为原点，x轴指东，y轴指北，z轴指天。它是所有轨迹评估的基准。
  • 机器人坐标系（Robot/Body Frame）：跟着机器人走的坐标系。原点在机器人中心，x轴朝前，y轴朝左，z轴朝上。传感器数据、控制指令都在这个坐标系下。

核心原则：所有传感器测量值都在机器人坐标系下，所有地图和轨迹都在世界坐标系下。两者之间通过一个变换矩阵连接。

我个人习惯在代码里用 world 和 body 作为命名前缀，比如 world_T_body 表示从机器人坐标系到世界坐标系的变换矩阵。这样一看就明白，不会搞混。

2.2 旋转矩阵：最直观的旋转表示

旋转矩阵，说白了就是一个3x3的矩阵，用来描述一个坐标系相对于另一个坐标系的朝向。它的每一列，就是原坐标系基向量在目标坐标系下的投影。

举个例子，机器人绕z轴旋转θ角，旋转矩阵长这样：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

旋转矩阵有个重要性质：它是正交矩阵，即 RT = R-1。这意味着求逆就是转置，计算上非常方便。

我的小技巧：验证一个矩阵是不是旋转矩阵，检查两件事：1) 行列式为+1；2) 矩阵乘以它的转置等于单位阵。我在项目中经常用这个来排查标定数据是否出错。

2.3 欧拉角：直观但容易踩坑

欧拉角用三个角度来描述旋转，比如偏航角(yaw)、俯仰角(pitch)、滚转角(roll)。听起来很直观对吧？但我要说——欧拉角是万恶之源。

为什么？因为：

  • 万向锁问题：当pitch接近±90°时，yaw和roll会失去一个自由度。我在做无人机仿真时就遇到过，飞机一抬头，姿态就乱跳。
  • 顺序依赖：ZYX、ZXY、YXZ……不同顺序结果完全不同。我曾经因为没统一顺序，两个模块算出来的姿态差了180°。
  • 插值困难：两个欧拉角之间做线性插值，得到的路径往往不是最短路径。

避坑指南：我曾经在轨迹评估中直接用欧拉角做误差分析，结果发现误差曲线剧烈抖动。后来改用四元数，问题立刻解决。所以我的建议是：人机交互用欧拉角，内部计算用四元数。

2.4 四元数：SLAM的标配

四元数，说白了就是一个四维向量 q = [w, x, y, z]，其中 w 是实部，x,y,z 是虚部。它没有万向锁，插值平滑，计算高效——SLAM领域几乎清一色用四元数。

四元数表示旋转的公式：

// 绕单位向量 u = [ux, uy, uz] 旋转 θ 角
q = [cos(θ/2), ux*sin(θ/2), uy*sin(θ/2), uz*sin(θ/2)]

两个四元数相乘表示旋转的复合：

q_result = q1 * q2  // 先做q2旋转，再做q1旋转

这里要注意：四元数乘法不满足交换律，q1*q2 和 q2*q1 结果不同。我刚开始学的时候经常搞反，后来养成了一个习惯——在代码里写注释说明旋转顺序。

实用建议：在轨迹评估中，我通常用四元数计算相对旋转误差。具体做法是：q_error = q_gt.inverse() * q_est，然后取角度部分作为误差指标。

2.5 齐次变换矩阵：把旋转和平移打包

齐次变换矩阵是一个4x4的矩阵，把旋转矩阵和平移向量打包在一起：

T = [R  t]
    [0  1]

其中 R 是3x3旋转矩阵，t 是3x1平移向量。用齐次坐标表示点 p = [x, y, z, 1]T，变换就是简单的矩阵乘法：

p' = T * p

这样做的好处是：多个变换可以连乘成一个矩阵。比如从机器人坐标系到世界坐标系的变换，可以分解为：

T_world_body = T_world_camera * T_camera_imu * T_imu_body

嗯，这里要注意顺序——矩阵乘法从右往左读，先做 body→imu，再做 imu→camera，最后 camera→world。

2.6 知识体系总览

下面这张图，是我自己总结的坐标系与刚体运动知识结构。每次做轨迹评估前，我都会对照这张图检查一遍：

坐标系与刚体运动知识体系 世界坐标系 机器人坐标系 齐次变换矩阵 T 旋转矩阵 R (3x3) 四元数 q (w,x,y,z) 欧拉角 (yaw,pitch,roll) 相互转换：旋转矩阵 ↔ 四元数 ↔ 欧拉角 轨迹评估：相对位姿误差 传感器标定：外参矩阵 运动规划：插值与平滑 核心：所有运动都是坐标系之间的变换

2.7 实战：坐标系变换的代码实现

最后，我分享一段我自己常用的坐标系变换代码。这段代码在轨迹评估中反复用到：

import numpy as np
from scipy.spatial.transform import Rotation as R

def quaternion_to_rotation_matrix(q):
    """四元数转旋转矩阵"""
    return R.from_quat(q).as_matrix()

def euler_to_rotation_matrix(yaw, pitch, roll):
    """欧拉角转旋转矩阵 (ZYX顺序)"""
    return R.from_euler('zyx', [yaw, pitch, roll]).as_matrix()

def build_transform_matrix(R, t):
    """构建齐次变换矩阵"""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def transform_point(T, p):
    """用齐次变换矩阵变换点"""
    p_h = np.append(p, 1)
    p_transformed = T @ p_h
    return p_transformed[:3]

# 示例：将机器人坐标系下的点变换到世界坐标系
q_body = [0.707, 0.0, 0.707, 0.0]  # 四元数
t_body = [1.0, 2.0, 0.0]           # 平移
R_body = quaternion_to_rotation_matrix(q_body)
T_world_body = build_transform_matrix(R_body, t_body)

p_body = [0.5, 0.0, 0.3]
p_world = transform_point(T_world_body, p_body)
print(f"世界坐标系下的点: {p_world}")

我的习惯：在代码里，我总会在变换矩阵变量名上标注坐标系关系，比如 T_world_body 表示从body到world的变换。这样半年后回来看代码，一眼就能明白。

好了，坐标系和刚体运动就聊到这里。记住一句话：没有错误的坐标系，只有没转对的变换。下一节我们聊聊传感器数据怎么处理，到时候这些变换知识会反复用到。

可可精品资料库

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