复合约束下基于点云的自适应机械臂人机物体传送方法、系统、机器可读存储介质及数据处理设备

本发明涉及移动机械臂视觉感知和运动规划领域，尤其涉及复合约束下基于点云的自适应机械臂人机物体传送方法、系统及数据处理设备。

背景技术：

1、随着服务机器人在日常生活和工业领域中的广泛应用，特别是在抓取和物体传送任务中应用，使得机械臂的自适应性和智能化成为关键挑战。传统的机械臂抓取方法通常依赖于预先定义的抓取姿态或者固定的抓取点，而这种方式在面对不同形状、尺寸的物体以及复杂的工作环境时，往往表现不佳，导致抓取的成功率和效率不高。此外，在人机物体传送的场景中，机械臂需要根据用户的状态和姿势进行抓取和传送操作，增加了任务的复杂性和难度。

2、针对上述问题，近年来涌现了许多基于视觉和深度感知的自适应机械臂控制方法。其中，机械臂手眼系统和点云处理技术的发展，为实现智能抓取和传送提供了重要支持。机械臂手眼系统利用在机械臂末端安装的相机，能够实时获取周围环境的图像信息，从而实现对物体位置、姿态等关键信息的感知。结合深度相机，即可获得更为丰富的三维点云数据，为机器人提供更准确的物体形状和位置信息。

技术实现思路

1、针对服务机器人在机械臂在执行抓取任务时，难以自适应的根据任务和用户当前状态进行人机物体传送的问题，本发明提供一种复合约束下基于点云的自适应机械臂人机物体传送方法、系统及数据处理设备，将点云匹配和姿态估计结合，实现了机械臂在抓取和物体传送过程中的自适应性和智能化，所估计出的自适应位姿结果误差较小，规划成功率高，以满足机械臂复杂的工作环境。

2、为实现上述目的，本发明提供了一种复合约束下基于点云的自适应机械臂人机物体传送方法，包括以下步骤：

3、s1、利用机械臂末端的深度相机，分别获取待抓取物体点云和用户人体点云；

4、s2、基于步骤s1获取的待抓取物体点云和用户人体点云，分别估计待抓取物体的抓取位姿和用户的物体传送位姿；

5、s3、基于步骤s2估计的待抓取物体的抓取位姿和用户的物体传送位姿，在复合约束下规划机械臂的运动轨迹，实现自适应机械臂人机物体传送规划。

6、优选的，步骤s2具体包括以下步骤：

7、s21、获取机械臂的相机坐标系相对于机械臂末端工具坐标系之间的欧氏旋转矩阵tooltcam，从而确定深度相机中目标物点云相对机械臂基座的坐标转换关系；

8、其具体包括以下步骤：

9、s211、设定机械臂的手眼系统闭环坐标系空间关系满足下式：

10、tooltcam＝tooltbase*basetcal*caltcam      (1)

11、式中，tooltcam为深度相机坐标系相对机械臂末端工具坐标系的欧式旋转矩阵；tooltbase为机械臂末端工具坐标系相对机械臂基座坐标系的旋转矩阵；basetcal为标定板相对机械臂基座坐标系的旋转矩阵；caltcam为标定板相对深度相机坐标系的旋转矩阵；

12、推导得到：

13、basetcal＝basettool*tooltcam*camtcal (2)

14、式中，basettool为机械臂基座坐标系相对机械臂末端工具坐标系的旋转矩阵；tooltcam为机械臂末端工具坐标系相对深度相机坐标系的旋转矩阵；camtcal为深度相机坐标系相对标定板的旋转矩阵；

15、s212、分别在t0和t1时刻取两次结果，得到：

16、

17、式中，为t0时刻机械臂基座坐标系相对机械臂末端工具坐标系的旋转矩阵；为t0时刻深度相机坐标系相对标定板的旋转矩阵；为t1时刻机械臂基座坐标系相对机械臂末端工具坐标系的旋转矩阵；为t1时刻机械臂基座坐标系相对机械臂末端工具坐标系的旋转矩阵；

18、在公式(3)的左右两边分别乘以和得到：

19、

20、式中，为t1时刻机械臂末端工具坐标系相对机械臂基座坐标系的旋转矩阵；为t0时刻标定板相对深度相机坐标系的旋转矩阵；

21、转换得到：

22、ax＝xb      (5)

23、其中：

24、

25、

26、x＝tooltcam       (8)；

27、s213、利用tsai-lenz方法解出所求深度相机坐标系相对机械臂末端工具坐标系的欧式旋转矩阵tooltcam的数值解；

28、s214、利用深度相机内参和欧式旋转矩阵tooltcam将用户的位置坐标转换到机械臂基座坐标系上：

29、

30、

31、式中，(upixel,vpixel)为用户在像素坐标系中的位置，其为识别框的中心点坐标；(x,y,z)为用户在相机坐标系中的三维坐标；z为用户所在像素点在深度图中读出的深度值；f为深度相机的焦距；cx和cy分别是深度相机在x轴和y轴的光心位置；

32、s22、分别估计待抓取物体的抓取位姿和用户的物体传送位姿：

33、针对待抓取物体点云，使用基于二维图像特征的模板匹配方法识别目标物，以实现对三维场景点云的分割，然后使用基于三维点云特征的粗匹配和精配准相结合的点云匹配方法，估计变换矩阵，从而自适应确定待抓取物体的抓取位姿；

34、针对用户人体点云，使用结合的点云滤波和聚类方法实现场景中三维人体点云的筛选和分割，然后使用基于点云矩阵奇异值分解的方法估计人体姿态，从而自适应确定用户的物体传送位姿。

35、优选的，在步骤s22中，针对待抓取物体点云，首先使用二维surf特征分别计算模板图像和场景图像的特征点，然后使用lowe算法和ransac算法筛选特征点并计算模板匹配的单应性变换矩阵，以在相机的二维rgb图像中划定感兴趣区域；然后将感兴趣区域内的点转换成同时包含三维xyz坐标和rgb颜色的点云，再根据深度值约束进行筛选，实现对三维环境点云的分割；

36、然后使用基于fpfh三维点云特征的sac-ia点云配准方法，来实现场景物体点云和模板点云的粗匹配，使用点对点的点云icp算法实现场景物体点云和模板点云的精配准，使用基于点云匹配的方法获得模板点云和场景物体点云的欧式变换矩阵，并将欧式变换矩阵应用到对应的人工最优抓取位姿上，计算出当前场景物体的最优抓取位姿；

37、其中，点云匹配步骤如下：

38、首先计算点云中所有点的fpfh特征，即计算点云中的每个点作为当前源点及其一定邻域范围内的所有点，以计算fpfh特征，并通过直方图统计表示，根据源点和邻域内一邻点的法向量和连线，建立坐标系，其中坐标系三轴的方向计算式如下：

39、u＝ns (11)

40、v＝(pt-ps)×u (12)

41、w＝u×v                                                     (13)

42、式中，u，v，w分别表示坐标系的三个轴；ps为当前计算特征的源点；pt为ps邻域内其中一点；ns为点ps的法向量；(pt-ps)表示点pt到点ps的连线；符号×表示向量的叉乘；

43、根据三轴坐标系，计算出如下三个特征值：

44、α＝v·nt                                                      (14)

45、

46、θ＝arctan(w·nt,u·nt)                                            (16)

47、式中，α为v轴和点pt法向量的夹角特征值；φ为源点法向量两点连线的夹角特征值；θ为描述u轴，w轴和邻点法向量方向的特征值；nt为点pt的法向量；||pt-ps||表示点pt到点ps的欧氏距离；表示点pt到点ps的连线方向上的单位向量；

48、然后，对源点邻域内的所有邻点分别计算α、φ、θ三个特征值，并通过直方图进行统计，使用把三个特征值域分别等分的方法，形成多维直方图特征，记为spf(ps)，对于k个邻点pt，分别再统计一次spf(pt)，并按该邻点pt与源点的距离计算权重加入spf(ps)中，得到源点的fpfh特征fpfh(ps)：

49、

50、式中，ωk表示第i个邻点pi和源点ps的距离；

51、分别计算模板点云和场景物体点云的fpfh特征后，利用sac-ia点云配准方法随机选择设定数量的特征匹配点，计算当前匹配情况下的欧式旋转矩阵和匹配误差；

52、通过设定次数的迭代选取不同的特征匹配点集合，将匹配误差最小的欧式旋转矩阵作为点云粗配准的结果；

53、最后利用点对点的点云icp算法，实现场景物体点云和模板点云的精配准：在点对点的点云icp算法中输入场景物体点云和模板点云以及粗配准的欧式变换矩阵，在粗配准的转换基础上迭代计算新的欧式变换矩阵直到达到设定误差要求。

54、优选的，在步骤s22中，针对用户人体点云，使用结合的点云滤波和聚类方法实现场景中三维人体点云的筛选和分割，具体步骤如下：

55、首先定义距离阈值tdist，并将距离大于tdist的点云删除，实现对环境点云的粗分割；

56、然后使用体素滤波的方法对粗分割后的环境点云进行下采样：体素滤波的方法将空间均分为半径为rvoxel的正方体体素格，对于有点云分布的体素格，通过计算该体素格中全部点云的质心，作为新的点云存储到该体素格中；

57、其中，在任意体素格内所有点的质心计算公式如下式：

58、

59、

60、

61、然后使用统计离群点滤波器对点云进行降噪处理：

62、对于数量为n的点云p＝{p1,p2,…,pn}中的任意一点pi，定义点pi附近的k个最近邻点为其邻域，并将点到其邻域内所有点的距离作为统计样本，满足正态分布，具有样本均值和样本方差；

63、其中，点pi到邻域内所有点的样本距离mi计算公式如下：

64、

65、式中，k为邻域内邻点的数量；pj表示点pi邻域内的第j个点；||pi-pj||表示点pi到点pj的距离；

66、再分别计算点pi到邻域内所有点的样本方差di：

67、

68、定义离群点阈值to，当离群点阈值to满足如下式条件时，认为点pi为统计离群点，被滤波器滤除：

69、to＜mi+λddi       (23)

70、式中，λd为标准差倍数；

71、最后，使用基于欧式距离的聚类分割算法，将人体点云分割出来：

72、对于数量为n的点云p＝{p1,p2,…,pn}中的任意一点pi，如果当前点未被分配到任何聚类中，则建立一个新聚类cj，并将点pi分配到聚类cj中，对于其邻域内一邻点pk，计算点pi和点pk之间的欧式距离dis(pi,pk)：

73、

74、式中，(xi,yi,zi)为点pi的坐标；(xk,yk,zk)为点pk的坐标；

75、定义一个距离阈值td，当相邻点欧式距离dis(pi,pk)小于距离阈值td时，则认为是同一个聚类的点，将其划入聚类cj中；对点邻域内的所有点进行欧式距离的阈值判断，对新加入聚类cj的点，继续对新加入点的邻域点进行迭代搜索，直至没有新加入的点；继续遍历未被分配到任何聚类的点，直到所有点都被分配到对应聚类为止。

76、优选的，在步骤s22中，使用基于点云矩阵奇异值分解的方法估计人体姿态，从而自适应确定用户的物体传送位姿，具体步骤如下：

77、首先，对点云进行奇异值分解以获得点云的主要特征方向的数学描述：

78、对于具有m个点的n维数据，描述成矩阵形式am×n，ai为第i个列向量，u为单位投影的列向量，对数据进行去中心化处理，使处理后的点云均值为0，设定点云的主要方向是数据分布中方差s2最大的方向上的单位向量e，定义如下目标函数：

79、

80、其中

81、

82、由于

83、((ai)t·e)2＝((ai)t·e)t·((ai)t·e)＝et·ai·(ai)t·e      (27)

84、故

85、

86、式中，为am×n的协方差矩阵，且记为c；

87、然后使用拉格朗日乘数法求解目标优化函数：

88、l(e,λ)＝etce+λ(1-ete)      (29)

89、

90、式中，l(e,λ)表示拉格朗日乘数法目标函数；

91、对式(30)化简即

92、ce＝λe         (31)

93、得到目标函数取极值时，λ为协方差矩阵c最大的特征值，e为最大特征值对应的特征向量，此时求解协方差矩阵c的特征值和特征向量问题，等同于求解点云矩阵am×n的奇异值分解问题，对am×n进行奇异值分解的结果如下式：

94、

95、式中，为(am×n)tam×n的特征向量，即协方差矩阵c的特征向量；um×m为am×n(am×n)t的特征向量，和um×m均为单位正交向量；

96、且um×m和中对应的特征向量按∑m×n奇异值大小排列，同时协方差矩阵c的特征值为∑m×n中奇异值的平方；

97、最后，将所求的点云方向进行正负方向的统一，即可组成三维欧式坐标系，并将三维欧式坐标系视为当前人体点云方向的坐标系。

98、优选的，在步骤s3中，设定人机物体传送任务为：以物体抓取位姿为起点，以物体传送位姿为终点，使用机械臂运动规划方法规划出有效路径，实现物体传送任务，并设定物体传送机械臂运动规划过程中的约束条件：被动链约束、环境约束、舒适度约束和任务约束。

99、优选的，在步骤s3中，还添加如下约束条件：使用基于机械臂末端四元数夹角的判断条件来约束相邻两路径节点运动过程中的姿态变化，判断条件如下式

100、2arccos(qn·qn′)≤tq      (33)

101、式中，qn为机械臂运动路径当前节点；qn′为规划出的路径中下个节点，arccos为反余弦函数，tq为姿态变化阈值。

102、所述的复合约束下基于点云的自适应机械臂人机物体传送方法的系统，包括机械臂，机械臂的末端安装有深度相机，深度相机与计算机相通讯，计算机包括：

103、标定模块，获取机械臂的相机坐标系相对于机械臂末端工具坐标系之间的欧氏变换矩阵；

104、感知和目标检测模块，用于利用深度相机获取待抓取物体点云和用户人体点云，获取机械臂的相机坐标系相对于机械臂末端工具坐标系之间的欧氏变换矩阵tooltcam，从而确定深度相机中目标物点云相对机械臂基座的坐标转换关系；

105、物体抓取位姿估计模块，用于根据待抓取物体点云，使用基于二维图像特征的模板匹配方法识别目标物，以实现对三维场景点云的分割，然后使用基于三维点云特征的粗匹配和精配准相结合的点云匹配方法估计变换矩阵，从而自适应确定待抓取物体的抓取位姿；

106、用户传送位姿估计模块，用于根据用户人体点云，使用结合的点云滤波和聚类方法实现场景中三维人体点云的筛选和分割，然后使用基于点云矩阵奇异值分解的方法估计人体姿态，从而自适应确定用户的物体传送位姿；

107、机械臂运动规划模块，用于基于待抓取物体抓取位姿和用户的物体传送位姿，在复合约束下，实现自适应机械臂人机物体传送规划。

108、一种机器可读存储介质，所述机器可读存储介质上存储有机器可执行指令，所述机器可执行指令在被一个或多个处理器执行时，实现所述的复合约束下基于点云的自适应机械臂人机物体传送方法。

109、一种数据处理设备，包括处理器和机器可读存储介质，所述机器可读存储介质上存储有机器可执行指令，所述机器可执行指令在被所述处理器执行时，实现所述的复合约束下基于点云的自适应机械臂人机物体传送方法。

110、本发明具有以下有益效果：

111、1、通过将点云匹配和姿态估计结合，实现了机械臂在抓取和物体传送过程中的自适应性和智能化，所估计出的自适应位姿结果误差较小，规划成功率高，以满足机械臂复杂的工作环境；

112、2、通过考虑规划过程中的复合约束，使机械臂有效地执行服务任务。

113、下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。