Pinocchion ABA算法核心公式 (基于aba.hxx)

(基于aba.hxx)

ABA (Articulated-Body Algorithm) 是一种高效的前向动力学算法，用于计算机器人关节加速度。以下是基于 aba.hxx 实现的核心数学公式：

1. 算法概述

ABA算法分为三步：

• 正向传播：计算关节位置、速度和科氏力
• 反向传播：计算铰接体惯性和力
• 正向计算：计算关节加速度
  

2. 正向步骤1：位置、速度和科氏力计算2.1 关节位置更新

oMi=oMi[parent]×liMioMi=oMi[parent]×liMi

• oMi：关节i在世界坐标系中的位姿
• liMi：关节i的局部位姿变换
• ×：位姿乘法
  

2.2 关节速度更新

ov=oMi⋅vjoint+ov[parent]ov=oMi⋅vjoint​+ov[parent]

• ov：关节i在世界坐标系中的速度
• v_{joint}：关节i的局部关节速度
• ·：位姿对运动的作用
  

2.3 科氏力计算

oa_gf=oMi⋅cjoint+(ov[parent]∧ov)oa_gf=oMi⋅cjoint​+(ov[parent]∧ov)

• oa\_gf：考虑重力的加速度场
• c_{joint}：关节i的局部科氏项
• \wedge：运动旋量叉乘
  

2.4 惯性和动量计算

oYaba=oYcrb=oMi⋅Ioh=oYcrb⋅ovoYaba=oYcrb=oMi⋅Ioh=oYcrb⋅ov

• oYaba：铰接体惯性矩阵（6x6）
• I：关节i的局部惯性张量
• oh：关节i的动量
  

2.5 力计算

of=ov∧ohof=ov∧oh

• of：关节i处的力旋量
• \wedge：运动旋量对动量旋量的叉乘
  

3. 反向步骤：铰接体惯性和力传播3.1 调整输入力

u=u−JT⋅ofu=u−JT⋅of

• u：关节i的输入力矩
• J：关节i的雅克比矩阵
• J^T：雅克比矩阵的转置
  

3.2 计算U和StU矩阵

U=oYaba⋅JStU=JT⋅U+armatureU=oYaba⋅JStU=JT⋅U+armature

• U：中间惯性矩阵（6×nv）
• StU：关节空间惯性矩阵的局部块（nv×nv）
• armature：转子惯性贡献
  

3.3 计算关节惯性矩阵逆和UDinv

Dinv=StU−1UDinv=U⋅DinvDinv=StU−1UDinv=U⋅Dinv

• Dinv：关节i的惯性矩阵逆（nv×nv）
• UDinv：中间矩阵（6×nv）
  

3.4 更新铰接体惯性

oYaba=oYaba−UDinv⋅UToYaba=oYaba−UDinv⋅UT

3.5 更新力

of=of+oYaba⋅oa_gf+UDinv⋅uof=of+oYaba⋅oa_gf+UDinv⋅u

• 将力传递给父关节：of[parent] = of[parent] + of
  

4. 正向步骤2：关节加速度计算4.1 更新加速度场

oa_gf=oa_gf+oa_gf[parent]oa_gf=oa_gf+oa_gf[parent]

4.2 计算关节加速度

ddq=Dinv⋅u−UDinvT⋅oa_gfddq=Dinv⋅u−UDinvT⋅oa_gf

• ddq：关节i的加速度
  

4.3 更新加速度场

oa_gf=oa_gf+J⋅ddqoa_gf=oa_gf+J⋅ddq

4.4 最终加速度和力

a=oa_gf+gravityof=oYcrb⋅oa_gf+ov∧oha=oa_gf+gravityof=oYcrb⋅oa_gf+ov∧oh

• a：关节i的绝对加速度
• gravity：重力加速度
  

5. 局部坐标系下的优化

abaLocalConvention 实现了局部坐标系下的ABA算法，主要区别在于：

• 速度和加速度在局部坐标系中表示：
  v=vjoint+liMi−1⋅v[parent]a_gf=cjoint+(v∧vjoint)+liMi−1⋅a_gf[parent]v=vjoint​+liMi−1⋅v[parent]a_gf=cjoint​+(v∧vjoint​)+liMi−1⋅a_gf[parent]
• 惯性矩阵直接使用局部表示：
  Yaba=I.matrix()Yaba=I.matrix()
• 力的传递考虑了坐标系变换：
  of[parent]=of[parent]+liMi⋅ofof[parent]=of[parent]+liMi⋅of
  
  

6. 惯性矩阵逆计算 (computeMinverse)

基于ABA的惯性矩阵逆计算：

6.1 反向步骤

Minv[i,i]=DinvMinv[i,children]=−JT⋅Fcrb[children]Minv[i,i]=DinvMinv[i,children]=−JT⋅Fcrb[children]

6.2 正向步骤

Minv[i,ancestors]=Minv[i,ancestors]−UDinvT⋅Fcrb[ancestors]Minv[i,ancestors]=Minv[i,ancestors]−UDinvT⋅Fcrb[ancestors]

7. 关键数学概念

• 铰接体惯性：表示关节对加速度的抵抗能力，考虑了所有子树的惯性
• 运动旋量：6维向量，表示刚体的速度（线性+角速度）
• 力旋量：6维向量，表示作用在刚体上的力（力+力矩）
• 雅克比矩阵：将关节速度映射到末端执行器速度的矩阵
• 科氏力：由于关节运动耦合产生的力
  

8. 算法复杂度

ABA算法的时间复杂度为O(n)，其中n是关节数量，这使得它非常适合实时控制应用。

9. 实现优化

• 模板元编程：支持多种关节类型和数值类型
• Eigen库优化：高效的矩阵运算
• 关节访问者模式：统一处理不同类型的关节
• 对称性利用：仅计算惯性矩阵的上三角部分
• 显式模板实例化：提高编译速度
  

通过这些优化，Pinocchio库实现了高效的ABA算法，能够满足机器人实时控制的需求。
</think_never_used_51bce0c785ca2f68081bfa7d91973934>

ABA算法数学公式详解 (基于aba.hxx)1. 算法概述

Articulated-Body Algorithm (ABA) 是一种高效的前向动力学算法，用于计算给定关节位置、速度和力矩时的关节加速度。该算法在 aba.hxx 中实现，分为三个核心步骤：

2. 正向步骤1：位置、速度与科氏力计算2.1 关节位姿与速度

oMi=oMi[parent]×liMiov=oMi⋅vjoint+ov[parent]oMiov​=oMi[parent]×liMi=oMi⋅vjoint​+ov[parent]​

• oMi：关节i在世界坐标系中的位姿变换
• liMi：关节i的局部位姿变换
• ov：关节i在世界坐标系中的速度旋量
• v_{joint}：关节i的局部关节速度
  

2.2 加速度场与科氏力

oa_gf=oMi⋅cjoint+(ov[parent]∧ov)oa_gf=oMi⋅cjoint​+(ov[parent]∧ov)

• oa\_gf：考虑重力的加速度场
• c_{joint}：关节i的局部科氏项
• \wedge：运动旋量叉乘
  

2.3 惯性与动量

oYaba=oMi⋅Ioh=oYaba⋅ovoYabaoh​=oMi⋅I=oYaba⋅ov​

• oYaba：铰接体惯性矩阵 (6×6)
• I：关节i的局部惯性张量
• oh：关节i的动量旋量
  

2.4 力旋量计算

of=ov∧ohof=ov∧oh

• of：关节i处的力旋量
  

3. 反向步骤：铰接体惯性与力传播3.1 输入力调整

u=u−JT⋅ofu=u−JT⋅of

• u：关节i的输入力矩
• J：关节i的雅克比矩阵
  

3.2 中间矩阵计算

U=oYaba⋅JStU=JT⋅U+armatureDinv=StU−1UDinv=U⋅DinvUStUDinvUDinv​=oYaba⋅J=JT⋅U+armature=StU−1=U⋅Dinv​

• U：中间惯性矩阵 (6×nv)
• StU：关节空间惯性矩阵局部块 (nv×nv)
• armature：转子惯性贡献
• Dinv：关节i的惯性矩阵逆
• UDinv：中间矩阵 (6×nv)
  

3.3 铰接体惯性更新

oYaba=oYaba−UDinv⋅UToYaba=oYaba−UDinv⋅UT

3.4 力传播

of[parent]=of[parent]+(oYaba⋅oa_gf+UDinv⋅u)of[parent]=of[parent]+(oYaba⋅oa_gf+UDinv⋅u)

4. 正向步骤2：关节加速度计算4.1 加速度场更新

oa_gf=oa_gf+oa_gf[parent]oa_gf=oa_gf+oa_gf[parent]

4.2 关节加速度计算

ddq=Dinv⋅u−UDinvT⋅oa_gfddq=Dinv⋅u−UDinvT⋅oa_gf

• ddq：关节i的加速度
  

4.3 加速度场最终更新

oa_gf=oa_gf+J⋅ddqoa_gf=oa_gf+J⋅ddq

4.4 最终加速度与力

a=oa_gf+gravityof=oYcrb⋅oa_gf+ov∧ohaof​=oa_gf+gravity=oYcrb⋅oa_gf+ov∧oh​

• a：关节i的绝对加速度
• gravity：重力加速度
  

5. 局部坐标系优化

abaLocalConvention 实现了局部坐标系下的ABA，主要优化：

• 局部速度与加速度：
  v=vjoint+liMi−1⋅v[parent]a_gf=cjoint+(v∧vjoint)+liMi−1⋅a_gf[parent]va_gf​=vjoint​+liMi−1⋅v[parent]=cjoint​+(v∧vjoint​)+liMi−1⋅a_gf[parent]​
• 直接使用局部惯性：
  Yaba=I.matrix()Yaba=I.matrix()
• 局部力传递：
  of[parent]=of[parent]+liMi⋅ofof[parent]=of[parent]+liMi⋅of
  
  

6. 惯性矩阵逆计算 (computeMinverse)6.1 反向步骤

Minv[i,i]=DinvMinv[i,children]=−JT⋅Fcrb[children]Minv[i,i]Minv[i,children]​=Dinv=−JT⋅Fcrb[children]​

6.2 正向步骤

Minv[i,ancestors]=Minv[i,ancestors]−UDinvT⋅Fcrb[ancestors]Minv[i,ancestors]=Minv[i,ancestors]−UDinvT⋅Fcrb[ancestors]

7. 算法特性

• 时间复杂度：O(n)，n为关节数量
• 空间复杂度：O(n)
• 数值稳定性：良好，适合实时应用
• 支持：外部力、转子惯性、多种关节类型
  

8. 关键优化

• 模板元编程：支持不同关节类型和数值类型
• 关节访问者模式：统一处理不同关节
• Eigen库：高效矩阵运算
• 对称性利用：仅计算惯性矩阵上三角部分
• 显式模板实例化：提高编译速度
  

ABA算法是机器人动力学中的核心算法，Pinocchio库的实现兼顾了效率和通用性，适用于各种机器人系统的实时控制和仿真。