第2章刚体运动学基础

2. 刚体运动学基础：位置、姿态、旋转矩阵、欧拉角、四元数

好，咱们正式开始啃六自由度机器人的核心——刚体运动学。

说白了，你要让机器人动起来，首先得知道它在哪儿、朝哪儿看。这就是位置和姿态的问题。我当年刚入行时，觉得这玩意儿不就是坐标嘛，有啥难的？结果第一次调试机械臂，末端执行器死活对不准目标点，折腾了一下午才发现是姿态表达搞混了。嗯，从那以后我再也不敢小看这部分基础了。

核心概念：刚体在三维空间中有6个自由度——3个平移自由度（位置） + 3个旋转自由度（姿态）。

2.1 位置描述

位置最简单。一个三维向量就搞定：

p = [x, y, z]^T

比如机械臂末端在基坐标系下的位置是 (0.5m, 0.3m, 0.2m)。这没啥好说的，初中数学水平。

但要注意：位置永远相对于某个坐标系。你跟我说末端在 (0.5, 0.3, 0.2)，我第一反应是——相对于哪个坐标系？基座标？工具坐标系？还是视觉坐标系？

我见过太多新手犯这个错。代码里写死一个位置，换了个安装位置就全乱套了。

2.2 姿态描述——这才是重头戏

姿态描述方式有好几种。我个人习惯把它们分成三类：

旋转矩阵：9个元素，冗余大，但直观
欧拉角：3个角度，简洁但有奇异性
四元数：4个参数，无奇异性，适合插值

2.2.1 旋转矩阵

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^T R = I，det(R) = 1。

它的每一列代表旋转后的坐标轴在原始坐标系中的投影。举个例子：

R = [[cosθ, -sinθ, 0],
     [sinθ,  cosθ, 0],
     [0,      0,    1]]

这是绕 Z 轴旋转 θ 角度的旋转矩阵。

我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，数值误差会累积，导致矩阵不再正交。这时候需要做正交化处理。我一般用 SVD 分解来修正：

import numpy as np

def orthogonalize(R):
    U, _, Vt = np.linalg.svd(R)
    return U @ Vt

小技巧：每次更新旋转矩阵后，都做一次正交化。虽然多花一点点计算量，但能避免后续姿态发散。

2.2.2 欧拉角

欧拉角用三个角度描述旋转。常见的有 ZYX 顺序（也叫RPY角：Roll、Pitch、Yaw）。

你想想看，为什么会有这么多顺序？因为旋转不可交换。先绕X转再绕Y转，和先绕Y转再绕X转，结果完全不同。

我建议初学者先掌握 ZYX 顺序：

Yaw（偏航）：绕Z轴转
Pitch（俯仰）：绕Y轴转
Roll（横滚）：绕X轴转

从旋转矩阵转欧拉角的公式：

def rotation_matrix_to_euler(R):
    sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(R[2,1], R[2,2])
        y = np.arctan2(-R[2,0], sy)
        z = np.arctan2(R[1,0], R[0,0])
    else:
        x = np.arctan2(-R[1,2], R[1,1])
        y = np.arctan2(-R[2,0], sy)
        z = 0
    return np.array([x, y, z])

警告：欧拉角有万向锁问题。当 Pitch = ±90° 时，Roll 和 Yaw 的旋转轴重合，丢失一个自由度。我曾经在无人机飞控里踩过这个坑——飞机做俯冲动作时姿态突然乱跳，就是因为万向锁。

2.2.3 四元数

四元数是我个人最喜欢的姿态表达方式。它没有奇异性，插值平滑，计算效率高。

四元数 q = [w, x, y, z]^T，其中 w 是实部，x,y,z 是虚部。它满足 w² + x² + y² + z² = 1。

从旋转矩阵转四元数：

def rotation_matrix_to_quaternion(R):
    tr = R[0,0] + R[1,1] + R[2,2]
    if tr > 0:
        S = np.sqrt(tr + 1.0) * 2
        w = 0.25 * S
        x = (R[2,1] - R[1,2]) / S
        y = (R[0,2] - R[2,0]) / S
        z = (R[1,0] - R[0,1]) / S
    elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
        S = np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2]) * 2
        w = (R[2,1] - R[1,2]) / S
        x = 0.25 * S
        y = (R[0,1] + R[1,0]) / S
        z = (R[0,2] + R[2,0]) / S
    elif R[1,1] > R[2,2]:
        S = np.sqrt(1.0 + R[1,1] - R[0,0] - R[2,2]) * 2
        w = (R[0,2] - R[2,0]) / S
        x = (R[0,1] + R[1,0]) / S
        y = 0.25 * S
        z = (R[1,2] + R[2,1]) / S
    else:
        S = np.sqrt(1.0 + R[2,2] - R[0,0] - R[1,1]) * 2
        w = (R[1,0] - R[0,1]) / S
        x = (R[0,2] + R[2,0]) / S
        y = (R[1,2] + R[2,1]) / S
        z = 0.25 * S
    return np.array([w, x, y, z])

经验之谈：做姿态插值时，用四元数的球面线性插值（SLERP）效果最好。欧拉角插值会出现奇怪的旋转路径，而四元数插值路径最短最自然。

2.3 三种表达方式的对比

特性	旋转矩阵	欧拉角	四元数
参数数量	9	3	4
奇异性	无	有（万向锁）	无
直观性	一般	好	差
插值	困难	有路径问题	容易（SLERP）
计算效率	低	高	高
存储	9个浮点数	3个浮点数	4个浮点数

2.4 刚体运动学知识体系

下面这张图是我梳理的刚体运动学核心知识结构，帮你理清思路：

2.5 实际应用中的选择建议

说了这么多理论，到底该用哪种？我根据实际项目经验给点建议：

做可视化调试：用欧拉角。人眼看得懂，调参方便。但注意避开万向锁区域。
做运动学计算：用旋转矩阵。矩阵运算成熟，库函数多。
做姿态插值和滤波：用四元数。SLERP插值平滑，卡尔曼滤波也常用四元数做状态量。
做存储和传输：用四元数或欧拉角。4个或3个浮点数，比9个浮点数省带宽。

避坑指南：我曾经在项目中直接用欧拉角做姿态控制，结果在某个特殊姿态下控制器发散，差点把机械臂撞到工件上。后来全部改用四元数做内部计算，只在人机界面显示时转成欧拉角。这个教训让我养成了一个习惯——内部计算统一用四元数，外部接口用欧拉角。

好了，刚体运动学基础就讲到这里。位置和姿态是机器人学的基石，后面的正运动学、逆运动学、轨迹规划全都建立在这之上。把这些搞扎实了，后面的路就好走了。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录

2. 刚体运动学基础：位置、姿态、旋转矩阵、欧拉角、四元数

2.1 位置描述

2.2 姿态描述——这才是重头戏

2.2.1 旋转矩阵

2.2.2 欧拉角

2.2.3 四元数

2.3 三种表达方式的对比

2.4 刚体运动学知识体系

2.5 实际应用中的选择建议