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

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

【技术实现步骤摘要】
一种非合作抛体的轨迹预测与捕获方法和装置


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

技术介绍

[0002]空间非合作抛体的轨迹预测与捕获方法是机器人的重要技术之一，对机器人落地应用有着重大意义，特别是球类机器人，通过此项技术，机器人可以快速捕捉空间运动物体，实现避障，抓取，互动等一系列操作。
[0003]目前，对于非合作抛体的轨迹预测与捕获主要基于动态捕捉系统。而该套系统具有两个主要弊端：一方面，动捕系统价格昂贵，且对试验场景的搭建有较高的要求。另一方面，动捕系统需要在抛体上安装传感器，这些都在很大程度上限制了该项技术广泛应用。

技术实现思路

[0004]本专利技术的目的在于针对现有技术的不足，提供了一种非合作抛体的轨迹预测与捕获方法和装置。该方法安装标定方便，价格低，精度高，稳定性好，对智能机器人的落地应用具有重要意义。
[0005]本专利技术的目的是通过以下技术方案来实现的：一种非合作抛体的轨迹预测与捕获方法，包括以下步骤：步骤一：基于高帧率RGB相机实现对目标抛体的粗定位；步骤二：基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测，得到粗预测结果；步骤三：基于粗预测结果控制带RGBD深度相机的移动机械臂移动到最佳视觉观测点后，基于RGBD深度相机实现对目标抛体的精定位；步骤四：基于多帧精定位结果进行目标抛体运动轨迹建模，并据此获得目标抛体的捕获点的精准坐标；步骤五：基于RGBD深度相机计算捕获点处目标抛体的6D姿态并实施捕获。
[0006]进一步地，所述步骤一具体为：将高帧率RGB相机固定于实验场景，调整拍摄角度，使高帧率RGB相机拍摄到目标抛体空间运动全过程信息，进而获得目标抛体的RGB图像；再使用张氏标定法对高帧率RGB相机进行内外参标定；随后对于获得的RGB图像，通过基于灰度差异的物体边缘检测方法获得目标抛体的检测框；通过目标抛体的检测框得到在图像坐标系下目标抛体检测框中心点的坐标以及检测框的宽和高；结合高帧率RGB相机的内参标定结果得到目标抛体检测框中心点在高帧率RGB相机坐标系下的坐标；再结合高帧率RGB相机的外参标定结果，将检测框中心点在高帧率RGB相机坐标系下的坐标转换到世界坐标系下检测框中心点的坐标，由此基于高帧率RGB相机实现目标抛体的粗定位，所述高帧率RGB相机的帧率大于100。
[0007]进一步地，所述步骤二具体为：
（2.1）使用目标抛体被抛出时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高对卡尔曼滤波跟踪器进行初始化；（2.2）使用目标抛体运动初期p帧的在图像坐标系下检测框中心点的坐标以及检测框宽和高作为真值对初始化后的卡尔曼滤波跟踪器进行校正；（2.3）使用校正后的尔曼滤波跟踪器预测生成p帧后目标抛体在图像坐标系下的运动轨迹，包括p帧后任一时刻在图像坐标系下检测框中心点的坐标以及检测框宽和高，随后结合高帧率RGB相机的内参标定结果得到在高帧率RGB相机坐标系下检测框中心点的坐标，再结合高帧率RGB相机的外参标定结果得到在世界坐标系下检测框中心点的坐标；在世界坐标系下检测框中心点的坐标作为目标抛体运动轨迹的粗预测结果，由此基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测。
[0008]进一步地，所述步骤三具体为：（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深度相机实现对目标抛体的精定位。
[0009]进一步地，所述步骤四具体为：（4.1）对于目标抛体的运动，考虑其受到重力以及空气阻力的影响，将其分解为x，y，z三轴的变加速度运动，进而对三轴运动分别建模，模型包含目标抛体在精定位初始帧时x，y，z三轴的初速度、空气阻力系数这些未知常量，使用多帧精定位结果通过最小二乘法拟合这些未知常量，从而获得目标抛体在空间的高精度预测轨迹；（4.2）基于目标抛体的运动轨迹的精预测曲线，计算目标抛体的捕获点的精准坐标，随后保持底座不移动，控制机械臂手臂移动到捕获点，准备捕获操作。
[0010]进一步地，所述步骤五具体为：
（5.1）利用姿态估计算法求得目标抛体在捕获点前b帧处目标抛体的6D姿态：首先将捕获点前b帧处目标抛体的RGBD图像输入训练好的分类深度神经网络中，得到相对应的目标抛体的6D姿态的快速匹配结果，然后使用ICP算法得到较为精确的目标抛体的6D姿态；b帧的选择与姿态估计算法速度以及机械手捕获精准度有关；（5.2）将捕获点前b帧处目标抛体的6D姿态作为捕获姿态，结合目标抛体的三维模型生成抓取位置，使用带RGBD深度相机的移动机械臂中的机械手对目标抛体进行捕获。
[0011]本专利技术还提供了一种非合作抛体的轨迹预测与捕获装置，包括一个或多个处理器，用于实现上述非合作抛体的轨迹预测与捕获方法。
[0012]本专利技术还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，用于实现上述非合作抛体的轨迹预测与捕获方法。
[0013]本专利技术的有益效果是：本专利技术提出了一种非合作抛体的轨迹预测与捕获方法和装置，相较现有方法具有低成本，高精度，高稳定性的优点。其核心创新点为高效利用了高帧率RGB相机及RGBD深度相机信息特点，基于不同功能要求实现了抛体的粗定位与精定位。此外，基于粗定位和精定位的结果，本专利技术进一步提出了不同的轨迹粗拟合和精拟合的算法，前者拟合速度快，满足机械臂快速响应控制要求，而本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种非合作抛体的轨迹预测与捕获方法，其特征在于，包括以下步骤：步骤一：基于高帧率RGB相机实现对目标抛体的粗定位；步骤二：基于校正后的卡尔曼滤波跟踪器实现对目标抛体运动轨迹的粗预测，得到粗预测结果；步骤三：基于粗预测结果控制带RGBD深度相机的移动机械臂移动到最佳视觉观测点后，基于RGBD深度相机实现对目标抛体的精定位；步骤四：基于多帧精定位结果进行目标抛体运动轨迹建模，并据此获得目标抛体的捕获点的精准坐标；步骤五：基于RGBD深度相机计算捕获点处目标抛体的6D姿态并实施捕获。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深度...

【专利技术属性】
技术研发人员：孟启炜，朱世强，寄珊珊，袁海辉，李特，顾建军，
申请(专利权)人：之江实验室，
类型：发明
国别省市：