机械臂动力学模型之 Pinocchio库

一、核心算法：牛顿-欧拉递推

Pinocchio 的动力学核心采用的是递归牛顿-欧拉算法（RNEA），而不是拉格朗日方程。

• 为什么选牛顿-欧拉？
  • 计算效率：递推算法的复杂度为 O(n)，与自由度成正比，而拉格朗日法直接计算质量矩阵的复杂度是 O(n³)。对于 6 轴机械臂，RNEA 每步只需几微秒，适合 1kHz 实时控制。
  • 数值稳定性：递推过程避免了拉格朗日法中繁琐的符号求导，由一系列矩阵乘法和向量叉积组成，数值非常稳定。
  • Pinocchio 的实现：利用刚体运动的空间向量代数，将线运动和角运动统一为 6 维向量，使得递推公式极其简洁高效。

递推过程分两步：

1. 正向递推：从基座到末端，计算每个连杆的角速度、角加速度、线加速度。
2. 反向递推：从末端到基座，根据牛顿-欧拉方程计算每个连杆所需的力矩，并传递到父连杆。

二、重力补偿

1. 重力补偿是核心功能

• 原理：在正向递推时，基座的线加速度被设置为重力加速度 −g，相当于给系统施加了一个虚拟的、向上的加速度场，与达朗贝尔原理的惯性力等效，模拟了真实的重力效果。
• 效果：调用 rnea(q, v, a) 计算的力矩中，已经包含了抵抗所有连杆重力所需的关节力矩。

2. 不包括摩擦力补偿

• 原因：摩擦是非线性、时变的物理现象，不是刚体动力学的一部分。不同的减速器、轴承，其摩擦特性完全不同。
• 如果想用 Pinocchio 做力矩前馈，必须在外部叠加摩擦项 τf，常用模型是：

其中 fc 是库仑摩擦系数，fv 是粘滞摩擦系数。

三、 Pinocchio 动力学参数

要调用 rnea 或 aba，需要为模型中的每一个连杆提供完整的惯性参数，在 Pinocchio 中可以通过 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 获取或辨识）并自行建模摩擦力。通过“静-动-力”三步校验法，利用伺服力矩反馈已有的六维力传感器，可以构建一个高度吻合真实硬件的数字孪生模型，为未来任何高阶应用做好准备。