第2章空间描述与变换基础

第二章空间描述与变换基础：位置与姿态的数学表示

各位同学，欢迎来到第二章。说实话，这一章是整个机器人学的「地基」。我见过太多人，一上来就急着写运动学代码，结果坐标系搞反了，机械臂直接撞到工作台上。嗯，这种事我也干过，所以咱们今天把基础打牢。

2.1 位置与姿态——机器人怎么「看」世界？

机器人要抓东西，首先得知道两件事：目标在哪儿，以及自己怎么转。前者叫位置，后者叫姿态。合在一起，就是空间描述。

你想想看，你让朋友帮你拿杯子，你会说「杯子在你右手边30厘米处，杯把朝左」。这不就是位置+姿态吗？机器人也一样，只不过它用的是数学语言。

位置的数学表示

在三维空间里，一个点用三个坐标就够了：

P = [x, y, z]ᵀ

这很简单，对吧？但注意，这个坐标是相对于某个坐标系的。换个坐标系，数值就变了。我在项目中吃过这个亏——有一次把传感器坐标系下的数据直接当成世界坐标系用，结果定位差了20厘米。

姿态的数学表示

姿态就有点意思了。一个物体在空间里可以旋转，怎么描述它转了多少？常用的有三种方式：

旋转矩阵：9个数字，但只有3个自由度
欧拉角：直观，但有万向锁问题
四元数：计算方便，没有奇点

咱们先从旋转矩阵讲起，因为它是基础中的基础。

2.2 旋转矩阵——姿态的「身份证」

旋转矩阵，说白了就是一个3×3的矩阵，它告诉你一个坐标系相对于另一个坐标系是怎么转的。

R = [r₁₁ r₁₂ r₁₃]
    [r₂₁ r₂₂ r₂₃]
    [r₃₁ r₃₂ r₃₃]

这个矩阵有个重要性质：它是正交矩阵。什么意思？就是它的逆等于它的转置：

R⁻¹ = Rᵀ

为什么会这样？因为旋转不改变向量的长度和夹角。你想想看，一个刚体转来转去，它上面的点之间的距离不会变，对吧？

核心要点：旋转矩阵的每一列，就是原坐标系各轴在新坐标系中的投影。

举个例子，绕Z轴旋转θ角：

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

绕X轴和Y轴的类似，我就不写了。记住一个规律：绕哪个轴转，哪个轴对应的行和列就不变。

我的小技巧：写旋转矩阵时，先确定旋转轴，再确定角度方向。右手法则——大拇指指向旋转轴正方向，四指弯曲方向就是正角度方向。

2.3 平移变换——不光要转，还要走

光有旋转不够，机器人还要移动。平移变换就是描述「从A点走到B点」的数学工具。

假设有个点P，在坐标系{A}中的坐标是^AP，在坐标系{B}中的坐标是^BP。如果{B}相对于{A}只有平移，没有旋转，那么：

^AP = ^BP + ^AP_BORG

其中^AP_BORG是{B}的原点在{A}中的位置。

但大多数时候，旋转和平移是同时发生的。这时候就需要把两者合在一起处理。

2.4 齐次坐标——把旋转和平移「打包」

你可能会问：为什么不能把旋转矩阵和平移向量分开算？当然可以，但那样写起来很啰嗦。比如：

^AP = ^AR_B · ^BP + ^AP_BORG

每次都要做一次矩阵乘法和一次向量加法。能不能一步到位？

能。用齐次坐标。

齐次坐标就是在三维坐标后面加一个1：

P_h = [x, y, z, 1]ᵀ

这样，旋转和平移就可以合并成一个4×4的矩阵：

^AT_B = [^AR_B   ^AP_BORG]
        [0 0 0    1     ]

这个^AT_B就是齐次变换矩阵。用它变换一个点：

^AP_h = ^AT_B · ^BP_h

一步到位，干净利落。

注意：齐次变换矩阵的最后一行的前三个元素必须是0 0 0，最后一个元素是1。我曾经见过有人写代码时把这一行写错了，结果变换出来的点全飞到天上去。

2.5 齐次变换矩阵的逆——怎么「回去」？

有了从{B}到{A}的变换，自然也需要从{A}到{B}的变换。直接求逆矩阵当然可以，但有个更聪明的方法：

^BT_A = [^AR_Bᵀ   -^AR_Bᵀ · ^AP_BORG]
        [0 0 0       1         ]

为什么？因为旋转矩阵的逆就是它的转置，而平移部分需要重新计算。这个公式我建议你记下来，写代码时直接用，省得每次都要调库求逆。

2.6 知识体系总览

下面这张图把本章的核心逻辑串起来了：

2.7 实战：用Python实现齐次变换

光说不练假把式。咱们写几行代码，把上面的理论跑一遍。

import numpy as np

def rotation_z(theta):
    """绕Z轴旋转的旋转矩阵"""
    c = np.cos(theta)
    s = np.sin(theta)
    return np.array([[c, -s, 0],
                     [s,  c, 0],
                     [0,  0, 1]])

def homogeneous_transform(R, t):
    """由旋转矩阵R和平移向量t构造齐次变换矩阵"""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

# 例子：绕Z轴转30度，再沿X轴平移5个单位
theta = np.radians(30)
R = rotation_z(theta)
t = np.array([5, 0, 0])

T = homogeneous_transform(R, t)
print("齐次变换矩阵：")
print(T)

# 变换一个点
p = np.array([1, 0, 0, 1])  # 齐次坐标
p_transformed = T @ p
print("变换后的点：", p_transformed[:3])

避坑指南：我曾经在写代码时忘了把角度转成弧度，结果旋转矩阵算出来全是错的。记住，numpy的三角函数默认用弧度！

2.8 本章小结

这一章的内容，说白了就是三件事：

位置用三维坐标表示
姿态用旋转矩阵表示
齐次变换矩阵把两者合在一起，方便计算

这些概念会贯穿整个机器人学课程。你后面学正运动学、逆运动学、动力学，都离不开它们。所以，别急着往下赶，把这一章吃透再说。

嗯，今天就到这儿。记住我的一句话：坐标系搞对了，问题就解决了一半。

📚 课程目录

第二章 空间描述与变换基础：位置与姿态的数学表示

2.1 位置与姿态——机器人怎么「看」世界？

位置的数学表示

姿态的数学表示

2.2 旋转矩阵——姿态的「身份证」

2.3 平移变换——不光要转，还要走

2.4 齐次坐标——把旋转和平移「打包」

2.5 齐次变换矩阵的逆——怎么「回去」？

2.6 知识体系总览

2.7 实战：用Python实现齐次变换

2.8 本章小结

第二章空间描述与变换基础：位置与姿态的数学表示