CN110340886B - 一种双目跟踪实现机器人路点迁移的方法及其系统 - Google Patents

Abstract

本发明涉及一种双目跟踪实现机器人路点迁移的方法及其系统，包括机器人按照多个第一预设位置对目标物体进行标定扫描，得到机器人手臂末端在机器人本体坐标系中的对应的多个第一标定路点位置坐标；获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；根据各部件位姿关系及预先对目标物体进行初次标定扫描过程得到的扫描仪坐标系和目标物体参考坐标系之间的初始位姿转换矩阵，获取机器人手臂末端坐标系和机器人本体坐标系之间的目标标定转换矩阵；根据目标标定转换矩阵对待标定路点进行标定得到目标路点。当部件位置发生变化时无需重新示教，新的路点通过第一次示教的路点求得。

Description

一种双目跟踪实现机器人路点迁移的方法及其系统

技术领域

本发明涉及智能光学室内跟踪技术领域，尤其涉及一种双目跟踪实现机器人路点迁移的方法及其系统。

背景技术

HyperScan光学追踪三维扫描仪(简称HyperScan)，其主要由ZG-Track跟踪器(简称跟踪器)和球形扫描仪(简称球扫)组成，球形扫描仪上包括靶球、相机和扫描器，且靶球设有多个标志反光点，通过跟踪器获取标志反光点在跟踪器坐标系(以跟踪器为中心构建的三维坐标系)中的坐标，球扫对目标物体进行扫描，分别获取目标物体表面的扫描点和标志反光点在球扫坐标系(以球扫为中心构建的三维坐标系)中的坐标，从而根据上述三个坐标构建目标物体表面，生成目标物体的三维模型。通常与轻型六自由度协作机器人结合使用时，可以实现对物体的快速定位和三维扫描检测，获取不同大小和尺寸的物体的三维数据，应用广泛。

但是在轻型六自由度协作机器人和HyperScan配合扫描过程中，机器人的路点示教(机器人的路点是指机器人手臂末端在机器人本体坐标系中的位置和姿态，机器人路点示教是指人工控制机器人手臂，依次设置和保存机器人路点)是非常繁琐耗时的。当各部件位置发生变化(例如工件位置移动、机器人位置移动、ZG-Track位置移动或者球扫与机器人手臂上的夹具角度变化等情况发生时)，都需要重新路点示教。例如，以车门为例，对车门进行扫描时，示教需要1个小时左右。但当车门位置移动、机器人位置移动、ZG-Track位置移动或者球扫与机器人手臂上的夹具角度变化等情况发生时，又需要重新示教，获取新的路点，示教又需要1个小时左右，非常繁琐耗时，不利于跟踪扫描的工作效率。

因此，亟需一种机器人路点迁移方法，实现在机器人和HyperScan配合追踪扫描过程中，只需要示教一次，当现场各部件位置发生变化时，可以快速地生成新的路点，无需重新路点示教。

发明内容

本发明所要解决的技术问题是针对上述现有技术的不足，提供一种双目跟踪实现机器人路点迁移的方法及其系统，当目标物体各部件位置发生变化(目标物体位置移动、机器人位置移动、跟踪器位置移动或者扫描仪与机器人手臂上的夹具角度变化等情况发生时)，无需重新示教机器人手臂末端的路点，新的路点只需要通过第一次示教的路点求得即可。

本发明解决上述技术问题的技术方案如下：

一种双目跟踪实现机器人路点迁移的方法，包括以下步骤：

步骤1：携带有扫描仪的机器人按照多个第一预设位置对目标物体进行标定扫描，得到在多个所述第一预设位置下，机器人手臂末端在机器人本体坐标系中对应的多个第一标定路点位置坐标；

步骤2：在所述标定扫描的过程中，分别获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及所述跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；

步骤3：根据所述第二位姿转换矩阵和多个所述第一标定路点位置坐标，得到所述跟踪器坐标系和所述机器人本体坐标系之间的第三位姿转换矩阵，以及得到所述扫描仪坐标系和机器人手臂末端坐标系之间的第四位姿转换矩阵；

步骤4：根据所述第一位姿转换矩阵、所述第三位姿转换矩阵和所述第四位姿转换矩阵，以及所述机器人预先对所述目标物体进行初次标定扫描的过程中，所述扫描仪坐标系和所述目标物体参考坐标系之间的初始位姿转换矩阵，获取所述机器人手臂末端坐标系和所述机器人本体坐标系之间的目标标定转换矩阵；

步骤5：根据所述目标标定转换矩阵对待标定路点进行标定，得到目标路点。

本发明的有益效果是：本发明中的坐标系包括以跟踪器为中心构建的跟踪器坐标系、以扫描仪为中心构建的坐标系、以目标物体的参考模型为中心构建的目标物体参考坐标系、以机器人的底座为中心构建的机器人本体坐标系和以机器人手臂末端为中心构建的机器人手臂末端坐标系；由于路点是指机器人手臂末端在机器人本体坐标系中的位姿，而当目标物体位置移动、机器人位置移动、跟踪器位置移动或者扫描仪与机器人手臂末端上的夹具角度变化等情况发生时，即在路点迁移过程中，扫描仪坐标系与目标物体参考坐标系之间的相对位置关系(即初始位姿转换矩阵)不变，而其他的坐标系之间的位姿转换关系会相应发生变化，因此为获得在路点迁移过程中，待标定路点和目标路点之间的转换关系(即目标标定转换矩阵)，通过预先对目标物体的初次标定扫描的过程中获得的扫描仪坐标系和目标物体参考坐标系之间的初始位姿转换矩阵，再通过按照多个第一预设位置对目标物体进行标定扫描，依次获得该标定扫描的过程中的第一位姿转换矩阵、第二位姿转换矩阵、第三位姿转换矩阵和第四位姿转换矩阵，求得在路点迁移过程中，待标定路点和目标路点之间的目标标定转换矩阵，通过该目标标定转换矩阵对待标定路点进行标定，即可得到在路点迁移过程中新的路点(即目标路点)；其中，多个第一标定路点位置坐标直接可根据对应的第一预设位置得到；

本发明双目跟踪实现机器人路点迁移的方法解决了机器人和HyperScan光学追踪三维扫描仪的配合扫描时，当各部件位置发生变化(目标物体位置移动、机器人位置移动、跟踪器位置移动或者扫描仪与机器人手臂上的夹具角度变化等情况发生时)，需要重新路点示教的问题，无需重新示教机器人手臂末端路点，新的路点可以通过第一次示教的路点求得，节省了目标物体的扫描时间，实现快速路点迁移和设置机器人新的路点的功能，大大提高扫描效率，特别适用于机器人和HyperScan组合扫描系统。

在上述技术方案的基础上，本发明还可以做如下改进：

进一步：在所述步骤1之前还包括以下步骤：

步骤0.1：对携带有所述扫描仪的所述机器人进行初次示教，得到初始示教路径；

步骤0.2：所述机器人按照所述初始示教路径自动扫描所述目标物体，在所述自动扫描的过程中，获取所述目标物体参考坐标系和所述跟踪器坐标系之间的第五位姿转换矩阵；

步骤0.3：所述机器人按照多个第二预设位置，预先对所述目标物体进行所述初次标定扫描，得到多个所述第二预设位置下，所述机器人手臂末端在所述机器人本体坐标系中对应的多个第二标定路点位置坐标；

步骤0.4：在所述初次标定扫描的过程中，分别获取所述跟踪器坐标系和所述机器人本体坐标系之间的第六位姿转换矩阵以及所述扫描仪坐标系和所述机器人手臂末端坐标系之间的第七位姿转换矩阵；

步骤0.5：根据所述第五位姿转换矩阵、所述第六位姿转换矩阵、所述第七位姿转换矩阵和多个所述第二标定路点位置坐标，得到所述初始位姿转换矩阵。

上述进一步方案的有益效果是：由于在路点迁移过程中，扫描仪坐标系与目标物体参考坐标系之间的相对位置关系(即初始位姿转换矩阵)不变，因此需要预先获取在各部件位置未发生变化之前的初始位姿转换矩阵；而该初始位姿转换矩阵需要通过在各部件位置未发生变换之前的第五位姿转换矩阵，以及按照多个第二预设位置进行初次标定扫描的过程中对应的多个第二标定路点位置坐标、第六位姿转换矩阵和第七位姿转换矩阵求得；其中，第五位姿转换矩阵是根据按照初始示教路径自动扫描目标物体得到的三维数据获得，初始示教路径中包含若干个初始示教路点，而第二预设位置为初始示教路点中选取的多个扫描点，因此可保证其对应的第二标定路点位置坐标为迁移之前的路点，并进一步保证按照该多个第二预设位置进行初次标定扫描得到的第六位姿转换矩阵和第七位姿转换矩阵也为路点迁移之前的位姿转换关系，从而保证得到路点迁移之前的初始位姿转换矩阵；

通过上述步骤获得初始位姿转换矩阵，便于后续根据该初始位姿转换矩阵得到目标标定转换矩阵，从而实现无需重新示教即可快速生成新的路点，大大节省目标物体的扫描效率。

进一步：所述第一预设位置和所述第二预设位置的数量均大于或等于20个。

上述进一步方案的有益效果是：由于一个预设位置对应一个第一标定路点位置坐标，而第三位姿转换矩阵和第四位姿转换矩阵中均分别有12个未知数，因此通过大于或等于20个以上的第一预设位置，可以方便后续联立公式得到对应的较为准确的第三位姿转换矩阵和第四位姿转换矩阵；同理，通过大于或等于20个以上的第二预设位置，可以方便后续联立公式得到对应的较为准确的第六位姿转换矩阵和第七位姿转换矩阵；其中，第一预设位置和第二预设位置可以相同，也可以不同。

进一步：在所述步骤2中，获取所述第一位姿转换矩阵的具体步骤包括：

步骤2.1：在所述标定扫描的过程中，获取在多个所述第一预设位置下，所述目标物体在所述跟踪器坐标系中对应的多个第一位置坐标；

步骤2.2：将多个所述第一位置坐标与所述目标物体参考坐标系进行拼接，得到所述第一位姿转换矩阵。

上述进一步方案的有益效果是：通过上述步骤可以方便获得在部件位置发生变化时，跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵，从而方便后续得到目标标定转换矩阵。

进一步：在所述步骤2中，获取所述第二位姿转换矩阵的具体步骤包括：

步骤2.3：在所述标定扫描的过程中，分别获取在多个所述第一预设位置下，所述扫描仪上的反光标志点在所述跟踪器坐标系中对应的多个第二位置坐标和在所述扫描仪坐标系中对应的多个第三位置坐标；

步骤2.4：根据任意至少三个所述第二位置坐标和对应的至少三个所述第三位置坐标，计算得到所述第二位姿转换矩阵；

在至少三个计算所述第二位姿转换矩阵的公式中，其中任一个公式具体为：

其中，J_StoT为所述第二位姿转换矩阵，

为所述反光标志点在所述跟踪器坐标系中的任一个所述第二位置坐标，

为所述反光标志点在所述球扫坐标系中，与所述第二位置坐标对应的一个所述第三位置坐标。

上述进一步方案的有益效果是：由于第二位姿转换矩阵

有12个未知数，而第二位置坐标和第三位置坐标均分别包含4个已知量，因此根据机器人和HyperScan组合扫描系统的原理，通过至少三个反光标志点在跟踪器坐标系中的第二位置坐标和球扫坐标系中的第三位置坐标，即可联立步骤2.4中的计算公式得到第二位姿转换矩阵；而每个第一预设位置会对应一个第二位置坐标和一个第三位置坐标，因此只需在多个第一预设位置处随机挑选至少三个，即可得到对应的第二位置坐标和第三位置坐标，进而实现第二位姿转换矩阵的求解。

进一步：所述步骤3的具体步骤包括：

步骤3.1：根据多个所述第一标定路点位置坐标分别得到所述机器人手臂末端坐标系和所述机器人本体坐标系之间对应的多个的参考标定转换矩阵；

步骤3.2：利用机器人标定函数关系式，根据所述第二位姿转换矩阵和所述参考标定转换矩阵，计算得到所述第三位姿转换矩阵和所述第四位姿转换矩阵；

所述机器人标定函数关系式为：

其中，J_EtoR″为所述参考标定转换矩阵，J_{EtoR_r}″为所述参考标定转换矩阵的旋转分量，J_{EtoR_t}″为所述参考标定转换矩阵的平移分量，J_TtoW为所述第一位姿转换矩阵，J_{StoT_r}为所述第二位姿转换矩阵的旋转分量，J_{StoT_t}为所述第二位姿转换矩阵的平移分量，J_TtoR为所述第三位姿转换矩阵，J_{TtoR_r}为所述第三位姿转换矩阵的旋转分量，J_{TtoR_t}为所述第三位姿转换矩阵的平移分量，J_StoE为所述第四位姿转换矩阵，J_{StoE_r}为所述第四位姿转换矩阵的旋转分量，J_{StoE_t}为所述第四位姿转换矩阵的平移分量。

上述进一步方案的有益效果是：由于多个第一预设位置为已知，因此根据该多个第一预设位置，可获得在路点迁移后的机器人手臂末端坐标系和机器人本体坐标系之间的多个参考标定转换矩阵，但该多个参考标定转换矩阵均并非目标标定转换矩阵，只是适用于对应的第一预设位置与原来的路点之间的迁移，而在机器人和HyperScan组合扫描系统的路点迁移过程中，包含大量的新的路点，因此结合机器人标定函数关系，可根据该多个参考标定转换矩阵以及步骤2中得到的第二位姿转换矩阵获得第三位姿转换矩阵和第四位姿转换矩阵，便于后续根据第一位姿转换矩阵、第三位姿转换矩阵、第四位姿转换矩阵以及初始位姿转换矩阵，得到适用于大量新的路点(即目标路点)情况下的目标标定转换矩阵，进一步得到该大量的新的目标路点，上述方法获得的目标标定转换矩阵通用性更强；

其中，针对机器人标定函数关系，由于从扫描仪坐标系转换到机器人本体坐标系有两种变换方式，包括从扫描仪坐标系变换到跟踪器坐标系再变换到机器人本体坐标系，和从扫描仪坐标系变换到机器人手臂末端坐标系再变换到机器人本体坐标系，而每一个位姿转换矩阵均包含旋转分量和平移分量，因此分别联立上述两种变换方式下旋转分量和平移分量的变换关系式，即可得到步骤3.2中所述的机器人标定函数关系式，从而便于求解第三位姿转换矩阵和第四位姿转换矩阵；其中，第三位姿转换矩阵和第四位姿转换矩阵中分别包含12个未知数，因此该机器人标定函数关系式包含24个未知数，通过至少20个的第一标定路点位置坐标一方面可列出至少40个方程组，即可求解出第三位姿转换矩阵和第四位姿转换矩阵，一方面还可以减小计算误差，提高计算准确率。

进一步：所述步骤4中的所述目标标定转换矩阵为：

其中，J_EtoR为所述目标标定转换矩阵，

为所述第四位姿转换矩阵的逆矩阵，J_StoW为所述初始位姿转换矩阵，

为所述第一位姿转换矩阵的逆矩阵。

上述进一步方案的有益效果是：根据各坐标系之间的变换，得到步骤4中的目标标定转换矩阵，从而方便根据该目标标定转换矩阵得到新的路点，即目标路点，而无需重复示教，即可实现快速生成新的路点，大大节省扫描效率。

依据本发明的另一方面，提供了一种双目跟踪实现机器人路点迁移的系统，应用于本发明中的双目跟踪实现机器人路点迁移的方法中，包括跟踪器、处理器和携带有扫描仪的机器人；

携带有所述扫描仪的所述机器人用于按照多个第一预设位置对目标物体进行标定扫描，得到在多个所述第一预设位置下，机器人手臂末端在机器人本体坐标系中对应的多个第一标定路点位置坐标；

所述跟踪器用于在所述标定扫描过程中，对所述机器人进行跟踪；

所述处理器用于：

在所述标定扫描的过程中，分别获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及所述跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；

根据所述第二位姿转换矩阵和多个所述第一标定路点位置坐标，得到所述跟踪器坐标系和所述机器人本体坐标系之间的第三位姿转换矩阵，以及得到所述扫描仪坐标系和机器人手臂末端坐标系之间的第四位姿转换矩阵；

根据所述第一位姿转换矩阵、所述第三位姿转换矩阵和所述第四位姿转换矩阵，以及所述机器人预先对所述目标物体进行初次标定扫描的过程中，所述扫描仪坐标系和所述目标物体参考坐标系之间的初始位姿转换矩阵，获取所述机器人手臂末端坐标系和所述机器人本体坐标系之间的目标标定转换矩阵；

根据所述目标标定转换矩阵对待标定路点进行标定，得到目标路点。

本发明的有益效果是：本发明双目跟踪实现机器人路点迁移的系统，解决了机器人和HyperScan光学追踪三维扫描仪的配合扫描时，当各部件位置发生变化(目标物体位置移动、机器人位置移动、跟踪器位置移动或者球扫与机器人手臂上的夹具角度变化等情况发生时)，需要重新路点示教的问题，无需重新示教机器人手臂末端路点，新的路点可以通过第一次示教的路点求得，节省了目标物体的扫描时间，实现快速路点迁移和设置机器人新的路点的功能，大大提高扫描效率，特别适用于机器人和HyperScan组合扫描系统。

在上述技术方案的基础上，本发明还可以做如下改进：

进一步：还包括控制器，所述控制器用于对所述机器人进行初次示教，得到初始示教路径；

所述机器人还用于按照所述初始示教路径自动扫描所述目标物体；还用于按照多个第二预设位置，预先对所述目标物体进行所述初次标定扫描，得到多个所述第二预设位置下，所述机器人手臂末端在所述机器人本体坐标系中对应的多个第二标定路点位置坐标；

所述处理器还用于：

在所述自动扫描的过程中，获取所述目标物体参考坐标系和所述跟踪器坐标系之间的第五位姿转换矩阵；

在所述初次标定扫描的过程中，分别获取所述跟踪器坐标系和所述目标物体参考坐标系之间的第六位姿转换矩阵以及所述跟踪器坐标系和所述扫描仪坐标系之间的第七位姿转换矩阵；

根据所述第五位姿转换矩阵、所述第六位姿转换矩阵、所述第七位姿转换矩阵和多个所述第二标定路点位置坐标，得到所述初始位姿转换矩阵。

上述进一步方案的有益效果是：通过控制器对目标物体的初次示教，以及处理器获取初始位姿转换矩阵，便于后续根据该初始位姿转换矩阵得到目标标定转换矩阵，从而实现无需重新示教即可快速生成新的路点，大大节省目标物体的扫描效率。

进一步：所述第一预设位置和所述第二预设位置的数量均大于或等于20个。

上述进一步方案的有益效果是：由于一个预设位置对应一个第一标定路点位置坐标，而第三位姿转换矩阵和第四位姿转换矩阵中均分别有12个未知数，因此通过大于或等于20个以上的第一预设位置，可以方便后续联立公式得到对应的第三位姿转换矩阵和第四位姿转换矩阵；同理，通过大于或等于20个以上的第二预设位置，可以方便后续联立公式得到对应的第六位姿转换矩阵和第七位姿转换矩阵；其中，第一预设位置和第二预设位置可以相同，也可以不同。

附图说明

图1为本发明实施例一中双目跟踪实现机器人路点迁移的方法的流程示意图；

图2为本发明实施例一中球扫坐标系的示意图；

图3为本发明实施例一中机器人本体坐标系和机器人手臂末端坐标系的示意图；

图4为本发明实施例一中获取初始位姿转换矩阵的流程示意图；

图5为本发明实施例一中获取第一位姿转换矩阵的流程示意图；

图6为本发明实施例一中获取第二位姿转换矩阵的流程示意图；

图7为本发明实施例一中获取第三位姿转换矩阵和第四位姿转换矩阵的流程示意图；

图8为本发明实施例一中获取第三位姿转换矩阵和第四位姿转换矩阵的模型示意图；

图9为本发明实施例一中获取目标标定转换矩阵的模型示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

下面结合附图，对本发明进行说明。

实施例一、如图1所示，一种双目跟踪实现机器人路点迁移的方法，包括以下步骤：

S1：携带有扫描仪的机器人按照多个第一预设位置对目标物体进行标定扫描，得到在多个所述第一预设位置下，机器人手臂末端在机器人本体坐标系中对应的多个第一标定路点位置坐标；

S2：在所述标定扫描的过程中，分别获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及所述跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；

S3：根据所述第二位姿转换矩阵和多个所述第一标定路点位置坐标，得到所述跟踪器坐标系和所述机器人本体坐标系之间的第三位姿转换矩阵，以及得到所述扫描仪坐标系和机器人手臂末端坐标系之间的第四位姿转换矩阵；

S4：根据所述第一位姿转换矩阵、所述第三位姿转换矩阵和所述第四位姿转换矩阵，以及所述机器人预先对所述目标物体进行初次标定扫描的过程中，所述扫描仪坐标系和所述目标物体参考坐标系之间的初始位姿转换矩阵，获取所述机器人手臂末端坐标系和所述机器人本体坐标系之间的目标标定转换矩阵；

步骤5：根据所述目标标定转换矩阵对待标定路点进行标定，得到目标路点。

本实施例双目跟踪实现机器人路点迁移的方法解决了机器人和HyperScan光学追踪三维扫描仪的配合扫描时，当各部件位置发生变化(目标物体位置移动、机器人位置移动、跟踪器位置移动或者扫描仪与机器人手臂上的夹具角度变化等情况发生时)，需要重新路点示教的问题，无需重新示教机器人手臂末端路点，新的路点可以通过第一次示教的路点求得，节省了目标物体的扫描时间，实现快速路点迁移和设置机器人新的路点的功能，大大提高扫描效率，特别适用于机器人和HyperScan组合扫描系统。

具体地，本实施例为六轴机器人和HyperScan光学追踪三维扫描仪构成的组合扫描系统，HyperScan光学追踪三维扫描仪由ZG-Track跟踪器(简称跟踪器)和球形扫描仪(简称球扫)组成，为方便后续获得各部件之间的相对位置关系(即各位姿转换矩阵)，将上述各部件构建出以下坐标系：

以ZG-Track跟踪器为中心构建的跟踪器坐标系(T系)；

以球扫为中心构建的球扫坐标系(S系)，如图2所示；

以目标物体的参考模型为中心构建的目标物体参考坐标系(W系)；

以机器人的底座为中心构建的机器人本体坐标系(R系)，如图3所示；

以机器人手臂末端为中心构建的机器人手臂末端坐标系(E系)，如图3所示。

因此本实施例所要得到的是在路点迁移过程中，待标定路点和目标路点之间的转换关系(即目标标定转换矩阵)，具体为J_EtoR。

具体地，本实施例的目标物体为车门。

优选地，如图4所示，在S1之前，即在首次工作现场(路点迁移之前)，包括以下步骤：

S0.1：对携带有球形扫描仪的所述机器人进行初次示教，得到初始示教路径；

S0.2：所述机器人按照所述初始示教路径自动扫描所述目标物体，在所述自动扫描的过程中，获取所述目标物体参考坐标系和所述跟踪器坐标系之间的第五位姿转换矩阵；

S0.3：所述机器人按照多个第二预设位置，预先对所述目标物体进行所述初次标定扫描，得到多个所述第二预设位置下，所述机器人手臂末端在所述机器人本体坐标系中对应的多个第二标定路点位置坐标；

S0.4：在所述初次标定扫描的过程中，分别获取所述跟踪器坐标系和所述机器人本体坐标系之间的第六位姿转换矩阵以及所述球扫坐标系和所述机器人手臂末端坐标系之间的第七位姿转换矩阵；

S0.5：根据所述第五位姿转换矩阵、所述第六位姿转换矩阵、所述第七位姿转换矩阵和多个所述第二标定路点位置坐标，得到所述初始位姿转换矩阵。

具体地，本实施例中ZG-Track跟踪器、六轴机器人、目标物体(车门)的位置摆放到位后，开始初次示教，并保存示教路点(即初始示教路径)；机器人按照初始示教路径自动扫描目标物体，得到目标物体在T系下的三维数据，并与目标物体的参考模型做拼接，得到T系和W系之间的第五位姿转换矩阵J_TtoW′；机器人携带球扫，在空中移动、旋转20个的位置，记录此时的路点即为第二标定路点位置坐标，对应20个已知的机器人手臂末端在R系中的位姿J_EtoR′；并获取此时T系和S系之间的第八位姿转换矩阵J′_StoT，再根据对应的机器人标定函数计算得到标定结果，包括T系和R系之间的J_TtoR′以及S系和E系之间的J_StoE′；

此时对应的机器人标定函数为：

最后根据20个已知的机器人手臂末端在R系中的位姿J_EtoR′，将_EoJ_tS′变换为J_StoR′,再通过J_TtoR′变换为J_StoT′，最后通过J_TtoW′得到J_StoW，其中J_StoW是机器人路点迁移中的不变量。

通过上述步骤获得初始位姿转换矩阵，便于后续根据该初始位姿转换矩阵得到目标标定转换矩阵，从而实现无需重新示教即可快速生成新的路点，大大节省目标物体的扫描效率。

优选地，如图5所示，在S2中，获取所述第一位姿转换矩阵的具体步骤包括：

S2.1：在所述标定扫描的过程中，获取在多个所述第一预设位置下，所述目标物体在所述跟踪器坐标系中对应的多个第一位置坐标；

S2.2：将多个所述第一位置坐标与所述目标物体参考坐标系进行拼接，得到所述第一位姿转换矩阵。

优选地，如图6所示，在S2中，获取所述第二位姿转换矩阵的具体步骤包括：

S2.3：在所述标定扫描的过程中，分别获取在多个所述第一预设位置下，所述球形扫描仪上的反光标志点在所述跟踪器坐标系中对应的多个第二位置坐标和在所述球扫坐标系中对应的多个第三位置坐标；

S2.4：根据任意至少三个所述第二位置坐标和对应的至少三个所述第三位置坐标，计算得到所述第二位姿转换矩阵；

在至少三个计算所述第二位姿转换矩阵的公式中，其中任一个公式具体为：

其中，J_StoT为所述第二位姿转换矩阵，

为所述反光标志点在所述跟踪器坐标系中的任一个所述第二位置坐标，

为所述反光标志点在所述球扫坐标系中，与所述第二位置坐标对应的一个所述第三位置坐标。

具体地，本实施例当把六轴机器人和HyperScan光学追踪三维扫描仪构成的组合扫描系统搬迁到新的工作现场时(即发生路点迁移时)，可通过人手持扫描仪，或机器人携带球扫，重新扫描目标物体(车门)，得到目标物体在T系下的三维数据，并与目标物体的参考模型做拼接，得到T系和W系之间的位姿关系，即第一位姿转换矩阵J_TtoW；然后将ZG-Track跟踪器、六轴机器人、目标物体的位置摆放到位，机器人携带球扫，再在空中移动、旋转20个的位置，记录此时的标定路点即为第一标定路点位置坐标，同时获得这20个位置下对应的第二位置坐标和第三位置坐标，分别随机挑选至少3个代入计算公式中求解，得到第二位姿转换矩阵J_StoT。

通过上述步骤可以方便获得在部件位置发生变化时，T系和W系之间的第一位姿转换矩阵以及S系和T系之间的第二位姿转换矩阵，从而方便后续得到目标标定转换矩阵。

优选地，如图7所示，S3的具体步骤包括：

S3.1：根据多个所述第一标定路点位置坐标分别得到所述机器人手臂末端坐标系和所述机器人本体坐标系之间对应的多个的参考标定转换矩阵；

S3.2：利用机器人标定函数关系式，根据所述第二位姿转换矩阵和所述参考标定转换矩阵，计算得到所述第三位姿转换矩阵和所述第四位姿转换矩阵；

所述机器人标定函数关系式为：

其中，J_EtoR″为所述参考标定转换矩阵，J_{EtoR_r}″为所述参考标定转换矩阵的旋转分量，J_{EtoR_t}″为所述参考标定转换矩阵的平移分量，J_TtoW为所述第一位姿转换矩阵，J_{StoT_r}为所述第二位姿转换矩阵的旋转分量，J_{StoT_t}为所述第二位姿转换矩阵的平移分量，J_TtoR为所述第三位姿转换矩阵，J_{TtoR_r}为所述第三位姿转换矩阵的旋转分量，J_{TtoR_t}为所述第三位姿转换矩阵的平移分量，J_StoE为所述第四位姿转换矩阵，J_{StoE_r}为所述第四位姿转换矩阵的旋转分量，J_{StoE_t}为所述第四位姿转换矩阵的平移分量。

具体地，将前述求得的J_StoT和已经记录的20个第一标定路点位置坐标(对应20个参考标定转换矩阵J_EtoR″)，代入S3.2中的机器人标定函数关系式，计算得到标定结果，包括T系和R系之间的J_TtoR以及S系和E之间的J_StoE，上述计算过程的模型示意图如图8所示。

通过上述步骤得到的第三位姿转换矩阵和第四位姿转换矩阵，可便于后续根据第一位姿转换矩阵、第三位姿转换矩阵、第四位姿转换矩阵以及初始位姿转换矩阵，得到适用于大量新的路点(即目标路点)情况下的目标标定转换矩阵，进一步得到该大量的新的目标路点。

优选地，S4中的所述目标标定转换矩阵为：

其中，J_EtoR为所述目标标定转换矩阵，

为所述第四位姿转换矩阵的逆矩阵，J_StoW为所述初始位姿转换矩阵，

为所述第一位姿转换矩阵的逆矩阵。

具体地，由于T系与W系之间存在位姿转换关系J_TtoW，T系与R系之间存在位姿转换关系J_TtoR，且前述S0.5得到的S系与W系之间存在位姿转换关系J_StoW，得到S系与R之间的位姿转换关系为：

再根据S系与E系之间的位姿转换关系J_StoE，得到E系与R系之间的位姿转换关系，即目标标定转换矩阵J_EtoR，上述计算过程的模型示意图如图9所示。

根据各坐标系之间的变换，得到目标标定转换矩阵，从而方便根据该目标标定转换矩阵得到新的路点，即目标路点，而无需重复示教，即可实现快速生成新的路点，大大节省扫描效率。

具体地，本实施例还调用机器人厂家提供的机械臂逆解函数，将得到的目标路点中的位置和姿态，转换为机器人的六个关节角，从而完整地实现路点的迁移，无需重新示教。

实施例二、一种双目跟踪实现机器人路点迁移的系统，应用于本发明中的双目跟踪实现机器人路点迁移的方法中，包括跟踪器、处理器和携带有扫描仪的机器人；

携带有所述扫描仪的所述机器人用于按照多个第一预设位置对目标物体进行标定扫描，得到在多个所述第一预设位置下，机器人手臂末端在机器人本体坐标系中对应的多个第一标定路点位置坐标；

所述跟踪器用于在所述标定扫描过程中，对所述机器人进行跟踪；

所述处理器用于：

在所述标定扫描的过程中，分别获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及所述跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；

根据所述第二位姿转换矩阵和多个所述第一标定路点位置坐标，得到所述跟踪器坐标系和所述机器人本体坐标系之间的第三位姿转换矩阵，以及得到所述球扫坐标系和机器人手臂末端坐标系之间的第四位姿转换矩阵；

根据所述第一位姿转换矩阵、所述第三位姿转换矩阵和所述第四位姿转换矩阵，以及所述机器人预先对所述目标物体进行初次标定扫描的过程中，所述扫描仪坐标系和所述目标物体参考坐标系之间的初始位姿转换矩阵，获取所述机器人手臂末端坐标系和所述机器人本体坐标系之间的目标标定转换矩阵；

根据所述目标标定转换矩阵对待标定路点进行标定，得到目标路点。

具体地，本实施例中的扫描仪为球形扫描仪，球形扫描仪和机器人分别如图2和图3所示。本实施例的双目跟踪实现机器人路点迁移的系统，解决了机器人和HyperScan光学追踪三维扫描仪的配合扫描时，当各部件位置发生变化(目标物体位置移动、机器人位置移动、跟踪器位置移动或者球扫与机器人手臂上的夹具角度变化等情况发生时)，需要重新路点示教的问题，无需重新示教机器人手臂末端路点，新的路点可以通过第一次示教的路点求得，节省了目标物体的扫描时间，实现快速路点迁移和设置机器人新的路点的功能，大大提高扫描效率，特别适用于机器人和HyperScan组合扫描系统。

优选地，还包括控制器，所述控制器用于对所述机器人进行初次示教，得到初始示教路径；

所述机器人还用于按照所述初始示教路径自动扫描所述目标物体；还用于按照多个第二预设位置，预先对所述目标物体进行所述初次标定扫描，得到多个所述第二预设位置下，所述机器人手臂末端在所述机器人本体坐标系的对应的多个第二标定路点位置坐标；

所述处理器还用于：

在所述自动扫描的过程中，获取所述目标物体参考坐标系和所述跟踪器坐标系之间的第五位姿转换矩阵；

在所述初次标定扫描的过程中，分别获取所述跟踪器坐标系和所述目标物体参考坐标系之间的第六位姿转换矩阵以及所述跟踪器坐标系和所述扫描仪坐标系之间的第七位姿转换矩阵；

根据所述第五位姿转换矩阵、所述第六位姿转换矩阵、所述第七位姿转换矩阵和多个所述第二标定路点位置坐标，得到所述初始位姿转换矩阵。

通过控制器对目标物体的初次示教，以及处理器获取初始位姿转换矩阵，便于后续根据该初始位姿转换矩阵得到目标标定转换矩阵，从而实现无需重新示教即可快速生成新的路点，大大节省目标物体的扫描效率。

具体地，本实施例中第一预设位置和第二预设位置的数量均为20个。

通过20个第一预设位置，可以方便后续联立公式得到对应的第三位姿转换矩阵和第四位姿转换矩阵；同理，通过20个第二预设位置，可以方便后续联立公式得到对应的第六位姿转换矩阵和第七位姿转换矩阵；其中，第一预设位置和第二预设位置可以相同，也可以不同。

本实施例中关于双目跟踪实现机器人路点迁移的方法的未尽细节，详见实施例一和图1至9的具体描述，此处不再赘述。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种双目跟踪实现机器人路点迁移的方法，其特征在于，包括以下步骤：

步骤1：携带有扫描仪的机器人按照多个第一预设位置对目标物体进行标定扫描，得到在多个所述第一预设位置下，机器人手臂末端在机器人本体坐标系中对应的多个第一标定路点位置坐标；

步骤2：在所述标定扫描的过程中，分别获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及所述跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；

步骤3：根据所述第二位姿转换矩阵和多个所述第一标定路点位置坐标，得到所述跟踪器坐标系和所述机器人本体坐标系之间的第三位姿转换矩阵，以及得到所述扫描仪坐标系和机器人手臂末端坐标系之间的第四位姿转换矩阵；

步骤4：根据所述第一位姿转换矩阵、所述第三位姿转换矩阵和所述第四位姿转换矩阵，以及所述机器人预先对所述目标物体进行初次标定扫描的过程中，所述扫描仪坐标系和所述目标物体参考坐标系之间的初始位姿转换矩阵，获取所述机器人手臂末端坐标系和所述机器人本体坐标系之间的目标标定转换矩阵；

步骤5：根据所述目标标定转换矩阵对待标定路点进行标定，得到目标路点。

2.根据权利要求1所述的双目跟踪实现机器人路点迁移的方法，其特征在于，在所述步骤1之前还包括以下步骤：

步骤0.1：对携带有所述扫描仪的所述机器人进行初次示教，得到初始示教路径；

步骤0.2：所述机器人按照所述初始示教路径自动扫描所述目标物体，在所述自动扫描的过程中，获取所述目标物体参考坐标系和所述跟踪器坐标系之间的第五位姿转换矩阵；

步骤0.3：所述机器人按照多个第二预设位置，预先对所述目标物体进行所述初次标定扫描，得到多个所述第二预设位置下，所述机器人手臂末端在所述机器人本体坐标系中对应的多个第二标定路点位置坐标；

步骤0.4：在所述初次标定扫描的过程中，分别获取所述跟踪器坐标系和所述机器人本体坐标系之间的第六位姿转换矩阵以及所述扫描仪坐标系和所述机器人手臂末端坐标系之间的第七位姿转换矩阵；

步骤0.5：根据所述第五位姿转换矩阵、所述第六位姿转换矩阵、所述第七位姿转换矩阵和多个所述第二标定路点位置坐标，得到所述初始位姿转换矩阵。

3.根据权利要求2所述的双目跟踪实现机器人路点迁移的方法，其特征在于，所述第一预设位置和所述第二预设位置的数量均大于或等于20个。

4.根据权利要求1所述的双目跟踪实现机器人路点迁移的方法，其特征在于，在所述步骤2中，获取所述第一位姿转换矩阵的具体步骤包括：

步骤2.1：在所述标定扫描的过程中，获取在多个所述第一预设位置下，所述目标物体在所述跟踪器坐标系中对应的多个第一位置坐标；

步骤2.2：将多个所述第一位置坐标与所述目标物体参考坐标系进行拼接，得到所述第一位姿转换矩阵。

5.根据权利要求4所述的双目跟踪实现机器人路点迁移的方法，其特征在于，在所述步骤2中，获取所述第二位姿转换矩阵的具体步骤包括：

步骤2.3：在所述标定扫描的过程中，分别获取在多个所述第一预设位置下，所述扫描仪上的反光标志点在所述跟踪器坐标系中对应的多个第二位置坐标和在所述扫描仪坐标系中对应的多个第三位置坐标；

步骤2.4：根据任意至少三个所述第二位置坐标和对应的至少三个所述第三位置坐标，计算得到所述第二位姿转换矩阵；

在至少三个计算所述第二位姿转换矩阵的公式中，其中任一个公式具体为：

其中，J_StoT为所述第二位姿转换矩阵，

为所述反光标志点在所述跟踪器坐标系中的任一个所述第二位置坐标，

为所述反光标志点在所述扫描仪坐标系中，与所述第二位置坐标对应的一个所述第三位置坐标。

6.根据权利要求5所述的双目跟踪实现机器人路点迁移的方法，其特征在于，所述步骤3的具体步骤包括：

步骤3.1：根据多个所述第一标定路点位置坐标分别得到所述机器人手臂末端坐标系和所述机器人本体坐标系之间对应的多个参考标定转换矩阵；

步骤3.2：利用机器人标定函数关系式，根据所述第二位姿转换矩阵和所述参考标定转换矩阵，计算得到所述第三位姿转换矩阵和所述第四位姿转换矩阵；

所述机器人标定函数关系式为：

其中，J_EtoR″为所述参考标定转换矩阵，J_{EtoR_r}″为所述参考标定转换矩阵的旋转分量，J_{EtoR_t}″为所述参考标定转换矩阵的平移分量，J_TtoW为所述第一位姿转换矩阵，J_{StoT_r}为所述第二位姿转换矩阵的旋转分量，J_{StoT_t}为所述第二位姿转换矩阵的平移分量，J_TtoR为所述第三位姿转换矩阵，J_{TtoR_r}为所述第三位姿转换矩阵的旋转分量，J_{TtoR_t}为所述第三位姿转换矩阵的平移分量，J_StoE为所述第四位姿转换矩阵，J_{StoE_r}为所述第四位姿转换矩阵的旋转分量，J_{StoE_t}为所述第四位姿转换矩阵的平移分量。

7.根据权利要求6所述的双目跟踪实现机器人路点迁移的方法，其特征在于，所述步骤4中的所述目标标定转换矩阵为：

其中，J_EtoR为所述目标标定转换矩阵，

为所述第四位姿转换矩阵的逆矩阵，J_StoW为所述初始位姿转换矩阵，

为所述第一位姿转换矩阵的逆矩阵。

8.一种双目跟踪实现机器人路点迁移的系统，其特征在于，应用于如权利要求1至7任一项权利要求所述的机器人路点迁移的方法中，包括跟踪器、处理器和携带有扫描仪的机器人；

携带有所述扫描仪的所述机器人用于按照多个第一预设位置对目标物体进行标定扫描，得到在多个所述第一预设位置下，机器人手臂末端在机器人本体坐标系中对应的多个第一标定路点位置坐标；

所述跟踪器用于在所述标定扫描过程中，对所述机器人进行跟踪；

所述处理器用于：

在所述标定扫描的过程中，分别获取跟踪器坐标系和目标物体参考坐标系之间的第一位姿转换矩阵以及所述跟踪器坐标系和扫描仪坐标系之间的第二位姿转换矩阵；

根据所述第二位姿转换矩阵和多个所述第一标定路点位置坐标，得到所述跟踪器坐标系和所述机器人本体坐标系之间的第三位姿转换矩阵，以及得到所述扫描仪坐标系和机器人手臂末端坐标系之间的第四位姿转换矩阵；

根据所述第一位姿转换矩阵、所述第三位姿转换矩阵和所述第四位姿转换矩阵，以及所述机器人预先对所述目标物体进行初次标定扫描的过程中，所述扫描仪坐标系和所述目标物体参考坐标系之间的初始位姿转换矩阵，获取所述机器人手臂末端坐标系和所述机器人本体坐标系之间的目标标定转换矩阵；

根据所述目标标定转换矩阵对待标定路点进行标定，得到目标路点。

9.根据权利要求8所述的双目跟踪实现机器人路点迁移的系统，其特征在于，还包括控制器，所述控制器用于对所述机器人进行初次示教，得到初始示教路径；

所述机器人还用于按照所述初始示教路径自动扫描所述目标物体；还用于按照多个第二预设位置，预先对所述目标物体进行所述初次标定扫描，得到多个所述第二预设位置下，所述机器人手臂末端在所述机器人本体坐标系中对应的多个第二标定路点位置坐标；

所述处理器还用于：

在所述自动扫描的过程中，获取所述目标物体参考坐标系和所述跟踪器坐标系之间的第五位姿转换矩阵；

在所述初次标定扫描的过程中，分别获取所述跟踪器坐标系和所述目标物体参考坐标系之间的第六位姿转换矩阵以及所述跟踪器坐标系和所述扫描仪坐标系之间的第七位姿转换矩阵；

根据所述第五位姿转换矩阵、所述第六位姿转换矩阵、所述第七位姿转换矩阵和多个所述第二标定路点位置坐标，得到所述初始位姿转换矩阵。

10.根据权利要求9所述的双目跟踪实现机器人路点迁移的系统，其特征在于，所述第一预设位置和所述第二预设位置的数量均大于或等于20个。

Images