基于分布式位姿采集和模型降维的机械臂标定方法及装置

1.本发明涉及机械臂标定技术领域，特别是涉及一种基于分布式位姿采集和模型降维的机械臂标定方法及装置。


背景技术：

2.机械臂标定能有效补偿dh(denavit-hartenberg)参数误差对机械臂末端绝对定位精度的影响。其中机械臂标定的关键技术包括末端位姿测量、标定模型建立以及误差辨识和补偿，位姿测量给标定模型提供机械臂位姿数据样本，标定模型表征了dh参数误差到末端位姿误差的映射关系，误差辨识是由标定模型求解dh参数误差，误差补偿则是辨识结果在机械臂运动控制环节的实际应用。在标定过程中测量、建模与辨识彼此密切关联。但在测量环节，传统标定方法通常将用于位姿采集的传感器集中安装在机械臂末端，仅可获得末端单个关节的位姿数据，因此后续环节只能建立单一的标定模型。但机械臂标定过程中中所有关节dh参数误差会共同决定模型中的末端位姿误差，而各误差影响权重却存在明显差异，因此不同的误差混合在单一标定模型中将导致雅克比矩阵病态，使误差辨识结果极易受测量环节噪声干扰，最终影响末端位姿补偿精度。而部分传统标定方法在模型中仅保留大权重的参数误差，以此来提高剩余误差的辨识精度，但这些主动丢失的误差信息在精度要求更高的场合中并不可忽略。综上，传统标定方法存在误差辨识结果精确性与完整性的矛盾，无法提升机械臂标定精度，机械臂精度差。


技术实现要素：

3.本发明要解决的技术问题是提供一种基于分布式位姿采集和模型降维的机械臂标定方法及装置，可解决现有传统机械臂标定方法误差辨识结果精确性与完整性矛盾的问题，提高机械臂精度，从而克服现有技术的不足。
4.为解决上述技术问题，本发明公开有一种基于位姿采集和模型降维的机械臂高精度标定方法，其包括如下步骤：
5.步骤一、在机械臂第i关节和第j关节分别安装一个位姿传感器，基于mdh规则建立机械臂各关节坐标系,且位姿传感器均可实时输出自身坐标系的位姿信息；其中第j关节为机械臂的末端关节，第i关节为机械臂的中间关节，且i和j均为正整数，i《j；
6.步骤二、选择测量构型(θ1，...，θi)
k1
(k1＝1,2,...,m1)和(θ1，...，θj)
k2
(k2＝1,2,...,m2)，其中(θ1，...，θi)表示测量构型对应的前i个关节的转角指令集合，(θ1，...，θj)表示测量构型对应全部关节的转角指令集合，k1、k2分别表示测量构型的序号，m1和m2表示测量构型的数量；其中在(θ1，...，θi)
k1
处由位于第i关节的位姿传感器获得第i关节实际位姿p
ia
，在(θ1，...，θj)
k2
处由位于第j关节的位姿传感器获得第j关节实际位姿p
ja
；
7.步骤三、基于已知的机械臂名义dh参数和机械臂正运动学，分别在(θ1，...，θi)
k1
处计算第i关节名义位姿pi，在(θ1，...，θj)
k2
处计算第j关节名义位姿pj；
8.步骤四、根据δpi＝p
ia-pi计算(θ1，...，θi)
k1
处的第i关节位姿误差δpi；
9.步骤五、建立前i个关节的标定模型δpi＝jaδua，并根据δpi＝jaδua求解δua；其中ja是依据机械臂正运动学和微分变换原理建立的雅克比矩阵，δua表示前i个关节的dh参数误差向量；
10.步骤六、根据δpj←a＝j
′aδua计算在(θ1，...，θj)
k2
处基于δua所导致的第j关节位姿误差δpj←a,其中j
′a是δua到第j关节在(θ1，...，θj)
k2
处位姿误差的雅克比矩阵；并根据δpj＝p
ja-p
j-δpj←a计算(θ1，...，θj)
k2
处的第j关节位姿误差δpj；
11.步骤七、建立全部关节的标定模型δpj＝jbδub，并根据δpj＝jbδub求解δub，其中jb是依据机械臂正运动学和微分变换原理建立的雅克比矩阵，δub表示全部关节的dh参数误差向量；
12.步骤八、通过修正关节指令的方式间接补偿dh参数误差δua和δub。
13.作为本发明的一种改进，所述步骤一中机械臂各关节坐标系至少包括基坐标系、第i关节坐标系、第j关节坐标系、位于第i关节位姿传感器的第一位姿传感器坐标系以及位于位于第j关节位姿传感器的第二位姿传感器坐标系。
14.作为本发明的一种改进，所述基坐标系的建立方法为：
15.首先仅转动机械臂的第一关节，用测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z1，作为第一关节坐标系的z1轴；
16.然后在第一个关节转角为0度时，即θ1＝0
°
处仅转动第二关节，测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z2，作为第二关节坐标系的z2轴；将向量x1＝z1×
z2定义为第一关节坐标系的x1轴，圆弧旋转轴向量z1和z2的公垂线与圆弧旋转轴向量z1的交点作为原点o1，即可建立第一关节坐标系f1；令基坐标系f0与f1重合，得到基坐标系f0。
17.作为本发明的进一步改进，所述第i关节坐标系的建立方法为：
18.在θ1＝...＝θi＝0
°
处仅转动第i+1关节，用测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
i+1
；在θ1＝...＝θi＝θ
i+1
＝0
°
处仅转动第i+2关节，采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
i+2
；圆弧旋转轴向量z
i+1
和z
i+2
的公垂线与圆弧旋转轴向量z
i+1
的交点作为原点o
i+1
；在θ1＝...＝θ
i-1
＝0
°
仅转动第i关节，测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量zi,过o
i+1
作zi的垂线，交zi于oi，作为第i关节坐标系的原点，即可建立第i关节坐标系fi。
19.作为本发明的进一种改进，所述位于第i关节位姿传感器的第一位姿传感器坐标系的建立方法为：
20.首先第一位姿传感器采用三个激光靶球；
21.在θ1＝...＝θi＝0
°
处用测量仪器分别采集第i关节上的3个激光靶球坐标；
22.选取其中一个激光靶球坐标作为共用坐标，结合另外两个激光靶球坐标构建单位向量v1和v2，将向量z
s1
＝v1×
v2作为第一位姿传感器坐标系的z
s1
轴，v1作为第一位姿传感器坐标系的x
s1
轴，共用的激光靶球坐标作为第一位姿传感器坐标系的原点坐标o
s1
，即可建立第一位姿传感器坐标系f
s1
。
23.作为本发明的进一种改进，所述第j关节坐标系的建立方法为：
24.首先在θ1＝...＝θ
j-2
＝θj＝0
°
处仅转动第j关节，用测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
j-1
；
25.在θ1＝...＝θj＝0
°
处仅转动第j关节，采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量zj，作为第j关节坐标系的zj轴；
26.将向量x6＝z5×
z6定义为第j关节坐标系的xj轴，圆弧旋转轴向量z
j-1
和zj的公垂线与圆弧旋转轴向量zj的交点作为原点oj，由此建立了第j关节坐标系fj。
27.作为本发明的进一种改进，所述位于第j关节位姿传感器的第二位姿传感器坐标系的建立方法为：
28.首先第二位姿传感器采用三个激光靶球；
29.在θ1＝...＝θj＝0
°
处用测量仪器分别采集第j关节上的3个激光靶球坐标；
30.选取其中一个激光靶球坐标作为共用坐标，结合另外两个激光靶球坐标构建单位向量v3和v4，将向量z
s2
＝v3×
v4作为第二位姿传感器坐标系的z
s2
轴，v3作为第二位姿传感器坐标系的x
s2
轴，共用的激光靶球坐标作为第二位姿传感器坐标系的原点坐标o
s2
，即可建立第二位姿传感器坐标系f
s2
。
31.作为本发明的进一种改进，所述步骤二中第i关节及第j关节的实际位姿的计算方法为：
32.首先基于得到第i关节坐标系fi、第一位姿传感器坐标系f
s1
、第j关节坐标系fj、第二位姿传感器坐标系f
s2
分别推导出第一位姿传感器坐标系到第i关节的齐次坐标变换矩阵以及第二位姿传感器坐标系到第j关节坐标系的齐次坐标变换矩阵:
33.*
t
#
＝(0t
*
)-1
(0t
#
)
34.其中*代表第一位姿传感器或第二位姿传感器；#代表第i关节或第j关节；
35.基于
*
t
#
＝(0t
*
)-1
(0t
#
)分别求出各测量构型(θ1，...，θi)
k1
(k1＝1,2,...,m1)和(θ1，...，θj)
k2
(k2＝1,2,...,m2)对应关节的实际(0t
#
)
ku
，即为(0t
#a
)
ku
，其中#代表第i关节或第j关节，对于测量构型(θ1，...，θi)
k1
(k1＝1,2,...,m1)，ku指代k1(k1＝1,2,...,m1)，对于测量构型(θ1，...，θj)
k2
(k2＝1,2,...,m2)，ku指代k2(k2＝1,2,...,m2)；
36.设定(0t
#a
)
ku
的各元素表征为：
[0037][0038]
其中#代表第i关节或第j关节，对于测量构型(θ1，...，θi)
k1
(k1＝1,2,...,m1)，ku指代k1(k1＝1,2,...,m1)，对于测量构型(θ1，...，θj)
k2
(k2＝1,2,...,m2)，ku指代k2(k2＝1,2,...,m2)；
[0039]
由(0t
#a
)
ku
即可得到对应关节的实际位姿向量(p
#a
)
ku
：
[0040]
[0041]
其中atan2(y,x)是双变量反正切函数。
[0042]
同时本发明还公开有一种采用上述机械臂高精度标定方法的机械臂高精度标定装置，其包括测量仪器和位姿传感器，所述位姿传感器至少包括两个，且其中一个位姿传感器安设于机械臂的末端关节；另一位姿传感器安设于机械臂的中间关节。
[0043]
作为本发明的进一种改进，所述机械臂采用六自由度机械臂，所述测量仪器采用激光跟踪仪，所述位姿传感器采用三个不共线的激光靶球，每个激光靶球分别通过靶球基座粘贴于一端有圆台的螺纹杆上；位于机械臂中间关节的三个激光靶球通过螺纹杆连接于剖分式连接圈，两段剖分式连接圈夹紧连接于机械臂中间关节；位于机械臂末端关节的三个激光靶球通过螺纹杆连接于末端关节的法兰端面的螺纹孔。
[0044]
采用这样的设计后，本发明至少具有以下优点：
[0045]
本发明通过在机械臂标定的测量环节将位姿传感器分布至不同关节，进而获取更为丰富的关节位姿数据，并由此分别建立了多个的低维度标定模型，将拥挤在传统单一标定模型中的dh参数误差疏散至多个子标定模型，从而显著改善了模型性态水平，实现了dh参数误差辨识精度的进一步提升，使得提高机械臂标定精度，同时也保证了误差辨识结果的完整性。
附图说明
[0046]
上述仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，以下结合附图与具体实施方式对本发明作进一步的详细说明。
[0047]
图1是本发明实施例机械臂高精度标定方法的流程示意图。
[0048]
图2是本发明实施例机械臂高精度标定装置中激光靶球的安装示意图。
[0049]
图3是本发明实施例基坐标系的建立流程示意图。
[0050]
图4是本发明实施例第一位姿传感器坐标系的建立流程示意图。
[0051]
图5是本发明实施例第二位姿传感器坐标系的建立流程示意图。
[0052]
图6是本发明实施例基于分布式激光靶球所建立机械臂坐标系的示意图。
[0053]
图7是本发明实施例标定方法与传统标定方法的误差辨识精度对比示意图。
[0054]
图8是本发明实施例标定方法与传统标定方法在补偿误差后残差对比示意图。
[0055]
图9是本发明实施例标定方法与传统标定方法在补偿误差后测试点对比示意图。
[0056]
附图中具体附图标记为：
[0057]
1-激光靶球；2-激光靶球；3-剖分式连接圈；4-法兰连接件。
具体实施方式
[0058]
下面结合附图对本公开实施例进行详细描述。
[0059]
以下通过特定的具体实例说明本公开的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本公开的其他优点与功效。显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。本公开还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本公开的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下
所获得的所有其他实施例，都属于本公开保护的范围。
[0060]
结合图2所示，本实施例中首先公开有一种基于分布式位姿采集和模型降维的机械臂标定装置，其包括测量仪器与位姿传感器，其中本实施例中测量仪器采用激光跟踪仪，而位姿传感器则采用激光靶球，本实施例中所标定对象为六自由度工业机械臂。
[0061]
具体的，本实施例中首先将3个不共线的激光靶球1通过第一连接件安装于机械臂的第二关节，其次将另外3个不共线的激光靶球2通过第二连接件安装在机械臂的第六关节。
[0062]
更详细的是，本实施例中位于第二关节的激光靶球1通过靶球基座粘贴在一端设有圆台的螺纹杆上，并将螺纹杆拧进剖分式连接圈3的螺纹孔中，然后将剖分式连接圈3套住第二关节的连杆上，并拧紧两侧的螺钉，使两段剖分式连接圈3合拢夹紧；同时拧紧剖分式连接圈3上其余用于间隙支撑的螺钉，此时螺钉与第二关节连杆表面顶紧，使剖分式连接圈3与第二关节连杆紧固连接为一个整体；本实施例中位于第六关节的激光靶球2也通过靶球基座粘贴在一端有圆台的螺纹杆上，然后将螺纹杆拧进第六关节所连接法兰连接件4的侧面螺纹孔中，之后用螺钉紧固法兰连接件4与第六节关节法兰端面。
[0063]
结合图3所示，装置安设完成后，首先仅转动机械臂的第一关节，并用激光跟踪仪采集第六关节上的某个激光靶球2的圆弧轨迹，拟合得到圆弧旋转轴向量z1，作为第一关节坐标系的z1轴；之后在θ1＝0
°
处仅转动第二关节，采集第六关节上的某个激光靶球2的圆弧轨迹，拟合得到圆弧旋转轴向量z2，作为第二关节坐标系的z2轴；随后将向量x1＝z1×
z2定义为第一关节坐标系的x1轴，圆弧旋转轴向量z1和z2的公垂线与圆弧旋转轴向量z1的交点作为原点o1，由此建立了第一关节坐标系f1；此时令基坐标系f0与第一关节坐标系f1重合，由此建立了基坐标系f0。
[0064]
如图4所示，本实施例在θ1＝θ2＝0
°
条件下仅转动第三关节，用激光跟踪仪采集第六关节上的某个激光靶球2的圆弧轨迹，拟合得到圆弧旋转轴向量z3；在θ1＝θ2＝θ3＝0
°
条件下仅转动第四关节，采集第六关节上的某个激光靶球2的圆弧轨迹，拟合得到圆弧旋转轴向量z4；圆弧旋转轴向量z3和z4的公垂线与圆弧旋转轴向量z3的交点作为原点o3；过原点o3作圆弧旋转轴向量z2的垂线，交圆弧旋转轴向量z2于o2点，该o2点作为第二关节坐标系的原点，由此建立了第二关节坐标系f2；在θ1＝θ2＝0
°
条件下用激光跟踪仪分别采集第二关节上的3个激光靶球1坐标：任选其中一个激光靶球1坐标作为为共用靶球坐标o
s1
，结合另外两个激光靶球1坐标构造单位向量v1和v2，将向量z
s1
＝v1×
v2作为第一传感器坐标系的z
s1
轴，向量v1作为x
s1
轴，共用靶球坐标o
s1
为原点坐标，由此建立了第一传感器坐标系f
s1
。
[0065]
由此基于上述可分别得到第二关节坐标系f2和第一传感器坐标系f
s1
在基坐标系f0下的齐次变换矩阵0t2和0t
s1
，进而推导出第一传感器坐标系f
s1
到第二关节坐标系f2的齐次坐标变换矩阵
s1
t2＝(0t
s1
)-1
(0t2)。
[0066]
进一步的，结合图5所示，本实施例在θ1＝θ2＝θ3＝θ4＝θ6＝0
°
条件下仅转动第五关节，用激光跟踪仪采集第六关节上的某个激光靶球2的圆弧轨迹，拟合得到圆弧旋转轴向量z5；在θ1＝θ2＝θ3＝θ4＝θ5＝0
°
条件下仅转动第六关节，采集第六关节上的某个激光靶球2的圆弧轨迹，拟合得到圆弧旋转轴向量z6，作为第六关节坐标系的z6轴；将向量x6＝z5×
z6定义为第六关节坐标系的x6轴，z5和z6的公垂线与z6的交点作为原点o6，由此建立了第六关节坐标系f6；如图6所示，在θ1＝θ2＝θ3＝θ4＝θ5＝θ6＝0
°
条件下用激光跟踪仪分别采集第六关节
上的3个激光靶球2坐标：可参照第一传感器坐标系f
s1
建立方法建立第二传感器坐标系f
s2
；由此分别得到第六关节坐标系f6和第二传感器坐标系f
s2
在基坐标系f0下的齐次变换矩阵0t6和0t
s2
，进而推导出第二传感器坐标系f
s2
到第六关节坐标系f6的齐次坐标变换矩阵
s2
t6＝(0t
s2
)-1
(0t6)。
[0067]
而基于mdh(modified denavit-hartenberg)规则可建立机械臂各关节坐标系，其中机械臂各关节的名义dh参数如表1所示：
[0068]
表1
[0069][0070]
本实施例进行机械臂标定前首先选择测量构型和其中(θ1,θ2)表示各测量构型对应的前两个关节的转角指令集合，(θ1,...,θ6)表示各测量构型对应的全部关节的转角指令集合，k1、k2分别表示测量构型的序号，m1和m2分别表示各测量构型的数量。例如本实施例在处由位于第二关节的位姿传感器获得第二关节实际位姿p
2a
，在处由位于第六关节的位姿传感器获得第六关节实际位姿p
6a
；本实施例中机械臂各关节的转动范围设定为θ1∈[-170
°
,170
°
]、θ2∈[-120
°
,70
°
]、θ3∈[-90
°
,70
°
]、θ4∈[-170
°
,170
°
],θ5∈[-110
°
,110
°
]以及θ6∈[-180
°
,180
°
]；随后分别在机械臂第二关节和第六关节的运动空间中挑选8个和16个相对分散、均匀的测量构型，即和
[0071]
随后用激光跟踪仪采集处的位于第二关节上的3个激光靶球1坐标，基于第一传感器坐标系f
s1
建立方法建立当前测量构型处实际的第一传感器坐标系(f
s1
)
k1
，进而得到实际的(0t
s1
)
k1
；再结合齐次坐标变换矩阵
s1
t2＝(0t
s1
)-1
(0t2)，求出实际的(0t2)
k1
，即(0t
2a
)
k1
。
[0072]
本实施例中预先设定(0t
2a
)
k1
的各元素表征为：
[0073][0074]
其中，角标x2，y2,z2分别表示第二关节齐次变换矩阵中的方位向量(n,o,a)或位置向量(p)在x方向、y方向、z方向上的分量。根据(0t
2a
)
k1
可得到对应的实际位姿向
量(p
2a
)
k1
：
[0075][0076]
其中atan2(y,x)是双变量反正切函数。
[0077]
基于上述步骤可测得处由位于第二关节的位姿传感器所获得第二关节的实际位姿p
2a
。
[0078]
随后基于已知的机械臂名义dh参数和机械臂正运动学，在处计算第二关节名义位姿p2；本实施例的机械臂正运动学结合各关节名义dh参数，参见表1，可得到当前测量构型处的名义(0t2)
k1
。本实施例中设定(0t2)
k1
的各元素表征为：
[0079][0080]
其中，角标标x2，y2,z2分别表示第二关节齐次变换矩阵中的方位向量(n,o,a)或位置向量(p)在x方向、y方向、z方向上的分量。则根据公式(3)可得到机械臂在对应测量构型的名义位姿向量(p2)
k1
：
[0081][0082]
其中，角标x2，y2,z2分别表示第二关节齐次变换矩阵中的方位向量(n,o,a)或位置向量(p)在x方向、y方向、z方向上的分量。根据公式(2)所得(p
2a
)
k1
和公式(4)所得(p2)
k1
可得第二关节在测量构型处的位姿误差(δp2)
k1
：
[0083]
(δp2)
k1
＝(p
2a
)
k1-(p2)
k1
ꢀꢀꢀ
(5)
[0084]
将所有(δp2)
k1
组合即可得到机械臂第二关节的位姿误差向量δp2：
[0085][0086]
实施例根据由表1可知，δua＝[δθ1,δd1,δa1,δα1,δθ2,δd2,δa2,δα2]
t
，其中角标a表示该向量元素为前两个关节的dh参数误差，角标1、2表示第一关节和第二关节。测量构型
处对应的雅克比矩阵为：
[0087][0088][0089]
其中，m和r均为3
×
1的微分变换向量，其分别表征了下标所对应的第i关节的dh参数误差(θ、d、a、α)到第二关节位姿误差的映射关系，其构造依据是基于机械臂正运动学、前两个关节的dh名义参数值、以及微分变换原理，属于现有技术，此处不再赘述。
[0090]
将所有处的(ja)
k1
组合即可得到ja：
[0091][0092]
同理的，本实施例采用激光跟踪仪采集处的第六关节上的3个激光靶球2坐标，并利用建立第二传感器坐标系f
s2
的方法建立各测量构型处实际的第二传感器坐标系(f
s2
)
k2
，进而得到实际的(0t
s2
)
k2
；之后结合齐次坐标变换矩阵
s2
t6＝(0t
s2
)-1
(0t6)；求出实际的(0t6)
k2
，即(0t
6a
)
k2
；本实施例设定(0t
6a
)
k2
各元素表征为：
[0093][0094]
其中，角标x6，y6,z6分别表示第六关节齐次变换矩阵中的方位向量(n,o,a)或位置向量(p)在x方向、y方向、z方向上的分量。根据(0t
6a
)
k1
可得到对应的实际位姿向量(p
6a
)
k2
：
[0095][0096]
其中，角标x6，y6,z6分别表示第六关节齐次变换矩阵中的方位向量(n,o,a)或位置向量(p)在x方向、y方向、z方向上的分量。基于上述步骤可测得在处由位于第六关节的位姿传感器获得第六关节实际位姿p
6a
。
[0097]
随后基于已知的机械臂名义dh参数和机械臂正运动学，在处计算第六关节名义位姿p6。本实施例的机械臂正运动学结合各关节名义dh参数，参见表1，可得到当前测量构型处的名义(0t6)
k2
，本实施例设定(0t6)
k2
的各元素表征为：
[0098][0099]
其中，角标x6，y6,z6分别表示第6关节齐次变换矩阵中的方位向量(n,o,a)或位置向量(p)在x方向、y方向、z方向上的分量。根据(0t6)
k2
可得到对应的名义位姿向量(p6)
k2
：
[0100][0101]
根据表1可知，δub＝[δθ3,δa3,δα3,δβ3,δθ4,δd4,δa4,δα4,δθ5,δd5,δa5,δα5,δθ6,δd6,δa6,δα6]
t
。其中，角标b表示该向量元素为后四个关节的dh参数误差，角标3、4、5、6表示第三、第四、第五和第六关节。而测量构型处对应的雅克比矩阵为：
[0102][0103][0104]
其中上式中m和r均为3
×
1的微分变换向量，其表征了其下标对应的第i关节的dh参数误差(θ、d、a、α)到第六关节位姿误差的映射关系，其构造依据是机械臂正运动学、前两个关节的dh名义参数值、以及微分变换原理，属于现有技术，此处不再赘述。
[0105]
通过将所有处的(jb)
k2
组合即可得到jb：
[0106][0107]
本实施例中关于δua到第六关节位姿误差的映射，可采用测量构型处的雅克比矩阵：
[0108][0109]
来表征。其中，
[0110][0111]
其中上式中m和r均为3
×
1的微分变换向量，其表征了下标对应的第i关节的dh参数误差(θ、d、a、α)到第六关节位姿误差的映射关系，其构造依据是机械臂正运动学、前两个
关节的dh名义参数值、以及微分变换原理，属于现有技术，此处不再赘述。
[0112]
通过将所有处的(j
′a)
k2
组合即可得到j
′a：
[0113][0114]
通过δua结合公式(19)可求出由δua导致的第六关节位姿误差δp6←a：
[0115]
δp6←a＝j
′aδuaꢀꢀꢀ
(20)
[0116]
根据上述公式(11)所得到的全部实际位姿向量(p
6a
)
k2
可组合为：
[0117][0118]
根据上述公式(13)所得到的全部名义位姿向量(p6)
k2
可组合为：
[0119][0120]
公式(21)和(22)可结合公式(20)所得的δp6←a，获得第六关节在测量构型处的位姿误差δp6：
[0121]
δp6＝p
6a-p
6-δp6←aꢀꢀꢀ
(23)
[0122]
根据公式(6)、(9)、(16)、(23)以及δua、δub可建立如下标定模型：
[0123][0124]
根据公式(24)可求解出δua和δub，之后通过修正关节转角指令θi的方式间接补偿机械臂第六关节位姿误差，即可提高机械臂末端绝对定位精度。其中，修正的θi由传统的逆运动学方法求得，不再赘述。
[0125]
需说明的是，本实施例的方法不局限于针对六自由度机械臂，针对其他机械臂也可采用上述方法，其中标定过程的所选择的关节位置也不局限于上述第二和第六关节，可针对具体情况进行选择，上述方法仅需进行适应性调整即可。
[0126]
本实施例通过对上述标定方法与传统单一模型标定方法进行性能对比，其中选择的对比指标为dh参数误差辨识精度和补偿后的机械臂绝对定位精度。
[0127]
本次对比过程中所选定的机械臂dh参数误差值如表2所示：
[0128]
表2
[0129][0130]
首先依据传统标定方法建立单一标定模型：
[0131]
设定共需要24个测量构型(θ1,...,θ6)k(k＝1,2,...,24)。采用j表示单一模型中的雅可比矩阵：
[0132][0133]
其中，(j)k(k＝1,2,...,24)表示在第k个测量构型处的雅可比矩阵块，具体的该雅可比矩阵块可表示为：
[0134]
[0135]
其中，由下式求得
[0136][0137]
其中m和r均为3
×
1的微分变换向量，表征了下标对应的第i关节的dh参数误差(θ、d、a、α)到第六关节位姿误差的映射关系。
[0138]
单一模型中需要求解的dh参数误差为而(θ1,...,θ6)k处的第六关节的名义位姿误差(p
″6)k和实际位姿误差(p
″
6a
)k可参照本实施例中公式(10)-(13)的方法获得，再由下式得到全部测量构型处的p
″6和p
″
6a
：
[0139][0140]
进而可获得第六关节在测量构型(θ1,...,θ6)k处的位姿误差δp
″6[0141]
δp
″6＝p
″
6a-p
″6ꢀꢀꢀ
(29)
[0142]
根据公式(25)、(29)以及δu建立如下标定模型：
[0143]
δp
″6＝jδu
ꢀꢀꢀ
(30)
[0144]
然后采用本实施例的标定方法进行标定：
[0145]
依据表1、表2以及机械臂正运动学，分别计算全部和(θ1,...,θ6)k处对应的关节实际位姿p
2a
、p
6a
和p
″
6a
，具体过程可参照上述公式(1)、(2)以及公式(10)、(11)的具体计算过程。
[0146]
同时依据表1和机械臂正运动学，分别计算全部和(θ1,...,θ6)k处对应的关节名义位姿p2、p6和p
″6，具体过程可参照上述公式(3)、(4)以及公式(12)、(13)的具体计算过程。
[0147]
由于本实施例需在同等测量噪声强度下对比两者的标定精度，因此本实施例还需模拟真实标定时的测量噪声，具体数值如表3所示。
[0148]
表3
[0149][0150]
根据表3所示噪声分布规律，对应生成δδp2、δδp6以及δδp
″6。
[0151]
之后根据上述所获得位姿数据p
2a
、p2、p
″
6a
、p
″6以及测量噪声δδp2和δδp
″6，先求出下列两个位姿误差数据：
[0152][0153]
再将公式(31)代入公式(24)和(30)分别求得δua和δu的估计值和
[0154]
根据求出的δua，参照公式(20)计算出δp6←a，并结合已获得的位姿数据p
6a
、p6以及测量噪声δδp6，求得：
[0155]
δp6＝p
6a-p
6-δp6←a+δδp6ꢀꢀꢀ
(32)
[0156]
之后将公式(32)代入公式(24)，求得δub的估计值
[0157]
本实施例通过分别重复本实施例与传统单一模型标定方法约105次，并将所得和代入下式，计算各dh参数误差在不同模型中的辨识精度：
[0158][0159]
其中mean[x]表示求x的平均值，下标“*”表示“a”、“b”或空值，ku表示δu
*
中的参数误差的序号，其中δua，ku＝1,2,...,8；δub，ku＝1,2,...,16；δu，ku＝1,2,...,24；δu
*
(ku)为表2中相应的设定值。最终对比结果可参见图7所示。
[0160]
然后将和分别代入下式求出补偿后的dh参数残余误差：
[0161][0162]
最后本实施例在机械臂运动空间内随机生成用于测试第六关节坐标系f6位姿残差测量构型(k
t
＝1,2,...,1000)。并令δu
(ab)r
＝[δu
ar
,δu
br
]
t
，然后将δu
(ab)r
和δur分别与机械臂正运动学结合，计算出二者导致的第六关节坐标系f6位姿残差，最终对比结果可参见图8所示。
[0163]
结合图7所示，本实施例的标定方法能够将绝大部分dh参数误差的辨识精度提高至相对均衡的水平，特别是δdi和δai这两个距离类的误差。结合图8和图9结果表明，机械臂在参数误差被补偿后，本实施例的标定方法在103个测试点处能够使机械臂的末端残差进
一步降低。并且本实施例针对距离类dh参数误差的辨识精度改善效果更为明显，满足对机械臂有更高定位精度要求的应用场合。
[0164]
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，本领域技术人员利用上述揭示的技术内容做出些许简单修改、等同变化或修饰，均落在本发明的保护范围内。

技术特征：
1.一种基于分布式位姿采集和模型降维的机械臂标定方法，其特征在于，包括如下步骤：步骤一、在机械臂第i关节和第j关节分别安装一个位姿传感器，基于mdh规则建立机械臂各关节坐标系,且位姿传感器均可实时输出自身坐标系的位姿信息；其中第j关节为机械臂的末端关节，第i关节为机械臂的中间关节，且i和j均为正整数，i<j；步骤二、选择测量构型(θ1，...，θ
i
)
k1
(k1＝1,2,...,m1)和(θ1，...，θ
j
)
k2
(k2＝1,2,...,m2)，其中(θ1，...，θ
i
)表示测量构型对应的前i个关节的转角指令集合，(θ1，...，θ
j
)表示测量构型对应全部关节的转角指令集合，k1、k2分别表示测量构型的序号，m1和m2表示测量构型的数量；其中在(θ1，...，θ
i
)
k1
处由位于第i关节的位姿传感器获得第i关节实际位姿p
ia
，在(θ1，...，θ
j
)
k2
处由位于第j关节的位姿传感器获得第j关节实际位姿p
ja
；步骤三、基于已知的机械臂名义dh参数和机械臂正运动学，分别在(θ1，...，θ
i
)
k1
处计算第i关节名义位姿p
i
，在(θ1，...，θ
j
)
k2
处计算第j关节名义位姿p
j
；步骤四、根据δp
i
＝p
ia-p
i
计算(θ1，...，θ
i
)
k1
处的第i关节位姿误差δp
i
；步骤五、建立前i个关节的标定模型δp
i
＝j
a
δu
a
，并根据δp
i
＝j
a
δu
a
求解δu
a
；其中j
a
是依据机械臂正运动学和微分变换原理建立的雅克比矩阵，δu
a
表示前i个关节的dh参数误差向量；步骤六、根据δp
j
←
a
＝j
′
a
δu
a
计算在(θ1，...，θ
j
)
k2
处基于δu
a
所导致的第j关节位姿误差δp
j
←
a
,其中j
′
a
是δu
a
到第j关节在(θ1，...，θ
j
)
k2
处位姿误差的雅克比矩阵；并根据δp
j
＝p
ja-p
j-δp
j
←
a
计算(θ1，...，θ
j
)
k2
处的第j关节位姿误差δp
j
；步骤七、建立全部关节的标定模型δp
j
＝j
b
δu
b
，并根据δp
j
＝j
b
δu
b
求解δu
b
，其中j
b
是依据机械臂正运动学和微分变换原理建立的雅克比矩阵，δu
b
表示全部关节的dh参数误差向量；步骤八、通过修正关节指令的方式间接补偿dh参数误差δu
a
和δu
b
。2.根据权利要求1所述的机械臂标定方法，其特征在于，所述步骤一中机械臂各坐标系至少包括基坐标系、第i关节坐标系、第j关节坐标系、位于第i关节位姿传感器的第一位姿传感器坐标系以及位于位于第j关节位姿传感器的第二位姿传感器坐标系。3.根据权利要求2所述的机械臂标定方法，其特征在于，所述基坐标系的建立方法为：首先仅转动机械臂的第一关节，用测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z1，作为第一关节坐标系的z1轴；然后在第一个关节转角为0度时，即θ1＝0
°
处仅转动第二关节，测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z2，作为第二关节坐标系的z2轴；将向量x1＝z1×
z2定义为第一关节坐标系的x1轴，圆弧旋转轴向量z1和z2的公垂线与圆弧旋转轴向量z1的交点作为原点o1，即可建立第一关节坐标系f1；令基坐标系f0与f1重合，得到基坐标系f0。4.根据权利要求1所述的机械臂标定方法，其特征在于，所述第i关节坐标系的建立方法为：在θ1＝...＝θ
i
＝0
°
处仅转动第i+1关节，用测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
i+1
；在θ1＝...＝θ
i
＝θ
i+1
＝0
°
处仅转动第i+2关节，采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
i+2
；圆弧旋转轴向量
z
i+1
和z
i+2
的公垂线与圆弧旋转轴向量z
i+1
的交点作为原点o
i+1
；在θ1＝...＝θ
i-1
＝0
°
仅转动第i关节，测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
i
,过o
i+1
作z
i
的垂线，交z
i
于o
i
，作为第i关节坐标系的原点，即可建立第i关节坐标系f
i
。5.根据权利要求1所述的机械臂标定方法，其特征在于，所述位于第i关节位姿传感器的第一位姿传感器坐标系的建立方法为：首先第一位姿传感器采用三个激光靶球；在θ1＝...＝θ
i
＝0
°
处用测量仪器分别采集第i关节上的3个激光靶球坐标；选取其中一个激光靶球坐标作为共用坐标，结合另外两个激光靶球坐标构建单位向量v1和v2，将向量z
s1
＝v1×
v2作为第一位姿传感器坐标系的z
s1
轴，v1作为第一位姿传感器坐标系的x
s1
轴，共用的激光靶球坐标作为第一位姿传感器坐标系的原点坐标o
s1
，即可建立第一位姿传感器坐标系f
s1
。6.根据权利要求1所述的机械臂标定方法，其特征在于，所述第j关节坐标系的建立方法为：首先在θ1＝...＝θ
j-2
＝θ
j
＝0
°
处仅转动第j关节，用测量仪器采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
j-1
；在θ1＝...＝θ
j
＝0
°
处仅转动第j关节，采集末端关节上的位姿传感器的圆弧轨迹，拟合得到圆弧旋转轴向量z
j
，作为第j关节坐标系的z
j
轴；将向量x6＝z5×
z6定义为第j关节坐标系的x
j
轴，圆弧旋转轴向量z
j-1
和z
j
的公垂线与圆弧旋转轴向量z
j
的交点作为原点o
j
，由此建立了第j关节坐标系f
j
。7.根据权利要求1所述的机械臂标定方法，其特征在于，所述位于第j关节位姿传感器的第二位姿传感器坐标系的建立方法为：首先第二位姿传感器采用三个激光靶球；在θ1＝...＝θ
j
＝0
°
处用测量仪器分别采集第j关节上的3个激光靶球坐标；选取其中一个激光靶球坐标作为共用坐标，结合另外两个激光靶球坐标构建单位向量v3和v4，将向量z
s2
＝v3×
v4作为第二位姿传感器坐标系的z
s2
轴，v3作为第二位姿传感器坐标系的x
s2
轴，共用的激光靶球坐标作为第二位姿传感器坐标系的原点坐标o
s2
，即可建立第二位姿传感器坐标系f
s2
。8.根据权利要求1所述的机械臂标定方法，其特征在于，所述步骤二中第i关节及第j关节的实际位姿的计算方法为：首先基于得到第i关节坐标系f
i
、第一位姿传感器坐标系f
s1
、第j关节坐标系f
j
、第二位姿传感器坐标系f
s2
分别推导出第一位姿传感器坐标系到第i关节的齐次坐标变换矩阵以及第二位姿传感器坐标系到第j关节坐标系的齐次坐标变换矩阵:
*
t
#
＝(0t
*
)-1
(0t
#
)其中*代表第一位姿传感器或第二位姿传感器；#代表第i关节或第j关节；基于
*
t
#
＝(0t
*
)-1
(0t
#
)分别求出各测量构型(θ1，...，θ
i
)
k1
(k1＝1,2,...,m1)和(θ1，...，θ
j
)
k2
(k2＝1,2,...,m2)对应关节的实际(0t
#
)
ku
，即为(0t
#a
)
ku
，其中#代表第i关节或第j关节，对于测量构型(θ1，...，θ
i
)
k1
(k1＝1,2,...,m1)，ku指代k1(k1＝1,2,...,m1)，对于测量构型(θ1，...，θ
j
)
k2
(k2＝1,2,...,m2)，ku指代k2(k2＝1,2,...,m2)；设定(0t
#a
)
ku
的各元素表征为：
其中#代表第i关节或第j关节，对于测量构型(θ1，...，θ
i
)
k1
(k1＝1,2,...,m1)，ku指代k1(k1＝1,2,...,m1)，对于测量构型(θ1，...，θ
j
)
k2
(k2＝1,2,...,m2)，ku指代k2(k2＝1,2,...,m2)；由(0t
#a
)
ku
即可得到对应关节的实际位姿向量(p
#a
)
ku
：其中atan2(y,x)是双变量反正切函数。9.一种基于权利要求1-8任一项所述机械臂标定方法的机械臂标定装置，其特征在于，包括测量仪器和位姿传感器，所述位姿传感器至少包括两个，且其中一个位姿传感器安设于机械臂的末端关节；另一位姿传感器安设于机械臂的中间关节。10.根据权利要求9所述的机械臂标定装置，其特征在于，所述机械臂采用六自由度机械臂，所述测量仪器采用激光跟踪仪，所述位姿传感器采用三个不共线的激光靶球，每个激光靶球分别通过靶球基座粘贴于一端有圆台的螺纹杆上；位于机械臂中间关节的三个激光靶球通过螺纹杆连接于剖分式连接圈，两段剖分式连接圈夹紧连接于机械臂中间关节；位于机械臂末端关节的三个激光靶球通过螺纹杆连接于末端关节的法兰端面的螺纹孔。

技术总结
本发明公开了一种基于分布式位姿采集和模型降维的机械臂标定方法及装置，该方法通过位姿传感器获得不同测量构型下各关节的实际位姿数据，并与对应关节的名义位姿数据对比分析，获取对应关节的位姿误差，同时利用前段关节的DH参数误差向量求解后段关节的位姿误差，进而剔除前段关节对末端关节位姿误差的影响，最后基于标定模型分别获得对应DH参数误差向量，并通过修正关节指令的方式间接补偿DH参数误差，该方法将拥挤在传统单一标定模型中的DH参数误差疏散至多个子标定模型，显著改善了模型性态水平，实现了DH参数误差辨识精度的进一步提升，提高了标定精度，同时保证了误差辨识结果的完整性。结果的完整性。结果的完整性。


技术研发人员：蒋周翔 秦鹏举 苏瑞 白龙 龙忠杰
受保护的技术使用者：北京信息科技大学
技术研发日：2023.07.24
技术公布日：2023/10/7