Pinocchio 核心算法之 CRBA

用户c995ffaf5c30 · 发表于 2026-1-25 11:11:27

一、CRBA算法概述

CRBA（Composite Rigid Body Algorithm）是计算机器人关节空间惯性矩阵（Joint Space Inertia Matrix, JSIM）的经典算法，由Featherstone于1983年提出，具有 O(n) 的时间复杂度，是机器人动力学中的核心算法之一。

核心功能

计算关节空间惯性矩阵 ( \mathbf{M}(\mathbf{q}) ) 的上三角部分
可选计算质心动量矩阵 ( \mathbf{A}_g(\mathbf{q}) )
支持不同的坐标系约定（LOCAL和WORLD）
考虑转子惯性效应（armature）

应用场景

机器人控制（如计算反馈控制器增益）
动力学仿真
轨迹优化
参数辨识

二、Pinocchio库中CRBA的实现结构文件结构

crba.hpp - 算法接口声明
crba.hxx - 算法详细实现
crba.txx - 模板显式实例化

主要类和函数

crba() - 主函数，根据坐标系约定调用不同实现
crbaLocalConvention() - 局部坐标系下的CRBA实现
crbaWorldConvention() - 世界坐标系下的CRBA实现
访问者模式实现不同关节类型的处理
- CrbaLocalConventionForwardStep - 局部坐标系前向遍历
- CrbaLocalConventionBackwardStep - 局部坐标系后向遍历
- CrbaWorldConventionForwardStep - 世界坐标系前向遍历
- CrbaWorldConventionBackwardStep - 世界坐标系后向遍历

三、CRBA算法原理1. 复合刚体模型

CRBA算法将机器人连杆系统视为一系列复合刚体，每个复合刚体包含该关节及其所有子关节。

2. 质量矩阵的数学表达

关节空间惯性矩阵可以表示为：
[
\mathbf{M}(\mathbf{q}) = \mathbf{J}^T(\mathbf{q}) \mathbf{I}_{\text{crb}}(\mathbf{q}) \mathbf{J}(\mathbf{q})
]
其中：

( \mathbf{J}(\mathbf{q}) ) 是关节雅可比矩阵
( \mathbf{I}_{\text{crb}}(\mathbf{q}) ) 是复合刚体的惯性张量

3. 算法步骤

CRBA算法分为两个主要阶段：

前向遍历（Forward Pass）

计算各关节的位姿变换
初始化各连杆的惯性张量

后向遍历（Backward Pass）

计算各复合刚体的惯性张量
计算关节雅可比矩阵
组装关节空间惯性矩阵

四、CRBA算法详细实现1. 局部坐标系约定（LOCAL）前向遍历

template<typename JointModel>static void algo( const JointModelBase<JointModel> & jmodel, JointDataBase<typename JointModel::JointDataDerived> & jdata, const Model & model, Data & data, const Eigen::MatrixBase<ConfigVectorType> & q){ const JointIndex & i = jmodel.id(); jmodel.calc(jdata.derived(), q.derived()); // 计算关节位姿变换 data.liMi = model.jointPlacements * jdata.M(); // 初始化连杆惯性张量 data.Ycrb = model.inertias;}

后向遍历
template<typename JointModel>static void algo_impl( const JointModelBase<JointModel> & jmodel, JointDataBase<typename JointModel::JointDataDerived> & jdata, const Model & model, Data & data){ // 计算F = Y * S（力矩阵） jmodel.jointCols(data.Fcrb) = data.Ycrb * jdata.S(); // 计算质量矩阵块：M[i, SUBTREE] = S' * F[1:6, SUBTREE] data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree).noalias() = jdata.S().transpose() * data.Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree); // 将当前连杆惯性张量传递给父连杆，形成复合刚体 const JointIndex & parent = model.parents; if (parent > 0) { data.Ycrb[parent] += data.liMi.act(data.Ycrb); // 更新力矩阵 Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree); Block iF = data.Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree); forceSet::se3Action(data.liMi, iF, jF); }}

2. 世界坐标系约定（WORLD）前向遍历
<pre style="border-color: rgb(49, 49, 49); border-style: solid; border-width: 1px; border-image: none 100% / 1 / 0 stretch; padding: 16px; border-radius: 3px; overflow: auto; text-wrap-mode: wrap;"><code data-line="117" class="code-line language-cpp" dir="auto" style="background-image: none; background-position: initial; background-size: initial; background-repeat: initial; background-attachment: initial; background-origin: initial; background-clip: initial; font-family: " jetbrains="" mono",="" consolas,="" "courier="" new",="" monospace;="" border-radius:="" 4px;="" font-size:="" 1em;="" line-height:="" 1.357em;="" display:="" inline-block;="" tab-size:="" 4;="" position:="" relative;"="">template<typename JointModel>

static void algo(

 const JointModelBase<JointModel> & jmodel,

 JointDataBase<typename JointModel::JointDataDerived> & jdata,

 const Model & model,

 Data & data,

 const Eigen::MatrixBase<ConfigVectorType> & q)

{

 const JointIndex & i = jmodel.id();

 jmodel.calc(jdata.derived(), q.derived());



 // 计算关节位姿变换

 data.liMi[i] = model.jointPlacements[i] * jdata.M();



 // 计算世界坐标系下的位姿

 const JointIndex & parent = model.parents[i];

 if (parent > 0)

data.oMi[i] = data.oMi[parent] * data.liMi[i];

 else

data.oMi[i] = data.liMi[i];



 // 计算关节雅可比矩阵

 jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S());



 // 计算世界坐标系下的惯性张量

 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);

}

</code></pre><h4 data-line="177" class="code-line" dir="auto" id="e5908ee59091e9818de58e86-1" style="margin-top: 24px; margin-bottom: 16px; line-height: 1.25; position: relative; font-family: -apple-system, BlinkMacSystemFont, " segoe="" wpc",="" "segoe="" ui",="" system-ui,="" ubuntu,="" "droid="" sans",="" sans-serif;"="">后向遍历</h4><pre style="border-color: rgb(49, 49, 49); border-style: solid; border-width: 1px; border-image: none 100% / 1 / 0 stretch; padding: 16px; border-radius: 3px; overflow: auto; text-wrap-mode: wrap;"><code data-line="148" class="code-line language-cpp" dir="auto" jetbrains="" mono",="" consolas,="" "courier="" new",="" monospace;="" background-image:="" none;="" background-position:="" initial;="" background-size:="" background-repeat:="" background-attachment:="" background-origin:="" background-clip:="" border-radius:="" 4px;="" font-size:="" 1em;="" line-height:="" 1.357em;="" display:="" inline-block;="" tab-size:="" 4;="" position:="" relative;"="">template<typename JointModel>

static void algo_impl(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)

{

 // 计算质心动量矩阵

 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);

 ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J);

 motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);



 // 计算质量矩阵

 data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =

J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]);



 // 更新复合刚体惯性张量

 const JointIndex & parent = model.parents[i];

 data.oYcrb[parent] += data.oYcrb[i];

}

</code></pre><h3 data-line="197" class="code-line" dir="auto" id="3-%E8%B4%A8%E9%87%8F%E7%9F%A9%E9%98%B5%E7%9A%84%E6%9C%80%E7%BB%88%E5%A4%84%E7%90%86" style="margin-top: 24px; margin-bottom: 16px; line-height: 1.25; font-size: 1.25em; position: relative; font-family: -apple-system, BlinkMacSystemFont, " segoe="" wpc",="" "segoe="" ui",="" system-ui,="" ubuntu,="" "droid="" sans",="" sans-serif;"="">3. 质量矩阵的最终处理</h3><p data-line="198" class="code-line" dir="auto" style="margin-bottom: 16px; position: relative; font-family: -apple-system, BlinkMacSystemFont, " segoe="" wpc",="" "segoe="" ui",="" system-ui,="" ubuntu,="" "droid="" sans",="" sans-serif;"="">无论使用哪种坐标系约定，最后都会添加转子惯性效应：<pre style="border-color: rgb(49, 49, 49); border-style: solid; border-width: 1px; border-image: none 100% / 1 / 0 stretch; padding: 16px; border-radius: 3px; overflow: auto; text-wrap-mode: wrap;"><code data-line="170" class="code-line language-cpp" dir="auto" jetbrains="" mono",="" consolas,="" "courier="" new",="" monospace;="" background-image:="" none;="" background-position:="" initial;="" background-size:="" background-repeat:="" background-attachment:="" background-origin:="" background-clip:="" border-radius:="" 4px;="" font-size:="" 1em;="" line-height:="" 1.357em;="" display:="" inline-block;="" tab-size:="" 4;="" position:="" relative;"="">// 添加转子惯性贡献

data.M.diagonal() += model.armature;

</code></pre><h2 data-line="205" class="code-line" dir="auto" id="%E4%BA%94%E8%B4%A8%E5%BF%83%E5%8A%A8%E9%87%8F%E7%9F%A9%E9%98%B5%E8%AE%A1%E7%AE%97" style="margin-top: 24px; margin-bottom: 16px; line-height: 1.25; padding-bottom: 0.3em; border-bottom: 1px solid rgba(255, 255, 255, 0.18); border-top-color: rgba(255, 255, 255, 0.18); border-right-color: rgba(255, 255, 255, 0.18); border-left-color: rgba(255, 255, 255, 0.18); position: relative; font-family: -apple-system, BlinkMacSystemFont, " segoe="" wpc",="" "segoe="" ui",="" system-ui,="" ubuntu,="" "droid="" sans",="" sans-serif;"="">五、质心动量矩阵计算</h2><p data-line="207" class="code-line" dir="auto" style="margin-bottom: 16px; position: relative; font-family: -apple-system, BlinkMacSystemFont, " segoe="" wpc",="" "segoe="" ui",="" system-ui,="" ubuntu,="" "droid="" sans",="" sans-serif;"="">在世界坐标系约定下，CRBA算法还会计算质心动量矩阵 ( \mathbf{A}_g(\mathbf{q}) )：<pre style="border-color: rgb(49, 49, 49); border-style: solid; border-width: 1px; border-image: none 100% / 1 / 0 stretch; padding: 16px; border-radius: 3px; overflow: auto; text-wrap-mode: wrap;"><code data-line="179" class="code-line language-cpp" dir="auto" style="background-image: none; background-position: initial; background-size: initial; background-repeat: initial; background-attachment: initial; background-origin: initial; background-clip: initial; font-family: " jetbrains="" mono",="" consolas,="" "courier="" new",="" monospace;="" border-radius:="" 4px;="" font-size:="" 1em;="" line-height:="" 1.357em;="" display:="" inline-block;="" tab-size:="" 4;="" position:="" relative;"="">// 计算总质量和质心位置

data.mass[0] = data.oYcrb[0].mass();

data.com[0] = data.oYcrb[0].lever();

// 更新质心动量矩阵的角速度部分

const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);

Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);

for (long i = 0; i < model.nv; ++i)

 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);</code></pre>
复制代码

质心动量矩阵 ( \mathbf{A}_g(\mathbf{q}) ) 满足：
[
\mathbf{h}_g(\mathbf{q}, \dot{\mathbf{q}}) = \mathbf{A}_g(\mathbf{q}) \dot{\mathbf{q}}
]
其中 ( \mathbf{h}_g ) 是机器人的质心动量。
六、算法特性与优化1. 时间复杂度
前向遍历：O(n)
后向遍历：O(n)
总复杂度：O(n)，其中n是关节数量

2. 内存优化
仅计算质量矩阵的上三角部分，通过对称性可恢复完整矩阵
使用块运算减少内存访问
利用机器人结构的树状特性，优化力矩阵的计算

3. 并行计算潜力
前向遍历可以并行化
后向遍历由于依赖关系，并行化难度较大

七、与其他动力学算法的关系1. 与RNEA的关系
RNEA（Recursive Newton-Euler Algorithm）：计算逆动力学，得到关节力矩
CRBA：计算关节空间惯性矩阵
两者都是基于牛顿-欧拉方程的递归算法
可以共享部分计算结果

2. 与ABA的关系
ABA（Articulated Body Algorithm）：计算正向动力学，得到关节加速度
CRBA：计算关节空间惯性矩阵
ABA的时间复杂度也是O(n)，但常数因子较大
CRBA更适合需要频繁计算质量矩阵的应用

3. 与质心动力学的关系
CRBA可以同时计算质心动量矩阵
质心动力学算法（如Centroidal Dynamics）可以基于CRBA的结果进行计算

八、应用场景1. 机器人控制
计算反馈控制器的增益矩阵
实现基于模型的控制器（如计算力矩控制）
设计自适应控制器

2. 动力学仿真
计算仿真所需的质量矩阵
实现高效的正向动力学仿真

3. 轨迹优化
计算优化问题的Hessian矩阵
实现基于动力学的轨迹优化

4. 参数辨识
辨识机器人的惯性参数
验证机器人模型的准确性

九、代码优化建议1. 内存访问优化
使用连续内存布局存储连杆数据
优化矩阵块的访问顺序，提高缓存命中率

2. 并行计算
对前向遍历进行并行化
利用GPU加速矩阵运算

3. 算法优化
针对特定机器人结构（如串联机器人）进行优化
实现自适应步长，根据机器人姿态调整计算精度

十、总结
CRBA算法是机器人动力学中的核心算法，具有高效、准确的特点。Pinocchio库的实现采用了模块化设计，支持不同的坐标系约定和关节类型，同时考虑了转子惯性效应。
主要优势
高效的O(n)时间复杂度
支持多种坐标系约定
可以同时计算质心动量矩阵
模块化设计，易于扩展
考虑了转子惯性效应

应用前景
随着机器人技术的发展，CRBA算法在机器人控制、仿真、优化等领域的应用将更加广泛。结合并行计算和GPU加速，CRBA算法可以处理更大规模的机器人系统，满足实时控制和仿真的需求。
CRBA算法的深入理解对于机器人动力学研究和应用具有重要意义，是机器人学领域的基础算法之一。

Pinocchio 核心算法之 CRBA

回复