CN114549592B - 一种非合作抛体的轨迹预测与捕获方法和装置 - Google Patents

Abstract

本发明公开了一种非合作抛体的轨迹预测与捕获方法和装置，其创新性地高效应用了高帧率RGB相机以及RGBD深度相机的感知信息，实现抛体在空间的快速精确定位。基于定位信息，本发明又提出了基于卡尔曼滤波的抛体轨迹粗拟合以及基于物理学建模的轨迹精拟合实现方案，通过融合轨迹粗拟合与精拟合信息，本发明方法可根据实际场景和任务特点，低成本，高精度，高稳定性的实现空间抛体的轨迹预测与捕获。

Description

一种非合作抛体的轨迹预测与捕获方法和装置

技术领域

本发明属于计算机视觉与智能机器人交叉创新领域，具体涉及一种非合作抛体的轨迹预测与捕获方法和装置。

背景技术

空间非合作抛体的轨迹预测与捕获方法是机器人的重要技术之一，对机器人落地应用有着重大意义，特别是球类机器人，通过此项技术，机器人可以快速捕捉空间运动物体，实现避障，抓取，互动等一系列操作。

目前，对于非合作抛体的轨迹预测与捕获主要基于动态捕捉系统。而该套系统具有两个主要弊端：一方面，动捕系统价格昂贵，且对试验场景的搭建有较高的要求。另一方面，动捕系统需要在抛体上安装传感器，这些都在很大程度上限制了该项技术广泛应用。

发明内容

本发明的目的在于针对现有技术的不足，提供了一种非合作抛体的轨迹预测与捕获方法和装置。该方法安装标定方便，价格低，精度高，稳定性好，对智能机器人的落地应用具有重要意义。

本发明的目的是通过以下技术方案来实现的：一种非合作抛体的轨迹预测与捕获方法，包括以下步骤：

步骤一：基于高帧率RGB相机实现对目标抛体的粗定位；

步骤二：基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测，得到粗预测结果；

步骤三：基于粗预测结果控制带RGBD深度相机的移动机械臂移动到最佳视觉观测点后，基于RGBD深度相机实现对目标抛体的精定位；

步骤四：基于多帧精定位结果进行目标抛体运动轨迹建模，并据此获得目标抛体的捕获点的精准坐标；

步骤五：基于RGBD深度相机计算捕获点处目标抛体的6D姿态并实施捕获。

进一步地，所述步骤一具体为：

将高帧率RGB相机固定于实验场景，调整拍摄角度，使高帧率RGB相机拍摄到目标抛体空间运动全过程信息，进而获得目标抛体的RGB图像；再使用张氏标定法对高帧率RGB相机进行内外参标定；随后对于获得的RGB图像，通过基于灰度差异的物体边缘检测方法获得目标抛体的检测框；通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标以及检测框的宽和高；结合高帧率RGB相机的内参标定结果得到目标抛体检测框中心点在高帧率RGB相机坐标系下的坐标；再结合高帧率RGB相机的外参标定结果，将检测框中心点在高帧率RGB相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于高帧率RGB相机实现目标抛体的粗定位，所述高帧率RGB相机的帧率大于100。

进一步地，所述步骤二具体为：

（2.1）使用目标抛体被抛出时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高对卡尔曼滤波跟踪器进行初始化；

（2.2）使用目标抛体运动初期p帧的在图像坐标系下检测框中心点的坐标以及检测框宽和高作为真值对初始化后的卡尔曼滤波跟踪器进行校正；

（2.3）使用校正后的尔曼滤波跟踪器预测生成p帧后目标抛体在图像坐标系下的运动轨迹，包括p帧后任一时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高，随后结合高帧率RGB相机的内参标定结果得到在高帧率RGB相机坐标系下检测框中心点的坐标，再结合高帧率RGB相机的外参标定结果得到在世界坐标系下检测框中心点的坐标；在世界坐标系下检测框中心点的坐标作为目标抛体运动轨迹的粗预测结果，由此基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测。

进一步地，所述步骤三具体为：

（3.1）在目标抛体捕获任务开始前，使用张氏标定法对带RGBD深度相机的移动机械臂进行离线标定，获得RGBD深度相机的内外参；所述带RGBD深度相机的移动机械臂包括底座、手臂、机械手和RGBD深度相机；标定结束后，得到RGBD深度相机在世界坐标系下的初始坐标；

（3.2）根据步骤（2.3）得到的目标抛体运动轨迹的粗预测结果，得到目标抛体在世界坐标系下的落地位置，从而计算得到最佳视觉观测点并触发带RGBD深度相机的移动机械臂中的底座快速响应到达最佳视觉观测点；所述最佳视觉观测点的位置的选取基于落地位置和带RGBD摄像头的移动机械臂的响应速度决定；在带RGBD深度相机的移动机械臂中的底座移动到最佳视觉观测点的过程中，记录底座移动过程中旋转和平移变换；并根据底座移动过程中旋转矩阵和平移向量计算得到最佳视觉观测点处RGBD深度相机在世界坐标系下的坐标；

（3.3）待底座移动到最佳视觉观测点后，通过RGBD深度相机获得场景的RGBD图像；随后待目标抛体进入RGBD深度相机的观测区域后，获得目标抛体的RGBD图像；对于获得的RGBD图像，通过基于深度学习的神经网络图像处理方法获得目标抛体的检测框；通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标，融合RGBD信息获得RGBD图像中目标抛体检测框中心点到RGBD深度相机平面的距离；结合RGBD深度相机的内参标定结果和检测框中心点到RGBD深度相机平面的距离得到目标抛体检测框中心点在RGBD深度相机坐标系下的坐标；再结合RGBD深度相机的外参标定结果，将检测框中心点在RGBD深度相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于RGBD深度相机实现对目标抛体的精定位。

进一步地，所述步骤四具体为：

（4.1）对于目标抛体的运动，考虑其受到重力以及空气阻力的影响，将其分解为x，y，z三轴的变加速度运动，进而对三轴运动分别建模，模型包含目标抛体在精定位初始帧时x，y，z三轴的初速度、空气阻力系数这些未知常量，使用多帧精定位结果通过最小二乘法拟合这些未知常量，从而获得目标抛体在空间的高精度预测轨迹；

（4.2）基于目标抛体的运动轨迹的精预测曲线，计算目标抛体的捕获点的精准坐标，随后保持底座不移动，控制机械臂手臂移动到捕获点，准备捕获操作。

进一步地，所述步骤五具体为：

（5.1）利用姿态估计算法求得目标抛体在捕获点前b帧处目标抛体的6D姿态：首先将捕获点前b帧处目标抛体的RGBD图像输入训练好的分类深度神经网络中，得到相对应的目标抛体的6D姿态的快速匹配结果，然后使用ICP算法得到较为精确的目标抛体的6D姿态；b帧的选择与姿态估计算法速度以及机械手捕获精准度有关；

（5.2）将捕获点前b帧处目标抛体的6D姿态作为捕获姿态，结合目标抛体的三维模型生成抓取位置，使用带RGBD深度相机的移动机械臂中的机械手对目标抛体进行捕获。

本发明还提供了一种非合作抛体的轨迹预测与捕获装置，包括一个或多个处理器，用于实现上述非合作抛体的轨迹预测与捕获方法。

本发明还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，用于实现上述非合作抛体的轨迹预测与捕获方法。

本发明的有益效果是：本发明提出了一种非合作抛体的轨迹预测与捕获方法和装置，相较现有方法具有低成本，高精度，高稳定性的优点。其核心创新点为高效利用了高帧率RGB相机及RGBD深度相机信息特点，基于不同功能要求实现了抛体的粗定位与精定位。此外，基于粗定位和精定位的结果，本发明进一步提出了不同的轨迹粗拟合和精拟合的算法，前者拟合速度快，满足机械臂快速响应控制要求，而后者通过运动学建模的方式对抛体轨迹实现预测，精度更高，满足抓取捕获等一系列高精度任务的要求。整体而言，本发明提出的抛体轨迹预测与捕获方法对智能机器人，物流装配等一系列产业具有积极意义。

附图说明

图1为本发明提供的一种非合作抛体的轨迹预测与捕获方法的流程示意图；

图2为本发明提供的一种非合作抛体的轨迹预测与捕获方法的实验场景设计图；

图3为本发明一种非合作抛体的轨迹预测与捕获装置的结构图。

具体实施方式

为了使本发明的目的、技术方案及优点更加明白清楚，结合附图和实施例，对本发明进一步的详细说明，应当理解，此处所描述的具体实施例仅仅用以解释本发明，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得 的所有其他实施例，均在本发明保护范围。

本发明的目的是提出一种非合作抛体的轨迹预测与捕获方法和装置，具有高精度、高稳定性以及相对低价的优点，下面结合图1-图3对本发明作进一步说明。

如图1-图2所示，一种非合作抛体的轨迹预测与捕获方法，包括以下步骤：

步骤一：基于高帧率RGB相机实现对目标抛体的粗定位；

步骤一具体通过以下步骤来实现的：

将高帧率RGB相机固定于实验场景，调整拍摄角度，使高帧率RGB相机拍摄到目标抛体空间运动全过程信息，进而获得目标抛体的RGB图像；再使用张氏标定法对高帧率RGB相机进行内外参标定；随后对于获得的RGB图像，通过基于灰度差异的物体边缘检测方法获得目标抛体的检测框；通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标以及检测框的宽和高；结合高帧率RGB相机的内参标定结果得到目标抛体检测框中心点在高帧率RGB相机坐标系下的坐标；再结合高帧率RGB相机的外参标定结果，将检测框中心点在高帧率RGB相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于高帧率RGB相机实现目标抛体的粗定位。

所述高帧率RGB相机的帧率大于100。所述基于灰度差异的物体边缘检测方法为物体检测算法的一种，基于灰度差异的物体边缘检测方法对于纯色抛体实现100fps以上的检测速度，符合高帧率相机的处理速度需求，且检测精度满足粗定位的要求，本实施例中，高帧率RGB相机的帧率为150。

对于获得的第i帧的RGB图像，通过基于灰度差异的物体边缘检测方法获得目标抛体的检测框，随后通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标为（x_i，y_i）以及检测框的宽和高分别为w_i、h_i；结合高帧率RGB相机的内参标定结果得到目标抛体检测框中心点在高帧率RGB相机坐标系下的坐标为（x_ci，y_ci，z_ci），通过公式（1）-（6）求得；公式（1）-（6）如下所示：

；

其中，下标i表示获得的第i帧的RGB图像，W、H为实际测量获得的目标抛体的宽和高，f_x1 、f_y1 、c_x1 、c_y1为高帧率RGB相机的内参；S_图像、S_物体分别为图像坐标系和相机坐标系下目标抛体检测框的面积。

再结合高帧率RGB相机的外参标定结果，将检测框中心点在高帧率RGB相机坐标系下的坐标（x_ci，y_ci，z_ci）转换到世界坐标系下检测框中心点的坐标（x_wi，y_wi，z_wi），通过公式（7）求得，由此基于高帧率RGB相机实现目标抛体的粗定位；公式（7）如下所示：

；

其中，R1、t1分别为高帧率RGB相机外参标定得到的旋转矩阵与平移向量；

在世界坐标系下检测框中心点的坐标（x_wi，y_wi，z_wi）作为粗定位的结果。

步骤二：基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测，得到粗预测结果；

步骤二具体通过以下步骤来实现的：

（2.1）使用目标抛体被抛出时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高对卡尔曼滤波跟踪器进行初始化；所述卡尔曼滤波跟踪器为一种滤波函数，可以实现运动轨迹的拟合与预测。

（2.2）使用初始化后的卡尔曼滤波跟踪器预测目标抛体下一时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高，但是这样位置预测的随机性较大，因此需要对卡尔曼滤波跟踪器进行校正；

使用目标抛体运动初期p帧的在图像坐标系下检测框中心点的坐标以及检测框宽和高作为真值对初始化后的卡尔曼滤波跟踪器进行校正；

所述p帧的取值与高帧率RGB相机以及目标抛体被抛出时刻在图像坐标系下检测框的宽高乘积有关；一般来说，高帧率RGB相机的帧率越高，或检测框的宽高乘积越小，p的取值相应增大，反之亦然，本实施例中p取值为100。

通过对卡尔曼滤波跟踪器进行校正，可以迭代优化卡尔曼滤波跟踪器的参数，减小预测值与实际值的差距，以提升预测准确性，以及降低位置预测的随机性，对短期跟踪具有良好的精度效果，可以满足运动轨迹粗预测的要求。

（2.3）使用校正后的卡尔曼滤波跟踪器预测生成p帧后目标抛体在图像坐标系下的运动轨迹，包括p帧后任一时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高，随后结合高帧率RGB相机的内参标定结果得到在高帧率RGB相机坐标系下检测框中心点的坐标，再结合高帧率RGB相机的外参标定结果得到在世界坐标系下检测框中心点的坐标；在世界坐标系下检测框中心点的坐标作为目标抛体运动轨迹的粗预测结果，由此基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测。

对于使用校正后的尔曼滤波跟踪器预测生成p帧后第n帧在图像坐标系下检测框中心点的坐标为（x_n，y_n）以及检测框宽和高分别为w_n、 h_n；与步骤一的粗定位相似，通过公式（1）-（7），结合高帧率RGB相机的内参标定结果得到检测框中心点在高帧率RGB相机坐标系下的坐标为（x_cn，y_cn，z_cn）；再结合高帧率RGB相机的外参标定结果得到在世界坐标系下检测框中心点的坐标（x_wn，y_wn，z_wn）；在世界坐标系下检测框中心点的坐标（x_wn，y_wn，z_wn）作为目标抛体运动轨迹的粗预测结果，由此基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测。

步骤三：基于粗预测结果控制带RGBD深度相机的移动机械臂移动到最佳视觉观测点后，基于RGBD深度相机实现对目标抛体的精定位；

步骤三具体通过以下步骤来实现的：

（3.1）在目标抛体捕获任务开始前，使用张氏标定法对带RGBD深度相机的移动机械臂进行离线标定，获得RGBD深度相机的内外参，此刻外参中包括了旋转矩阵R2和平移向量t2；

标定结束后，得到RGBD深度相机在RGBD深度相机坐标系下的初始坐标为（0,0,0），并结合旋转矩阵R2和平移向量t2得到RGBD深度相机在世界坐标系下的初始坐标为（x_rgbd，y_rgbd，z_rgbd），通过公式（8）求得；

；

所述带RGBD深度相机的移动机械臂包括底座、手臂、机械手和RGBD深度相机；由遨博机器人公司的aubo-r200产品实现，此RGBD深度相机帧率为30，但不限于此。

（3.2）根据步骤（2.3）得到的目标抛体运动轨迹的粗预测结果，得到目标抛体在世界坐标系下的落地位置，从而计算得到最佳视觉观测点并触发带RGBD深度相机的移动机械臂中的底座快速响应到达最佳视觉观测点；所述最佳视觉观测点的位置的选取基于落地位置和带RGBD摄像头的移动机械臂的响应速度决定，若落地点坐标与移动机械臂初始位置较近，或机械臂响应时间较小，则可选择在落地点较近处作为最佳观测点，反之，则应选择在落地点较远处开始观测；在带RGBD深度相机的移动机械臂中的底座移动到最佳视觉观测点的过程中，记录底座移动过程中旋转和平移变换，得到旋转矩阵R3和平移向量t3，根据旋转矩阵R3和平移向量t3计算得到最佳视觉观测点处RGBD深度相机在世界坐标系下的坐标

，计算过程如公式（9）所示：

；

本实施例中选取粗预测轨迹中目标抛体到达落地位置前300帧处的位置作为最佳视觉观测点。

（3.3）待底座移动到最佳视觉观测点后，通过RGBD深度相机获得场景的RGBD图像；随后待目标抛体进入RGBD深度相机的观测区域后，获得目标抛体的RGBD图像；对于获得的RGBD图像，通过基于深度学习的神经网络图像处理方法获得目标抛体的检测框；通过目标抛体的检测框可得到在图像坐标系下目标抛体检测框中心点的坐标，融合RGBD信息获得RGBD图像中目标抛体检测框中心点到RGBD深度相机平面的距离；结合RGBD深度相机的内参标定结果和检测框中心点到RGBD深度相机平面的距离得到目标抛体检测框中心点在RGBD深度相机坐标系下的坐标；再结合RGBD深度相机的外参标定结果，将检测框中心点在RGBD深度相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于RGBD深度相机实现对目标抛体的精定位。

所述基于深度学习的神经网络图像处理方法为物体检测算法中的另一种方法，相较于基于灰度差异的物体边缘检测方法可实现更高的检测精度，且速率达到30fps以上，满足深度相机处理速度需求。

对于获得的第j帧目标抛体的RGBD图像，通过基于深度学习的神经网络图像处理方法获得目标抛体的检测框，随后通过目标抛体的检测框可得到在图像坐标系下目标抛体检测框中心点的坐标为（x_j，y_j），融合RGBD信息获得RGBD图像中目标抛体检测框中心点到RGBD深度相机平面的距离d_j；结合RGBD深度相机的内参标定结果以及距离d_j得到目标抛体检测框中心点在RGBD深度相机坐标系下的坐标为（x_dj，y_dj，z_dj），通过公式（10）-（12）求得；公式（10）-（12）如下所示：

；

其中，下标j表示获得的第j帧目标抛体的RGBD图像，f_x2、f_y2、c_x2、c_y2为1RGBD深度相机的内参；

再结合RGBD深度相机的外参标定结果，将检测框中心点在RGBD深度相机坐标系下的坐标（x_dj，y_dj，z_dj）转换到世界坐标系下检测框中心点的坐标（x_wj，y_wj，z_wj），通过公式（13）求得，由此基于RGBD深度相机实现目标抛体的精定位；公式（13）如下所示：

；

在世界坐标系下检测框中心点的坐标（x_wj，y_wj，z_wj）作为精定位的结果。

步骤四：基于多帧精定位结果进行目标抛体运动轨迹建模，并据此获得目标抛体的捕获点的精准坐标；

步骤四具体通过以下步骤来实现的：

（4.1）对于目标抛体的运动，考虑其受到重力以及空气阻力的影响，将其分解为x，y，z三轴的变加速度运动，进而对三轴运动分别建模，模型包含目标抛体在精定位初始帧时x，y，z三轴的初速度、空气阻力系数这些未知常量，使用多帧精定位结果通过最小二乘法拟合这些未知常量，从而获得目标抛体在空间的高精度预测轨迹。

具体实现方法如下，通过公式（14）-（22）对目标抛体三轴运动分别建模，再与目标抛体在空间运动前a帧精定位结果（j=1,2,…,a）进行拟合。目标抛体在空间运动过程中，考虑其受到空气阻力和重力作用，假定目标抛体质量为m，重力加速度为g，目标抛体空气阻力系数为k；公式（14）-（22）如下所示：

；

其中，k为未知常量，m、g为已知常量；a_xj、a_yj、a_zj分别表示在第j帧目标抛体的RGBD图像中目标抛体在x、y、z轴方向的加速度，V_xj、V_yj、V_zj分别表示在第j帧目标抛体的RGBD图像中目标抛体在x、y、z轴方向的速度，V_x(j+1)、V_y(j+1) 、V_y(j+1) 分别表示在第j+1帧目标抛体的RGBD图像中目标抛体在x、y、z轴方向的速度；

为RGBD深度相机相邻两帧的间隔时间，

为已知常量；

、

、

分别表示第j帧目标抛体的RGBD图像中目标抛体与第j+1帧目标抛体的RGBD图像中目标抛体之间在三轴方向的位置变化量；

对于通过公式（14）-（22）建模得到的运动模型，使用步骤三精定位结果获得前a帧精定位结果（x_wj=1，y_wj=1，z_wj=1）、（x_wj=2，y_wj=2，z_wj=2）…（x_wj=a，y_wj=a，z_wj=a），j=1,2,…a进行拟合，得到未知常量的拟合结果，所述未知常量包括空气阻力系数k以及目标抛体在x，y，z三轴的初速度（V_xj=1,V_yj=1,V_zj=1）。

具体拟合过程如下：1）通过目标抛体在精定位初始帧时x，y，z三轴的初速度（V_xj=1,V_yj=1,V_zj=1）基于公式（17）、（19）、（21）计算得到前a帧的目标抛体的速度（V_xj=2,V_yj=2,V_zj=2）、（V_xj=3,V_yj=3,V_zj=3）…（V_{xj=a,Vyj=a,Vzj=a}），求得的速度（V_xj=2,V_yj=2,V_zj=2）、（V_xj=3,V_yj=3,V_zj=3）…（V_{xj=a,Vyj=a,Vzj=a}）均仅与初速度（V_xj=1,V_yj=1,V_zj=1）和空气阻力系数k有关；随后将前a帧的目标抛体的速度（V_xj=1,V_yj=1,V_zj=1）、（V_xj=2,V_yj=2,V_zj=2）、（V_xj=3,V_yj=3,V_zj=3）…（V_{xj=a,Vyj=a,Vzj=a}）代入公式（18）、（20）、（22），计算得到前a帧帧间位置变化的预测值（∆x_j=1,∆y_j=1, ∆z_j=1）、（∆x_j=2, ∆y_j=2, ∆z_j=2）…（∆x_j=a-1, ∆y_j=a-1, ∆z_j=a-1），均与初速度（V_xj=1,V_yj=1,V_zj=1）和空气阻力系数k有关；2）通过步骤三得到的前a帧精定位结果（x_wj=1，y_wj=1，z_wj=1）、（x_wj=2，y_wj=2，z_wj=2）…（x_wj=a，y_wj=a，z_wj=a）计算得到前a帧帧间位置变化的真值

、

…

；3）对于计算得到的前a帧帧间位置变化的预测值和真值，通过最小二乘法迭代优化空气阻力系数k以及目标抛体在精定位初始帧时x，y，z三轴的初速度（V_xj=1,V_yj=1,V_zj=1），最终得到空气阻力系数k以及初速度（V_xj=1,V_yj=1,V_zj=1）的拟合值；4）得到拟合值后，再代入公式（14）-（22），即得到a帧后任一时刻目标抛体在世界坐标系下的估计坐标，随后得到目标抛体的运动轨迹的精预测轨迹曲线，对目标抛体实现精预测。

（4.2）基于目标抛体的运动轨迹的精预测曲线，计算目标抛体的捕获点的精准坐标，随后保持底座不移动，控制机械臂手臂移动到捕获点，准备捕获操作。

捕获点精准坐标的计算与机械臂响应速度和规划时间有关，对于响应和规划速度较快的机械臂选择a帧后较短时间处的精预测位置作为捕获点，反之则选择a帧后较长时间处的精预测位置进行捕获。本实施例中选取a帧后0.7s处（精预测轨迹中a+21帧处）的位置作为捕获点。

步骤五：基于RGBD深度相机计算捕获点处目标抛体的6D姿态并实施捕获；

步骤五具体通过以下步骤来实现的：

（5.1）利用姿态估计算法求得目标抛体在捕获点前b帧处目标抛体的6D姿态：首先将捕获点前b帧处目标抛体的RGBD图像输入训练好的分类深度神经网络中，得到相对应的目标抛体的6D姿态的快速匹配结果，然后使用ICP算法得到较为精确的目标抛体的6D姿态；

b帧的选择与步骤（5.1）中姿态估计算法速度以及机械手捕获精准度有关，一般来说，算法速度快或捕获精度低，则应选择较小的b值以确保捕获成功率，反之则可选用较大的b值。本实施例中b选为5，即为捕获点前5帧 /精预测轨迹中a+16帧处物体的位姿作为捕获姿态。

（5.2）将捕获点前b帧处目标抛体的6D姿态作为捕获姿态，结合目标抛体的三维模型生成抓取位置，使用带RGBD深度相机的移动机械臂中的机械手对目标抛体进行捕获。

本实施例为对发明内容的进一步解释以及在实际场景下具体应用方法的说明，核心体现了RGB高帧率相机与RGBD深度相机感知信息的融合应用，以及基于卡尔曼滤波的快速轨迹粗拟合以及基于物理学建模的高精度轨迹拟合与预测的创新点。注意本实施例并非对工程实现方法的限定，基于不同的实际场景，本领域的技术人员可围绕上述两核心创新点进行工程方案设计，实现低成本，高精度和高稳定性的非合作抛体轨迹预测与抓取任务。

参见图3，本发明实施例提供的一种非合作抛体的轨迹预测与捕获装置，包括一个或多个处理器，用于实现上述实施例中的非合作抛体的轨迹预测与捕获方法。

本发明非合作抛体的轨迹预测与捕获装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图3所示，为本发明非合作抛体的轨迹预测与捕获装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图3所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。

上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的非合作抛体的轨迹预测与捕获方法。

所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是任意具备数据处理能力的设备的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡（Smart Media Card，SMC）、SD卡、闪存卡（Flash Card）等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (8)

1.一种非合作抛体的轨迹预测与捕获方法，其特征在于，包括以下步骤：

步骤一：基于高帧率RGB相机实现对目标抛体的粗定位；将高帧率RGB相机固定于实验场景，调整拍摄角度，使高帧率RGB相机拍摄到目标抛体空间运动全过程信息，进而获得目标抛体的RGB图像；

步骤二：基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测，得到粗预测结果；

步骤三：基于粗预测结果控制带RGBD深度相机的移动机械臂移动到最佳视觉观测点后，基于RGBD深度相机实现对目标抛体的精定位；

步骤四：基于多帧精定位结果进行目标抛体运动轨迹建模，并据此获得目标抛体的捕获点的精准坐标；

步骤五：基于RGBD深度相机计算捕获点处目标抛体的6D姿态并实施捕获；使用带RGBD深度相机的移动机械臂中的机械手对目标抛体进行捕获。

2.根据权利要求1所述的一种非合作抛体的轨迹预测与捕获方法，其特征在于，所述步骤一具体为：

将高帧率RGB相机固定于实验场景，调整拍摄角度，使高帧率RGB相机拍摄到目标抛体空间运动全过程信息，进而获得目标抛体的RGB图像；再使用张氏标定法对高帧率RGB相机进行内外参标定；随后对于获得的RGB图像，通过基于灰度差异的物体边缘检测方法获得目标抛体的检测框；通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标以及检测框的宽和高；结合高帧率RGB相机的内参标定结果得到目标抛体检测框中心点在高帧率RGB相机坐标系下的坐标；再结合高帧率RGB相机的外参标定结果，将检测框中心点在高帧率RGB相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于高帧率RGB相机实现目标抛体的粗定位，所述高帧率RGB相机的帧率大于100。

3.根据权利要求1所述的一种非合作抛体的轨迹预测与捕获方法，其特征在于，所述步骤二具体为：

（2.1）使用目标抛体被抛出时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高对卡尔曼滤波跟踪器进行初始化；

（2.2）使用目标抛体运动初期p帧的在图像坐标系下检测框中心点的坐标以及检测框宽和高作为真值对初始化后的卡尔曼滤波跟踪器进行校正；

（2.3）使用校正后的尔曼滤波跟踪器预测生成p帧后目标抛体在图像坐标系下的运动轨迹，包括p帧后任一时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高，随后结合高帧率RGB相机的内参标定结果得到在高帧率RGB相机坐标系下检测框中心点的坐标，再结合高帧率RGB相机的外参标定结果得到在世界坐标系下检测框中心点的坐标；在世界坐标系下检测框中心点的坐标作为目标抛体运动轨迹的粗预测结果，由此基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测。

4.根据权利要求3所述的一种非合作抛体的轨迹预测与捕获方法，其特征在于，所述步骤三具体为：

（3.1）在目标抛体捕获任务开始前，使用张氏标定法对带RGBD深度相机的移动机械臂进行离线标定，获得RGBD深度相机的内外参；所述带RGBD深度相机的移动机械臂包括底座、手臂、机械手和RGBD深度相机；标定结束后，得到RGBD深度相机在世界坐标系下的初始坐标；

（3.2）根据步骤（2.3）得到的目标抛体运动轨迹的粗预测结果，得到目标抛体在世界坐标系下的落地位置，从而计算得到最佳视觉观测点并触发带RGBD深度相机的移动机械臂中的底座快速响应到达最佳视觉观测点；所述最佳视觉观测点的位置的选取基于落地位置和带RGBD摄像头的移动机械臂的响应速度决定；在带RGBD深度相机的移动机械臂中的底座移动到最佳视觉观测点的过程中，记录底座移动过程中旋转和平移变换；并根据底座移动过程中旋转矩阵和平移向量计算得到最佳视觉观测点处RGBD深度相机在世界坐标系下的坐标；

（3.3）待底座移动到最佳视觉观测点后，通过RGBD深度相机获得场景的RGBD图像；随后待目标抛体进入RGBD深度相机的观测区域后，获得目标抛体的RGBD图像；对于获得的RGBD图像，通过基于深度学习的神经网络图像处理方法获得目标抛体的检测框；通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标，融合RGBD信息获得RGBD图像中目标抛体检测框中心点到RGBD深度相机平面的距离；结合RGBD深度相机的内参标定结果和检测框中心点到RGBD深度相机平面的距离得到目标抛体检测框中心点在RGBD深度相机坐标系下的坐标；再结合RGBD深度相机的外参标定结果，将检测框中心点在RGBD深度相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于RGBD深度相机实现对目标抛体的精定位。

5.根据权利要求1所述的一种非合作抛体的轨迹预测与捕获方法，其特征在于，所述步骤四具体为：

（4.1）对于目标抛体的运动，考虑其受到重力以及空气阻力的影响，将其分解为x，y，z三轴的变加速度运动，进而对三轴运动分别建模，模型包含目标抛体在精定位初始帧时x，y，z三轴的初速度、空气阻力系数这些未知常量，使用多帧精定位结果通过最小二乘法拟合这些未知常量，从而获得目标抛体在空间的高精度预测轨迹；

（4.2）基于目标抛体的运动轨迹的精预测曲线，计算目标抛体的捕获点的精准坐标，随后保持底座不移动，控制机械臂手臂移动到捕获点，准备捕获操作。

6.根据权利要求1所述的一种非合作抛体的轨迹预测与捕获方法，其特征在于，所述步骤五具体为：

（5.1）利用姿态估计算法求得目标抛体在捕获点前b帧处目标抛体的6D姿态：首先将捕获点前b帧处目标抛体的RGBD图像输入训练好的分类深度神经网络中，得到相对应的目标抛体的6D姿态的快速匹配结果，然后使用ICP算法得到较为精确的目标抛体的6D姿态；b帧的选择与姿态估计算法速度以及机械手捕获精准度有关；

（5.2）将捕获点前b帧处目标抛体的6D姿态作为捕获姿态，结合目标抛体的三维模型生成抓取位置，使用带RGBD深度相机的移动机械臂中的机械手对目标抛体进行捕获。

7.一种非合作抛体的轨迹预测与捕获装置，其特征在于，包括一个或多个处理器，用于实现权利要求1-6中任一项所述的非合作抛体的轨迹预测与捕获方法。

8.一种计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时，用于实现权利要求1-6中任一项所述的非合作抛体的轨迹预测与捕获方法。

Images