基于抓取成本预测和L加速梯度重启优化的抓取规划方法

本发明属于抓取规划领域，具体涉及基于抓取成本预测和l加速梯度重启优化的抓取规划方法。

背景技术：

1、机器人操作在工业应用中具有巨大的潜力。机械臂运输货物要求动作平稳、安全，接近并牢牢抓住物体。传统的手臂系统的接触级抓取规划和运动规划，都是以强解耦的方式来解决的。首先通过抓取规划，生成可行的机械臂末端抓取点集，然后选择一个合适的抓取点进行轨迹规划，最终生成抓取规划路径。但是生成的抓取点集是有限的，无法包含所有可行的抓取点集，因此可能会忽略一些良好的抓取点。同时由于运动学限制或与环境的冲突，抓取规划器产生的结果不一定能够通过轨迹规划生成可行的轨迹，或者可能需要更多的时间才能生成可行轨迹，从而产生更大的时间成本。因此研究如何将抓取规划与运动规划耦合来求解成本低、成功率高的抓取规划轨迹是一个重要的研究课题。

2、神经隐式函数(nifs)是一种神经网络，它接受查询q和可选的上下文嵌入z来输出一个标量值，该值表示与底层分布的关系。与显式函数不同，nifs不受分辨率的限制，因为它们可以在任何查询点预测一个值，并且还可以更好地表示不相交的底层分布。加速梯度下降算法是改进后的梯度下降算法，通过增加动量项来加快收敛速度，重启算法能够减少加速梯度下降算法对于参数选择的依赖。

技术实现思路

1、为解决现有技术的不足，避免抓取规划和运动规划强解耦导致的最终轨迹不可行或者成本高的目的，本发明采用如下的技术方案：

2、基于抓取成本预测和l加速梯度重启优化的抓取规划方法，包括如下步骤：

3、步骤1：构建预测机械臂末端位姿与最近邻可行抓取位姿距离的神经抓取距离场(ngdf)；

4、步骤2：构建包含抓取成本、高斯先验成本、障碍成本的综合优化目标函数；

5、步骤3：设计基于l加速梯度下降重启的轨迹规划算法，求解最优抓取轨迹。

6、进一步地，所述步骤3包括如下步骤：

7、步骤3.1：输入初始轨迹θ0、初始协方差k0，目标泛函f(θ0(t))和对应导数初始化权衡系数η，泛函导数的收敛阈值ftol和障碍项收敛阈值obstol；

8、步骤3.2：初始化李普希兹常量lf；

9、步骤3.3：初始化轨迹θ0,单步优化率可接受范围下限clow和上限cup；初始化迭代次数k＝1，更新系数αk,βk,λk，计算

10、步骤3.4：计算真实的改进值trueimprove和模型预测的改进值modelimprove，若真实的改进值trueimprove除模型预测的改进值modelimprove的结果在clow～cup范围内，则更新李普希兹常量lf并跳转到步骤3.3，将θ0,均初始化为θk；否则，更新李普希兹常量lf；

11、步骤3.5：计算泛函梯度若的二阶范数小于ftol，则跳转至步骤3.7；

12、步骤3.6：更新更新k＝k+1，返回步骤3.4，直至达到迭代次数条件；

13、步骤3.7：计算所述障碍成本中的避障代价若小于障碍项收敛阈值obstol，则算法结束，输出无碰撞轨迹θk，否则，调整权衡系数η＝η*1.1，并返回步骤3.2。

14、进一步地，所述步骤3.2中，李普希兹常量lf的估计公式如下：

15、

16、

17、其中，δ表示θ与之间的距离。

18、进一步地，所述步骤3.3中，系数αk、βk、λk的更新公式如下：

19、

20、

21、λk＝βk。

22、进一步地，所述步骤3.3中，θ0,均初始化为上一次迭代结束的轨迹，clow和cup表达式如下：

23、

24、

25、其中，θj,分别为上一次李普希兹迭代结束的轨迹和对应梯度。

26、进一步地，所述步骤3.4中，真实的改进值trueimprove的计算公式如下：

27、

28、进一步地，所述步骤3.4中，模型预测的改进值modelimprove的计算公式如下：

29、

30、其中，clow-check表示比较下限时，采用其前述公式，cup-check表示比较上限时采用其前述公式。

31、进一步地，所述步骤3.4中，结果在clow～cup范围内时，李普希兹常量lf的更新公式如下：

32、

33、

34、其中，

35、结果不在clow～cup范围内时，李普希兹常量lf的更新公式如下：

36、

37、进一步地，所述步骤1包括如下步骤：

38、步骤1.1：构建训练神经抓取距离场ngdf网络数据集；ngdf训练包含点云、有效抓取位姿和查询位姿的数据集；在目标网格质心的一定半径内采样多个随机抓取位姿，包含三维空间位置以及三维旋转姿态，将不可行抓取位姿去除后，对每个采样位姿寻找有效抓取集中最近的抓取位姿，从而形成点云-有效抓取位姿-查询位姿数据集；

39、步骤1.2：构建神经抓取距离场ngdf网络；输入的点云使用编码器网络转换为点云向量z，ngdf的输入是该点云向量z与查询位姿p(位置和姿态四元数)的连接，输出是查询位姿和最近邻可行抓取位姿的距离，通过固定在末端的一系列点的距离和计算；ngdf网络基于deepsdf网络，由8个多层感知机(mlp)组成，每层512个单元，隐藏层采用relu激活函数。

40、进一步地，所述步骤2包括如下步骤：

41、步骤2.1：构建抓取成本fgrasp[θ]＝d，其中d为使用神经抓取距离场ngdf得到的当前轨迹最后一点末端位姿和最近邻可行抓取位姿的距离，θ表示轨迹，可以是n*6的矩阵，n代表轨迹点的个数，6代表机械臂关节个数；

42、步骤2.2：构建高斯先验成本其中μ为先验高斯过程的均值，k为先验高斯过程的协方差矩阵；

43、通过线性时变随机微分方程构建高斯过程，线性时变随机微分方程的具体形式以及本发明的参数设置如下，其中w(t)是期望值为0的高斯白噪声：

44、

45、a(t)＝0，u(t)＝0，f(t)＝i

46、根据上述方程构建高斯过程，高斯过程的均值和协方差矩阵如下所示：

47、

48、

49、其中，μ0和k0是t＝0时刻的初始均值和协方差，φ(t,s)为状态转移矩阵；

50、步骤2.3：构建障碍成本其中是工作空间成本函数，x表示包围球球心的笛卡尔坐标(空间意义上的xyz)，可以通过正运动学、关节角度θi以及包围球球心在连杆上的位置来计算；表示包围球球心的速度，即x对时间求导；c(x)表示x这个三维空间点所在位置对应的一个障碍代价，该障碍代价可以理解为空间中的一个势能，每个空间三维点都有对应的势能；r3表示三维实向量，即x，r表示实数，代表c(x)是实数，r3→r表示一个函数将三维空间的点映射成一个实数；t0～tn是指轨迹的初始时间和终点时间；n表示轨迹点个数；b表示作为包围球球心的检测点集；

51、碰撞代价函数c如下：

52、

53、其中d(x)将碰撞检测盒位置映射成一有向距离值，通过有向距离场sdf实现。x为当前角度构型下包围盒的笛卡尔三维位置。ε是设定的值，用来反映希望离障碍物多远碰撞代价为0；

54、步骤2.4：构建联合目标函数f[θ]＝λ1fgrasp[θ]+λ2fgp[θ]+λ3ηfobs[θ]，其中λ1,λ2,λ3是各成本项系数，η用于在规划的过程中调节障碍项系数从而确保产生无碰撞的轨迹。

55、本发明的优势和有益效果在于：

56、本发明将机械臂末端位姿与最近可行抓取点的距离构建成抓取成本，从而对抓取规划与轨迹规划耦合求解。通过l加速梯度重启算法进行目标函数的优化，从而加快了算法的收敛速度。本发明不需要考虑离散抓取点集的生成，能够实现抓取规划的联合优化，具有规划成本低，成功率高的优点。