机器人动力学

用李群李代数语言描述刚体的运动与力，是机器人动力学表示的基础。核心是利用 $SO(3)$ 描述姿态、$SE(3)$ 描述位姿。

姿态表示

• 旋转矩阵：$R \in SO(3) = \{R \in \mathbb{R}^{3\times 3}: R^T R = I, \det R = 1\}$，见特殊正交群
• 角速度：$\dot{R} = R [\omega]_\times$，其中 $[\omega]_\times \in \mathfrak{so}(3)$ 是反对称矩阵（李代数），$\omega$ 为角速度向量
• 姿态误差可用李括号/对易子表示：$R_1^T R_2$ 度量相邻姿态的相对变化

位姿表示

• 齐次变换矩阵：$T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix} \in SE(3)$，见特殊欧式群
• 瞬时速度（旋量）：$\dot{T} T^{-1} \in \mathfrak{se}(3)$，包含角速度与线速度

运动学与动力学方程

• 前向运动学：$T = T_1 T_2 \cdots T_n$（链式乘法）
• 速度（Jacobian）：$v = J(q) \dot{q}$，$J$ 可由旋量坐标构造
• 动力学（Euler-Lagrange）：$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau$

李代数优势

• 指数映射 $\exp: \mathfrak{se}(3) \to SE(3)$ 将"单位时间速度"积分为"有限位移"
• 无奇异点的姿态插值/平滑
• 迭代求解逆运动学的几何方法（如 Newton-Raphson 在群上）