机械臂动力学建模（Pinocchio库为例）

建立一套完整的动力学模型，是为未来功能升级（如高速高精度轨迹跟踪、全臂碰撞检测、拖动示教等）打下基础的关键一步，运动学使用 Pinocchio 库，天然支持高效的刚体动力学算法，构建成本并不高，以下是一套以 Pinocchio 为例，落地的动力学模型构建方案：

一、动力学模型构建的基础

1. 模型数学描述

机械臂的动力学可以表示为：

2. 所需基础参数

对于每个连杆（包括末端工具整体），需要以下惯性参数（在各自关节坐标系或质心坐标系下定义）：

• 质量：m_i
• 质心位置： c_i=[x_c,y_c,z_c]T（相对于连杆坐标系的质心向量）
• 惯性张量：I_i（3×3 矩阵，在质心处定义，或相对于连杆坐标系原点）

此外，关节级还需要：

• 关节类型：旋转/平移
• 轴方向
• 关节摩擦参数：（库仑摩擦系数 fc、粘滞摩擦系数 fv）
• 转子惯量、减速比：若需要高精度，通常整合到 M(q)或摩擦模型中

二、构建要求

1. 运动学一致性：动力学模型必须与 Pinocchio 中已实现的正逆运动学共享完全相同的运动学参数（连杆长度、关节轴方向、DH/URDF 参数），避免出现两层皮。
2. 参数精度：惯性参数的相对误差应控制在 10% 以内，关键连杆（大臂、俯仰）最好达到 5%，否则力矩前馈或碰撞检测效果会打折扣，摩擦参数可通过简单实验辨识。
3. 实时性预算：若未来用于实时控制（如力矩前馈、模型预测控制），逆动力学（RNEA）的计算需在 1 个控制周期（如 1ms）内完成。Pinocchio 的 RNEA 效率极高，对于 6 轴臂仅需几微秒，完全满足实时要求。
4. 外力接口：动力学算法需要接受末端外力 F_ext 作为输入，六维力传感器经过补偿后，正好提供这个外力，这是得天独厚的条件。
5. 计算框架：推荐使用 Pinocchio 的 C++ 接口或 Python 绑定，直接调用 rnea（逆动力学）和 crba（质量矩阵）等函数。

三、详细落地步骤

步骤 1：准备 URDF 模型文件

这是模型构建的核心，URDF文件描述了机器人的运动学和动力学。

• 基于现有 DH 参数，编写 URDF 文件，定义每个关节和连杆。
• 关键：为每个连杆添加 <inertial> 标签，填入：
  • <origin>：质心相对于连杆坐标系的位置（xyz）和姿态（rpy）。
  • <mass>：质量（kg）。
  • <inertia>：惯性张量（Ixx,Ixy,Ixz,Iyy,Iyz,Izz），相对于质心坐标系。
• 对于机械臂构型：
  • SCARA 的升降轴是移动关节，其他为旋转关节。
  • 两个俯仰轴（J5、J6）的轴线方向一致，需在 URDF 中准确指定旋转轴。
  • 末端工具（清洁模组）应作为一个固定关节（或虚拟连杆）附加在最后一个俯仰关节之后，并填入之前辨识出的工具惯性参数（m,rcom,Isensor）。注意，工具的惯性张量是相对于传感器下安装面的，需转换到质心或连杆坐标系，URDF 期望相对质心的惯性张量。

步骤 2：获取精确的惯性参数

方法 A（首选，95% 场景足够）：从 CAD 模型自动提取，在 SolidWorks、Fusion360 等软件中，赋予每个零件真实材料密度，为每个连杆建立单独的坐标系（对齐 URDF 中的连杆坐标系），使用质量属性工具，读取该连杆（包含所有紧固件、电机定子等）的质量、质心位置、惯性张量，将数据直接填入 URDF。

方法 B（校准优化）：整体参数辨识，若 CAD 数据不全或实际装配偏差大，可以用最小惯性参数集辨识。通过设计一系列激励轨迹，采集关节力矩和运动数据，用最小二乘法估算连杆的惯性参数（通常是质量、一阶矩、惯性张量的组合）。此项较复杂，对于清洁场景，CAD 法已足够。

末端工具：通过负载辨识已得到 m,rcom，并可用 CAD 或质点近似得到 I，直接作为最后一根连杆的参数。

步骤 3：关节摩擦模型辨识（可选但推荐）

若未来要用动力学模型做力矩前馈或碰撞检测，摩擦模型能显著提高精度。

• 模型：库仑+粘滞摩擦 τf=fc⋅sign(q˙)+fvq˙
• 辨识实验：
  1. 让单个关节以多种恒定速度（正反方向）做匀速运动。
  2. 由于无加速度和重力变化，电机力矩（或驱动器反馈电流转换）近似用于克服摩擦。
  3. 记录不同速度下的平均力矩，线性回归得到 fc,fv。
• 将摩擦参数加入动力学公式，或直接在 Pinocchio 的外部实现。

步骤 4：在 Pinocchio 中构建动力学模型

利用已有的 Pinocchio 环境：

• 从 URDF 构建模型：pinocchio.buildModelFromUrdf(urdf_path)。
• 初始化数据对象：data = model.createData()。
• 此时，可以调用：
  • 逆动力学：pinocchio.rnea(model, data, q, dq, ddq)，内部会考虑重力、质量矩阵等，若需要包含外力，可使用 fext 参数，这正是六维力传感器数据。
  • 正动力学：pinocchio.aba(model, data, q, dq, tau)，输入关节力矩，输出加速度。
  • 质量矩阵：pinocchio.crba(model, data, q)。
• 集成外力：将补偿后的六维力 Fcontact作为末端外力传入，计算关节力矩：pinocchio.rnea(model, data, q, dq, ddq, fext)，假设在传感器坐标系的外力 F_ext，需转换到基座标系或工具连杆坐标系，fext 是一个力向量列表，对应每个连杆受到的外力，通常只给末端连杆设置

步骤 5：离线验证与参数校准

在实际部署到实时控制前，进行充分的离线验证：

1. 空载零力验证：不接触任何物体，让机械臂做一系列慢速运动，采集关节力矩（通过驱动器电流反馈或力矩传感器），与 RNEA 计算的无外力力矩对比（此时 Fext=0）。两者应接近，差值主要来自摩擦和参数误差。
2. 静态重力验证：在不同姿态下锁定机械臂，测量关节力矩，与 RNEA 计算的重力项 G(q)对比，可校准质量和质心。
3. 动态轨迹验证：执行高速正弦摆动，比较预测力矩和实测力矩。根据偏差微调惯性参数。
4. 工具负载验证：利用已知的工具负载（已通过力传感器补偿验证），进一步确认末端惯性参数的准确性。

步骤 6：实时部署准备（当需要时）

一旦决定将动力学模型嵌入实时控制器：

• 使用 Pinocchio 的 C++ API 并编译优化（-O3），确保计算延迟在微秒级。
• 在控制循环（1kHz）中：
  1. 读取关节位置、速度、加速度（加速度可通过速度差分加滤波得到，或由关节传感器获取）。
  2. 读取补偿后的六维力 Fcontact。
  3. 调用 rnea(q, dq, ddq, fext) 计算力矩前馈值 τff。
  4. 将 τff 作为前馈发送给关节伺服驱动器（如果支持），或用于碰撞检测残差计算。

四、注意事项

1. 冗余俯仰的动力学解耦：J5 和 J6 轴线平行且距离近，动力学上可能耦合较强。在参数获取时，要精确测量两轴之间的连杆质量和惯量分布，避免因小惯量连杆的误差导致力矩计算偏差。
2. 末端外力直接可用：六维力传感器在工具端，补偿后得到纯净接触力。在逆动力学中外力直接作用于工具连杆，使模型更贴合实际。
3. 移动底盘的处理：动力学建模仅考虑机械臂本体，底盘视为固定基座。当底盘运动时，若需全系统动力学，则必须加入底盘的运动加速度作为基座扰动，但这属于移动操纵动力学，更复杂，目前不必。

五、总结

即使现在不部署实时动力学，完成上述步骤 1~5 也是非常有价值的：

• 得到了一个高保真的仿真模型，可用于算法预研、轨迹优化、故障模拟。
• 未来只需将模型加载到实时控制器，并接通传感器数据，即可快速激活力矩前馈、碰撞检测等功能，无需从头摸索。
• Pinocchio 的高效性让你随时可以从“运动学盒子”升级为“动力学盒子”，成本极低。

一句话：利用已有的 CAD 和 Pinocchio，用 URDF 把动力学模型“建档”，就是最务实的落地方案。