CN119458332A 一种基于视-触觉融合的高精度物体位姿估计与精细操作方法 （北京理工大学）

号一种基于视-触觉融合的高精度物体位姿估本发明提供了一种基于视触觉融合的高精应对能力；本发明针对FMB基准在内的13种具有2分别估计主物体和从物体的初始位姿，并实时跟踪主物物体相对于主物体的位姿估计误差转移到从物体相对于机器人末端执行器的位姿估计误步骤33、对这一轮的粒子添加小噪声，记录当前时刻下界坐标系的位姿和主物体相对于世界坐标系的位姿神,主物体和从物体的3D空间模型表主物体和从物体的接触程度，表示机器人和2.如权利要求1所述的一种基于视_触觉融合的高精度物体位姿估计与精细操作方法，3首先，使用FoundationPose方法分别估计主物体和从物体的初始位姿结果和则两者的位姿真值分别表示为其中em和es分别表示主物体和从物体位在上式中，从物体在世界坐标系的位姿通过从物体和末端执行器在世界坐标系的位姿来估计；从物体相对于主物体的位姿进一步写为在使用触觉信息进行位姿修正之后，基于从物体在主物体坐标系中3.如权利要求1或2所述的一种基于视一触觉先对原始数据应用平滑滤波，除去边缘和一些噪声点，然后计基于特征点的计算提取，根据特征计算接触区域中心的实时位移，最终从标记物旋转角度沿旋转中心的不均匀分布推断出旋转滑移其特征在于，所述旋转滑移由计算得到，其中4将从物体相对于主物体的位姿估计误差转移到从物体相对于机器人末端执行器的位姿估5于世界坐标系的位姿和主物体相对于世界坐标系的位姿主物体和从物体的3D空间[0020]首先，使用FoundationPose方法分别估计主物体和从物体的初始位姿结果和则两者的位姿真值分别表示为和其中em和es分别表示主物体和从物整合到一个总体误差项eo中，该总体误差项eo指的是从物体和主物体的位姿估计的总体误写为在使用触觉信息进行位姿修正之后，基于从物体在6[0025]较佳的，所述步骤2中估计从物体在机器人末端执行器坐标系7[0039]在详细介绍技术方案之前，先解释各参数或符号的物体含义：公式中从物体机器人完成高精度操作任务。物体位姿描述了3D空间中给定坐标系中物体的位置和姿态，[0041]如图1所示，(a)本发明先利用FoundationPose方法(Unified6DPoseEstimationandTrackingofNovelObjects，物体6D位姿估计与跟踪方法)分别估计主体和从物体的初始位姿结果和他们和真值关系为和其中e表示位姿可以进一步写为其中误差eo'仅需要在从物体相对于机器人末端[0048]利用实时计算的末端执行器机器人能够在各种动态环境条件下执行高精度的8[0049]将从物体相对于主物体的位姿估计误差转移到从物体相对于机器人末端执行姿结果但由于从物体可能在机器人末端执行器中发生滑动，因此本发明提出了一个实[0055]机器人系统模型:考虑到从物对于世界坐标系的位姿为xt表示在粒子滤波中时刻t每个粒子所代表的位姿(即可状态空间模型中的状态：手部物体位姿)，主物体和从物体的3D空间模型分别为Om和Os，函数9的状态观察模型结果代表着主物体和从物体的接触程度，其越接近于1代表着接触程度越[0059](1)初始位姿假设生成:将主物体和从物体的位姿误差集中在手部物体位姿估计先利用FoundationPose方法实时估计并跟踪从物体的位姿xest，本发明在合理范围内均匀和而指的是时刻T_1的预测值，最终近似于的加权求和。[0067]本发明通过计算当前位姿假设下主物体和次物体之间的距离来更新每个位姿粒了时刻T下的机器人末端执行器相对于世界坐标系的位姿和主物体相对于世界坐标系的其中将由状态空间模型中的系统模型给出，Om,Os,最小阈值dminth和最大阈值dmaxth之间的距离值，本发明会利用公式计算得到，[0071]基于粒子群的权重分布本发明通过加权采样产生新的粒子群来不断更[0077]如图3所示，效果对比说明：动作选择模块(a)和权重更新模块(b)的位姿估计精取从物体并与主物体进行接触，其中机器人是装有一对触觉传感器Tac3D_A1的Franka[0081]从FMB基准的物体模型中选择9种不同的钉孔形状(长度为150mm)作为实验物体，[0082]基于粒子更新的权重，本发明对位姿预测结果进行加权，最终的结果如表1所选择策略的有效性，所提出的方法与随机动作选择方法(本发明称之为'随机')进行了比接触次数下实现位姿估计。所提出的动作选择方法随着交互次数的增加，性能快速提高