多基站激光跟踪仪协同转站的双臂机器人测试方法及装置与流程

本发明涉及机器人测试技术领域，更具体涉及多基站激光跟踪仪协同转站的双臂机器人测试方法及装置。

背景技术：

双臂工业机器人是指采用一套控制系统，控制两套操作机共同完成一项或一组操作任务的工业机器人。双臂机器人模仿了人体双臂的协作原理，具备双臂分别操作功能，由两个空间关节工业机器人单臂构成，具有高速高精度、超大作业空间、长寿命、模块化、高集成度的特点，适合定向空间负载作业，可采用植入式结构，特别适合板材制造上下料作业。在机器人行业与自动化生产中存在许多复杂的装配任务和零件搬运任务，双臂机器人系统为其提供了协作解决方法。双臂机器人目前成为机器人领域的热点，尤其在协作机器人的开发设计方面至关重要。

激光跟踪仪是一台以激光为测距手段配以反射标靶的仪器，它同时配有绕两个轴转动的测角机构，形成一个完整的球坐标测量系统。可以用它来测量静止目标，跟踪和测量移动目标或它们的组合。在利用激光跟踪仪测量各类部件或装配对象的时候，当测量范围不够或激光“视线”被挡住，需要移动激光跟踪仪并进行转站。转站后两次测量的坐标系一致，这样才能完整描述测量对象的位置、姿态信息同样，当多台跟踪仪在不同位置时，也要通过转站使它们的测量坐标系一致才能描述物体的位姿信息。

转站实质上就是测量一些在外部坐标系下已知坐标值的点，经过计算得到当前激光跟踪仪相对于外部坐标系的位姿，应用程序将该位姿参数写入跟踪仪控制器，这样跟踪仪就能将在仪器坐标系下测得的值经过坐标转换得到它们在外部坐标系下的值并发送给应用程序。所以，转站精度将极大的影响测量精度。

现有的单台激光跟踪仪测试方法只能测量单臂机器人，无法测量双臂机器人或无法达到双臂机器人要求的测量精度，需要应用多基站激光跟踪仪进行测量。多基站激光跟踪仪在不同位置时，要通过协同转站使它们的测量坐标系统一，才能描述物体的位姿信息。

中国专利申请号201910510332.7，公开了一种基于激光跟踪仪的双臂机器人组合操作测试方法，所述双臂机器人包括第一操作机和第二操作机，第一操作机和第二操作机分别操控工件1和工件2工作时，使用激光跟踪仪分别测量第一操作机和第二操作机的位姿参数，并通过计算第一操作机和第二操作机位姿参数的偏差值和加权平均值，求得双臂组合位姿准确度和重复性、双臂组合轨迹准确度和速度准确度。该发明只是描述双臂机器人对工件组合体组合操控的完成度，并不涉及具体的基于多基站激光跟踪仪协同转站控制，不能实现多基站激光跟踪仪的坐标系统一。

技术实现要素：

本发明所要解决的技术问题在于现有技术缺乏多基站激光跟踪仪协同转站的双臂机器人测试方法及装置，不能实现多基站激光跟踪仪的坐标系统一。

本发明通过以下技术手段实现解决上述技术问题的：多基站激光跟踪仪协同转站的双臂机器人测试方法，双臂机器人包括第一操作机和第二操作机，用两台激光跟踪仪测量第一操作机和第二操作机，为保证最终测量的第一操作机和第二操作机的坐标系一致，通过激光跟踪仪协同转站的方法将两坐标系统一，所述方法包括：

步骤一：激光跟踪仪测量获取目标点空间坐标；

步骤二：将目标点空间坐标作为转站前点坐标，根据转站前点坐标获取转站后点坐标的表达式；

步骤三：构建线性eiv模型；

步骤四：根据线性eiv模型构建拉格朗日极值条件式；

步骤五：对拉格朗日极值条件式中各分量求偏导并令偏导等于零求得当前待求参数向量，将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

本发明双臂机器人采用一套控制系统，控制两套操作机共同完成一项或一组操作任务，本发明通过激光跟踪仪协同转站的方式，分别用两台激光跟踪仪测量第一操作机和第二操作机，为保证最终测量的第一操作机和第二操作机的坐标系一致，通过激光跟踪仪转站的方式将两坐标系统一，通过本发明的转站算法将第一操作机的基座坐标系和第二操作机的基座坐标系作为参考坐标系变换到世界坐标系，完成转站过程，统一坐标系，提供多基站激光跟踪仪协同转站的双臂机器人测试方法及装置，弥补现有技术的欠缺。

进一步地，所述步骤一包括：

通过激光跟踪仪测量其与目标点的距离d及其水平方向的偏转角α和垂直方向的偏转角β，通过公式p(x,y,z)＝f(d,α,β)获取目标点空间坐标，其中，

进一步地，所述步骤二包括：

将目标点空间坐标作为转站前点坐标，对于每个站位通过公式pi′＝srpi+t＝spi+t获取转站后点坐标的表达式，其中，pi为第i组目标点转站前点坐标，t＝(t1,t2,t3)^t，t表示平移向量且该向量中元素为未知待测量，r为旋转矩阵且该矩阵中元素为预设常数，s为比例因子，s为比例旋转矩阵且该矩阵中元素为未知待测量；

将目标点空间坐标作为系数矩阵的元素转换表达形式，通过公式li＝aiξ获取观测向量，其中，ai为系数矩阵且

xi、yi、zi为第i组目标点空间坐标也即第i组目标点转站前点坐标；

ξ为待求参数向量且

ξ＝[r1r2r3r4r5r6r7r8r9t1t2t3]。

进一步地，所述步骤三包括：

通过公式l+el＝(a+ea)ξ构建线性eiv模型，其中，l表示观测向量的真值，a表示系数矩阵的真值，ea表示系数矩阵的误差矩阵且vec表示拉直算子，vec(ea)表示系数矩阵的误差矩阵按列拉伸后的向量，el表示观测向量的误差向量且ql表示观测向量的真值的协方差矩阵，qa表示系数矩阵的真值的协方差矩阵。

进一步地，所述步骤四包括：

根据观测向量的误差向量、系数矩阵的误差矩阵按列拉伸后的向量以及待求参数向量，通过公式

对线性eiv模型进行转化，其中，表示克罗内克积，ξ0表示初始待求参数向量，表示初始误差值，δξ表示第一改正值，δea表示第二改正值，且ξ＝ξ0+δξ，

通过公式构建拉格朗日极值条件式，其中，λ为第一预设常数，μ为第二预设常数。

进一步地，所述步骤五包括：

假设拉格朗日极值条件式中函数连续，对拉格朗日极值条件式中各分量求偏导并令偏导等于零，求解得其中，

将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

进一步地，还包括步骤六：

利用公式获取单位权重误差，利用单位权重误差判断转站准确度，其中，n为观测数，m为未知量个数。

本发明还提供多基站激光跟踪仪协同转站的双臂机器人测试装置，双臂机器人包括第一操作机和第二操作机，用两台激光跟踪仪测量第一操作机和第二操作机，为保证最终测量的第一操作机和第二操作机的坐标系一致，通过激光跟踪仪协同转站的方法将两坐标系统一，所述装置包括：

目标点坐标获取模块，用于激光跟踪仪测量获取目标点空间坐标；

表达式获取模块，用于将目标点空间坐标作为转站前点坐标，根据转站前点坐标获取转站后点坐标的表达式；

模型构建模块，用于构建线性eiv模型；

拉格朗日极值条件式构建模块，用于根据线性eiv模型构建拉格朗日极值条件式；

转站后点坐标获取模块，用于对拉格朗日极值条件式中各分量求偏导并令偏导等于零求得当前待求参数向量，将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

进一步地，所述目标点坐标获取模块用于：

通过激光跟踪仪测量其与目标点的距离d及其水平方向的偏转角α和垂直方向的偏转角β，通过公式p(x,y,z)＝f(d,α,β)获取目标点空间坐标，其中，

进一步地，所述表达式获取模块用于：

将目标点空间坐标作为转站前点坐标，对于每个站位通过公式pi′＝srpi+t＝spi+t获取转站后点坐标的表达式，其中，pi为第i组目标点转站前点坐标，t＝(t1,t2,t3)^t，t表示平移向量且该向量中元素为未知待测量，r为旋转矩阵且该矩阵中元素为预设常数，s为比例因子，s为比例旋转矩阵且该矩阵中元素为未知待测量；

将目标点空间坐标作为系数矩阵的元素转换表达形式，通过公式li＝aiξ获取观测向量，其中，ai为系数矩阵且

xi、yi、zi为第i组目标点空间坐标也即第i组目标点转站前点坐标；

ξ为待求参数向量且

ξ＝[r1r2r3r4r5r6r7r8r9t1t2t3]。

进一步地，所述模型构建模块用于：

通过公式l+el＝(a+ea)ξ构建线性eiv模型，其中，l表示观测向量的真值，a表示系数矩阵的真值，ea表示系数矩阵的误差矩阵且vec表示拉直算子，vec(ea)表示系数矩阵的误差矩阵按列拉伸后的向量，el表示观测向量的误差向量且ql表示观测向量的真值的协方差矩阵，qa表示系数矩阵的真值的协方差矩阵。

进一步地，所述拉格朗日极值条件式构建模块用于：

根据观测向量的误差向量、系数矩阵的误差矩阵按列拉伸后的向量以及待求参数向量，通过公式

对线性eiv模型进行转化，其中，表示克罗内克积，ξ0表示初始待求参数向量，表示初始误差值，δξ表示第一改正值，δea表示第二改正值，且ξ＝ξ0+δξ，

通过公式构建拉格朗日极值条件式，其中，λ为第一预设常数，μ为第二预设常数。

进一步地，所述转站后点坐标获取模块用于：

假设拉格朗日极值条件式中函数连续，对拉格朗日极值条件式中各分量求偏导并令偏导等于零，求解得其中，

将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

进一步地，还包括转站准确度判断模块，用于：

利用公式获取单位权重误差，利用单位权重误差判断转站准确度，其中，n为观测数，m为未知量个数。

本发明的优点在于：本发明双臂机器人采用一套控制系统，控制两套操作机共同完成一项或一组操作任务，本发明通过激光跟踪仪协同转站的方式，分别用两台激光跟踪仪测量第一操作机和第二操作机，为保证最终测量的第一操作机和第二操作机的坐标系一致，通过激光跟踪仪转站的方式将两坐标系统一，通过本发明的转站算法将第一操作机的基座坐标系和第二操作机的基座坐标系作为参考坐标系变换到世界坐标系，完成转站过程，统一坐标系，提供多基站激光跟踪仪协同转站的双臂机器人测试方法及装置，弥补现有技术的欠缺。

附图说明

图1为现有技术双臂机器人联合操作示意图；

图2为本发明实施例所提供的多基站激光跟踪仪协同转站的双臂机器人测试方法的流程图；

图3为本发明实施例所提供的多基站激光跟踪仪协同转站的双臂机器人测试方法中迭代求解待求参数向量的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

多基站激光跟踪仪协同转站的双臂机器人测试方法，如图1所示，双臂机器人包括第一操作机1和第二操作机2，用两台激光跟踪仪测量第一操作机1和第二操作机2，为保证最终测量的第一操作机1和第二操作机2的坐标系一致，通过激光跟踪仪协同转站的方法将两坐标系统一，如图2和图3所示，所述方法包括：

步骤s1：激光跟踪仪测量获取目标点空间坐标，具体过程为：

通过激光跟踪仪测量其与目标点的距离d及其水平方向的偏转角α和垂直方向的偏转角β，通过公式p(x,y,z)＝f(d,α,β)获取目标点空间坐标，其中，

步骤s2：将目标点空间坐标作为转站前点坐标，根据转站前点坐标获取转站后点坐标的表达式，具体过程为：

将目标点空间坐标作为转站前点坐标，对于每个站位通过公式pi′＝srpi+t＝spi+t获取转站后点坐标的表达式，其中，pi为第i组目标点转站前点坐标，t＝(t1,t2,t3)^t，t表示平移向量且该向量中元素为未知待测量，r为旋转矩阵且该矩阵中元素为预设常数，s为比例因子，s为比例旋转矩阵且该矩阵中元素为未知待测量；

将目标点空间坐标作为系数矩阵的元素转换表达形式，通过公式li＝aiξ获取观测向量，其中，ai为系数矩阵且

xi、yi、zi为第i组目标点空间坐标也即第i组目标点转站前点坐标；

ξ为待求参数向量且

ξ＝[r1r2r3r4r5r6r7r8r9t1t2t3]。

步骤s3：构建线性eiv模型，具体过程为：

通过公式l+el＝(a+ea)ξ构建线性eiv模型，其中，l表示观测向量的真值，a表示系数矩阵的真值，ea表示系数矩阵的误差矩阵且表示vec(ea)服从参数为0，……，的正态分布，vec表示拉直算子，vec(ea)表示系数矩阵的误差矩阵按列拉伸后的向量，el表示观测向量的误差向量且表示el服从参数为0，……，的正态分布，ql表示观测向量的真值的协方差矩阵，qa表示系数矩阵的真值的协方差矩阵。

步骤s4：根据线性eiv模型构建拉格朗日极值条件式，具体过程为：

根据观测向量的误差向量、系数矩阵的误差矩阵按列拉伸后的向量以及待求参数向量，通过公式

对线性eiv模型进行转化，其中，表示克罗内克积，ξ0表示初始待求参数向量，表示初始误差值，δξ表示第一改正值，δea表示第二改正值，且ξ＝ξ0+δξ，

通过公式构建拉格朗日极值条件式，其中，λ为第一预设常数，μ为第二预设常数。

步骤s5：对拉格朗日极值条件式中各分量求偏导并令偏导等于零求得当前待求参数向量，将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标，具体过程为：

参阅图3，假设拉格朗日极值条件式中函数连续，对拉格朗日极值条件式中各分量求偏导并令偏导等于零，求解得其中，

将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

步骤s6：获取单位权重误差，利用单位权重误差得出转站准确度，具体过程为：

利用公式获取单位权重误差，利用单位权重误差判断转站准确度，其中，n为观测数，m为未知量个数。这一过程中还可以通过单位权重误差的值的大小判断需不需要返回前述步骤进行参数更新优化待求参数向量，理论上单位权重误差越小越好，所以对于单位权重误差可以根据需要设定一个阈值，超过阈值的话返回前述步骤调整参数，如果没有超过阈值就不需要调整参数，本发明对于优化的过程不做具体描述，本申请旨在将双臂机器人的整体基座坐标系作为世界坐标系也即本发明中的转站前的点坐标，然后第一操作机1的坐标以及第二操作机2的坐标为上述算法中转站后的点坐标，这样通过上述算法就能将第一操作机1和第二操作机2都统一到双臂机器人的整体基座坐标系下，达到统一坐标的作用。

通过以上技术方案，本发明提供的多基站激光跟踪仪协同转站的双臂机器人测试方法，双臂机器人采用一套控制系统，控制两套操作机共同完成一项或一组操作任务，本发明通过激光跟踪仪协同转站的方式，分别用两台激光跟踪仪测量第一操作机1和第二操作机2，为保证最终测量的第一操作机1和第二操作机2的坐标系一致，通过激光跟踪仪转站的方式将两坐标系统一，通过本发明的转站算法将第一操作机1的基座坐标系和第二操作机2的基座坐标系作为参考坐标系变换到世界坐标系，完成转站过程，统一坐标系，提供多基站激光跟踪仪协同转站的双臂机器人测试方法及装置，弥补现有技术的欠缺。

实施例2

与本发明实施例1相对应的，本发明实施例2还提供多基站激光跟踪仪协同转站的双臂机器人测试装置，双臂机器人包括第一操作机1和第二操作机2，用两台激光跟踪仪测量第一操作机1和第二操作机2，为保证最终测量的第一操作机1和第二操作机2的坐标系一致，通过激光跟踪仪协同转站的方法将两坐标系统一，所述装置包括：

目标点坐标获取模块，用于激光跟踪仪测量获取目标点空间坐标；

表达式获取模块，用于将目标点空间坐标作为转站前点坐标，根据转站前点坐标获取转站后点坐标的表达式；

模型构建模块，用于构建线性eiv模型；

拉格朗日极值条件式构建模块，用于根据线性eiv模型构建拉格朗日极值条件式；

转站后点坐标获取模块，用于对拉格朗日极值条件式中各分量求偏导并令偏导等于零求得当前待求参数向量，将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

具体的，所述目标点坐标获取模块用于：

通过激光跟踪仪测量其与目标点的距离d及其水平方向的偏转角α和垂直方向的偏转角β，通过公式p(x,y,z)＝f(d,α,β)获取目标点空间坐标，其中，

具体的，所述表达式获取模块用于：

将目标点空间坐标作为转站前点坐标，对于每个站位通过公式pi′＝srpi+t＝spi+t获取转站后点坐标的表达式，其中，pi为第i组目标点转站前点坐标，t＝(t1,t2,t3)^t，t表示平移向量且该向量中元素为未知待测量，r为旋转矩阵且该矩阵中元素为预设常数，s为比例因子，s为比例旋转矩阵且该矩阵中元素为未知待测量；

将目标点空间坐标作为系数矩阵的元素转换表达形式，通过公式li＝aiξ获取观测向量，其中，ai为系数矩阵且

xi、yi、zi为第i组目标点空间坐标也即第i组目标点转站前点坐标；

ξ为待求参数向量且

ξ＝[r1r2r3r4r5r6r7r8r9t1t2t3]。

具体的，所述模型构建模块用于：

通过公式l+el＝(a+ea)ξ构建线性eiv模型，其中，l表示观测向量的真值，a表示系数矩阵的真值，ea表示系数矩阵的误差矩阵且vec表示拉直算子，vec(ea)表示系数矩阵的误差矩阵按列拉伸后的向量，el表示观测向量的误差向量且ql表示观测向量的真值的协方差矩阵，qa表示系数矩阵的真值的协方差矩阵。

具体的，所述拉格朗日极值条件式构建模块用于：

根据观测向量的误差向量、系数矩阵的误差矩阵按列拉伸后的向量以及待求参数向量，通过公式

对线性eiv模型进行转化，其中，表示克罗内克积，ξ0表示初始待求参数向量，表示初始误差值，δξ表示第一改正值，δea表示第二改正值，且ξ＝ξ0+δξ，

通过公式构建拉格朗日极值条件式，其中，λ为第一预设常数，μ为第二预设常数。

具体的，所述转站后点坐标获取模块用于：

假设拉格朗日极值条件式中函数连续，对拉格朗日极值条件式中各分量求偏导并令偏导等于零，求解得其中，

将当前待求参数向量作为初始待求参数向量返回执行步骤四，直到当前待求参数向量与上一次迭代求得的待求参数向量差值小于预设值则输出最终的待求参数向量，根据最终的待求参数向量获取转站后点坐标。

具体的，还包括转站准确度判断模块，用于：

利用公式获取单位权重误差，利用单位权重误差判断转站准确度，其中，n为观测数，m为未知量个数。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。