一种机器人路径规划式自动三维扫描测量方法

1.本发明属于三维形貌测量技术领域，更具体的讲，涉及一种机器人路径规划式自动三维扫描测量方法。


背景技术：

2.近年来随着机器视觉的快速发展，结构光测量技术由于生成的点云质量高精度好等优点，被广泛的应用到撞击检测、文物重建、医疗扫描等一些高精度测量领域。由于结构光测量设备受限于相机的成像范围和拍摄视角，在进行实际的三维形貌测量任务时，不可能一次性的将被测物的三维形貌信息获取完整。一般的解决方法是需要将结构光测量设备移动至各个视角所对应的空间位姿，从多个视角获取被测物的点云信息，然后将多视角点云信息进行拼接，从而使得被测对象的点云信息完整。但是由于高精度结构光设备的质量较大，传统的人为挪动方式不能准确把握各个采集视角的位姿状态。在一些负载性大重复性高的环境中机器人能够很好的代替传统人力工作，并且可以对机器人进行预先的路径规划来完成测量任务。
3.在工厂中，传统机器人的测量路径生成通常采用人工示教的方式，由操作人员机器人逐个选择机器人测量任务的路径点，这对操作人员的技术水平，以及现场工作环境的熟悉程度要求较高。由于单视角点云精度很大程度的受结构光设备的测距影响，在面对类似结构光测量仪这种对拍摄距离有要求的测量设备时，操作人员还需要手动测量设备与被测物之间的距离，并判断是否满足拍摄距离要求。然而在一些难以获得测量设备与被测物距离的测量场合，如高温环境中，如何确保每个测量位姿下测量设备与被测物之间的距离符合要求，成为了一个难题。因此有必要采取一种切实有效的测量方法，降低测量任务中机器人的操作难度并能够精确的获取测量设备与被测物之间的距离，从而获得更高质量的单视角点云


技术实现要素：

4.本发明的目的在于克服现有技术的不足，提供一种机器人路径规划式自动三维扫描测量方法，以提高结构光测量仪的工作效率，缩短了任务周期，并为后续精拼接过程提供良好的初值，并减轻后续配准任务的负担，。
5.为实现上述发明目的，本发明机器人路径规划式自动三维扫描测量方法，其特征在于，包括：
6.(1)、虚拟仿真环境搭建
7.1.1)、根据现实测量环境，在装有开源机器人系统的计算机上搭建虚拟仿真环境，并将机器人的描述文件导入到虚拟仿真环境中，生成相应的机器人，同时，在虚拟仿真环境中，将结构光测量仪以及深度相机安装在机器人末端法兰盘的固定夹具上；
8.1.2)、对被测物进行仿真，并放置于机器人和结构光测量仪的正前方，确定结构光测量仪对被测物进行拍摄的多个测量面以及每个测量面存在的多条测量路径，其中，第k个
测量面的第i条测量路径记为ks
i k＝1,2,...,k,i＝1,2,...,mk，其中，k为测量面的个数，mk为第k个测量面上测量路径的条数，同时每一条测量路径上还包含多个测量点，被测物第k个测量面上的第i条路径中的第j个测量点即测量位姿记为kp
ij
,j＝1,2,...,nk，nk为第k个测量面测量路径上测量位姿的数量；
9.结构光测量仪的视野范围为长m厘米，宽n厘米大小的矩形，被测物第k个测量面的外接矩形长为ka，宽为kb，测量路径为沿长度方向，一条条的纵向路径，则需要满足约束：同一测量面上的测量路径条数mk≥ka/m，同一测量路径上的测量点数nk≥kb/n，根据这一约束，得到第k个测量面的第i条测量路径的第j个测量位姿kp
ij
,j＝1,2,...,nk；
10.(2)、测量路径规划
11.2.1)、对于第k个测量面的第i条测量路径，首先在虚拟仿真环境中，拖拽机器人末端到测量位姿kp
ij
，其中，测量位姿kp
ij
为(kpx
ij,k
py
ij,k
pz
ij,k
rx
ij,k
ry
ij,k
rz
ij
)，(kpx
ij,k
py
ij,k
pz
ij
)为机器人末端的位置坐标，(krx
ij,k
ry
ij,k
rz
ij
)为机器人末端的姿态坐标；
12.2.2)、在机器人的测量位姿kp
ij
处，通过使用深度相机采集被测物图像，得到rgb-d图像并转换为灰度图像后，进行阈值分割过滤背景，再提取被测物轮廓信息，得到被测物轮廓所在的最小外接矩形之后在rgb-d图像相对应的最小外接矩形范围中遍历所有像素点的深度信息d，找到被测物与结构光测量仪之间的最短距离d
min
，同时记录下最短距离的像素点坐标a(u,v)，然后判断最短距离d
min
是否满足结构光测量仪的测量范围：d
min
∈[d-δ,d+δ]，其中，d为结构光测量仪进行相机标定时的焦距，δ为结构光测量仪测量允许的误差范围；若不满足，则进入步骤2.3)，对测量位姿进行调整，若满足，则进入步骤2.4)；
[0013]
2.3)、将像素点坐标a(u,v)经过坐标系转换获得与其相对应的空间坐标(x
(u,v)
,y
(u,v)
,z
(u,v)
)，然后结合机器人末端的位置坐标(kpx
ij,k
py
ij,k
pz
ij
)确定一条空间直线：
[0014][0015]
其中，x,y,z为空间直线上的坐标；
[0016]
然后，从位置坐标(kpx
ij,k
py
ij,k
pz
ij
)起，沿着该空间直线，计算得到一个位置坐标满足测量条件：
[0017][0018]
得到满足测量条件的位置坐标之后，结合原有的位姿坐标(krx
ij,k
ry
ij,k
rz
ij
)，组成一个更新测量位姿kp
ij
即根据测量位姿kp
ij
，由机器人运动学逆解出机器人对应关节状态，实现位姿调整；
[0019]
2.4)、将测量位姿kp
ij
记录下来，返回步骤2.1)，进行下一个测量位姿kp
i(j+1)
设置，直到测量路径ksi所有测量位姿kp
ij
的最短距离d
min
判断完毕，然后，进入步骤(3)，其中，在进行下一个测量位姿kp
i(j+1)
设置前，如果：
[0020]
a、测量位姿kp
ij+1
设置在机器人自由度退化，逆运动学无解的空间位姿，即奇异点
附近时，为了保证机器人运动的流畅性需要设置一个过渡点来避开机器人的奇异点；
[0021]
b、当被测物的形貌不规则，被测物表面有凸起时，为了获得更好的测量效果以及避免在机器人承载结构光测量仪运动的过程中与不规则被测物相碰撞，在测量位姿kp
i(j+1)
设置时需要考虑机器人实际的测量位姿以及被测物的形貌特征，在测量位姿kp
ij
和测量位姿kp
i(j+1)
之间设置一个过渡点
[0022]
过渡点处不需要进行距离检测以及调整，过渡点为人为地对机器人的测量路径进行调整；
[0023]
2.5)、对测量路径ksi进行路径评价
[0024]
所有测量位姿以及所有过度点构成一条完整的测量路径ksi，对测量路径ksi进行路径评价，步骤如下：
[0025]
步骤2.5.1)、在虚拟仿真环境中执行测量路径ksi，机器人将从测量位姿kp
i1
连续地运动到测量位姿将运动过程进行等时间采样记录过程点集合构成机器人的实际运动路径为
[0026]
在测量路径ksi中，其起始位置坐标为(kpx
i1,k
py
i1,k
pz
i1
),终止位置坐标为则终止位置与起始位置之间的最短距离为：
[0027][0028]
在实际运动路径中，其起始位置坐标为终止位置坐标为则机器人实际运动的长度为：
[0029][0030]
两式相减可得：
[0031]
l＝l
2-l1[0032]
则评价函数f1(l)表示为：f1(l)＝(δ
1-l)/δ1,0≤l≤δ1，式中δ1为设置的最大误差阈值；
[0033]
步骤2.5.2)、在机器人的实际运动路径中找到一位姿到被测物质心坐标(px
obj
,py
obj
,pz
obj
)的距离l3最小，则距离l3表示为：
[0034][0035]
则评价函数f2(l3)表示为：
[0036]
f2(l3)＝(l
3-δ2)/l3[0037]
其中，δ2为保证结构光测量仪与被测物不碰撞的最小距离；
[0038]
2.5.3)、对于测量路径ksi的综合评价函数
kfi
为：
[0039]
kfi
＝(0.5f1(l)+0.5f(l3))*100
[0040]
根据综合评价函数
kfi
对所设置的测量路径进行评价，当综合评价函数
kfi
＞g时通过评价，进入步骤2.7)，否则进行步骤2.6)，其中g为根据不同测量场景由测量人员认为定义的阈值，0＜g＜100；
[0041]
2.6)、遍历测量路径ksi中的所有位姿，找到与位姿距离最近的位姿kp
is
，之后将机器人运动到测量位姿kp
is
处，拖拽机器人末端手动增加当前机器人测量位姿与被测物之间的距离d
min
，如果位姿kp
is
是测量位姿，则调整范围仍需满足结构光测量仪的测量条件：d
min
∈[d-δ,d+δ]，如果位姿kp
is
是过渡点，需要根据过渡点的设置条件调整距离d
min
，调整后的位姿替换位姿kp
is
，从而完成对测量路径ksi的修正，然后进入步骤2.7)；
[0042]
2.7)、对所有测量面的所有测量路径按照步骤2.1)到2.6)进行处理，完成测量路径规划仿真，将规划好的测量路径转换为机器人能够识别的通信报文由虚拟仿真环境发送给实体机器人；
[0043]
(3)、现实环境测量
[0044]
3.1)、进行手眼标定，得相机坐标系相对于机器人末端法兰坐标系的旋转平移矩阵即手眼关系矩阵h
cg
；
[0045]
3.2)、完成手眼标定后，开始对被测物进行测量，实体机器人接收到由虚拟仿真环境发送的测量路径后，依次运动到测量位姿kp
ij
处；
[0046]
3.3.)、在到达测量位姿kp
ij
处时，实体机器人将测量位姿kp
ij
发送给结构光测量仪，通过双目视觉相机获取当前测量位姿处被测物的单视角点云ki
ij
，并使用手眼关系矩阵h
cg
、根据测量位姿获得的机器人末端法兰坐标系到机器人基座标系的位姿矩阵将单视角点云ki
ij
转换到机器人基座标系下；
[0047]
3.4.)、重复步骤3.2)、3.3)，得到机器人基座标系下的被测物点云ib即被测物点云粗拼接结果，然后观察点云ib是否完整即是否存在漏拍的现象，若发生漏拍则按照以下步骤进行补拍：
[0048]
3.4.1)、对于漏拍点云位置，确定与漏拍点云位置相邻的两幅点云ki
ij
、ki
i(j+1)
，进而确定机器人的测量位姿kp
ij
与kp
i(j+1)
，最后确定漏拍位置实体机器人的测量位姿
[0049][0050]
3.4.2)、操作实体机器人到达测量位姿获取当前测量位姿处被测物的单视角点云，并根据步骤3.3)将单视角点云转换到机器人的基座标系下，完成漏拍点云的补全。
[0051]
本发明的发明目的是这样实现的：
[0052]
本发明机器人路径规划式自动三维扫描测量方法，首先根据现实测量环境在装有开源机器人系统(ros)的计算机上搭建虚拟仿真环境；然后在保证三维形貌测量完整的前提条件下，在虚拟仿真环境上完成测量位姿以及测量路径的设置并对测量路径进行路径评价；然后在现实测量环境中完成结构光测量仪的相机标定以及结构光测量仪与机器人之间的手眼标定，并将通过路径评价的测量路径发送给实体机器人，由实体机器人承载结构光测量仪依次到达所设置的测量位姿处进行拍摄，获取多视角的点云图像；之后根据手眼标定结果，通过坐标系转换将多视角点云统一到机器人基座坐标系下完成点云粗拼接过程。最后判断机器人基座坐标系下的被测物点云粗拼接图像是否完整，并在漏拍处对漏拍点云
进行补全，直至完成整个被测物的三维形貌测量。本发明通过虚拟仿真平台进行机器人路径规划有效降低了多视角测量的试错风险；通过将点云转换到机器人基座坐标系下完成点云粗配准，为后续的精配准过程提供了良好的先验条件，减轻了后续配准任务的负担，提高了结构光测量仪的工作效率，缩短了任务周期。
[0053]
具体而言，本发明具有以下优点与创新性：
[0054]
(1)、本发明在ros上完成虚拟仿真场景的搭建，可以在仿真环境中进行全仿真实验，降低了测量位姿选择时的试错风险；
[0055]
(2)、通过距离检测判断当前位姿下结构光测量仪到被测物的距离是否满足结构光测量仪测距要求，有效地提高了测量精度以及拍摄效果，对于测距不合适的测量位姿实现了测量位姿的自动调整；
[0056]
(3)、根据特定的路径评价函数给出了更高效更安全路径的选择标准，提高了系统整体的测量效率保障测量安全；
[0057]
(4)、将不同测量位姿下拍摄的被测物点云统一到机器人基座坐标系中，实现了点云的粗配准，为之后的精配准过程提供良好的初值。
[0058]
(5)整个测量过程通过使用双目结构光测量仪，采用无接触式的测量方式对被测物进行测量，不需要在被测物表面贴标记点，保留更多被测物的形貌信息的同时也具有精度高速度快的优点。
附图说明
[0059]
图1是本发明机器人路径规划式自动三维扫描测量方法一种具体实施方式的流程图；
[0060]
图2是确定测量路径以及测量位姿的示意图；
[0061]
图3是粗拼接坐标系转换示意图；
[0062]
图4是搭建的虚拟仿真环境具体实例示意图；
[0063]
图5是被测物同一测量面上测量位姿设置示意图以及单视角点云图；
[0064]
图6是被测物不同测量面设置示意图以及单个测量面粗拼接结果；
[0065]
图7是最终点云粗拼接结果。
具体实施方式
[0066]
下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。
[0067]
图1是本发明机器人路径规划式自动三维扫描测量方法一种具体实施方式的流程图。
[0068]
在本实施例中，如图1所示，本发明机器人路径规划式自动三维扫描测量方法包括以下步骤：
[0069]
步骤s1：虚拟仿真环境搭建
[0070]
步骤s1.1：生成机器人并安装结构光测量仪以及深度相机
[0071]
根据现实测量环境，在装有开源机器人系统的计算机上搭建虚拟仿真环境，并将
机器人的描述文件导入到虚拟仿真环境中，生成相应的机器人，同时，在虚拟仿真环境中，将结构光测量仪以及深度相机安装在机器人末端法兰盘的固定夹具上。
[0072]
其中，所述的描述文件为urdf(universal robot description format即通用机器人描述格式)描述文件，包含的内容有：连杆、关节，运动学和动力学参数、可视化模型、碰撞检测模型等。
[0073]
步骤s1.2：确定测量路径
[0074]
图2是确定测量路径以及测量位姿的示意图。在本实施例中，如图2所示，为了能够更完整更精确的获得被测物的三维形貌特征，并且保证相邻点云之间存在重叠区域，对被测物进行仿真，并放置于机器人和结构光测量仪的正前方，确定结构光测量仪对被测物进行拍摄的多个测量面以及每个测量面存在的多条测量路径，其中，第k个测量面的第i条测量路径记为ks
i k＝1,2,...,k,i＝1,2,...,mk，其中，k为测量面的个数，mk为第k个测量面上测量路径的条数，同时每一条测量路径上还包含多个测量点，被测物第k个测量面上的第i条路径中的第j个测量点即测量位姿记为kp
ij
,j＝1,2,...,nk，nk为第k个测量面测量路径上测量位姿的数量。
[0075]
结构光测量仪的视野范围为长m厘米，宽n厘米大小的矩形，被测物第k个测量面的外接矩形长为ka，宽为kb，测量路径为沿长度方向，一条条的纵向路径，为了保证相邻两条测量路径以及相邻两个测量点的点云信息存在公共区域，则需要满足约束：同一测量面上的测量路径条数mk≥ka/m，同一测量路径上的测量点数nk≥kb/n，根据这一约束，得到第k个测量面的第i条测量路径的第j个测量位姿kp
ij
,j＝1,2,...,nk，如图2所示，这样水平方向以及纵向都存在重叠区域。
[0076]
步骤s2：测量路径规划
[0077]
步骤s2.1：拖拽机器人末端到测量位姿
[0078]
对于第k个测量面的第i条测量路径，首先在虚拟仿真环境中，拖拽机器人末端到测量位姿kp
ij
，其中，测量位姿kp
ij
为(kpx
ij,k
py
ij,k
pz
ij,k
rx
ij,k
ry
ij,k
rz
ij
)，(kpx
ij,k
py
ij,k
pz
ij
)为机器人末端的位置坐标，(krx
ij,k
ry
ij,k
rz
ij
)为机器人末端的姿态坐标。
[0079]
步骤s2.2：最短距离检测与判断
[0080]
在机器人的测量位姿kp
ij
处，通过使用深度相机采集被测物图像，得到rgb-d图像并转换为灰度图像后，进行阈值分割过滤背景，再提取被测物轮廓信息，得到被测物轮廓所在的最小外接矩形之后在rgb-d图像相对应的最小外接矩形范围中遍历所有像素点的深度信息d，找到被测物与结构光测量仪之间的最短距离d
min
，同时记录下最短距离的像素点坐标a(u,v)，然后判断最短距离d
min
是否满足结构光测量仪的测量范围：d
min
∈[d-δ,d+δ]，其中，d为结构光测量仪进行相机标定时的焦距，δ为结构光测量仪测量允许的误差范围；若不满足，则进入步骤s2.3，对测量位姿进行调整，若满足，则进入步骤s2.4。
[0081]
步骤s2.3：位姿调整
[0082]
将像素点坐标a(u,v)经过坐标系转换获得与其相对应的空间坐标(x
(u,v)
,y
(u,v)
,z
(u,v)
)，然后结合机器人末端的位置坐标(kpx
ij,k
py
ij,k
pz
ij
)确定一条空间直线：
[0083]
[0084]
其中，x,y,z为空间直线上的坐标；
[0085]
然后，从位置坐标(kpx
ij,k
py
ij,k
pz
ij
)起，沿着该空间直线，计算得到一个位置坐标满足测量条件：
[0086][0087]
得到满足测量条件的位置坐标之后，结合原有的位姿坐标(krx
ij,k
ry
ij,k
rz
ij
)，组成一个更新测量位姿kp
ij
即根据测量位姿kp
ij
，由机器人运动学逆解出机器人对应关节状态，实现位姿调整。
[0088]
步骤s2.4:重复步骤s2.1-s2.3，直到测量路径所有测量位姿最短距离判断与位姿调整完毕
[0089]
将测量位姿kp
ij
记录下来，返回步骤s2.1，进行下一个测量位姿kp
i(j+1)
设置，直到测量路径ksi所有测量位姿kp
ij
的最短距离d
min
判断完毕，然后，进入步骤s2.5，其中，在进行下一个测量位姿kp
i(j+1)
设置前，如果：
[0090]
a、测量位姿kp
i(j+1)
设置在机器人自由度退化，逆运动学无解的空间位姿，即奇异点附近时，为了保证机器人运动的流畅性需要设置一个过渡点来避开机器人的奇异点；
[0091]
b、当被测物的形貌不规则，被测物表面有凸起时，为了获得更好的测量效果以及避免在机器人承载结构光测量仪运动的过程中与不规则被测物相碰撞，在测量位姿kp
i(j+1)
设置时需要考虑机器人实际的测量位姿以及被测物的形貌特征，在测量位姿kp
ij
和测量位姿kp
i(j+1)
之间设置一个过渡点
[0092]
过渡点处不需要进行距离检测以及调整，过渡点为人为地对机器人的测量路径进行调整。
[0093]
步骤s2.5：对测量路径ksi进行路径评价
[0094]
所有测量位姿以及所有过度点构成一条完整的测量路径ksi。为了衡量测量路径ksi是否能作为完成测量任务机器人的可执行路径，从效率和安全性的角度对测量路径ksi进行路径评价，步骤如下：
[0095]
步骤s2.5.1：在虚拟仿真环境中执行测量路径ksi，机器人将从测量位姿kp
i1
连续地运动到测量位姿将运动过程进行等时间采样记录过程点集合构成机器人的实际运动路径为
[0096]
在测量路径ksi中，其起始位置坐标为(kpx
i1,k
py
i1,k
pz
i1
),终止位置坐标为则终止位置与起始位置之间的最短距离为：
[0097]
[0098]
在实际运动路径中，其起始位置坐标为终止位置坐标为则机器人实际运动的长度为：
[0099][0100]
两式相减可得：
[0101]
l＝l
2-l1[0102]
则评价函数f1(l)表示为：f1(l)＝(δ
1-l)/δ1,0≤l≤δ1，式中δ1为设置的最大误差阈值。
[0103]
步骤s2.5.2：在机器人的实际运动路径中找到一位姿到被测物质心坐标(px
obj
,py
obj
,pz
obj
)的距离l3最小，则距离l3表示为：
[0104][0105]
则评价函数f2(l3)表示为：
[0106]
f2(l3)＝(l
3-δ2)/l3[0107]
其中，δ2为保证结构光测量仪与被测物不碰撞的最小距离。
[0108]
步骤s2.5.3：对于测量路径ksi的综合评价函数
kfi
为：
[0109]
kfi
＝(0.5f1(l)+0.5f(l3))*100
[0110]
根据综合评价函数
kfi
对所设置的测量路径进行评价，当综合评价函数
kfi
＞g时通过评价，进入步骤2.7，否则进行步骤s2.6，其中g为根据不同测量场景由测量人员认为定义的阈值，0＜g＜100。
[0111]
步骤s2.6：修正路径
[0112]
遍历测量路径ksi中的所有位姿，找到与位姿距离最近的位姿kp
is
，之后将机器人运动到测量位姿kp
is
处，拖拽机器人末端手动增加当前机器人测量位姿与被测物之间的距离d
min
，如果位姿kp
is
是测量位姿，则调整范围仍需满足结构光测量仪的测量条件：d
min
∈[d-δ,d+δ]，如果位姿kp
is
是过渡点，需要根据过渡点的设置条件调整距离d
min
，调整后的位姿替换位姿从而完成对测量路径ksi的修正，然后进入步骤s2.7。
[0113]
步骤s2.7：对所有测量面的所有测量路径按照步骤s2.1到s2.6进行处理，完成测量路径规划仿真，将规划好的测量路径转换为机器人能够识别的通信报文由虚拟仿真环境发送给实体机器人。
[0114]
步骤s3：现实环境测量
[0115]
步骤s3.1：进行手眼标定，得相机坐标系相对于机器人末端法兰坐标系的旋转平移矩阵即手眼关系矩阵h
cg
。
[0116]
图3是粗拼接坐标系转换示意图。如图3所示，实体机器人、被测物以及结构光扫描仪(相机)的位置关系以及坐标系之间的转换如图3所示，其中，相机坐标系用下标c表示，机器人末端法兰坐标系用下标g表示，机器人基座坐标系用下标b表示。在机器人承载扫描仪完成测量任务之前需要先进行手眼标定，求解出结构光测量仪与机器人末端法兰盘的相对
位置关系矩阵h
cg
，具体为：
[0117]
步骤s3.1.1：首先调整结构光扫描仪的双目结构光相机，使其左右相机都能够拍摄到被测物对象，并根据张正友标定法完成相机标定。
[0118]
步骤s3.1.2：手动操作机器人分别从第q＝1,2,......r个位姿拍摄标定板，获取对应位姿下的标定板图片，同时记录机器人末端法兰盘的位姿坐标。
[0119]
步骤s3.1.3：根据张氏标定法获得每组标定板图片中标定板相对于相机的旋转向量r
cq
和平移向量t
cq
，并由其构成旋转平移矩阵，即得到相机的外参矩阵根据机器人末端法兰盘的位姿坐标可以计算出机器人的位姿矩阵
[0120]
步骤s3.1.4：由于任意两次(第p、q)位姿变换之间机器人基座到标定板的关系则有：
[0121][0122][0123]
其中，为第p次拍摄时机器人末端法兰盘到机器人基座坐标系的旋转平移矩阵，为第p次拍摄时的手眼关系矩阵，为第p次拍摄时相机到机器人坐标系的旋转平移矩阵。具有相同与第p次拍摄时相同的含义。
[0124]
由于两次拍摄中机器人基座到标定板之间的关系保持不变，则有机器人基座到标定板的旋转平移矩阵h
rc
：
[0125][0126]
由于两次变换中机器人法兰末端与相机的位姿关系保持不变，则有手眼关系矩阵h
cg
：
[0127][0128]
则经过处理后的手眼标定方程可写为：
[0129]
ax＝xb
[0130]
式中
[0131]
步骤s3.1.5：使用tsai-lenz算法求解手眼标定方程，进而求得相机坐标系相对于机器人末端法兰坐标系的旋转平移矩阵即手眼关系矩阵h
cg
。
[0132]
步骤s3.2：完成手眼标定后，开始对被测物进行测量，实体机器人接收到由虚拟仿真环境发送的测量路径后，依次运动到测量位姿kp
ij
处；
[0133]
步骤s3.3：在到达测量位姿kp
ij
处时，实体机器人将测量位姿kp
ij
发送给结构光测量仪，通过双目视觉相机获取当前测量位姿处被测物的单视角点云ki
ij
，并使用手眼关系矩阵h
cg
、根据测量位姿获得的机器人末端法兰坐标系到机器人基座标系的位姿矩阵将单视角点云ki
ij
转换到机器人基座标系下，具体为：
[0134]
步骤s3.3.1：在测量位姿kp
ij
处结构光测量仪的左右相机分别拍摄一幅深度图像，并根据双目相机原理进行点云重构，得到测量位姿kp
ij
下的单视角点云ki
ij
，并根据测量位姿kp
ij
获得位姿矩阵
[0135]
步骤s3.3.2：将单视角点云ki
ij
转换到机器人基座标系下构成新点云转换方法如下:
[0136][0137]
步骤s3.4：重复步骤s3.2、s3.3，得到机器人基座标系下的被测物点云ib即被测物点云粗拼接结果，机器人基座坐标系下的点云ib可以表示为：
[0138][0139]
然后观察点云ib是否完整即是否存在漏拍的现象，若发生漏拍则按照以下步骤进行补拍：
[0140]
步骤s3.4.1：对于漏拍点云位置，确定与漏拍点云位置相邻的两幅点云ki
ij
、ki
i(j+1)
，进而确定机器人的测量位姿kp
ij
与kp
i(j+1)
，最后确定漏拍位置实体机器人的测量位姿
[0141][0142]
步骤s3.4.2：操作实体机器人到达测量位姿获取当前测量位姿处被测物的单视角点云，并根据步骤s3.3将单视角点云转换到机器人的基座标系下，完成漏拍点云的补全。
[0143]
实例
[0144]
本实例中被测物是一个600mm*450mm*200mm的长方体，首先根据现实测量场景在装有ubuntu系统以及ros操作系统的计算机中搭建1:1虚拟仿真测量环境。虚拟仿真环境如图4所示，结构光测量仪安装与六自由度机器人末端的法兰盘上，被测物放置于机器人正前方的测量平台上。其次按照步骤s2中所描述的路径规划方式进行测量路径规划，选择被测物的一个面进行测量点的设置。之后选择其他测量面，同样按照步骤s2进行测量路径规划，所有测量路径设置完成后将路径信息发送个实体六自由度机器人。
[0145]
在现实测量环境中根据步骤s3.1-s3.4完成相机标定以及手眼标定，求解出手眼矩阵h
cg
，之后由六自由度机器人承载结构光测量仪根据不同的路径到达所设置的测量点处进行拍摄。图5为单个测量面下机器人承载结构光测量仪按照规划好的测量路径获取到的被测物的单视角点云。单个测量面上的单视角点云通过坐标系转换，统一到机器人基座坐标系下的结果如图6所示，可以看出在同一测量面上的单视角点云能够拼接成功。图7为最终的点云粗拼接结果，可以看出不同测量面上的点云经过点云粗拼接之后能够基本还原出被测物的三维形貌。
[0146]
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术邻域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术邻域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

技术特征：
1.一种机器人路径规划式自动三维扫描测量方法，其特征在于，包括：(1)、虚拟仿真环境搭建1.1)、根据现实测量环境，在装有开源机器人系统的计算机上搭建虚拟仿真环境，并将机器人的描述文件导入到虚拟仿真环境中，生成相应的机器人，同时，在虚拟仿真环境中，将结构光测量仪以及深度相机安装在机器人末端法兰盘的固定夹具上；1.2)、对被测物进行仿真，并放置于机器人和结构光测量仪的正前方，确定结构光测量仪对被测物进行拍摄的多个测量面以及每个测量面存在的多条测量路径，其中，第k个测量面的第i条测量路径记为
k
s
i k＝1,2,...,k,i＝1,2,...,m
k
，其中，k为测量面的个数，m
k
为第k个测量面上测量路径的条数，同时每一条测量路径上还包含多个测量点，被测物第k个测量面上的第i条路径中的第j个测量点即测量位姿记为
k
p
ij
,j＝1,2,...,n
k
，n
k
为第k个测量面测量路径上测量位姿的数量；结构光测量仪的视野范围为长m厘米，宽n厘米大小的矩形，被测物第k个测量面的外接矩形长为
k
a，宽为
k
b，测量路径为沿长度方向，一条条的纵向路径，则需要满足约束：同一测量面上的测量路径条数m
k
≥
k
a/m，同一测量路径上的测量点数n
k
≥
k
b/n，根据这一约束，得到第k个测量面的第i条测量路径的第j个测量位姿
k
p
ij
,j＝1,2,...,n
k
；(2)、测量路径规划2.1)、对于第k个测量面的第i条测量路径，首先在虚拟仿真环境中，拖拽机器人末端到测量位姿
k
p
ij
，其中，测量位姿
k
p
ij
为(
k
px
ij
,
k
py
ij
,
k
pz
ij
,
k
rx
ij
,
k
ry
ij
,
k
rz
ij
)，(
k
px
ij
,
k
py
ij
,
k
pz
ij
)为机器人末端的位置坐标，(
k
rx
ij
,
k
ry
ij
,
k
rz
ij
)为机器人末端的姿态坐标；2.2)、在机器人的测量位姿
k
p
ij
处，通过使用深度相机采集被测物图像，得到rgb-d图像并转换为灰度图像后，进行阈值分割过滤背景，再提取被测物轮廓信息，得到被测物轮廓所在的最小外接矩形之后在rgb-d图像相对应的最小外接矩形范围中遍历所有像素点的深度信息d，找到被测物与结构光测量仪之间的最短距离d
min
，同时记录下最短距离的像素点坐标a(u,v)，然后判断最短距离d
min
是否满足结构光测量仪的测量范围：d
min
∈[d-δ,d+δ]，其中，d为结构光测量仪进行相机标定时的焦距，δ为结构光测量仪测量允许的误差范围；若不满足，则进入步骤2.3)，对测量位姿进行调整，若满足，则进入步骤2.4)；2.3)、将像素点坐标a(u,v)经过坐标系转换获得与其相对应的空间坐标(x
(u,v)
,y
(u,v)
,z
(u,v)
)，然后结合机器人末端的位置坐标(
k
px
ij,k
py
ij,k
pz
ij
)确定一条空间直线：其中，x,y,z为空间直线上的坐标；然后，从位置坐标(
k
px
ij
,
k
py
ij
,
k
pz
ij
)起，沿着该空间直线，计算得到一个位置坐标满足测量条件：
得到满足测量条件的位置坐标之后，结合原有的位姿坐标(
k
rx
ij
,
k
ry
ij
,
k
rz
ij
)，组成一个更新测量位姿
k
p
ij
即根据测量位姿
k
p
ij
，由机器人运动学逆解出机器人对应关节状态，实现位姿调整；2.4)、将测量位姿
k
p
ij
记录下来，返回步骤2.1)，进行下一个测量位姿
k
p
i(j+1)
设置，直到测量路径
k
s
i
所有测量位姿
k
p
ij
的最短距离d
min
判断完毕，然后，进入步骤(3)，其中，在进行下一个测量位姿
k
p
i(j+1)
设置前，如果：a、测量位姿
k
p
ij+1
设置在机器人自由度退化，逆运动学无解的空间位姿，即奇异点附近时，为了保证机器人运动的流畅性需要设置一个过渡点来避开机器人的奇异点；b、当被测物的形貌不规则，被测物表面有凸起时，为了获得更好的测量效果以及避免在机器人承载结构光测量仪运动的过程中与不规则被测物相碰撞，在测量位姿
k
p
i(j+1)
设置时需要考虑机器人实际的测量位姿以及被测物的形貌特征，在测量位姿
k
p
ij
和测量位姿
k
p
i(j+1)
之间设置一个过渡点过渡点处不需要进行距离检测以及调整，过渡点为人为地对机器人的测量路径进行调整；2.5)、对测量路径
k
s
i
进行路径评价所有测量位姿以及所有过度点构成一条完整的测量路径
k
s
i
，对测量路径
k
s
i
进行路径评价，步骤如下：步骤2.5.1)、在虚拟仿真环境中执行测量路径
k
s
i
，机器人将从测量位姿
k
p
i1
连续地运动到测量位姿将运动过程进行等时间采样记录过程点集合构成机器人的实际运动路径为在测量路径
k
s
i
中，其起始位置坐标为(
k
px
i1,k
py
i1,k
pz
i1
),终止位置坐标为则终止位置与起始位置之间的最短距离为：在实际运动路径中，其起始位置坐标为终止位置坐标为则机器人实际运动的长度为：两式相减可得：l＝l
2-l1则评价函数f1(l)表示为：f1(l)＝(δ
1-l)/δ1,0≤l≤δ1，式中δ1为设置的最大误差阈值；步骤2.5.2)、在机器人的实际运动路径中找到一位姿到被测物质心坐标(px
obj
,py
obj
,pz
obj
)的距离l3最小，则距离l3表示为：
则评价函数f2(l3)表示为：f2(l3)＝(l
3-δ2)/l3其中，δ2为保证结构光测量仪与被测物不碰撞的最小距离；2.5.3)、对于测量路径
k
s
i
的综合评价函数
k
f
i
为：
k
f
i
＝(0.5f1(l)+0.5f(l3))*100根据综合评价函数
k
f
i
对所设置的测量路径进行评价，当综合评价函数
k
f
i
＞g时通过评价，进入步骤2.7)，否则进行步骤2.6)，其中g为根据不同测量场景由测量人员认为定义的阈值，0＜g＜100；2.6)、遍历测量路径
k
s
i
中的所有位姿，找到与位姿距离最近的位姿
k
p
is
，之后将机器人运动到测量位姿
k
p
is
处，拖拽机器人末端手动增加当前机器人测量位姿与被测物之间的距离d
min
，如果位姿
k
p
is
是测量位姿，则调整范围仍需满足结构光测量仪的测量条件：d
min
∈[d-δ,d+δ]，如果位姿
k
p
is
是过渡点，需要根据过渡点的设置条件调整距离d
min
，调整后的位姿替换位姿
k
p
is
，从而完成对测量路径
k
s
i
的修正，然后进入步骤2.7)；2.7)、对所有测量面的所有测量路径按照步骤2.1)到2.6)进行处理，完成测量路径规划仿真，将规划好的测量路径转换为机器人能够识别的通信报文由虚拟仿真环境发送给实体机器人；(3)、现实环境测量3.1)、进行手眼标定，得相机坐标系相对于机器人末端法兰坐标系的旋转平移矩阵即手眼关系矩阵h
cg
；3.2)、完成手眼标定后，开始对被测物进行测量，实体机器人接收到由虚拟仿真环境发送的测量路径后，依次运动到测量位姿
k
p
ij
处；3.3.)、在到达测量位姿
k
p
ij
处时，实体机器人将测量位姿
k
p
ij
发送给结构光测量仪，通过双目视觉相机获取当前测量位姿处被测物的单视角点云
k
i
ij
，并使用手眼关系矩阵h
cg
、根据测量位姿获得的机器人末端法兰坐标系到机器人基座标系的位姿矩阵
k
h
ijg
，将单视角点云
k
i
ij
转换到机器人基座标系下；3.4.)、重复步骤3.2)、3.3)，得到机器人基座标系下的被测物点云i
b
即被测物点云粗拼接结果，然后观察点云i
b
是否完整即是否存在漏拍的现象，若发生漏拍则按照以下步骤进行补拍：3.4.1)、对于漏拍点云位置，确定与漏拍点云位置相邻的两幅点云
k
i
ij
、
k
i
i(j+1)
，进而确定机器人的测量位姿
k
p
ij
与
k
p
i(j+1)
，最后确定漏拍位置实体机器人的测量位姿，最后确定漏拍位置实体机器人的测量位姿3.4.2)、操作实体机器人到达测量位姿获取当前测量位姿处被测物的单视角点云，并根据步骤3.3)将单视角点云转换到机器人的基座标系下，完成漏拍点云的补全。

技术总结
本发明公开了一种机器人路径规划式自动三维扫描测量方法，首先搭建虚拟仿真环境；然后在虚拟仿真环境上完成测量位姿以及测量路径的设置并对测量路径进行路径评价；然后在现实测量环境中完成结手眼标定，实体机器人承载结构光测量仪依次到达所设置的测量位姿处进行拍摄，获取多视角的点云图像；之后根据手眼标定结果，通过坐标系转换将多视角点云统一到机器人基座坐标系下完成点云粗拼接过程，最后在漏拍处对漏拍点云进行补全。本发明通过虚拟仿真平台进行机器人路径规划有效降低了多视角测量的试错风险，通过将点云转换到机器人基座坐标系下完成点云粗配准，为后续的精配准过程提供了良好的先验条件，减轻了后续配准任务的负担，提高了结构光测量仪的工作效率，缩短了任务周期。了任务周期。


技术研发人员：殷春 闫中宝 高延 李致博 陈凯 杨澳琳
受保护的技术使用者：电子科技大学
技术研发日：2023.03.17
技术公布日：2023/7/13