• 导纳控制需要机械臂末端装有六维力传感器，传感器可以记录下来外力及外力矩，传感器的价格至少一万元以上一个
  • 考虑到成本，以及力控精度不需要很准的情况下，无需安装传感器，可以对末端力进行估算算法的开发。

一.末端六维力估计计算方法

\tau _{measured}=(M(q)q\ddot{}+C(q)q\dot{}+G(q)+Fric(q\dot{}))+J^{^{T}}F_{ext} （1）

M(q)q\ddot{}+C(q)q\dot{}+G(q)+Fric(q\dot{})=\tau_{model}

公式1,左侧为关节力矩传感器测量得到的实际关节力矩。公式1在机械臂的任意实际状态下都成立，即当手臂受外力时，将机械臂的状态都代入到动力学模型中，加上末端外力/力矩转换到关节后，恒等于左侧。

令\tau _{measured}-\tau _{model}=\tau_{ext}

\tau_{ext}=J^{^{T}}F_{ext}

J维度是7*6, 外力矩的维度是6*1的，7个方程，6个未知数，因此这是个超正定方程，没有精确解，转化成为一个求最优解的问题，即

\parallel J^{T}F-\tau _{ext}\parallel ^{2}=L(F)

将其展开,

L=F^{T}(JJ^{T})F-2F^{T}(J\tau _{ext})+\tau _{ext}^{^{T}}\tau _{ext}

使用最小二乘进行求解，由于接近奇异位置时，JJ^{^{T}}接近奇异，F_{ext}会无限大。需要使用带有惩罚项的最小二乘方式，防止F会无限大。

L(F)=\parallel J^{T}F-\tau _{ext}\parallel ^{2}+\lambda ^{^{2}}\parallel F\parallel ^{2}

对F求导数

\frac{\partial L}{\partial F}=2(JJ^{T})F-2(J\tau _{ext})+2\lambda ^2F=0

(JJ^{T}+\lambda ^2)F=J\tau _{ext}                    (2)

公式2可看作是Ax=b,可以进行线性方程求解

A=(JJ^{T}+\lambda ^2)

x=F

b=J\tau _{ext}

由于A本身为对称矩阵，不需要求逆，使用LDL^{T}对A进行分解，可以快速稳定的求解出来x

# 机器人 # 算法
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

τ0-VLA：一种基于世界模型引导测试时计算的分层机器人基础模型

avatar DAMO开发者矩阵
cover

WholeBodyWAM：基于 WBC-协同将预训练“世界-动作”先验泛化至人形机器人的移动与操作任务

avatar DAMO开发者矩阵
cover

TVA具身智能运行机理（14）：从“概率猜测”到“科学推理”的跃迁

avatar DAMO开发者矩阵