Pinocchion 核心算法RNEA 实现详解

rnea.hxx 文件实现了 Recursive Newton-Euler Algorithm (RNEA) 算法家族，这是机器人动力学中的经典算法，主要用于计算：

• 关节力矩：给定机器人的位置、速度和加速度，计算所需的关节驱动力矩
• 非线性效应：计算科氏力和离心力
• 广义重力：计算重力对关节的影响
• 静力矩：计算静态平衡时的关节力矩
• 科氏矩阵：计算机器人动力学方程中的科氏矩阵
  

2. 算法原理与数学公式2.1 基本RNEA算法

RNEA算法采用两步法：

• 前向传播：从基坐标系到末端坐标系计算各关节的速度和加速度
• 后向传播：从末端坐标系到基坐标系计算各关节的力和力矩
  

前向传播公式

vi=vjoint,i+iXparent(i)vparent(i)ai=ajoint,i+iXparent(i)aparent(i)+vi×vjoint,ivi​ai​​=vjoint,i​+iXparent(i)​vparent(i)​=ajoint,i​+iXparent(i)​aparent(i)​+vi​×vjoint,i​​

• v_i：关节i的速度（局部坐标系）
• a_i：关节i的加速度（局部坐标系）
• {}^{i}X_{parent(i)}：从父关节到当前关节的坐标变换
  

后向传播公式

fi=Iiai+vi×(Iivi)−fext,iτi=SiTfi+τparent(i)fi​τi​​=Ii​ai​+vi​×(Ii​vi​)−fext,i​=SiT​fi​+τparent(i)​​

• f_i：关节i处的力（局部坐标系）
• I_i：关节i的惯性张量
• S_i：关节i的运动旋量矩阵
• \tau_i：关节i的力矩
  

2.2 非线性效应计算

非线性效应（科氏力和离心力）的计算公式：

nlei=τi−M(q)ai−G(q)nlei​=τi​−M(q)ai​−G(q)

其中 M(q) 是惯性矩阵，G(q) 是重力向量。

2.3 广义重力计算

广义重力向量 G(q) 的计算公式：

G(q)=τi∣v=0,a=0G(q)=τi​∣v=0,a=0​

即在速度和加速度为零时的关节力矩。

2.4 科氏矩阵计算

科氏矩阵 C(q, v) 满足：

C(q,v)v=nlei−G(q)C(q,v)v=nlei​−G(q)

其元素可通过以下公式计算：

Cij=12(∂Mij∂qkvk+∂Mik∂qjvk−∂Mjk∂qivk)Cij​=21​(∂qk​∂Mij​​vk​+∂qj​∂Mik​​vk​−∂qi​∂Mjk​​vk​)

3. 代码结构与实现细节3.1 核心类结构[td]

类名	功能	关键方法
RneaForwardStep	RNEA前向传播	计算速度、加速度和力
RneaBackwardStep	RNEA后向传播	计算关节力矩
NLEForwardStep	非线性效应前向传播	计算速度和力
NLEBackwardStep	非线性效应后向传播	计算非线性效应力矩
ComputeGeneralizedGravityForwardStep	广义重力前向传播	计算加速度和力
ComputeGeneralizedGravityBackwardStep	广义重力后向传播	计算重力力矩
CoriolisMatrixForwardStep	科氏矩阵前向传播	计算速度、惯性和雅可比
CoriolisMatrixBackwardStep	科氏矩阵后向传播	计算科氏矩阵

3.2 核心函数实现3.2.1 RNEA主函数
、

1. // ============================================================================
   
2. // 3.2.1 主算法：递归牛顿-欧拉算法 (RNEA)
   
3. // ============================================================================
   
4. template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
   
5.          typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
   
6. const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
   
7. rnea(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
   
8.      DataTpl<Scalar,Options,JointCollectionTpl> & data,
   
9.      const Eigen::MatrixBase<ConfigVectorType> & q,
   
10.      const Eigen::MatrixBase<TangentVectorType1> & v,
    
11.      const Eigen::MatrixBase<TangentVectorType2> & a)
    
12. {
    
13.   // 初始化根节点
    
14.   data.tau.setZero();
    
15.   data.v[0].setZero();
    
16.   data.a_gf[0] = -model.gravity;  // 根节点加速度为重力加速度的负值
    
17.   
    
18.   // -------------------------------------------------------------------------
    
19.   // 前向传播：计算各关节的空间速度和空间加速度
    
20.   // -------------------------------------------------------------------------
    
21.   typedef RneaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Pass1;
    
22.   
    
23.   for(JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
    
24.   {
    
25.     Pass1::run(model.joints[i], data.joints[i],
    
26.                typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
    
27.   }
    
28.   
    
29.   // -------------------------------------------------------------------------
    
30.   // 后向传播：计算各关节的力和力矩
    
31.   // -------------------------------------------------------------------------
    
32.   typedef RneaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
    
33.   
    
34.   for(JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
    
35.   {
    
36.     Pass2::run(model.joints[i], data.joints[i],
    
37.                typename Pass2::ArgsType(model, data));
    
38.   }
    
39.   
    
40.   // 添加转子惯性（电枢）贡献：τ += I_rotor * q̈
    
41.   data.tau.array() += model.armature.array() * a.array();
    
42.   
    
43.   return data.tau;
    
44. }
    
45. 
    
46. // ============================================================================
    
47. // 3.2.2 前向传播步骤
    
48. // ============================================================================
    
49. template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
    
50.          typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
    
51. template<typename JointModel>
    
52. void RneaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2>::
    
53. algo(const JointModelBase<JointModel> & jmodel,
    
54.      JointDataBase<typename JointModel::JointDataDerived> & jdata,
    
55.      const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
    
56.      DataTpl<Scalar,Options,JointCollectionTpl> & data,
    
57.      const Eigen::MatrixBase<ConfigVectorType> & q,
    
58.      const Eigen::MatrixBase<TangentVectorType1> & v,
    
59.      const Eigen::MatrixBase<TangentVectorType2> & a)
    
60. {
    
61.   const JointIndex i = jmodel.id();
    
62.   const JointIndex parent = model.parents[i];
    
63.   
    
64.   // 1. 计算关节运动学（变换矩阵、局部速度等）
    
65.   jmodel.calc(jdata.derived(), q.derived(), v.derived());
    
66.   
    
67.   // 2. 计算相对于父节点的位姿变换
    
68.   data.liMi[i] = model.jointPlacements[i] * jdata.M();
    
69.   
    
70.   // 3. 计算空间速度（局部坐标系）
    
71.   //    v_i = v_parent + v_joint
    
72.   data.v[i] = jdata.v();
    
73.   if(parent > 0)
    
74.   {
    
75.     data.v[i] += data.liMi[i].actInv(data.v[parent]);
    
76.   }
    
77.   
    
78.   // 4. 计算空间加速度（局部坐标系）
    
79.   //    a_i = a_parent + c + v × v_joint + S*q̈
    
80.   data.a_gf[i] = jdata.c() + data.v[i].cross(jdata.v());  // 科里奥利加速度
    
81.   data.a_gf[i] += jdata.S() * jmodel.JointMappedVelocitySelector(a.derived());  // 关节加速度投影
    
82.   
    
83.   if(parent > 0)
    
84.   {
    
85.     data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
    
86.   }
    
87.   
    
88.   // 5. 计算空间力
    
89.   //    h = I * v  (空间动量)
    
90.   //    f = I * a + v × h  (空间力)
    
91.   const InertiaTpl<Scalar,Options> & I = model.inertias[i];
    
92.   I.__mult__(data.v[i], data.h[i]);
    
93.   I.__mult__(data.a_gf[i], data.f[i]);
    
94.   data.f[i] += data.v[i].cross(data.h[i]);
    
95. }
    
96. 
    
97. // ============================================================================
    
98. // 3.2.3 后向传播步骤
    
99. // ============================================================================
    
100. template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
     
101. template<typename JointModel>
     
102. void RneaBackwardStep<Scalar,Options,JointCollectionTpl>::
     
103. algo(const JointModelBase<JointModel> & jmodel,
     
104.      JointDataBase<typename JointModel::JointDataDerived> & jdata,
     
105.      const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
     
106.      DataTpl<Scalar,Options,JointCollectionTpl> & data)
     
107. {
     
108.   const JointIndex i = jmodel.id();
     
109.   const JointIndex parent = model.parents[i];
     
110.   
     
111.   // 1. 投影空间力到关节轴上得到关节力矩：τ = S^T * f
     
112.   jmodel.JointMappedVelocitySelector(data.tau) += jdata.S().transpose() * data.f[i];
     
113.   
     
114.   // 2. 将力传递到父关节：f_parent += X * f_i
     
115.   if(parent > 0)
     
116.   {
     
117.     data.f[parent] += data.liMi[i].act(data.f[i]);
     
118.   }
     
119. }

复制代码

4. 关键技术点解析4.1 坐标变换处理

RNEA算法在不同坐标系中进行计算，关键的坐标变换包括：

• liMi：关节i的局部位姿变换矩阵
• actInv：将父关节的速度/加速度转换到当前关节的局部坐标系
• act：将当前关节的力转换到父关节的坐标系
  
  

4.2 运动旋量计算

运动旋量是RNEA算法的核心概念，用于表示关节的运动：

• jdata.v()：关节的局部速度旋量
• jdata.c()：关节的科氏项旋量
• jdata.S()：关节的运动旋量矩阵
  

4.3 惯性张量处理

惯性张量在不同坐标系中的转换：

• model.inertias：关节i的局部惯性张量
• data.oYcrb：关节i在世界坐标系中的惯性张量
  

4.4 雅可比矩阵计算

雅可比矩阵用于科氏矩阵的计算：

• data.J：关节的雅可比矩阵
• data.dJ：雅可比矩阵的时间导数
  

5. 代码优化技术

• 模板元编程：使用模板实现泛型编程，支持不同的关节类型和数值类型
• 关节访问者模式：使用 JointUnaryVisitorBase 统一处理不同类型的关节
• 表达式模板：利用Eigen库的表达式模板技术，减少临时变量
• 内存复用：在 Data 结构中预分配内存，避免运行时内存分配
• 向量化计算：使用Eigen库的向量化功能，提高计算效率
  

6. 应用场景

RNEA算法在以下场景中具有广泛应用：

• 机器人控制：计算实时控制所需的关节力矩
• 轨迹优化：在轨迹规划中考虑动力学约束
• 仿真模拟：进行机器人动力学仿真
• 参数辨识：通过实验数据辨识机器人的惯性参数
• 力控制：实现基于力反馈的机器人控制
  

7. 输入输出示例输入：

1. #include <pinocchio/multibody/model.hpp>
   
2. #include <pinocchio/multibody/data.hpp>
   
3. #include <pinocchio/algorithm/rnea.hpp>
   
4. #include <pinocchio/parsers/urdf.hpp>
   
5. 
   
6. using namespace pinocchio;
   
7. using namespace Eigen;
   
8. 
   
9. int main()
   
10. {
    
11.     // =========================================================================
    
12.     // 1. 模型初始化
    
13.     // =========================================================================
    
14.    
    
15.     // 方法A：从URDF文件加载机器人模型
    
16.     Model model;
    
17.     const std::string urdf_filename = "path/to/robot.urdf";
    
18.     pinocchio::urdf::buildModel(urdf_filename, model);
    
19.    
    
20.     // 方法B：手动构建模型（示例）
    
21.     // Model model;
    
22.     // ... 使用 model.addJoint() 等API构建 ...
    
23.    
    
24.     // 创建数据缓存（存储中间计算结果）
    
25.     Data data(model);
    
26.    
    
27.     // =========================================================================
    
28.     // 2. 输入状态定义
    
29.     // =========================================================================
    
30.    
    
31.     // 关节位置 [rad] (nq x 1)
    
32.     VectorXd q(model.nq);
    
33.     q << M_PI/4, -M_PI/6, 0.0;  // 示例：3关节机器人
    
34.    
    
35.     // 关节速度 [rad/s] (nv x 1)
    
36.     VectorXd v(model.nv);
    
37.     v << 0.5, -0.3, 0.1;
    
38.    
    
39.     // 关节加速度 [rad/s²] (nv x 1)
    
40.     VectorXd a(model.nv);
    
41.     a << 0.1, 0.0, -0.2;
    
42.    
    
43.     // =========================================================================
    
44.     // 3. 执行RNEA计算
    
45.     // =========================================================================
    
46.    
    
47.     // 计算逆动力学：给定(q, v, a)，求所需关节力矩 τ
    
48.     // 物理意义：τ = M(q)*a + C(q, v)*v + G(q)
    
49.     VectorXd tau = rnea(model, data, q, v, a);
    
50.    
    
51.     // =========================================================================
    
52.     // 4. 结果解析
    
53.     // =========================================================================
    
54.    
    
55.     // tau 维度: (nv x 1)，每个元素对应一个自由度
    
56.     // 对于固定基座机器人：
    
57.     // - tau[0]: 关节1所需的力矩/力 [Nm 或 N]
    
58.     // - tau[1]: 关节2所需的力矩/力 [Nm 或 N]
    
59.     // - ...
    
60.    
    
61.     std::cout << "关节力矩 [Nm]:" << std::endl;
    
62.     for (int i = 0; i < model.nv; ++i)
    
63.     {
    
64.         std::cout << "  Joint " << i+1 << ": " << tau[i] << std::endl;
    
65.     }
    
66.    
    
67.     // 输出示例（2关节机器人）：
    
68.     // tau = [ 5.234, -2.156 ]
    
69.     // 解释：
    
70.     // - tau[0] = 5.234 Nm  → 关节1需要提供正方向5.234牛米的力矩
    
71.     // - tau[1] = -2.156 Nm → 关节2需要提供负方向2.156牛米的力矩
    
72.    
    
73.     return 0;
    
74. }

复制代码

8. 总结

rnea.hxx 文件实现了一套完整的RNEA算法家族，包括：

• 核心RNEA算法：计算关节力矩
• 非线性效应：计算科氏力和离心力
• 广义重力：计算重力影响
• 静力矩：计算静态平衡力矩
• 科氏矩阵：计算科氏矩阵
  

这些实现采用了现代C++技术，如模板元编程、关节访问者模式和表达式模板，确保了计算效率和代码可维护性。RNEA算法是机器人动力学分析的基础，广泛应用于机器人控制、仿真和优化等领域。

该实现的核心优势在于：

• 计算效率：O(n)时间复杂度，适用于实时应用
• 通用性：支持多种关节类型和机器人结构
• 可扩展性：易于集成到更大的机器人系统中
• 精度：提供准确的动力学计算结果
  

通过理解和应用这些算法，可以更好地分析和控制机器人系统，实现更复杂的机器人任务。

3.2 部分，加上代码块就会好很多

为什么公式不显示？

ustc-tiger 发表于 2026-2-5 13:55
为什么公式不显示？

下次我用图片

ustc-tiger 发表于 2026-2-5 13:55
为什么公式不显示？

已添加图片