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

权利要求书

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

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

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

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

所述步骤1中，基于视觉的位姿估计方法为三维模板匹配方法，融合了RGB信息，深度信息及点云信息，具体包括：

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

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

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

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

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

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

所述步骤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：使用最佳的动力学参数预测目标的位姿；

所述步骤3中，轨迹规划基于模仿学习，具体包括：

步骤3.1：在现实场景下建立漂浮物体慢速运动的场景，利用动作捕捉系统采集人抓捕目标的轨迹，轨迹包含了手末端位置和姿态的时间序列，建立示教数据集其中N为轨迹数量，D＝7为每个点的纬度，包括T为时间序列；

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

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

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

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

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

的计算方式为：

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

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