当前位置：首页 > news >正文

具身智能2.1刚体运动学：坐标系变换、旋转矩阵、四元数

news 2026/3/26 21:13:57

2.1 第一部分：位姿与旋转矩阵

在纯粹的数字世界（如 ChatGPT）中，信息的最小单位是 Token；而在具身智能的物理世界中，信息的最小单位是位姿（Pose）。

如果你无法精确地用数学语言描述“我的手在哪里”以及“我的手朝向哪里”，那么任何高层的规划（Planning）都无法落地为执行（Control）。运动学（Kinematics）是研究运动的几何性质而不考虑受力的学科，它是连接“感知”与“行动”的第一个数学桥梁。

1. 刚体假设与位姿描述 (Position and Orientation)

在具身智能中，我们通常将机器人（如机械臂、人形机器人的躯干）视为刚体（Rigid Body）。

定义：刚体上任意两点之间的距离在运动过程中保持不变。这意味着我们不需要像流体力学那样计算每一个粒子的状态，只需计算刚体上固连坐标系（Body Frame）的状态。

1.1 自由度 (Degrees of Freedom, DOF)

在三维空间中，描述一个自由浮动的刚体（例如无人机，或尚未固定的机械手末端）需要 6 个参数，即6-DOF：

位置 (Position, 3-DOF)：描述刚体在空间中的坐标 (x, y, z)。
姿态 (Orientation, 3-DOF)：描述刚体在空间中的朝向（滚转、俯仰、偏航）。

1.2 坐标系的映射关系

机器人学本质上是处理不同坐标系之间变换的学科。

世界坐标系 (World Frame,)：固定的、绝对的参考系（例如机械臂的底座，或房间的角落）。
机体坐标系 (Body Frame,)：附着在运动物体上的坐标系（例如机械臂的末端执行器，或机器狗的躯干中心）。

核心任务：我们需要找到一种数学描述，能够将系中的点，映射到系中表示。

数学表达为：

其中：

：点 P 在世界坐标系下的坐标。
：点 P 在机体坐标系下的坐标。
：描述相对于的旋转（本节重点）。
：描述原点相对于原点的平移。

2. 旋转矩阵 (Rotation Matrix) 的推导与本质

平移（Translation）只是简单的向量加法，线性且直观。旋转（Rotation）是具身智能中数学复杂性的主要来源（也是导致非线性的主要原因）。

最基础的描述方式是旋转矩阵。

2.1 从基向量投影的角度理解

不要死记硬背旋转公式。从投影的角度理解最为严谨。

假设坐标系的三个主轴单位向量为。坐标系的三个主轴单位向量为。

我们想描述相对于的姿态。这意味着，我们需要知道这三个向量在坐标系里的分量是多少。

根据向量点积（Dot Product）的几何意义（投影长度），我们可以写出：

将这三个列向量拼合在一起，就形成了旋转矩阵：

实例说明：

假设一个安装在机器人头顶的摄像头，其坐标系相对于机器人底座旋转了 90度（绕 Z 轴）。

摄像头的 X 轴（的前方）指向了底座的 Y 轴方向。
摄像头的 Y 轴（的左方）指向了底座的 -X 轴方向。
摄像头的 Z 轴（的上方）依然指向底座的 Z 轴方向。

则旋转矩阵为：

第一列正是的 X 轴在中的表示。

2.2 旋转矩阵的群性质（SO(3)）

这一点对于编写鲁棒的控制代码至关重要。旋转矩阵不是任意的矩阵，它必须属于特殊正交群 (Special Orthogonal Group, SO(3))。

一个矩阵 R 要成为有效的旋转矩阵，必须满足两个严格约束：

正交性 (Orthogonality)：（或者说）。

物理含义：坐标系的轴必须两两垂直，且长度为 1（单位向量）。旋转不会改变向量的长度，也不会改变向量间的夹角。

行列式为 +1 (Determinant = +1)：。

物理含义：遵循右手定则。如果，则意味着坐标系发生了“镜像”或“翻转”，这在刚体物理运动中是不可能通过连续旋转发生的（除非你把机器人的左手拆下来装成右手）。

工程隐患实例：在长时间的数值计算（如SLAM或机器人积分推演）中，由于浮点数误差，矩阵 R可能会逐渐偏离 SO(3) 流形（比如列向量不再正交）。

后果：机器人的数字模型会认为自己的手臂在“变长”或“剪切变形”。
对策：必须定期对旋转矩阵进行正交化（Orthogonalization）修正（例如使用 Gram-Schmidt 过程或 SVD 分解）。

3. 基本旋转矩阵 (Basic Rotations)

为了构建复杂的姿态，我们通常从绕主轴（X, Y, Z）的基本旋转开始。根据右手定则，拇指指向轴的正方向，四指弯曲方向为正旋转方向。

绕 X 轴旋转角 ():

绕 Y 轴旋转角 ():

(注意：这里 sin 的符号位置与其他两个不同，这是由右手定则在三维轮换中的顺序决定的)

绕 Z 轴旋转角 ():

我们已经建立了刚体运动学的地基：

位姿是具身智能的数据原子。
旋转矩阵本质上是坐标轴的投影描述。
SO(3) 约束保证了物理世界的刚体性质不被数学计算破坏。

然而，旋转矩阵有明显的缺点：它用了 9 个数字来描述 3 个自由度（冗余），计算量大，且难以插值。如果我们要描述“先绕X轴转，再绕Y轴转”，就会引入欧拉角的概念，但这又会带来致命的万向节死锁。

第二部分：欧拉角与万向节死锁

1. 欧拉角 (Euler Angles)：直觉的代价

欧拉角的核心思想是将一次复杂的旋转分解为三次绕特定轴的简单旋转。

最常见的描述方式是RPY 角（Roll-Pitch-Yaw），广泛应用于航空、无人机和机器人末端执行器。

Roll (滚转,)：绕 X 轴旋转（如飞机翻滚）。
Pitch (俯仰,)：绕 Y 轴旋转（如飞机机头抬升）。
Yaw (偏航,)：绕 Z 轴旋转（如飞机水平转向）。

1.1 旋转顺序的重要性 (Sequence Matters)

这是初学者最容易犯错的地方。矩阵乘法不满足交换律（），因此，旋转的顺序决定了最终的姿态。

如果你告诉机器人：“先绕 X 轴转 90 度，再绕 Y 轴转 90 度”，与“先绕 Y 轴转 90 度，再绕 X 轴转 90 度”，其结果是截然不同的。

在工业机器人和 ROS (Robot Operating System) 中，最常用的标准是Z-Y-X 顺序（对应 Yaw-Pitch-Roll）。即：先绕 Z 轴转，再绕新的 Y 轴转，最后绕新的 X 轴转。

1.2 静态轴与动态轴 (Extrinsic vs. Intrinsic)

为了逻辑缜密，必须区分两种旋转参考：

静态轴 (Extrinsic / Fixed Frame)：每次旋转都绕着固定的世界坐标系轴旋转。

数学操作：左乘旋转矩阵 ()。

动态轴 (Intrinsic / Body Frame)：每次旋转都绕着物体变换后的新轴旋转。

数学操作：右乘旋转矩阵 ()。

具身智能中的惯例：我们通常谈论的是动态轴（Intrinsic），因为传感器（IMU）和执行器是绑在机器人身上的。

2. 万向节死锁 (Gimbal Lock)：数学奇点

欧拉角虽然直观且节省存储（只用 3 个数），但它存在一个无法根除的缺陷：万向节死锁。

这并不是指机械关节被物理卡住了，而是数学描述上的自由度丢失（Singularity）。

2.1 现象描述

当中间的旋转轴（在 Z-Y-X 顺序中是 Y 轴，即 Pitch）旋转到时，第三个旋转轴（X 轴）就会与第一个旋转轴（Z 轴）在空间中重合（或者由平行变为共线）。

此时，机器人在空间中虽然看起来还可以转动，但从数学控制的角度看，它失去了一个自由度。系统无法区分“绕 Z 轴旋转”和“绕 X 轴旋转”，因为它们现在的效果是一样的。

2.2 严密的数学证明

为了深入理解，我们展开 Z-Y-X 顺序的旋转矩阵乘积。

设。

乘积结果的通式非常复杂，但我们只关注死锁时刻。当Pitch时，。

代入后的矩阵变为：

利用三角函数和差公式：

逻辑推演结论：观察矩阵中的项，所有的变量都变成了的形式。这意味着，无论你怎么改变 Roll () 或 Yaw ()，只要它们的和不变，得到的旋转矩阵就是完全一样的。

方程组有无穷多解。
控制器无法解算出唯一的和。
后果：机器人会发生剧烈的抖动，或者在执行路径规划时，因为雅可比矩阵（Jacobian）行列式为 0 而报错，导致程序崩溃。

3. 实例：现实世界中的灾难

3.1 工业机械臂的“奇异点”

当你控制一个 6 轴机械臂去焊接一个正上方的点时，如果不幸机械臂的第 5 轴（Wrist Pitch）转到了 90 度，第 4 轴和第 6 轴就会对齐。此时，如果你命令机器人“向左微调”，机器人可能会突然疯了一样快速旋转第 4 轴和第 6 轴 180 度，试图凑出这个位姿。这种剧烈运动极易打断线缆或撞坏工件。