一种舒适空间约束下的机械臂运动规划方法及规划系统

本发明涉及服务机器人末端路径规划，尤其涉及一种舒适空间约束下的机械臂运动规划方法及规划系统。

背景技术：

1、随着机器人技术的不断发展，越来越多的机器人出现在了我们的日常生活中。服务机器人作为机器人中的一个重要分支，是当今机器人产业中的一个重要发展趋势。服务机器人的工作场景中，往往需要其可以同时完成感知和执行的任务，因此在移动底盘上搭载协作机械臂，配合完成感知系统和执行系统的移动机械臂在现今服务机器人的行业发展中占据了重要的位置。

2、同时由于在各式各样的工作场景中，服务机器人面对的服务对象大多是人。因此，从处理人机关系的角度出发，为了提高与服务机器人进行人机交互时的舒适感，基于舒适空间建模的规划算法应运而生。

3、在人机交互中，机器人的动作和移动方式对于用户的舒适度有着直接的影响。设计机器人时考虑到这一点至关重要，特别是在机器人需要与人类在相对密集的空间中共存和互动的情况下，考虑用户的舒适度可以提高用户与机器人的互动体验。如果机器人的移动方式不适当或者侵犯到人的个人空间，用户可能会感到不安或者不舒服，这会影响到用户对机器人的信任感和接受度。相反，一个能够尊重个人空间的机器人设计可以增加用户对机器人的好感，并且更容易与之合作和互动。然而，现在广泛使用的基于舒适空间建模的规划算法只考虑了移动底盘在二维地图中的舒适空间交互，而无法解决在移动机械臂运动规划过程中，服务机器人手臂对人舒适空间侵入的问题。

技术实现思路

1、为解决服务机器人在机械臂运动规划过程中较少考虑人机交互舒适度的问题，本发明提供一种舒适空间约束下的机械臂运动规划方法及规划系统，其通过考虑社会属性的三维舒适空间模型和基于启发式搜索的舒适空间约束下六自由度机械臂运动规划，实现了对机械臂末端路径的规划，使其能尽可能减少对被服务人舒适空间的入侵，具有更小的综合代价和较好的稳定性。

2、为实现上述目的，本发明提供了一种舒适空间约束下的机械臂运动规划方法，包括以下步骤：

3、s1、构建被服务人的三维舒适空间模型，并通过机械臂的末端运动基元制作用于启发式搜索的地图；

4、s2、根据三维舒适空间模型在地图中引入舒适代价；

5、s3、使用ara*算法对地图进行启发式搜索，依据评估函数扩展节点，再对每个新扩展的节点做碰撞检测以确定节点的可用性，直至获得综合代价最小且无关节碰撞的最优路径。

6、优选的，步骤s3后还包括s4、借助在搭载在机械臂上的深度相机，配合深度卷积神经网络对被服务人进行目标检测，从而对被服务人的舒适空间距离进行验证。

7、优选的，步骤s1具体包括以下步骤：

8、s11、利用设置在机械臂上的深度相机配合眼在手上的手眼系统的标定方法，标定得到深度相机坐标系相对机械臂末端工具坐标系的欧式旋转矩阵tooltcam；

9、s12、通过深度相机获取环境信息点云，使用八叉树地图octomap存储环境点云，构造障碍物地图；

10、八叉树地图是以八叉树结构存储的地图，每个子结点都可以逐级向上找到其对应的父节点，其中每个节点存储一概率值来表示该节点空间被占据的可能性，当概率值大于一个设定阈值时，则认为该节点空间内存在障碍物，具有存储容量小，更新速度快，数据直观，使用方便的优点。

11、s13、利用深度相机采集被服务人的rgb图像，使用深度卷积神经网络对rgb图像做人脸目标检测，获取被服务人的位置和社会属性，并深度相机内参和欧式旋转矩阵tooltcam将被服务人的位置坐标转换到机械臂基座坐标系上；

12、s14、根据被服务人的位置和社会属性，构建被服务人的三维舒适空间模型，其中社会属性包括被服务人的性别和年龄。

13、优选的，步骤s11所述标定方法具体包括以下步骤：

14、s111、设定手眼系统的闭环坐标系空间关系满足下式：

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

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

17、推导得到：

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

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

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

21、

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

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

24、

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

26、转换得到：

27、ax＝xb                                                   (5)

28、其中：

29、

30、

31、x＝tooltcam                                                  (8)；

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

33、优选的，步骤s13所述的深度相机内参为用于将深度相机采集的二维rgb图像坐标转换到三维深度图坐标的固定相机参数，其中，将识别框的中心点坐标设定为被服务人在像素坐标系中的位置(u,v)；

34、通过深度相机的内参矩阵转换获得被服务人在深度相机坐标系中的三维坐标(x,y,z)：

35、

36、

37、式中，z为被服务人所在像素点在深度图中读出的深度值；f为深度相机的焦距；cx和cy分别是深度相机在x轴和y轴的光心位置。

38、优选的，步骤s14所述的三维舒适空间模型为基于hall个人空间理论的三维圆柱形舒适空间模型；

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

40、s141、初始设定：三维圆柱形舒适空间模型的圆心位置o表示被服务人的位置；三维圆柱形舒适空间模型的圆半径r表示被服务人的最小的舒适空间半径值；三维圆柱形舒适空间模型的圆柱高h表示被服务人所在位置的垂直空间；

41、同时，针对被服务人的性别和社会属性设定最小的舒适空间半径值，并将其作为被服务人的舒适空间距离；

42、在本实施例中，圆柱高度h被设置为无限大，即机械臂不允许规划出经过被服务人上方的路径。被服务人的位置，即圆心位置o，圆半径r主要决定了舒适空间的大小，对于不同年龄和性别的人，具有不同的舒适空间大小。最小的舒适空间半径值(0.45m)，取自hall的个人空间理论。

43、表1不同年龄和性别的人的最小的舒适空间半径值

44、

45、s142、使用二维对称高斯分布构建平面圆形舒适空间，在高度上保持相同的舒适代价值，从而构成三维圆柱形舒适空间模型：

46、以机械臂的基座中心点为原点构建世界坐标系xyz，已知被服务人坐标为(xh,yh,zh)和机械臂工作空间内任意一点(x,y,z)，构建如下三维舒适空间模型代价值：

47、

48、其中：

49、

50、

51、式中，l为二维对称高斯分布的幅度，决定了在舒适空间内通行的代价大小，其与对于两个相邻运动基元的路径通行代价成比例；d表示机械臂工作空间内任意一点(x,y,z)与被服务人之间的距离；σ为二维对称高斯分布的方差，反映了当前被服务人舒适空间的大小和分布情况，由当前被服务人的舒适空间距离rh决定，可随着社会属性的变化而变化。该高斯代价值反映了入侵者在该点对被服务人的入侵程度，整体上呈现出离被服务人的距离越近，入侵程度越大的趋势；rh为被服务人的舒适空间距离。

52、优选的，步骤s3具体包括以下步骤：

53、s31、使用机械臂末端运动基元构建连通lattice图；

54、运动基元(motion primitive)是机器人运动规划中的基本运动单元，用于描述机器人在运动中的基本动作，可以理解为机器人在单位时间内的一次基本运动。且运动基元是由机械结构和运动任务共同决定的，而不取决于机械臂当前的运动状态，例如在六自由度的机械臂运动规划过程中，机械臂的运动基元可以表示为一个6维的矩阵，分别对应各个自由度的平移和旋转。当确定了运动基元后，就可以将搜索空间离散成状态的连通图。将起始点作为起始状态节点，以运动基元作为单位路径来扩展新节点，并对该路径进行碰撞检测，如果是可行路径，则作为lattice连通图的边。

55、s32、使用舒适空间约束下ara*算法在lattice图上搜索，依据评估函数扩展节点，对扩展的节点进行碰撞检测以确定节点的可用性，并将节点的代价和位置信息使用open表、closed表和incons表存储，迭代搜索直至规划出最优路径；

56、其中，open表是一个优先队列，用于存储将要扩展的节点；closed表用于存储已经被扩展并搜索完全的节点，即先前迭代的搜索结果；incons表用于存储在先前搜索迭代过程中的不一致节点。不一致节点是指在扩张该节点时，由于权重ε较大，使其估计出的启发式代价大于实际代价，不满足相容性条件。在降低权重ε迭代搜索的过程中，incons表中的节点可能由于启发式估计函数h(n)的下降而在新的迭代中调整父节点，因此incons表的设置有助于提高算法的搜索效率。

57、优选的，在步骤s32中，舒适空间约束下ara*算法使用如下带权值ε的评估函数来扩展节点：

58、f(n)＝g(n)+εh(n)                                             (14)

59、式中，f(n)为评估函数；g(n)为路径中的节点n的实际代价，其表示从起始节点到当前节点n的实际路径代价；h(n)为启发式估计函数，其表示从当前节点到目标节点的启发式估计路径代价；

60、设定x,y,z分别为节点n′的x轴，y轴，z轴坐标，得到：

61、c(n,n′)＝cp(n,n′)+cc(n,n′)                                        (15)

62、c(n,n′)＝g(n′)-g(n)                                            (16)

63、cc(n,n′)＝fc(x,y,z)                                              (17)

64、式中，g(n′)为起点到相邻的下一节点n′的实际代价；cp(n,n′)为路径通行代价，其反映了机械臂从节点n移动到节点n′需要的能量资源损耗，其值大小由机械结构确定，并与机械臂当时所处的位置和状态无关；cc(n,n′)为舒适代价，其反映了机械臂从节点n移动到节点n′对被服务人的舒适空间的入侵程度，其值大小取决于机械臂末端在当前节点状态的位置；

65、启发式估计函数h(n)的表达式如下：

66、h(n)＝αmxyz(n,ngoal)+βmrpy(n,ngoal)                                 (18)

67、式中，mxyz(n,ngoal)为当前节点n到目标节点ngoal的平移曼哈顿距离；mrpy(n,ngoal)为当前节点n到目标节点ngoal的旋转曼哈顿距离；α和β分别为mxyz(n,ngoal)和mrpy(n,ngoal)的比例权值。

68、基于上述分析可知，权值ε用于改变启发式估计函数h(n)的大小。改变启发式函数的大小能直接改变规划算法的搜索结果。当启发式估计函数h(n)满足相容性条件时，规划算法一定能找到全局最优解，且h(n)越小，搜索过程中所需要扩展的点就越多，需要的搜索时间就越长。当估计函数h(n)恰好等于实际的代价时，搜索过程就只会按照最优路径扩展，也即最理想的情况。当h(n)大于实际代价时，搜索结果会按照局部最优解逐步向下搜索，类似于贪心算法，大幅提升运行时间，但是可能得到不是全局最优的最短路径。

69、优选的，在步骤s32中对于扩展的节点，基于包围球的碰撞检测算法使用球体包围障碍物；

70、对于新扩展的节点，需要对其进行碰撞检测。基于包围球的碰撞检测算法使用简单的球体包围障碍物，因为球体数学模型上仅有圆心位置和半径两个参数具有较快的计算效率。

71、此时，在对机械臂的连杆进行碰撞检测时，因为自身具有一定的物理形状，是不能将其视为质点的，因而利用半径大于机械臂连杆半径的连杆圆柱体代替连杆的物理形状，并设定连杆圆柱体的中心点为m，连杆圆柱体的两个端点分别为e和f，连杆圆柱体的半径为rm，障碍物中心点为n，包围障碍物的球体半径为rn，此时问题转化成三维空间中圆柱体和球体，其距离无法简单地使用m和n的距离直接表示，而需要考虑其空间关系，以找到其中最短的距离dmin；

72、考虑空间几何关系，最短距离分为三种情况：连杆圆柱体e端靠近球体，连杆圆柱体f端靠近球体以及连杆圆柱体与球体垂直，此时该空间几何关系由δnef的形状表示，表达式如下：

73、

74、式中，dmin表示连杆圆柱体与球体之间的最短距离。

75、如图5和图6所示，舒适空间下ara*算法的核心思想是在有限的计算时间内执行一系列迭代搜索，每次搜索都具有不同的启发式权重。首先使用一个较大的权重ε，算法可以在初始搜索中快速找到一个相对较快但不一定是最优的规划路径，然后在之后的每次的迭代搜索过程中逐次降低权重ε，以改进规划路径的质量。算法会一直迭代搜索直到达到以下三种情况之一：找到最优规划路径、权重ε降低到1、达到计算时间上限，然后返回当前已找到的最佳规划路径。

76、一种规划系统，用于执行一种舒适空间约束下的机械臂运动规划方法；

77、规划系统包括机械臂、设置在机械臂末端且眼在手上的深度相机和计算机，机械臂和深度相机均与计算机相通讯，机械臂为6自由度ur5e机械臂，深度相机为realsensed435深度相机，计算机为用于计算三维舒适空间模型和地图的nuc计算机；

78、计算机包括：

79、标定模块，用于获取眼在手上的深度相机坐标系相对于机械臂末端工具坐标系的欧式旋转矩阵tooltcam；

80、感知和目标检测模块，用于利用深度卷积神经网络进行人脸目标检测，然后利用标定出的欧氏变换矩阵tooltcam将感知出的环境信息和被服务人信息转化到机械臂的基座坐标系下；

81、三维舒适空间建模模块，用于根据人脸目标检测的得到的被服务人的位置和社会属性，建立的被服务人三维舒适空间模型；

82、机械臂运动规划模块，用于通过运动基元建立lattice图，并使用舒适空间约束下的ara*算法基于lattice图进行启发式图搜索，获取最优路径。

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

84、利用舒适空间ara*算法能够考虑不同人的部分社会属性，建立三维舒适空间模型，从而增加了对人的舒适度考量，可有效地避免过度侵入人的舒适空间，且ara*算法具有较好的一致性，规划成功率比较高，能够满足服务机器人的任务要求。

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