基于机器人测量-磨抛一体化系统标定的并行求解方法

本发明属于机器人测量-磨抛自动化加工，涉及一种基于机器人测量-磨抛一体化系统标定的并行求解方法。

背景技术：

1、面向高端装备大型复杂构件的磨抛加工仍采用人工磨抛，其优势在于面向复杂构件适应性强，但由于人工操作存在主观性致使磨抛质量一致性差，因此亟需开发基于工业机器人的测量-磨抛一体化系统，以具有高柔性、高刚性的工业机器人代替人手，以可实现大面域、高精度在位测量的非接触式测量系统代替人眼，实现大型复杂构件的智能制造。

2、考虑大型复杂构件毛坯装夹后与数模尺寸存在差异，无法根据数模直接规划加工，须进行在位测量获取构件毛坯实际形态从而指导加工。目前，如三坐标等接触式测量装置，虽然测量精度高，但是面向复杂构件适应性较低，且测量速度较慢，智能化自动加工过程中应用性较差。如结构光扫描仪等非接触式测量装置，配合协作机器人可完成复杂构件大空间、大面域实时测量，实现测量-磨抛自动化加工。由于机器人测量-磨抛一体化系统子单元多，单元之间位姿变换标定是实现测量-加工协同作业的前提，标定过程涉及测量-构件-加工集成单元之间的位姿变换矩阵标定，过程繁琐且标定误差随标定步骤累积、放大。因此，如何实现机器人测量-磨抛一体化系统多单元之间位姿变换的精确标定是亟需解决的难题之一。

3、现有技术文献1“a general approach to hand-eye calibration through theoptimization of atomic transformations”，该文献提出了一种基于最小二乘法求解位姿变换矩阵的方法，并使用重投影误差作为指导优化过程的指标，能够精确预测手-眼矩阵方差，然而该方法属于分步标定，在标定过程中易将第一步标定误差传递到下一步标定流程，造成误差累积问题，致使机器人测量-磨抛自动加工系统标定精度难控；

4、现有技术文献2“probabilistic approaches to the axb＝ycz calibrationproblemin multi-robot systems”，该文献将axb＝ycz方程传统求解方法与刚体姿态的概率估计方法相结合，该文献所提方法可降低测量噪音对标定精度的影响，但由于引入大量迭代求解计算，导致求解过程耗时较长，难以适用到自动加工系统中。

技术实现思路

1、本发明面向高端装备大型复杂构件结构特殊与磨抛高一致性需求，针对常规数控磨削机床难以实现此类构件数控磨削，建立测量-磨抛一体化系统，发明一种基于重投影误差最小化的并行求解算法，避免分步标定过程中误差传递累积，提高系统标定精度。首先，集成“在位测量-自动磨抛”一体化系统，系统包括结构光扫描仪、测量协作机器人、磨抛工业机器人子单元；其次建立测量-构件子系统标定模型，将位姿变换矩阵转换为可同时表示旋转矩阵及平移矩阵的对偶四元数并进行svd封闭式求解，以封闭式计算结果作为目标函数迭代求解的初值，确保迭代寻优过程正确可靠；然后建立测量-加工子系统标定模型，结合相机成像模型中重投影误差最小化求解位姿变换矩阵，在矩阵计算误差最小的基础上虑及图像点测量误差，进一步提高标定精度。该方法可有效完成多功能系统间协同作业，且标定精度高，对大型复杂构件表面低损伤磨抛加工具有重要的指导意义。

2、本发明采用的技术方案是一种基于机器人测量-磨抛一体化系统标定的并行求解方法。其特征在于，该方法基于面向大型复杂构件集成的测量-磨抛一体化系统，首先建立测量-构件子系统标定模型，采用对偶四元数及svd同时求解旋转矩阵及平移矩阵，并以上述封闭式求解作为迭代优化求解的初值，确保寻优结构可靠；其次建立测量-加工子系统标定模型，并以测量-构件子系统标定模型中求解的固定位姿变换矩阵作为已知量，采用重投影误差最小化并行求解位姿变换矩阵，不仅考虑了矩阵的计算误差，也考虑了扫描仪相机成像的计算误差，有效提高并行标定结果的精度；最终实现测量-磨抛一体化系统各全局位姿变换矩阵高精度标定。

3、方法的具体步骤如下：

4、步骤1，搭建基于非接触式测量的机器人测量-磨抛一体化系统

5、非接触式测量的机器人测量-磨抛一体化系统由测量协作机器人1，测量扫描仪2，扫描仪右相机3，扫描仪左相机4，构件装夹装置5，待加工构件6，磨抛机器人7，换刀架8组成，化分为在位测量子单元、构件及装夹子单元、磨抛加工子单元。在位测量子单元结合安装在六自由度机械臂末端的结构光三维测量扫描仪实现复杂构件大空间、大面域、高精度曲面测量；构件及装夹子单元实现构件的装夹与定位，并为在位测量以及磨抛加工提供支持；磨抛加工子单元由工业机器人、打磨工具组成，依据在位测量构件实际廓形进行磨抛轨迹规划、位置和姿态高精度定位控制，并由机器人末端搭载浮动磨头进行构件恒力打磨。自动磨抛系统集在位测量、轨迹规划、随型打磨等功能于一体，实现构件自动磨抛加工，开发面向大型复杂构件的测量-加工一体化系统。

6、步骤2，测量-构件子系统间位姿变换矩阵并行求解

7、1)建立测量-构件子系统标定模型

8、基于测量协作机器人、结构光扫描仪、构件及装夹装置建立测量-构件子系统间标定模型。待构件在装夹装置上固定后，在装夹装置固定立柱上贴好标记点，结构光三维扫描仪固定于测量协作机器人末端通过改变m次机器人位姿，获取m组扫描测量数据，以及m组测量协作机器人末端相对于基坐标系位姿变换关系。

9、基于m组测量数据，设测量协作机器人基坐标系{w}到测量协作机器人末端法兰坐标系{q}位姿变换矩阵为a，装夹装置固定立柱上标记点坐标系{p}到扫描仪坐标系{c}位姿变换矩阵为b，测量协作机器人末端法兰坐标系{q}到扫描仪坐标系{c}位姿变换矩阵为x，测量协作机器人基坐标系{w}到装夹装置固定立柱上标记点坐标系{p}位姿变换矩阵为y，构建测量协作机器人-构件间的位姿变换矩阵关系式为：

10、ax＝yb  (1)

11、其中，a＝aiai-1，ai(i＝1,2,3,...,m)为在m个位姿下测量协作机器人基坐标系到测量协作机器人末端法兰坐标系位姿变换矩阵，可由测量协作机器人正向运动学获得；b＝bibi-1，bi(i＝1,2,3,...,m)可由测量协作机器人在m个位姿下扫描仪获取的标定点坐标，可由扫描仪读取数据后求解位姿变换矩阵。

12、2)模型位姿变换矩阵并行求解

13、依据测量协作机器人-构件子系统标定模型，采用对偶四元数及迭代式求解实现位姿变换矩阵中旋转矩阵及平移矩阵并行求解，避免顺次求解过程中旋转矩阵计算误差引入到平移矩阵计算过程中，引起标定结果的误差累积与放大。模型中位姿变换矩阵可分解为旋转矩阵r和平移矩阵t两部分，根据式(1)可得：

14、

15、式中，ra,ta分别为测量协作机器人基坐标系{w}到测量协作机器人末端法兰坐标系{q}位姿变换矩阵a的旋转矩阵和平移矩阵，由测量协作机器人正向运动学获得；rb,tb分别为装夹装置固定立柱上标记点坐标系{p}到扫描仪坐标系{c}位姿变换矩阵b的旋转矩阵和平移矩阵，可由扫描仪测量点云后计算获得；rx,tx分别为测量协作机器人末端法兰坐标系{q}到扫描仪坐标系{c}位姿变换矩阵x的旋转矩阵和平移矩阵；ry,ty分别为构件上测量协作机器人基坐标系{w}到装夹装置固定立柱上标记点坐标系{p}位姿变换矩阵y的旋转矩阵和平移矩阵。

16、通过展开式(2)齐次变换矩阵，可推导出：

17、rarx＝ryrb  (3)

18、ratx+ta＝rytb+ty  (4)

19、为同时求解位姿变换矩阵的旋转矩阵r以及平移矩阵t，采用对偶四元数可同时表示空间旋转和平移，位姿变换矩阵的对偶四元数形式为其中qr,qt分别为旋转单位四元数和平移单位四元数，ε为对偶算子，满足ε≠0,ε2＝0，因此式(3)、(4)齐次变换矩阵基于对偶四元数形式可表达为：

20、

21、

22、式中，分别为位姿变换矩阵a的旋转单位四元数和平移单位四元数，分别为位姿变换矩阵b的旋转单位四元数和平移单位四元数，分别为位姿变换矩阵x的旋转单位四元数和平移单位四元数，分别为位姿变换矩阵y的旋转单位四元数和平移单位四元数。

23、将式(5)、(6)进行整理可得：

24、

25、由于标定时测量机器人移动m次、扫描仪获取m次位姿下标记点的坐标，因此存在多组数据，将式(7)中的多组测量求解量记为ki(i＝1,2,...,m)，未知待求量记为q，式(7)可简化为：

26、kiq＝0  (8)

27、其中，ki(i＝1,2,...,m)是由位姿变换矩阵a和b的对偶四元数构成的矩阵，q是由位姿变换矩阵x和y的对偶四元数构成的矩阵，然后对矩阵k＝[k1,k2,...,km]进行svd分解，结合对偶四元数约束qq-1＝1,||q||2＝1可同时求解位姿变换矩阵x和y对应的旋转单位四元数和平移单位四元数，记为并作为迭代求解的初值。

28、由于封闭式求解过程易受环境干扰而不稳定，因此采用迭代优化求解方法，并以封闭式求解结果作为迭代求解初始值，提高求解过程稳定性，加快求解收敛速度。首先建立对应目标函数f(x,y)＝||ax-yb||2，对于m组测量协作机器人移动的位姿，基于式(5)和(6)构建最优化成本函数为：

29、

30、采用lm法对目标函数进行求解，以式(8)求中求解结果作为迭代求解过程的初值并给定初始参数值，然后按照算法迭代步长δxs进行迭代：

31、

32、式中，s为迭代次数；令为目标函数f(x)关于x的雅可比矩阵；μs为阻尼系数；i为单位矩阵。通过lm迭代求解，当迭代步长满足条件时，停止迭代，输出目标函数f(x)的最优解然后根据式求出位姿变换矩阵x和y的对偶四元数，并将对偶四元数转换为位姿变换矩阵，获得测量协作机器人末端法兰坐标系{q}到扫描仪坐标系{c}位姿变换矩阵x，测量协作机器人基坐标系{w}到装夹装置固定立柱上标记点坐标系{p}位姿变换矩阵y。

33、步骤3，测量-加工子系统基于重投影误差最小化参数优化求解

34、1)建立测量协作机器人-磨抛加工子系统标定模型

35、基于测量协作机器人、打磨工具及磨抛机器人建立测量-加工子系统标定模型。首先建立测量协作机器人、扫描仪、磨抛工业机器人、打磨工具间位姿变换关系模型，在工业机器人打磨工具末端放置标定板，结构光三维扫描仪固定于测量协作机器人末端，改变n次测量协作机器人位姿，磨抛工业机器人相应改变n次磨抛加工机器人位姿，获取n组扫描标定板测量数据，同时可获取n组测量协作机器人末端相对于基坐标系位姿变换关系以及n组磨抛工业机器人末端相对于基坐标系位姿变换关系。

36、基于n组测量数据，设测量协作机器人基坐标系{w}到测量协作机器人末端法兰坐标系{q}位姿变换矩阵为a，扫描仪坐标系{c}到磨抛工业机器人末端打磨工具坐标系{t}位姿变换矩阵为d，磨抛工业机器人基坐标系{ob}到磨抛工业机器人末端法兰坐标系{s}位姿变换矩阵为e，磨抛工业机器人末端法兰坐标系{s}到磨抛工业机器人末端打磨工具坐标系{t}位姿变换矩阵为p，测量协作机器人基坐标系{w}到磨抛工业机器人基坐标系{ob}位姿变换矩阵为z，由于步骤2中测量协作机器人-构件子系统标定模型所求出的测量协作机器人末端法兰坐标系{q}到扫描仪坐标系{c}位姿变换矩阵x是固定不变的，不随测量协作机器人移动而改变，因此将矩阵x作为已知量应用于测量-加工子系统标定模型求解中，构建测量-加工系统间的位姿变换矩阵关系式为：

37、axd＝zep  (11)

38、其中，a＝ajaj-1，aj(j＝1,2,3,...,n)为在n个位姿下测量协作机器人基坐标系{w}到测量协作机器人末端法兰坐标系{q}位姿变换矩阵，可由测量协作机器人正向运动学获得；e＝ejej-1，ej(j＝1,2,3,...,n)为在n个位姿下磨抛工业机器人基坐标系{ob}到磨抛工业机器人末端法兰坐标系{s}位姿变换矩阵，可由磨抛工业机器人正向运动学获得；d＝djdj-1，dj(j＝1,2,3,...,n)为测量协作机器人移动n次扫描仪坐标系{c}到磨抛工业机器人末端打磨工具坐标系{t}位姿变换矩阵，可由扫描仪读取数据后求解位姿变换矩阵；矩阵x由步骤2中测量协作机器人-构件子系统标定模型求出，在测量-加工系统标定模型中作为已知量；p为磨抛工业机器人末端法兰坐标系{s}到磨抛工业机器人末端打磨工具坐标系{t}位姿变换矩阵；z为测量协作机器人基坐标系{w}到磨抛工业机器人基坐标系{ob}位姿变换矩阵。

39、将式(11)中等式左边已知量记为g，即g＝axd，并令q＝p-1，式(11)可写为：

40、gq＝ze  (12)

41、式中，位姿变换矩阵a、e可直接根据测量协作机器人、磨抛工业机器人正向运动情况由示教器读取并计算获得，位姿变换矩阵d可由扫描仪测量数据后进行求解，测量协作机器人末端法兰与工具之间位姿变换矩阵x由步骤2求出，且二者之间是刚性固定的。

42、2)基于重投影误差最小化参数优化模型求解

43、由于世界坐标系下空间点的实际位置坐标pjk与扫描仪相机成像平面上计算值ujk存在误差利用投影误差最小化求解最优的位姿变换矩阵，其中k为扫描仪相机内参，t为扫描仪坐标系与世界坐标系位姿变换矩阵。采用基于重投影误差最小化参数优化方法求解式(12)，通过最小化成本函数来同时求解位姿变换矩阵q、z的旋转矩阵rq,rz和平移矩阵tq,tz，令g＝zeq-1，根据式(12)构建基于重投影误差最小化目标函数为：

44、

45、式中，j＝1,2,3,...,n为测量协作机器人的移动次数；k为扫描仪视野范围打磨工具上标定板的棋盘格点数；pjk为第j个位姿时世界坐标系下测量的k个棋盘格点数的实际位置坐标，ujk为测量点在扫描仪相机成像平面上投影的像素坐标，d为测量点在扫描仪坐标系下的深度。

46、重复步骤2并行求解算法，基于对偶四元数封闭式求解方法获取式(12)中位姿变换矩阵并作为重投影误差最小化目标函数初值，给定参数d、k，对最小化目标函数f(rq,tq,rz,tz)进行lm迭代求解，求得磨抛工业机器人末端法兰坐标系{s}到磨抛工业机器人末端打磨工具坐标系{t}位姿变换矩阵p＝q-1以及测量协作机器人基坐标系{w}到磨抛工业机器人基坐标系{ob}位姿变换矩阵z。

47、步骤4，基于机器人测量-磨抛一体化系统全局标定

48、首先基于所搭建的机器人测量-磨抛一体化系统，根据测量-构件子系统标定模型以及测量-加工子系统标定模型求解结果，获得一体化系统中各设备间位姿变换关系；然后将测量协作机器人1导引扫描仪2在位测量构件6的完整廓形通过位姿变换矩阵x转换到测量协作机器人末端法兰坐标系{q}下，再经位姿变换矩阵a转换到测量协作机器人基坐标系{w}下，并根据构件结构、形状进行分区磨抛加工轨迹规划，将规划的加工轨迹后置为磨抛机器人7可读取的加工路径，并将加工路径经位姿变换矩阵z转换到磨抛机器人7基坐标系{ob}下，根据位姿变换矩阵e、p转换到磨抛工业机器人末端打磨工具坐标系{t}下，完成测量-构件-加工各子单元间数据统一，实现多功能协同加工；最后将加工路径输入磨抛工业机器人控制端并设定加工速度、加速度，实现机器人测量-磨抛一体化系统自动磨抛加工。

49、本发明的显著效果和益处是：基于所搭建的测量-磨抛一体化系统，以所提出的并行求解算法在测量-构件子系统模型标定过程中采用对偶四元数同时表述位姿变换关系的旋转矩阵和平移矩阵，避免矩阵顺次求解过程中误差累积效应，结合svd封闭式求解标定模型结果，并以上述结果作为目标优化函数迭代求解的初值，为目标函数提供了可靠的初始点，避免优化求解陷入局部最优；同时将测量-构件子系统标定模型中求解的固定位姿变换矩阵应用于测量-加工子系统模型标定过程中，以封闭式求解方法计算目标函数初值，结合相机成像原理建立基于重投影误差最小化的目标函数求解坐标变换关系，不仅考虑了矩阵的计算误差，也考虑了扫描仪相机成像的测量误差，有效提高并行标定结果的精度，从而使大型复杂构件在不断变化的在位测量-自动磨抛系统中实现准确的机器人手眼协同加工。该方法在同时求解位姿变换关系的旋转矩阵和平移矩阵基础上引入最小化重投影误差进行优化求解，协调在位测量与磨抛加工的运行稳定性，可有效完成多功能系统间协同作业，充分发挥自动化加工性能，进而提高机器人测量-磨抛一体化系统加工质量。