机械臂动力学模型之 Pinocchio库
### 一、核心算法：牛顿-欧拉递推 Pinocchio 的动力学核心采用的是**递归牛顿-欧拉算法（RNEA）**，而不是拉格朗日方程。 - **为什么选牛顿-欧拉？** - **计算效率**：递推算法的复杂度为 O(n)，与自由度成正比，而拉格朗日法直接计算质量矩阵的复杂度是 O(n³)。对于 6 轴机械臂，RNEA 每步只需几微秒，适合 1kHz 实时控制。 - **数值稳定性**：递推过程避免了拉格朗日法中繁琐的符号求导，由一系列矩阵乘法和向量叉积组成，数值非常稳定。 - **Pinocchio 的实现**：利用刚体运动的**空间向量代数**，将线运动和角运动统一为 6 维向量，使得递推公式极其简洁高效。 **递推过程分两步**： 1. **正向递推**：从基座到末端，计算每个连杆的角速度、角加速度、线加速度。 1. **反向递推**：从末端到基座，根据牛顿-欧拉方程计算每个连杆所需的力矩，并传递到父连杆。  ### 二、重力补偿 #### 1. 重力补偿**是核心功能** - **原理**：在正向递推时，基座的线加速度被设置为重力加速度 −g，相当于给系统施加了一个虚拟的、向上的加速度场，与达朗贝尔原理的惯性力等效，模拟了真实的重力效果。 - **效果**：调用 `rnea(q, v, a)` 计算的力矩中，已经包含了抵抗所有连杆重力所需的关节力矩。 #### 2. 不包括摩擦力补偿 - **原因**：摩擦是非线性、时变的物理现象，不是刚体动力学的一部分。不同的减速器、轴承，其摩擦特性完全不同。 - 如果想用 Pinocchio 做力矩前馈，必须在外部叠加摩擦项 τf，常用模型是：  其中 fc 是库仑摩擦系数，fv 是粘滞摩擦系数。  ### 三、 Pinocchio 动力学参数 要调用 `rnea` 或 `aba`，需要为模型中的**每一个连杆**提供完整的惯性参数，在 Pinocchio 中可以通过 URDF 文件加载。 |参数|物理含义|获取方法| | --- | --- | --- | |质量|连杆总质量，包括电机转子（若视为一体）、紧固件等|CAD 模型设定真实材料密度后读取，或用高精度电子秤称重| |质心位置|质心在该连杆坐标系下的三维坐标 [xc,yc,zc]T|CAD 模型质量属性中读取| |惯性张量|相对于质心的 3×3对称惯性矩阵，对角元是绕各轴的转动惯量，非对角元是惯性积|CAD 模型质量属性中读取，注意选择相对于质心坐标系的输出| |关节轴与类型|旋转/平移、轴线方向、减速比|根据机械设计图纸准确填入 URDF。减速比在计算电机力矩时使用，不影响关节空间动力学| **针对清洁工具**： - 末端清洁模组（包括六维力传感器之后的所有部件）作为一个独立的虚拟连杆，固定在 J6 之后。 - 其 m,rcom,I可以通过负载辨识获取，可直接填入 URDF。 --- ### 四、动力学模型校验  这是最关键的一步，校验的核心思想是**比较预测力矩和实测力矩，需要能够获取机械臂各个关节的实时力矩信号**。从伺服驱动器读取电流，乘以力矩常数和减速比，即可得到实测关节力矩。 **步骤 1：静态重力校验** - **做法**：让机械臂在不接触物体的情况下，摆出 6~8 个姿态，静止后记录各关节实测力矩，同时，用 Pinocchio 的 `rnea` 计算预测力矩（设速度和加速度为零）。 - **预期**：二者之差即关节摩擦的静摩擦分量，在各姿态下应保持合理范围内（如电机额定力矩的 5%~10%），如果误差巨大，则说明连杆质量或质心有误。 - **应用**：这是校准质量 m 和质心 c 的最直接方法。 **步骤 2：低速匀速运动校验** - **做法**：让单个关节（如 J5、J6）以多种恒定速度（如 10°/s, 30°/s, 60°/s）做正反向运动，记录整个行程的力矩，比较实测力矩与 `rnea` 预测力矩的差值。 - **预期**：这个差值主要由摩擦力和模型参数误差造成，通过分析差值拟合出该关节的摩擦参数 fc,fv，在扣除摩擦后，观察残余力矩是否接近零，验证惯性参数准确性。 **步骤 3：动态轨迹校验** - **做法**：设计一段激励轨迹（如正弦扫频波），覆盖清洁任务中常见的关节运动范围和速度，运行轨迹，采集全程关节力矩。 - **分析**：将实测力矩、Pinocchio 预测力矩、以及（预测力矩+辨识的摩擦力矩）三者画在同一张图上进行对比。 - 通过标准：在高速动态部分，摩擦力补偿后的残差应小于电机额定力矩的 15%~20%。残差中的低频成分代表模型参数误差（惯性张量不准），高频尖峰则可能是传动误差或弹性形变。 #### **步骤 4：**利用六维力传感器校验 这是得天独厚的优势，在动态校验时，让清洁模组接触一个已知的柔性目标，并记录六维力数据。 - **正向校验**：将补偿后的接触力 F_ext作为外力，输入 Pinocchio 的 `rnea，`计算的关节力矩，应等于机械臂空载力矩 + 抵抗外力所需的力矩。与实测力矩对比，可以验证整个力传导链的准确性。 - **反向校验（残差观测）**：将实测力矩减去空载动力学力矩，得到外力矩残差，再通过雅可比矩阵转置，将这个残差映射到末端力，并与六维力传感器的读数对比。这是协作机器人**无传感器力估计**的标准方法，可以用来检验你的传感器和动力学模型的一致性。 --- **总结**：Pinocchio 为提供了高效、精准的刚体动力学核心，包含了重力效应，只需要提供准确的惯性参数（从 CAD 获取或辨识）并自行建模摩擦力。通过“静-动-力”三步校验法，利用伺服力矩反馈已有的六维力传感器，可以构建一个高度吻合真实硬件的数字孪生模型，为未来任何高阶应用做好准备。
