2. 机器人机构建模的数学基础

多体动力学仿真的第一步不是写方程，而是把“机器人长什么样、各构件在空间中处于什么位置和姿态”用一套统一的数学语言描述清楚。本节围绕五个相互衔接的问题展开：刚体位姿如何描述、旋转矩阵与四元数如何表达姿态、齐次变换如何统一平移与旋转、关节约束如何写成数学等式、机构简图如何转换为可计算的拓扑模型。这五步构成了从物理样机到仿真模型的“数学接口”。

一、刚体位姿描述：位置与姿态分离

刚体在三维空间中有 6 个自由度：3 个平移、3 个转动。描述时通常把两者分开：位置用一个三维向量 p = [x, y, z]ᵀ 表示，姿态用一个 3×3 旋转矩阵 R 表示。二者组合成 4×4 齐次变换矩阵 T，即

T = [ R  p ]
    [ 0  1 ]

这里约定：所有坐标系均为右手系，旋转矩阵按“从局部坐标系到参考坐标系”的方向定义，即 p_ref = R · p_local + p_origin。全文及图示均采用这一约定，切换参照系时必须显式说明。

二、旋转矩阵与四元数：两种姿态表达

旋转矩阵的优点是可直接与向量相乘、便于复合，缺点是 9 个元素中只有 3 个独立，存在冗余且数值积分时容易失去正交性。四元数 q = [w, x, y, z]ᵀ 用 4 个分量表达姿态，满足 w² + x² + y² + z² = 1，无奇异、插值平滑，适合动力学积分。二者可互相转换：给定单位四元数，旋转矩阵为

R = [1-2(y²+z²)   2(xy-wz)    2(xz+wy) ]
    [2(xy+wz)     1-2(x²+z²)  2(yz-wx) ]
    [2(xz-wy)     2(yz+wx)    1-2(x²+y²)]

实际仿真中，姿态积分通常用四元数，输出与显示时再转成旋转矩阵或欧拉角。需要注意：欧拉角存在万向节死锁，仅用于人机交互显示，不用于内部状态积分。

三、齐次变换：统一平移与旋转

齐次变换把“先旋转再平移”合并为一次矩阵乘法，使多级坐标变换可以连乘。例如从基座到末端执行器的变换为

T_0n = T_01 · T_12 · … · T_(n-1)n

每个 T_(i-1)i 由关节变量决定。转动关节的旋转部分随角度变化，移动关节的平移部分随位移变化。这样，机构的正运动学就转化为矩阵连乘，便于程序化实现。

四、关节约束的数学表达

关节约束的本质是限制两个相邻构件之间的相对运动。以转动关节为例，约束可写为：两构件在关节处的相对平移为零，只保留绕关节轴的一个转动自由度。设关节轴单位向量为 a，则约束方程可表示为

Φ_trans = p_i - p_j - d = 0
Φ_rot   = (I - a aᵀ)(R_i - R_j) = 0

其中 d 为关节安装偏置。移动关节则相反：相对转动被约束，只保留沿 a 的平移。把这些约束写成通用形式 Φ(q, t) = 0，再对时间求导得到速度约束 Φ_q · q̇ = 0，即可嵌入多体动力学方程。约束的雅可比矩阵 Φ_q 直接决定后续求解的规模与稳定性。

五、从机构简图到拓扑模型

机构简图是工程师的“图纸语言”，拓扑模型是仿真软件的“数据结构”。转换过程可概括为：识别构件（刚体）→ 识别关节（约束）→ 确定连接关系（父子关系）→ 分配坐标系与参数。以典型六轴串联机器人为例，其拓扑是一条从基座到末端的链，每个关节连接一个父构件和一个子构件，形成树状结构。若存在闭链（如并联机构），则需引入约束方程而非简单父子关系。

下面这张图给出从机构简图到拓扑模型的转换关系，同时标出旋转矩阵、四元数、齐次变换与关节约束在其中的位置。

转换时最容易出错的是坐标系方向与关节轴定义。建议在建模初期就固定一套命名规则：每个构件一个局部坐标系，每个关节一个关节坐标系，关节轴用单位向量 a 表示，并记录在拓扑模型的边属性中。这样，后续无论是用拉格朗日法还是递推牛顿-欧拉法，都能直接从拓扑模型读取参数，避免重复推导。

把上述数学基础落实为可执行步骤，可以按以下顺序推进：先为每个刚体建立局部坐标系并写出初始位姿；再为每个关节定义类型、轴向量和零位偏置；然后按拓扑顺序连乘齐次变换，得到各构件在参考系中的位姿；最后把关节约束写成 Φ(q,t)=0 并组装雅可比矩阵。完成这四步后，机构简图就真正变成了可参与动力学求解的拓扑模型。