CN111325768A - 一种基于3d视觉和模仿学习的自由漂浮目标抓捕方法 - Google Patents

Abstract

本发明公开一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法。主要包含三个部分：首先，使用深度相机实时获取目标抓捕点的位置和姿态；其次，根据历史数据，使用卡尔曼滤波估计运动状态，根据运动信息预测自由漂浮未来一段时间内的轨迹；最后：确定抓捕时机及抓捕点在这一时刻的位姿，采集人的抓捕数据，建立技能模型，将其迁移到机器人上进行抓捕。本发明基于视觉、模仿学习完成了过程中的位姿估计，轨迹预测及机械臂轨迹规划技术，实现自由漂浮目标的自主抓捕，可以很好的满足抓捕需求。

Description

一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法

技术领域

本发明涉及空间机器人领域，具体涉及一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法。

背景技术

人类开发和利用外太空的步伐不断加快。全球每年发射卫星80～130颗，其中10％左右的卫星因故障宣告失败，这些碎片占用了宝贵的轨道资源；部分航天器发射入轨后可能出现零部件失效、轨道偏离等问题，或者在完成预定任务、燃料耗尽时其主要零部件仍能正常工作，对其他在轨航天器构成了严重的安全；利用空间机器人完成轨道碎片清理、在轨维修、维护升级、空间站的组装等任务对提高太空轨道利用率，降低成本、挽回经济损失，延长航天器的寿命，提升航天器的在轨性能，提高经济效益有重要的作用，而对漂浮目标进行抓捕是其中一项关键技术。

目前在轨机器人抓捕大多为遥操作或航天员在轨辅助机器人，自主性不高，并且控制信号的时延也会带来不稳定性，导致精度差，成本高。

自主太空机器人集成视觉、力等多种传感器，可以代替航天员或者地面操作人员控制进行抓捕作业，因此，各个航天大国都在对空间机器人在轨抓捕关键技术进行研究和演示验证，但目前研究成果大多处于概念设计阶段，距离实际应用还有很大差距。

发明内容

鉴于以上现有技术的不足，本发明的目的在于提供一种基于3D视觉和模仿学习的自由漂浮目标抓捕技术，该技术集成了目标位姿估计，轨迹预测，机械臂轨迹规划等技术，保证精度和抓捕速度，能够实现目标的自主抓捕。

为了解决上述问题，本发明采用如下技术方案：

一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，其特征在于，包括以下步骤：

步骤1：基于视觉的位姿估计，实时反馈目标的位置和姿态；

步骤2：轨迹预测，根据历史一段时间位置和姿态轨迹，动态预测后一段时间的位置和姿态轨迹；

步骤3：基于模仿学习的轨迹规划，采集人的抓捕数据，建立技能模型，将其迁移到机器人上，根据步骤2所预测的轨迹，确定合适的抓捕时机进行抓捕。

在上述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，所述步骤1中，基于视觉的位姿估计方法为三维模板匹配方法，融合了RGB信息，深度信息及点云信息，具体包括：

步骤1.1：建立模板库，利用OpenGL，直接采集漂浮物体抓取点三维模型不同视点不同距离的模板，模板特征为RGB图像边缘梯度向量与深度图像法向量；

其中，梯度向量计算公式为：

其中，x表示像素位置，R,G,B分别表示图像的三个通道，法向量的计算方法为通过最小二乘法求取最优的深度梯度，通过深度梯度求取法向量；

步骤1.2：计算自由漂浮目标初始位姿，根据RGB与深度信息，进行窗口搜索，得到目标的初始位姿，使用如下公式进行相似度计算：

步骤1.3：根据初始位姿进行ICP点云匹配，得到目标的精确位姿，使用如下公式进行计算：

其中p_i为目标点云中的点，q_i为源点云中与之对应的点，R为旋转变换，t为平移量。

在上述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，所述步骤2中，轨迹预测基于欧拉动力学，具体包括：

步骤2.1：根据所测量位姿，找到一组合适的动力学参数来解析表示目标的位姿运动；

目标的动力学模型为：

其中I_xx,I_yy,I_zz分别为刚体中心主惯量矩，ω_x,ω_y,ω_z为刚体角速度在中心惯量主轴坐标系中的投影分量，ψ_w,θ_w,

为刚体相对于中心惯量主轴坐标系转过的角度；

令E为刚体的动能，L为刚体的角动量，刚体运动可以看作潘索椭球

令

目标的动力学模型可表示为：

J_x,J_y,α为所需要估计的参数；

步骤2.2：求解最佳的动力学参数，使得这些动力学参数预测的位姿与观测结果吻合最好；

根据根据约束条件求解如下优化目标得到最佳动力学参数：

其中ω(J_x,J_y,J_z,α,t_i)为t_i时刻预测值，ω_l(t_i)为t_i时刻观测值；

步骤2.3：使用最佳的动力学参数预测目标的位姿。

在上述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，所述步骤3中，轨迹规划基于模仿学习，具体包括：

步骤3.1：在现实场景下建立漂浮物体慢速运动的场景，利用动作捕捉***采集人抓捕目标的轨迹，轨迹包含了手末端位置和姿态的时间序列，建立示教数据集

其中N为轨迹数量，D＝7为每个点的纬度，包括

T为时间序列；

步骤3.2：建立模仿学习的模型，利用采集数据对模型进行训练，将人抓捕目标的技能迁移到机器人上，可以生成对应目标位姿的抓捕轨迹。

在上述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，所述步骤3.2模仿学习的过程按照以下步骤进行：

步骤A：模型训练，将位姿看作

6个独立的变量，对每一个维度分别训练，对每一条轨迹分别变换到起点和终点坐标系进行GMM编码，求去轨迹点的概率分布模型；

步骤B：将所得GMM模型分别进行GMR回归，将时间作为输入，轨迹的6维位姿坐标作为输出，对应的条件概率分布为：

其中

分别为ξ_t,μ_i,Σ_i切分成输入输出而成

的计算方式为：

步骤C：抓取轨迹生成，将步骤B所得GMR模型起点和终点坐标系分别变换到机械臂现在的坐标系与目标坐标系，把对应的高斯分布进行乘积，计算公式为：

其中

对应新的起点和终点坐标系，

分别对应训练得到的起点和终点坐标系下t时刻轨迹点的均值和协方差矩阵，μ_t,Σ_t分别为新生成轨迹点的均值和协方差矩阵。

本发明由于采用以上技术方案，具有以下优点：

1.位姿估计中充分利用了彩色信息与深度信息，提高了识别的速度与准确性；

2.融合了视觉，控制，机器学习等技术，具有较高的自主性；

3.将人的抓捕技巧迁移到机器人上，实现简单，速度快。

附图说明

图1是本发明抓捕场景示意图。

图2是本发明位姿估计与轨迹预测流程图。

图3是本发明模仿学习流程图。

图4是本发明总体抓捕策略流程图。

具体实施方式

下面结合附图来对本发明提供的协作搬运***进行详细说明，所述是对本发明的解释而不是限定。

首先介绍本发明的方法原理，包括以下步骤：

步骤一：基于视觉的位姿估计，实时反馈目标的位置和姿态；

步骤二：轨迹预测，根据历史一段时间位置和姿态轨迹，动态预测后一段时间的位置和姿态轨迹；

步骤三：基于模仿学习的轨迹规划，采集人的抓捕数据，建立技能模型，将其迁移到机器人上，根据步骤二所预测的轨迹，确定合适的抓捕时机进行抓捕。

可选的，抓捕***基于ROS(Robot Operating System)平台，能对慢速运动的自由漂浮目标实现位姿估计、轨迹预测、轨迹规划，最后实现精准抓捕。

可选的，所述基于视觉的位姿估计方法为三维模板匹配方法，融合了RGB信息，深度信息及点云信息，按照以下步骤进行：

步骤一：建立模板库，利用OpenGL，直接采集漂浮物体抓取点三维模型不同视点不同距离的模板，模板特征为RGB图像边缘梯度向量与深度图像法向量；

其中，梯度向量计算公式为：

其中，x表示像素位置，R,G,B分别表示图像的三个通道

法向量的计算方法为通过最小二乘法求取最优的深度梯度，通过深度梯度求取法向量。

步骤二：计算自由漂浮目标初始位姿，根据RGB与深度信息，进行窗口搜索，得到目标的初始位姿，使用如下公式进行相似度计算：

步骤三：根据初始位姿进行ICP点云匹配，得到目标的精确位姿，使用如下公式进行计算：

其中p_i为目标点云中的点，q_i为源点云中与之对应的点，R为旋转变换，t为平移量。

可选的：所述轨迹预测方法为基于欧拉动力学的方法，按照以下步骤进行：

步骤一：根据所测量位姿，找到一组合适的动力学参数来解析表示目标的位姿运动；

目标的动力学模型为：

其中I_xx,I_yy,I_zz分别为刚体中心主惯量矩，ω_x,ω_y,ω_z为刚体角速度在中心惯量主轴坐标系中的投影分量，ψ_w,θ_w,

为刚体相对于中心惯量主轴坐标系转过的角度。

令E为刚体的动能，L为刚体的角动量，刚体运动可以看作潘索椭球

令

目标的动力学模型可表示为：

J_x,J_y,J_z,α为所需要估计的参数。

步骤二：求解最佳的动力学参数，使得这些动力学参数预测的位姿与观测结果吻合最好；

根据根据约束条件求解如下优化目标得到最佳动力学参数：

其中ω(J_x,J_y,J_z,α,t_i)为t_i时刻预测值，ω_l(t_i)为t_i时刻观测值。

步骤三：使用最佳的动力学参数预测目标的位姿。

可选的：所述轨迹规划方法为基于模仿学习的方法，按照以下步骤进行：

步骤一：在现实场景下建立漂浮物体慢速运动的场景，利用动作捕捉***采集人抓捕目标的轨迹，轨迹包含了手末端位置和姿态的时间序列，建立示教数据集

其中N为轨迹数量，D＝7为每个点的纬度，包括

T为时间序列；

步骤二：建立模仿学习的模型，利用采集数据对模型进行训练，将人抓捕目标的技能迁移到机器人上，可以生成对应目标位姿的抓捕轨迹。

可选的，所述步骤二模仿学习的过程按照以下步骤进行：

步骤一：模型训练，将位姿看作

6个独立的变量，对每一个维度分别训练，对每一条轨迹分别变换到起点和终点坐标系进行GMM编码，求去轨迹点的概率分布模型。

步骤二：将所得GMM模型分别进行GMR回归，将时间作为输入，轨迹的6维位姿坐标作为输出，对应的条件概率分布为：

其中

分别为ξ_t,μ_i,Σ_i切分成输入输出而成

的计算方式为：

步骤三：抓取轨迹生成，将步骤二所得GMR模型起点和终点坐标系分别变换到机械臂现在的坐标系与目标坐标系，把对应的高斯分布进行乘积，计算公式为：

其中

对应新的起点和终点坐标系，

分别对应训练得到的起点和终点坐标系下t时刻轨迹点的均值和协方差矩阵，μ_t,Σ_t分别为新生成轨迹点的均值和协方差矩阵。

下面结合附图对本发明的发明原理进行具体说明

本发明的实施过程如图4所示，首先基于ROS搭建模拟抓捕场景，包括深度相机，可以获取RGB图像，深度图像和点云，两个六轴机械臂，其中一个机械臂用来拖动模拟漂浮物体匀速运动，另一个机械臂用来抓捕模拟漂浮目标。

首先建立模板库，利用OpenGL，直接采集漂浮物体抓取点三维模型不同视点不同距离的模板，模板特征为RGB图像边缘梯度向量与深度图像法向量；利用RGB边缘梯度特征和深度图法向量特征进行模板匹配得到初始位姿，利用点云信息进行ICP位姿修正，实时获取目标的位姿信息，该方法充分利用了深度相机的彩色信息，深度信息及点云信息，首先利用彩色和深度信息的特征进行模板匹配，找到初始位姿，再利用点云信息进行位姿修正，匹配速度快，精度高，能够在目标慢速运动情况下进行实时的位姿估计。

其次利用历史一段时间的位姿进行轨迹预测。根据欧拉方程建立自由漂浮目标的动力学模型，找到一组合适的动力学参数来解析表示目标的位姿运动。根据历史数据对参数进行优化，使动力学参数预测的位姿与观测位姿吻合度最高。这组参数即为最优参数。使用此参数计算目标的角速度，线速度等进行长期预测。此方法实现简单，精度高，能够实现目标的长期稳定预测。

最后进行轨迹规划，根据预测结果和机械臂末端可达到的范围判断合适的抓捕时机，使用模仿学习的方法进行轨迹规划。建立人进行抓捕的技能模型，使用机械臂拖动目标模拟空间自由漂浮物体，使用动作捕捉***采集人手进行抓捕的数据，利用采集的数据对模型进行训练，将人的抓捕技能迁移到机械臂上，在机械臂得到合适的抓捕时机时，快速规划，完成目标的抓捕。此方法基于机器学习，不需要复杂的建模工作，规划速度快，并且能够根据位姿的变化动态的规划轨迹。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (5)

1.一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，其特征在于，包括以下步骤：

步骤1：基于视觉的位姿估计，实时反馈目标的位置和姿态；

步骤2：轨迹预测，根据历史一段时间位置和姿态轨迹，动态预测后一段时间的位置和姿态轨迹；

步骤3：基于模仿学习的轨迹规划，采集人的抓捕数据，建立技能模型，将其迁移到机器人上，根据步骤2所预测的轨迹，确定合适的抓捕时机进行抓捕。

2.根据权利要求1所述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，其特征在于：所述步骤1中，基于视觉的位姿估计方法为三维模板匹配方法，融合了RGB信息，深度信息及点云信息，具体包括：

步骤1.1：建立模板库，利用OpenGL，直接采集漂浮物体抓取点三维模型不同视点不同距离的模板，模板特征为RGB图像边缘梯度向量与深度图像法向量；

其中，梯度向量计算公式为：

其中，x表示像素位置，R,G,B分别表示图像的三个通道，法向量的计算方法为通过最小二乘法求取最优的深度梯度，通过深度梯度求取法向量；

步骤1.2：计算自由漂浮目标初始位姿，根据RGB与深度信息，进行窗口搜索，得到目标的初始位姿，使用如下公式进行相似度计算：

步骤1.3：根据初始位姿进行ICP点云匹配，得到目标的精确位姿，使用如下公式进行计算：

其中p_i为目标点云中的点，q_i为源点云中与之对应的点，R为旋转变换，t为平移量。

3.根据权利要求1所述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，其特征在于：所述步骤2中，轨迹预测基于欧拉动力学，具体包括：

步骤2.1：根据所测量位姿，找到一组合适的动力学参数来解析表示目标的位姿运动；

目标的动力学模型为：

其中I_xx,I_yy,I_zz分别为刚体中心主惯量矩，ω_x,ω_y,ω_z为刚体角速度在中心惯量主轴坐标系中的投影分量，ψ_w,θ_w,

为刚体相对于中心惯量主轴坐标系转过的角度；

令E为刚体的动能，L为刚体的角动量，刚体运动可以看作潘索椭球

令

目标的动力学模型可表示为：

J_x,J_y,J_z,α为所需要估计的参数；

步骤2.2：求解最佳的动力学参数，使得这些动力学参数预测的位姿与观测结果吻合最好；

根据根据约束条件求解如下优化目标得到最佳动力学参数：

其中ω(J_x,J_y,J_z,α,t_i)为t_i时刻预测值，ω_l(t_i)为t_i时刻观测值；

步骤2.3：使用最佳的动力学参数预测目标的位姿。

4.根据权利要求1所述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，其特征在于：所述步骤3中，轨迹规划基于模仿学习，具体包括：

步骤3.1：在现实场景下建立漂浮物体慢速运动的场景，利用动作捕捉***采集人抓捕目标的轨迹，轨迹包含了手末端位置和姿态的时间序列，建立示教数据集

其中N为轨迹数量，D＝7为每个点的纬度，包括

T为时间序列；

步骤3.2：建立模仿学习的模型，利用采集数据对模型进行训练，将人抓捕目标的技能迁移到机器人上，可以生成对应目标位姿的抓捕轨迹。

5.根据权利要求4所述的一种基于3D视觉和模仿学习的自由漂浮目标抓捕方法，其特征在于：所述步骤3.2模仿学习的过程按照以下步骤进行：

步骤A：模型训练，将位姿看作

6个独立的变量，对每一个维度分别训练，对每一条轨迹分别变换到起点和终点坐标系进行GMM编码，求去轨迹点的概率分布模型；

步骤B：将所得GMM模型分别进行GMR回归，将时间作为输入，轨迹的6维位姿坐标作为输出，对应的条件概率分布为：

其中

分别为ξ_t,μ_i,Σ_i切分成输入输出而成

的计算方式为：

步骤C：抓取轨迹生成，将步骤B所得GMR模型起点和终点坐标系分别变换到机械臂现在的坐标系与目标坐标系，把对应的高斯分布进行乘积，计算公式为：

其中

对应新的起点和终点坐标系，

分别对应训练得到的起点和终点坐标系下t时刻轨迹点的均值和协方差矩阵，μ_t,Σ_t分别为新生成轨迹点的均值和协方差矩阵。

Images