机器人学中的刚体运动表示与应用

1. 刚体运动基础概念解析

刚体运动学是机器人学中最基础也最重要的数学工具之一。所谓刚体，指的是在运动过程中形状和大小始终保持不变的理想化物体。在实际机器人应用中，机械臂的连杆、末端执行器、移动机器人的底盘等部件都可以近似看作刚体。

1.1 构型空间(C-space)的表示方法

构型空间(Configuration Space)是描述刚体所有可能状态的数学空间。对于一个自由刚体，其构型空间是6维的——3个平移自由度加3个旋转自由度。在传统力学教材中，通常会使用欧拉角或RPY角等最小坐标集来表示旋转，但这种方法存在奇点问题。

现代机器人学更倾向于采用隐式表示法，即使用旋转矩阵或四元数等非最小坐标表示。这种表示虽然会引入冗余参数，但能避免奇点问题，更适合计算机实现。例如，旋转矩阵用3×3矩阵表示旋转，虽然只有3个自由度却用了9个参数。

注意：隐式表示虽然参数冗余，但在数值计算中更稳定。实际编程时要注意保持旋转矩阵的正交性，避免累积误差导致矩阵性质退化。

1.2 坐标系与右手定则

在描述刚体运动时，我们需要建立参考坐标系。坐标系由原点和三个互相垂直的坐标轴(x,y,z)组成。机器人学中统一采用右手坐标系，这是行业标准约定。

右手定则的实用技巧：

1. 伸直右手，食指指向x轴正方向
2. 中指与食指垂直指向y轴正方向
3. 拇指自然竖起指向z轴正方向

这个规则也适用于叉积运算：x轴单位向量(î) × y轴单位向量(ĵ) = z轴单位向量(k̂)。在实际应用中，电机旋转方向、力矩方向等都需要用右手定则判断。

2. 刚体运动的数学表示

2.1 旋转的表示方法

2.1.1 旋转矩阵

旋转矩阵R是3×3正交矩阵(RᵀR=I)，行列式为1。它可以将向量从一个坐标系变换到另一个坐标系。例如，将机体坐标系中的向量转换为世界坐标系表示：

v_world = R * v_body

旋转矩阵的列向量实际上是新坐标系基向量在原坐标系中的表示。例如，R的第一列就是新x轴在原坐标系中的方向余弦。

2.1.2 欧拉角与万向锁

欧拉角用三个绕特定轴的旋转角度表示方向，常见的有ZYX、ZYZ等约定。虽然直观，但存在万向锁问题：当中间旋转角为±90°时，会丢失一个自由度。

python复制# Python示例：欧拉角转旋转矩阵(scipy实现)
from scipy.spatial.transform import Rotation
r = Rotation.from_euler('zyx', [yaw, pitch, roll], degrees=True)
R = r.as_matrix()

2.1.3 四元数

四元数q = [w, x, y, z] = w + xi + yj + zk，其中w是实部，(x,y,z)是虚部。相比旋转矩阵，它只有4个参数且无奇点；相比欧拉角，它计算效率更高。

四元数乘法对应旋转组合：
q_final = q2 * q1 (表示先旋转q1再旋转q2)

实操建议：在实际编程中，建议内部使用四元数表示旋转，只在人机交互时转换为欧拉角。这样可以兼顾计算效率和用户友好性。

2.2 齐次变换矩阵

为了统一表示旋转和平移，我们使用4×4齐次变换矩阵：

T = [ R t ]
[ 0 1 ]

其中R是3×3旋转矩阵，t是3×1平移向量。齐次坐标下点的变换：

p' = T * p

齐次变换矩阵的逆矩阵有简便求法：

T⁻¹ = [ Rᵀ -Rᵀt ]
[ 0 1 ]

3. 刚体运动的速度表示

3.1 角速度与线速度

刚体速度不能用构型的时间导数简单表示。我们需要分别考虑：

1. 线速度v：坐标系原点平移速度
2. 角速度ω：坐标系旋转速度

两者组合成空间速度旋量V = [v; ω]。注意角速度ω不是任何角度的导数，而是瞬时旋转轴乘以旋转速率。

3.2 速度旋量的坐标系表示

速度旋量可以表示在任意坐标系中。设V_b是机体坐标系下的速度，V_s是空间坐标系下的速度，则有：

V_s = [ R 0 ] * V_b
[ 0 R ]

其中R是从机体到空间坐标系的旋转矩阵。

3.3 力旋量

类似地，力旋量F = [f; τ]组合线力f和力矩τ。功率计算遵循对偶关系：

P = VᵀF = v·f + ω·τ

4. 机器人学中的实际应用

4.1 机械臂运动学

在机械臂正向运动学中，我们通过连杆变换矩阵的连乘得到末端位姿：

T_0n = T_01 * T_12 * ... * T_(n-1)n

每个T_i(i+1)表示相邻连杆间的齐次变换。

4.2 移动机器人定位

移动机器人的位姿通常表示为ξ = [x, y, θ]ᵀ。虽然只有3个参数，但旋转部分需要用旋转矩阵或四元数精确表示，特别是在进行滤波或优化时。

python复制# 2D位姿转齐次矩阵
def pose_to_matrix(x, y, theta):
    return np.array([
        [np.cos(theta), -np.sin(theta), x],
        [np.sin(theta),  np.cos(theta), y],
        [0,             0,             1]
    ])

4.3 传感器数据融合

IMU提供角速度和线加速度测量，需要结合刚体运动学进行积分得到位姿变化。注意加速度计测量的是比力，包含重力分量：

a_measured = a_true - g

5. 常见问题与调试技巧

5.1 旋转矩阵不正交

问题表现：连续旋转后物体变形或缩放
解决方法：

1. 定期正交化：R = (R + Rᵀ)/2
2. 改用四元数表示旋转
3. 检查数值积分步长是否过大

5.2 万向锁导致控制异常

问题表现：特定姿态下姿态控制失效
解决方案：

1. 改用四元数进行内部计算
2. 使用两套欧拉角约定切换使用
3. 采用旋转矢量表示

5.3 速度积分漂移

问题表现：长时间积分后位置误差累积
应对措施：

1. 引入零速修正(ZUPT)
2. 融合其他传感器(如视觉、GPS)
3. 使用卡尔曼滤波估计误差

6. 进阶话题与扩展阅读

对于希望深入学习的读者，建议研究：

1. 螺旋理论(Screw Theory)与指数坐标
2. 李群与李代数在机器人学中的应用
3. 运动学与动力学的微分几何表示
4. 现代优化方法在运动规划中的应用

在实际机器人开发中，推荐使用成熟的数学库如Eigen( C++)或scipy.spatial(Python)来处理刚体运动计算，避免重复造轮子。同时要注意不同库和工具间的坐标系约定差异，这是许多bug的根源。