一种多机器人协同三维测量作业方法

背景技术：

1、近年来随着机器视觉的快速发展，结构光测量技术由于生成的点云质量高精度好等优点，被广泛的应用到撞击检测、文物重建、医疗扫描等一些高精度测量领域。由于结构光测量设备受限于相机的成像范围和拍摄视角，在进行实际的三维形貌测量任务时，不可能一次性的将被测物的三维形貌信息获取完整。一般的解决方法是需要将结构光测量设备移动至各个视角所对应的空间位姿，从多个视角获取被测物的点云信息，然后将多视角点云信息进行拼接，从而使得被测对象的点云信息完整。但是由于高精度结构光设备的质量较大，传统的人为挪动方式不能准确把握各个采集视角的位姿状态。在一些负载性大重复性高的环境中机器人能够很好的代替传统人力工作，并且可以对机器人进行预先的路径规划来完成测量任务。

2、在工厂中，传统机器人的测量路径生成通常采用人工示教的方式，由操作人员机器人逐个选择机器人测量任务的路径点，这对操作人员的技术水平，以及现场工作环境的熟悉程度要求较高。由于单视角点云精度很大程度的受结构光设备的测距影响，在面对类似结构光测量仪这种对拍摄距离有要求的测量设备时，操作人员还需要手动测量设备与被测物之间的距离，并判断是否满足拍摄距离要求。然而在一些难以获得测量设备与被测物距离的测量场合，如高温环境中，如何确保每个测量位姿下测量设备与被测物之间的距离符合要求，成为了一个难题。因此有必要采取一种切实有效的测量方法，降低测量任务中机器人的操作难度并能够精确的获取测量设备与被测物之间的距离，从而获得更高质量的单视角点云

技术实现思路

1、本发明的目的在于克服现有技术的不足，提供一种多机器人协同三维测量作业方法，以提高结构光测量仪的工作效率，缩短了任务周期，并为后续精拼接过程提供良好的初值，减轻后续配准任务的负担。

2、为实现上述发明目的，本发明多机器人协同三维测量作业方法，其特征在于，包括：

3、(1)、虚拟仿真环境搭建

4、1.1)、根据现实测量环境，在装有开源机器人系统的计算机上搭建虚拟仿真环境，并将双机器人的描述文件导入到虚拟仿真环境中，生成相应的机器人1、机器人2，同时，在虚拟仿真环境中，将结构光测量仪1以及深度相机1安装在机器人1末端法兰盘的固定夹具上，将结构光测量仪2以及深度相机2安装在机器人2末端法兰盘的固定夹具上；

5、1.2)、对被测物进行仿真，并放置于机器人1、2之间，结构光测量仪1、与结构光测量仪2均正对被测物，确定结构光测量仪1、2对被测物进行拍摄的多个测量面以及每个测量面存在的多条测量路径，对于结构光测量仪1：第k个测量面的第i条测量路径记为其中，kr1为测量面的个数，mr1_k为第k个测量面上测量路径的条数，同时每一条测量路径上还包含多个测量点，被测物第k个测量面上的第i条路径中的第j个测量点即测量位姿记为为第k个测量面测量路径上测量位姿的数量，r1代表机器人1，对于结构光测量仪2：第k个测量面的第i条测量路径记为其中，kr2为测量面的个数，mr2_k为第k个测量面上测量路径的条数，被测物第k个测量面上的第i条路径中的第j个测量点即测量位姿记为为第k个测量面测量路径上测量位姿的数量，r2代表机器人2；

6、结构光测量仪1、2的视野范围均为长m厘米，宽n厘米大小的矩形，对于结构光测量仪1，被测物第k个测量面的外接矩形长为宽为测量路径为沿长度方向，一条条的纵向路径，则需要满足约束：同一测量面上的测量路径条数同一测量路径上的测量点数根据这一约束，得到第k个测量面的第i条测量路径的第j个测量位姿对于结构光测量仪2，被测物第k个测量面的外接矩形长为宽为测量路径为沿长度方向，一条条的纵向路径，则需要满足约束：同一测量面上的测量路径条数同一测量路径上的测量点数根据这一约束，得到第k个测量面的第i条测量路径的第j个测量位姿

7、(2)、测量路径规划

8、2.1)、对于机器人1的第k个测量面的第i条测量路径首先在虚拟仿真环境中，拖拽机器人末端到测量位姿其中，测量位姿为为机器人1末端的位置坐标，为机器人1末端的姿态坐标；

9、2.2)、在机器人1的测量位姿处，通过使用深度相机采集被测物图像，得到rgb-d图像并转换为灰度图像后，进行阈值分割过滤背景，再提取被测物轮廓信息，得到被测物轮廓所在的最小外接矩形之后在rgb-d图像相对应的最小外接矩形范围中遍历所有像素点的深度信息d，找到被测物与结构光测量仪之间的最短距离dmin，同时记录下最短距离的像素点坐标a(u,v)，然后判断最短距离dmin是否满足结构光测量仪的测量范围：dmin∈[d-δ,d+δ]，其中，d为结构光测量仪进行相机标定时的焦距，δ为结构光测量仪测量允许的误差范围；若不满足，则进入步骤2.3)，对测量位姿进行调整，若满足，则进入步骤2.4)；

10、2.3)、将像素点坐标a(u,v)经过坐标系转换获得与其相对应的空间坐标(x(u,v),y(u,v),z(u,v))，然后结合机器人1末端的位置坐标确定一条空间直线：

11、

12、其中，x,y,z为空间直线上的坐标；

13、然后，从位置坐标起，沿着该空间直线，计算得到一个位置坐标满足测量条件：

14、

15、得到满足测量条件的位置坐标之后，结合原有的位姿坐标组成一个测量位姿作为测量位姿根据测量位姿由机器人运动学逆解出机器人对应关节状态，实现位姿调整；

16、2.4)、将调整后的测量位姿记录下来，返回步骤2.1)，进行下一个测量位姿设置，直到测量路径ksi所有测量位姿kpij的最短距离dmin判断完毕，然后，进入步骤(3)，其中，在进行下一个测量位姿设置前，如果：

17、a、测量位姿设置在机器人自由度退化，逆运动学无解的空间位姿，即奇异点附近时，为了保证机器人运动的流畅性需要设置一个过渡点来避开机器人的奇异点；

18、b、当被测物的形貌不规则，被测物表面有凸起时，为了获得更好的测量效果以及避免在机器人承载结构光测量仪运动的过程中与不规则被测物相碰撞，在测量位姿设置时需要考虑机器人实际的测量位姿以及被测物的形貌特征，在测量位姿和测量位姿之间设置一个过渡点

19、过渡点处不需要进行距离检测以及调整，过渡点为人为地对机器人的测量路径进行调整；

20、2.5)、对测量路径进行路径评价

21、所有测量位姿以及所有过度点构成一条完整的测量路径对测量路径进行路径评价，步骤如下：

22、步骤2.5.1)、在虚拟仿真环境中执行测量路径机器人1将从测量位姿连续地运动到测量位姿将运动过程进行等时间采样记录过程点t为时刻，t为运动终点的时刻，集合构成机器人的实际运动路径为

23、在测量路径中，其起始位置坐标为终止位置坐标为则终止位置与起始位置之间的最短距离为：

24、

25、在实际运动路径中，其起始位置坐标为终止位置坐标为则机器人实际运动的长度为：

26、

27、两式相减可得：

28、l＝l2-l1

29、则评价函数f1(l)表示为：f1(l)＝(δ1-l)/δ1,0≤l≤δ1，式中δ1为设置的最大误差阈值；

30、步骤2.5.2)、在机器人1的实际运动路径中找到一位姿到被测物质心坐标(pxobj,pyobj,pzobj)的距离l3最小，则距离l3表示为：

31、

32、其中，为位姿的位置坐标；

33、则评价函数f2(l3)表示为：

34、f2(l3)＝(l3-δ2)/l3

35、其中，δ2为保证结构光测量仪与被测物不碰撞的最小距离；

36、2.5.3)、对于测量路径的综合评价函数kfi为：

37、

38、根据综合评价函数kfi对所设置的测量路径进行评价，当综合评价函数时通过评价，进入步骤2.7)，否则进行步骤2.6)，其中g为根据不同测量场景由测量人员认为定义的阈值，0＜g＜100；

39、2.6)、遍历测量路径中的所有位姿，找到与位姿距离最近的位姿之后将机器人运动到测量位姿处，拖拽机器人末端手动增加当前机器人测量位姿与被测物之间的距离dmin，如果位姿是测量位姿，则调整范围仍需满足结构光测量仪的测量条件：dmin∈[d-δ,d+δ]，如果位姿是过渡点，需要根据过渡点的设置条件调整距离dmin，调整后的位姿替换位姿从而完成对测量路径的修正，然后进入步骤2.7)；

40、2.7)、对所有测量面的所有测量路径按照步骤2.1)到2.6)进行处理，完成测量路径规划仿真，将规划好的测量路径转换为机器人能够识别的通信报文由虚拟仿真环境发送给实体机器人1；

41、对于机器人2，按照步骤2.1)-2.7)的方法，获得规划好的测量路径并换为机器人能够识别的通信报文由虚拟仿真环境发送给实体机器人2；

42、(3)、现实环境测量

43、3.1)、对于实体机器人1进行手眼标定，得其相机坐标系相对于实体机器人1末端法兰坐标系的旋转平移矩阵即手眼关系矩阵r1hcg；

44、3.2)、完成手眼标定后，开始对被测物进行测量，实体机器人1接收到由虚拟仿真环境发送的测量路径后，依次运动到测量位姿处；

45、3.3.)、在到达测量位姿处时，实体机器人1将测量位姿发送给结构光测量仪，通过双目视觉相机获取当前测量位姿处被测物的单视角点云并使用手眼关系矩阵r1hcg、根据测量位姿获得的实体机器人1末端法兰坐标系到实体机器人1基座标系的位姿矩阵将单视角点云转换到实体机器人1基座标系下；

46、这样，得到实体机器人1基座标系下的被测物点云r1ib即被测物点云粗拼接结果；

47、3.4.)、对于实体机器人2，按照步骤3.1)-3.3)的方法，获得实体机器人1基座标系下的被测物点云r2ib即被测物点云粗拼接结果；

48、3.5)、获取实体机器人2的基座坐标系通过旋转平移到达实体机器人1基座坐标系的旋转平移矩阵

49、3.5.1)、首先在实体机器人1和实体机器人2末端法兰盘上分别固定一个探针1和探针2，并在实体机器人端进行探针1和探针2的tcp工具坐标系的设定；

50、3.5.2)、完成探针1和探针2的tcp工具坐标系设定后，手动操作实体机器人1和实体机器人2，将探针1和探针2同时指向两个实体机器人工作空间内的同一个点，记录下探针1末端在实体机器人1坐标系下的位姿p1和探针2末端在机器人2坐标系下的位姿p2，根据位姿p1和p2获得探针末端tcp坐标系到实体机器人1、2基座坐标系之间的旋转平移矩阵hg1和hg2，则可以表示为:

51、

52、3.6)、获取将被测物点云r2ib转换到实体机器人1坐标系下，得到被测物点云r2i′b：

53、

54、这样得到被测物通过双机器人获取的点云ib：

55、ib＝r1ib+r2i′b。

56、本发明的发明目的是这样实现的：

57、本发明多机器人协同三维测量作业方法，首先根据现实测量环境在装有开源机器人系统(ros)的计算机上搭建虚拟仿真环境；然后在保证三维形貌测量完整的前提条件下，在虚拟仿真环境上完成双机器人的测量位姿以及测量路径的设置并对测量路径进行路径评价；然后在现实测量环境中完成结构光测量仪的相机标定以及结构光测量仪与双机器人之间的手眼标定，并将通过路径评价的测量路径发送给对应的实体机器人1、2，由实体机器人1、2承载结构光测量仪依次到达所设置的测量位姿处进行拍摄，获取多视角的点云图像；之后根据手眼标定结果，通过坐标系转换将多视角点云统一到实体机器人1、2基座坐标系下，得到相应的被测物点云，最后，根据实体机器人2的基座坐标系通过旋转平移到达实体机器人1基座坐标系的旋转平移矩阵，将实体机器人2的点云转换到实体机器人1基座坐标系下，得到被测物通过双机器人获取的点云ib。本发明通过虚拟仿真平台进行机器人路径规划有效降低了多视角测量的试错风险；通过将点云转换到机器人基座坐标系下完成点云粗配准，为后续的精配准过程提供了良好的先验条件，减轻了后续配准任务的负担，提高了结构光测量仪的工作效率，缩短了任务周期，同时，使用双机器人使得测量范围更大，相对于单机器人能够完成更大物件的测量任务。

58、具体而言，本发明具有以下优点与创新性：

59、(1)、本发明在ros上完成虚拟仿真场景的搭建，可以在仿真环境中进行全仿真实验，降低了测量位姿选择时的试错风险；

60、(2)、通过距离检测判断当前位姿下结构光测量仪到被测物的距离是否满足结构光测量仪测距要求，有效地提高了测量精度以及拍摄效果，对于测距不合适的测量位姿实现了测量位姿的自动调整；

61、(3)、根据特定的路径评价函数给出了更高效更安全路径的选择标准，提高了系统整体的测量效率保障测量安全；

62、(4)、将不同测量位姿下拍摄的被测物点云统一到机器人基座坐标系中，实现了点云的粗配准，为之后的精配准过程提供良好的初值。

63、(5)整个测量过程通过使用双目结构光测量仪，采用无接触式的测量方式对被测物进行测量，不需要在被测物表面贴标记点，保留更多被测物的形貌信息的同时也具有精度高速度快的优点。