第 68B 章 机器人动力学与感知¶
前置：机器人运动学 (Ch68A) · 矩阵分析 (Ch14) · 正定矩阵 (Ch16) · 概率统计
本章脉络：从几何到力学 \(\to\) 拉格朗日动力学 (Lagrangian Dynamics) 的矩阵形式 \(\to\) 核心算子：质量矩阵 (Mass Matrix) \(M(q)\) \(\to\) 离心力、科里奥利力与重力项的线性表示 \(\to\) 正向动力学（求加速度）与逆向动力学（求力矩） \(\to\) 机器人感知：传感器标定与坐标变换 \(\to\) 数据融合：基于线性代数的卡尔曼滤波 (Kalman Filter) 初步 \(\to\) 状态估计中的协方差演化 \(\to\) 应用：动态补偿控制、碰撞检测、移动机器人定位 (SLAM)
延伸：机器人动力学是线性代数的“能量表达”；它证明了复杂的机械力学行为可以被封装为一个随构型演化的正定矩阵算子，而感知则是通过矩阵的不确定性传播来寻找真相的过程，是构建闭环智能系统的物理核心
如果说运动学研究的是“在哪里”，那么动力学研究的就是“如何动起来”。机器人动力学（Robot Dynamics）描述了关节力矩与产生的加速度之间的因果关系。通过定义质量矩阵（Mass Matrix），我们将机械能的分布转化为矩阵的代数性质。与此同时，机器人感知通过矩阵变换和统计滤波（如卡尔曼滤波），将充满噪声的传感器数据转化为对真实状态的精确估计。本章将介绍这一作为机器人“大脑”与“肌肉”连接纽带的代数体系。
68B.1 动力学方程的矩阵形式¶
定义 68B.1 (标准动力学方程)
$\(M(q) \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau\)$ - \(M(q)\)：惯性矩阵（对称正定）。 - \(C(q, \dot{q})\)：科里奥利力与离心力矩阵。 - \(g(q)\)：重力向量。 - \(\tau\)：关节力矩输入。
68B.2 感知与卡尔曼滤波初步¶
技术：线性状态估计
在感知中，系统状态的更新遵循线性方程 \(x_k = Ax_{k-1} + Bu_k + w\)。 卡尔曼滤波利用协方差矩阵 \(P\) 的演化来权衡“预测”与“观测”的可靠性。其更新步涉及求逆运算，本质上是解一个最小二乘问题。
68B.3 坐标变换与标定¶
定义 68B.2 (传感器外参)
从传感器（如激光雷达）到机器人本体的变换矩阵 \(T_{sensor}^{body}\) 称为外参。通过手眼标定（解矩阵方程 \(AX=XB\)），可以确定这一关键几何结构。
练习题¶
1. [基础] 证明机器人的惯性矩阵 \(M(q)\) 始终是正定的。
参考答案
物理证明： 1. 系统的动能定义为 \(K = \frac{1}{2} \dot{q}^T M(q) \dot{q}\)。 2. 根据物理规律，只要机器人有运动（\(\dot{q} \neq 0\)），其动能必然大于 0。 3. 二次型 \(v^T M v > 0\) 正是正定矩阵（见 Ch16）的定义。 意义：这保证了动力学方程的数值稳定性。
2. [计算] 设 \(M = \operatorname{diag}(2, 1)\)，若希望产生加速度 \(\ddot{q} = (1, 5)^T\)，忽略其他项，需要多少关节力矩？
参考答案
计算步骤： \(\tau = M \ddot{q} = \begin{pmatrix} 2 & 0 \\ 0 & 1 \end{pmatrix} \begin{pmatrix} 1 \\ 5 \end{pmatrix} = \begin{pmatrix} 2 \\ 5 \end{pmatrix}\)。 结论：关节 1 需要 2 单位力矩，关节 2 需要 5 单位力矩。
3. [性质] 什么是动力学中的“反对称性质”？
参考答案
结论：\(\dot{M} - 2C\) 是一个反对称矩阵。 意义：这一性质反映了物理系统中的能量守恒。它在设计稳健的非线性控制器（如自适应控制）时起到了核心作用，允许我们通过代数抵消项来证明闭环系统的稳定性。
4. [感知] 传感器数据带有方差 \(\sigma^2\)，在矩阵中这对应什么位置？
参考答案
结论：对角线。 观测噪声协方差矩阵 \(R\) 的对角元 \(R_{ii}\) 即为第 \(i\) 个传感器的测量方差。非对角元代表不同传感器间的噪声相关性（通常假设为 0）。
5. [卡尔曼滤波] 在卡尔曼增益 \(K = PH^T(HPH^T + R)^{-1}\) 中，求逆操作的物理含义是什么？
参考答案
代数解释： 求逆部分对应于信息量的反比（即精度）。 当观测噪声 \(R\) 非常大时，逆矩阵变得很小，导致增益 \(K\) 变小。这意味着算法更倾向于相信预测，而忽略不靠谱的观测数据。
6. [计算] 两个传感器测量同一个量，方差分别为 \(P_1=1, P_2=4\)。最优融合后的方差是多少？
参考答案
公式： \(P_{combined} = (P_1^{-1} + P_2^{-1})^{-1} = (1 + 0.25)^{-1} = 1/1.25 = 0.8\)。 这对应于算子调和平均（见 Ch46B）。由于融合了更多信息，最终的不确定性减小了。
7. [标定] 解释 \(AX=XB\) 方程在机器人标定中的意义。
参考答案
这是手眼标定的标准形式。 - \(A\)：末端执行器的两次运动之间的相对变换（已知）。 - \(B\)：相机观测到的目标物两次相对变换（已知）。 - \(X\)：待求的相机相对于末端的安装位姿。 解此方程本质上是在寻找使两个运动链闭合的唯一相似变换矩阵。
8. [动力学] 什么是“逆动力学”控制？
参考答案
解释： 利用计算机高速计算出当前时刻所需的力矩 \(\tau_{calc} = M\ddot{q}_d + C\dot{q} + g\)，并直接作为电机指令。线性代数证明了通过这种代数补偿，可以将复杂的非线性机械臂“线性化”为一系列简单的二阶解耦系统。
9. [移动机器人] 在 SLAM 中，地图点的坐标为什么要存储在状态向量中？
参考答案
因为地图点的位置是不确定的。通过将路标坐标与机器人位姿合并为一个巨大的状态向量 \(\mathbf{X}\)，卡尔曼滤波可以利用协方差矩阵的非对角元来记录“机器人位姿”与“路标位置”之间的相关性。当机器人看到已知路标时，这种相关性能帮助纠正整个历史轨迹的累积误差。
10. [应用] 简述“质量矩阵”在碰撞检测中的作用。
参考答案
通过实时测量电流推算实际力矩 \(\tau_{act}\)，并与理论计算的 \(\tau_{calc}\) 比较。 若差值 \(\Delta \tau\) 对应的能量项 \(\Delta \tau^T M^{-1} \Delta \tau\) 超过阈值，则判定发生了非预期的碰撞。这里 \(M^{-1}\) 起到了将力矩空间转换为加速度（动能）空间的转换器作用。
本章小结¶
机器人动力学与感知是线性代数在物理世界中的“闭环逻辑”：
- 力学的代数化：惯性矩阵 \(M(q)\) 将复杂的质量分布转化为正定算子，确立了控制力矩与运动响应之间的严密因果链。
- 感知的概率测度：卡尔曼滤波通过协方差矩阵的线性演化，证明了“真相”可以通过对观测残差的最优加权投影而得到，为处理现实世界的不确定性提供了数学准则。
- 系统的综合：从 \(AX=XB\) 的几何校准到动力学补偿控制，线性代数实现了从机械结构到感知信息的全方位数学封装，是现代工业机器人与自动驾驶汽车的底层操作系统。