基于RGB-D置信度融合的挂装点六维位姿实时获取方法与流程

本发明涉及机器人感知，尤其涉及基于rgb-d置信度融合的挂装点六维位姿实时获取方法。

背景技术：

1、在救援机器人对伤员执行自主转运的过程中，机械臂需要获取战术背心上挂装点的六维位姿信息，以执行精确的抓取动作。六维位姿包括挂装点在相机坐标系下的三维空间位置和表面法向量。rgb-d深度相机可同时提供彩色图像和深度图像，是获取六维位姿的常用传感器。

2、然而，战术背心通常采用黑色尼龙材质，该材质对红外光具有较高的吸收率，导致深度图像中频繁出现深度空洞。现有方法对不同形态的深度空洞采用统一的填充方式，未区分孤立空洞与大面积空洞的差异，在大面积空洞上强行填充会引入显著的位置偏差。

3、此外，通过局部点云主成分分析获得的法向量，在目标表面弯曲或深度噪声过大时会严重偏离真实方向，而现有方法不附带法向量质量评估，导致低质量法向量被无差别使用。逐帧独立计算的法向量受深度噪声影响存在帧间跳变，且仅依赖模型检测置信度的单一评估方式无法全面反映位姿数据的可靠性。上述问题导致机械臂在执行挂装动作时存在位姿偏差过大的风险。

技术实现思路

1、本发明的目的在于：为了解决上述问题，而提出的基于rgb-d置信度融合的挂装点六维位姿实时获取方法。

2、为了实现上述目的，本发明采用了如下技术方案：

3、基于rgb-d置信度融合的挂装点六维位姿实时获取方法，包括：

4、通过深度相机获取对齐的rgb图像和深度图像;

5、将所述rgb图像输入关键点检测模型,输出各挂装点的像素坐标和模型检测置信度;

6、在所述深度图像中读取所述挂装点处的深度值,当所述深度值为零时根据邻域内有效深度像素的分布情况执行分级处理以获取填充后的深度值;

7、利用相机内参矩阵将所述像素坐标和所述深度值反投影至相机坐标系,得到所述挂装点的三维坐标;

8、提取所述挂装点的局部点云并通过主成分分析计算表面法向量,同时计算法向量质量指标并将所述法向量质量指标与预设质量阈值进行比较以评估所述表面法向量的可靠性;

9、确定所述表面法向量指向相机方向;将所述模型检测置信度与深度有效比例相乘得到复合置信度,输出所述挂装点的六维位姿及所述复合置信度。

10、优选地，所述根据邻域内有效深度像素的分布情况执行分级处理包括:统计目标像素8邻域内有效深度像素的数量;当所述有效深度像素的数量大于等于5时,判定为孤立空洞,以所述目标像素为中心提取邻域内所有有效深度值并计算中位数作为填充值;当所述有效深度像素的数量小于5且沿某一方向存在连续零值带时,判定为连续空洞,沿空洞长轴方向确定两端的有效深度值并对空洞内部像素执行线性插值填充;当邻域内有效深度像素占比低于预设比例阈值时,判定为大面积空洞,将该挂装点标记为深度不可恢复状态并将该挂装点的复合置信度置零。

11、优选地，所述通过主成分分析计算表面法向量包括:提取所述挂装点的局部邻域,将局部邻域内有效深度像素反投影为三维点云;计算所述三维点云的协方差矩阵并执行特征值分解,得到三个特征值及对应的特征向量;取最小特征值对应的特征向量作为所述表面法向量的初始估计;其中,所述法向量质量指标为所述最小特征值与最大特征值的比值,当所述法向量质量指标大于所述预设质量阈值时,放弃当前表面法向量估计并回退至上一帧该挂装点的有效法向量。

12、优选地，所述确定所述表面法向量指向相机方向包括:判断所述表面法向量在相机坐标系z轴上的分量,当所述分量大于零时将所述表面法向量取反。

13、优选地，所述深度有效比例为局部邻域内有效深度像素数占局部邻域总像素数的比值。

14、优选地，所述在输出所述挂装点的六维位姿之前还包括:对连续帧之间同一挂装点的表面法向量执行球面线性插值,得到平滑后的法向量,其中球面线性插值的权重为根据所述复合置信度确定的自适应插值权重;对所述三维坐标执行指数移动平均,得到平滑后的三维坐标;将所述平滑后的法向量和所述平滑后的三维坐标作为最终输出。

15、优选地，所述自适应插值权重为将所述复合置信度限制在0.1至0.9范围内的数值,当所述复合置信度较高时所述自适应插值权重较大,当所述复合置信度较低时所述自适应插值权重较小。

16、优选地，所述根据所述复合置信度将挂装点分为三个可靠性等级:当所述复合置信度大于0.7时判定为高可靠性;当所述复合置信度大于0.3且小于等于0.7时判定为中可靠性;当所述复合置信度小于等于0.3时判定为低可靠性。

17、优选地，所述深度相机配备940nm波段的红外补光灯。

18、综上所述，由于采用了上述技术方案，本发明的有益效果是：

19、1、本发明通过采用基于空洞形态特征的分级处理策略，对孤立空洞、连续空洞和大面积空洞分别执行中位数填充、线性插值和标记深度不可恢复状态的差异化处理；由于不同形态的深度空洞具有不同的空间分布特征和可恢复性，分级处理使每类空洞均获得与其特征相匹配的处理方式，对大面积空洞标记为深度不可恢复状态而非强行填充，避免了虚假深度值被用于位姿计算。

20、2、本发明通过针对法向量帧间跳变问题，通过球面线性插值对连续帧的表面法向量执行时域平滑，并以复合置信度动态调节自适应插值权重；高复合置信度帧获得较大的当前帧权重，低复合置信度帧获得较小的当前帧权重，时域平滑在抑制帧间跳变的同时保留了对真实位姿变化的响应能力。

1.基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，包括：

2.根据权利要求1所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，根据邻域内有效深度像素的分布情况执行分级处理包括:统计目标像素8邻域内有效深度像素的数量;当所述有效深度像素的数量大于等于5时,判定为孤立空洞,以所述目标像素为中心提取邻域内所有有效深度值并计算中位数作为填充值;当所述有效深度像素的数量小于5且沿某一方向存在连续零值带时,判定为连续空洞,沿空洞长轴方向确定两端的有效深度值并对空洞内部像素执行线性插值填充;当邻域内有效深度像素占比低于预设比例阈值时,判定为大面积空洞,将该挂装点标记为深度不可恢复状态并将该挂装点的复合置信度置零。

3.根据权利要求1所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，通过主成分分析计算表面法向量包括:提取所述挂装点的局部邻域,将局部邻域内有效深度像素反投影为三维点云;计算所述三维点云的协方差矩阵并执行特征值分解,得到三个特征值及对应的特征向量;取最小特征值对应的特征向量作为所述表面法向量的初始估计;其中,所述法向量质量指标为所述最小特征值与最大特征值的比值,当所述法向量质量指标大于所述预设质量阈值时,放弃当前表面法向量估计并回退至上一帧该挂装点的有效法向量。

4.根据权利要求1所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，确定所述表面法向量指向相机方向包括:判断所述表面法向量在相机坐标系z轴上的分量,当所述分量大于零时将所述表面法向量取反。

5.根据权利要求1所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，深度有效比例为局部邻域内有效深度像素数占局部邻域总像素数的比值。

6.根据权利要求5所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，在输出所述挂装点的六维位姿之前还包括:对连续帧之间同一挂装点的表面法向量执行球面线性插值,得到平滑后的法向量,其中球面线性插值的权重为根据所述复合置信度确定的自适应插值权重;对所述三维坐标执行指数移动平均,得到平滑后的三维坐标;将所述平滑后的法向量和所述平滑后的三维坐标作为最终输出。

7.根据权利要求6所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，自适应插值权重为将所述复合置信度限制在0.1至0.9范围内的数值,当所述复合置信度较高时所述自适应插值权重较大,当所述复合置信度较低时所述自适应插值权重较小。

8.根据权利要求7所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，根据所述复合置信度将挂装点分为三个可靠性等级:当所述复合置信度大于0.7时判定为高可靠性;当所述复合置信度大于0.3且小于等于0.7时判定为中可靠性;当所述复合置信度小于等于0.3时判定为低可靠性。

9.根据权利要求8所述的基于rgb-d置信度融合的挂装点六维位姿实时获取方法，其特征在于，深度相机配备940nm波段的红外补光灯。