CN102062576A - 基于激光跟踪测量的附加外轴机器人自动标定装置及方法 - Google Patents

Abstract

本发明公开了一种基于激光跟踪测量的附加外轴机器人自动标定装置及方法。它包括系统平台、靶标法兰、靶标安装孔、机器人导轨、机器人、激光跟踪仪、工件、数控精加工机床、三坐标数控定位器、上位机；系统平台上安装有9个通过高精密数控机床加工出的靶标安装孔、三坐标数控定位器、机器人导轨，在三坐标数控定位器上固定有工件，在机器人导轨上安装有机器人，机器人上安装有靶标法兰，靶标法兰上有6个靶标安装孔，在靶标安装孔上设有靶标，系统平台外侧设有数控精加工机床、激光跟踪仪、上位机。本发明的优点在于：(1)能够自动化标定带附加外轴的机器人坐标系统；(2)标定精度高、标定结果可靠；(3)标定操作简单、效率高。

Description

基于激光跟踪测量的附加外轴机器人自动标定装置及方法 

技术领域

本发明涉及一种基于激光跟踪测量的附加外轴机器人自动标定装置及方法。 

背景技术

飞机数字化装配系统中的大量模块化工装和自动化设备系统，比如系统平台、靶标法兰、靶标、机器人导轨、机器人、激光跟踪仪、工件、数控精加工机床、三坐标数控定位器，都是在自身的局部坐标系下伺服运动。为了提高系统的运动性能和精度，必须构建系统全局坐标系与各组成子系统局部坐标系的转换关系。各组成子系统局部坐标系的创建与标定是系统初始化必须完成的重要工作之一，其创建与标定将直接影响到系统的精度。 

带附加外轴的机器人以其极高的重复定位精度、较高的绝对定位精度、高度的柔性、较小的安装空间需求和可编程控制等特点，保证其具有比较大的灵活作业空间的同时，保证了装配系统总体布局设计上的结构紧凑性和作业空间开敞性，在飞机数字化装配系统中具有广泛应用。 

构建和标定机器人坐标系可以较为精确地确定靶标、工具、工件、移动导轨坐标系在全局坐标系下的位姿，机器人移动时便可准确定位机器人及其工具。 

构建和标定机器人坐标系的传统方法仅仅提出了个别坐标系的构建和标定过程而不够系统全面，有的要求特殊的标准试件而难以加工，大多数没有给出标定后具体精度的数值指标。 

带附加外轴的机器人系统坐标系的构建与标定过程是一个极其繁琐而复杂的过程，本发明提出了一种快速简单、精度较高的方法来构建与标定系统各坐标系，采用激光跟踪仪来测量一系列点并进行拟合计算，可以实现大批量全自动化构建与标定坐标系组。该方法具有操作简单、精度高、可靠性强等优点。 

发明内容

本发明的目的是针对上述带附加外轴的机器人系统坐标系的构建，提供一种基于激光跟踪测量的附加外轴机器人自动标定装置及方法。 

基于激光跟踪测量的附加外轴机器人自动标定装置包括系统平台、靶标法兰、靶标安装孔、机器人导轨、机器人、激光跟踪仪、工件、数控精加工机床、三坐标数控定位器、上位机；系统平台上安装有9个通过高精密数控机床加工 出的靶标安装孔、三坐标数控定位器、机器人导轨，在三坐标数控定位器上固定有工件，在机器人导轨上安装有机器人，机器人上安装有靶标法兰，靶标法兰上有6个靶标安装孔，在靶标安装孔上设有靶标，系统平台外侧设有数控精加工机床、激光跟踪仪、上位机。 

所述的机器人的定位精度为±0.692mm。 

所述的上位机、机器人、激光跟踪仪之间通过RS485以太网连接实现Socket通讯，以字符串的形式发送数据、命令、请求，通过上位机完成测量数据和计算数据的备份和保存。 

基于激光跟踪测量的附加外轴机器人自动标定方法包括标定世界坐标系、标定靶标工具坐标系、标定BASE坐标系和标定TRACK坐标系，标定BASE坐标系包括标定BASE坐标系各坐标轴方向和标定BASE坐标系原点， 

标定世界坐标系的步骤： 

a)测量9个靶标在激光跟踪仪坐标系下的位置； 

b)将靶标在激光跟踪仪坐标系下的空间实际位置与理论位置进行匹配计算，获取激光跟踪仪坐标系在世界坐标系下的姿态； 

c)根据激光跟踪仪坐标系与世界坐标系的位姿转换关系，将激光跟踪仪测量坐标系转站到世界坐标系下； 

标定BASE坐标系各坐标轴方向的步骤： 

d)使机器人沿BASE坐标系的X轴运动，连续测量靶标的位置，拟合一条直线即机器人BASE坐标系的X轴方向；使机器人沿BASE坐标系的Y轴运动，连续测量靶标的位置，拟合一条直线即机器人BASE坐标系Y轴的方向； 

e)通过机器人BASE坐标系的X轴和Y轴叉乘，计算出Z＝X×Y，即机器人BASE坐标系Z轴的方向； 

标定靶标工具坐标系的步骤： 

f)激光跟踪仪坐标系转站，使激光跟踪仪坐标系的方向转站后与机器人BASE坐标系的方向一致； 

g)激光跟踪仪控制机器人以4种不同的姿态使靶标达到空间同一点，分别记录机器人的工具中心点在BASE坐标系下的位姿； 

h)经过旋转矩阵变换，计算靶标在TOOL0坐标系中的位置； 

标定BASE坐标系原点的步骤： 

i)根据激光跟踪仪坐标系与世界坐标系的位姿转换关系，将激光跟踪仪 测量坐标系转站到世界坐标系下； 

i)上位机控制机器人回零，激光跟踪仪测量靶标在世界坐标系下的位置； 

k)经过位姿矩阵变换，计算BASE坐标系原点在世界坐标系中的位置；标定TRACK坐标系的步骤： 

l)上位机控制机器人沿TRACK坐标系X轴运动一段直线，每隔一段距离激光跟踪仪测量靶标在世界坐标系下的位置； 

m)将测量得到的一组数据进行直线拟合，得到TRACK坐标系X轴在世界坐标系下的姿态作为TRACK坐标系的方向。 

步骤b)所述的将靶标在激光跟踪仪坐标系下的空间实际位置与设定位置进行匹配计算，获取激光跟踪仪坐标系在世界坐标系下的姿态的步骤为： 

记测量3～9个靶标的实际坐标为P_A、理论坐标为P_B，P_Ai，i＝1，2，...，m为空间固定靶标的实际坐标，P_Bi，i＝1，2，...，m为空间固定靶标的理论坐标； 

姿态计算是实际测量坐标值到理论坐标值一个刚性转换^AT_B： 

P_B＝^AT_B×P_A              (1) 

该刚性变换^AT_B可表达为一个旋转变化R和一个平移变换t的组合，即： 

P_B＝^AT_B×P_A＝R×P_A+t     (2) 

由于测量误差的存在，变换后的结果不能完全等于理论的坐标值，即： 

ζ＝P_Bi-(R×P_Ai+t)，ζ为误差矩阵。 

(3) 

式中，{x_Ai’，y_Ai’，z_Ai’}为P_Ai经旋转变化和一个平移变换后的坐标值，{x_Ai，y_Ai，z_Ai}为P_Bi的坐标值。 

令D_i，i＝1，2，...，m为靶标对应测量点转换后与理论点之间的距离，即 

$D_i=\sqrt{{({x_{\mathrm{Ai}}}^{\′}-x_{\mathrm{Bi}})}^2+{({y_{\mathrm{Ai}}}^{\′}-y_{\mathrm{Bi}})}^2+{({z_{\mathrm{Ai}}}^{\′}-z_{\mathrm{Bi}})}^2}---\left(4\right)$

令 

导出相对应点的位置误差的均方差表达式为 

${\mathrm{\σ}}_D=\sqrt{\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{(D_i-\stackrel{\‾}{D})}^2}{m-1}}---\left(5\right)$

以位置误差的均方差为目标函数，进行最小二乘拟合求解得到R和t，从而 得到测量的实际坐标值到理论坐标值的位姿变换矩阵，即可将当前激光跟踪仪坐标系转站到世界坐标系。 

步骤d)所述的拟合一条直线即机器人BASE坐标系的X轴方向的步骤为： 

设表达式 

为在激光跟踪仪坐标系下机器人BASE坐标系的X轴的拟合曲线方程，需要拟合计算出各系数a₁、b₁、c₁、a₂、b₂、c₂的值。 

采用激光跟踪仪测量m，m≥3个点都在该直线上，将这些点的坐标代入到该直线方程，构成m个方程组，写成矩阵形式： 

$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_m& y_m& z_m\end{array}\right]\·\left[\begin{array}{cc}{a}_1& a_2\\ b_1& b_2\\ c_1& c_2\end{array}\right]=\left[\begin{array}{cc}-1& -1\\ -1& -1\\ \·& \·\\ \·& \·\\ \·& \·\\ -1& -1\end{array}\right]---\left(6\right)$

式中，{x_i，y_i，z_i}为m，m≥3个点在激光跟踪仪坐标系下的坐标值。 

即Q·K＝C                (7) 

其中 $Q=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_m& y_m& z_m\end{array}\right],$ $K=\left[\begin{array}{cc}{a}_1& a_2\\ b_1& b_2\\ c_1& c_2\end{array}\right],$ $C=\left[\begin{array}{cc}-1& -1\\ -1& -1\\ \·& \·\\ \·& \·\\ \·& \·\\ -1& -1\end{array}\right]$

式中，(x_i，y_i，z_i)为Q_i(i＝1，2，…，m)的坐标值。 

通过最小二乘法求解式，得 

K＝(Q^TQ)^-1Q^TC            (8) 

通过求解出来的a₁，b₁，c₁，a₂，b₂，c₂即可得到该直线的一组方向数(l，m，n)，即该直线的方向矢量 

$l=\left|\begin{array}{cc}{b}_1& c_1\\ b_2& c_2\end{array}\right|,m=\left|\begin{array}{cc}{c}_1& a_1\\ c_2& a_2\end{array}\right|,n=\left|\begin{array}{cc}{a}_1& b_1\\ a_2& b_2\end{array}\right|$

归一化， 

(n_测，o_测，a_测)为该直线的归一化方向矢量，即机器人BASE坐标系的X轴方向。 

步骤e)所述的通过机器人BASE坐标系的X轴和Y轴叉乘的步骤为：设拟合出来坐标系的X、Y轴方向分别用矢量[n_x测 o_x测 a_x测]、 

表示。测量过程中X、Y轴不一定正交，需要进行正交化处理， 

则可 

拟合出来坐标系的X、Y、Z轴方向相对于激光跟踪仪坐标系的旋转矩阵 

本发明与现有技术相比具有的有益效果： 

1)建立带附加外轴的机器人坐标系自动化标定体系和方法； 

2)标定过程稳定可靠，系统整体精度高； 

3)操作简单，具有可拓展性，可以满足多种带附加外轴的机器人坐标系自动化标定； 

4)带附加外轴的机器人系统中具有多个坐标系需要标定，在标定过程中，每个坐标系的精度可以得到评价，有利于后续的对整个系统精度的评价； 

5)整个标定过程由上位机、附加外轴机器人、激光跟踪仪协同完成，操控过程简单。 

附图说明

图1为带附加外轴的机器人坐标系自动化标定系统图； 

图2为机器人手腕位姿测量测点布局图； 

图3为带附加外轴的机器人坐标系自动化标定流程图； 

图4为机器人末端靶标的对尖法示意图； 

图5为机器人坐标系示意图； 

图6为机器人BASE坐标系测量示意图。 

图中，系统平台1、靶标法兰2、靶标3、机器人导轨4、机器人5、激光跟踪仪6、工件7、数控精加工机床8、三坐标数控定位器9、上位机10、世界坐标系11、工具坐标系12、BASE坐标系13、TRACK坐标系14、工件坐标系15、激光跟踪仪坐标系16。 

具体实施方式

如图1所示，基于激光跟踪测量的附加外轴机器人自动标定装置包括系统平台1、靶标法兰2、靶标安装孔3、机器人导轨4、机器人5、激光跟踪仪6、工件7、数控精加工机床8、三坐标数控定位器9、上位机10；系统平台1上安装有9个通过高精密数控机床加工出的靶标安装孔3、三坐标数控定位器9、机器人导轨4，在三坐标数控定位器9上固定有工件7，在机器人导轨4上安装有机器人5，机器人5上安装有靶标法兰2，靶标法兰2上有6个靶标安装孔3，在靶标安装孔3上设有靶标，系统平台1外侧设有数控精加工机床8、激光跟踪仪6、上位机10。 

所述的机器人5的定位精度为±0.692mm。 

所述的上位机10、机器人5、激光跟踪仪6之间通过RS485以太网连接实现Socket通讯，以字符串的形式发送数据、命令、请求，通过上位机10完成测量数据和计算数据的备份和保存。 

基于激光跟踪测量的附加外轴机器人自动标定方法包括标定世界坐标系、标定靶标工具坐标系、标定BASE坐标系和标定TRACK坐标系，标定BASE坐标系包括标定BASE坐标系各坐标轴方向和标定BASE坐标系原点， 

标定世界坐标系11的步骤： 

a)测量9个靶标在激光跟踪仪坐标系16下的位置； 

b)将靶标在激光跟踪仪坐标系16下的空间实际位置与理论位置进行匹配计算，获取激光跟踪仪坐标系16在世界坐标系11下的姿态； 

c)根据激光跟踪仪坐标系16与世界坐标系11的位姿转换关系，将激光跟踪仪测量坐标系16转站到世界坐标系11下； 

标定BASE坐标系13各坐标轴方向的步骤： 

d)使机器人5沿BASE坐标系13的X轴运动，连续测量靶标3的位置， 拟合一条直线即机器人BASE坐标系13的X轴方向；使机器人沿BASE坐标系13的Y轴运动，连续测量靶标的位置，拟合一条直线即机器人BASE坐标系13Y轴的方向； 

e)通过机器人BASE坐标系13的X轴和Y轴叉乘，计算出Z＝X×Y，即机器人BASE坐标系13Z轴的方向； 

标定靶标工具坐标系12的步骤： 

f)激光跟踪仪6坐标系转站，使激光跟踪仪坐标系16的方向转站后与机器人BASE坐标系13的方向一致； 

g)如图4所示，激光跟踪仪6控制机器人5以4种不同的姿态使靶标达到空间同一点，分别记录机器人5的工具中心点在BASE坐标系13下的位姿； 

h)经过旋转矩阵变换，计算靶标在TOOL0坐标系中的位置； 

标定BASE坐标系13原点的步骤： 

i)根据激光跟踪仪坐标系16与世界坐标系11的位姿转换关系，将激光跟踪仪测量坐标系16转站到世界坐标系11下； 

j)上位机10控制机器人5回零，激光跟踪仪6测量靶标在世界坐标系11下的位置； 

k)经过位姿矩阵变换，计算BASE坐标系13原点在世界坐标系11中的位置； 

标定TRACK坐标系14的步骤： 

l)上位机10控制机器人5沿TRACK坐标系14X轴运动一段直线，每隔一段距离激光跟踪仪6测量靶标3在世界坐标系11下的位置； 

m)将测量得到的一组数据进行直线拟合，得到TRACK坐标系14X轴在世界坐标系11下的姿态作为TRACK坐标系14的方向。 

步骤b)所述的将靶标在激光跟踪仪坐标系16下的空间实际位置与设定位置进行匹配计算，获取激光跟踪仪坐标系16在世界坐标系11下的姿态的步骤为： 

记测量3～9个靶标的实际坐标为P_A、理论坐标为P_B，P_Ai，i＝1，2，...，m为空间固定靶标的实际坐标，P_Bi，i＝1，2，...，m为空间固定靶标的理论坐标； 

姿态计算是实际测量坐标值到理论坐标值一个刚性转换^AT_B： 

P_B＝^AT_B×P_A                    (1) 

该刚性变换^AT_B可表达为一个旋转变化R和一个平移变换t的组合，即： 

P_B＝^AT_B×P_A＝R×P_A+t           (2) 

由于测量误差的存在，变换后的结果不能完全等于理论的坐标值，即： 

ζ＝P_Bi-(R×P_Ai+t)，ζ为误差矩阵。(3) 

式中，{x_Ai’，y_Ai’，z_Ai’}为P_Ai经旋转变化和一个平移变换后的坐标值，{x_Ai，y_Ai，z_Ai}为P_Bi的坐标值。 

令D_i，i＝1，2，...，m为靶标对应测量点转换后与理论点之间的距离，即 

$D_i=\sqrt{{({x_{\mathrm{Ai}}}^{\′}-x_{\mathrm{Bi}})}^2+{({y_{\mathrm{Ai}}}^{\′}-y_{\mathrm{Bi}})}^2+{({z_{\mathrm{Ai}}}^{\′}-z_{\mathrm{Bi}})}^2}---\left(4\right)$

令 

导出相对应点的位置误差的均方差表达式为 

${\mathrm{\σ}}_D=\sqrt{\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{(D_i-\stackrel{\‾}{D})}^2}{m-1}}---\left(5\right)$

以位置误差的均方差为目标函数，进行最小二乘拟合求解得到R和t，从而得到测量的实际坐标值到理论坐标值的位姿变换矩阵，即可将当前激光跟踪仪坐标系16转站到世界坐标系11。 

步骤d)所述的拟合一条直线即机器人BASE坐标系13的X轴方向的步骤为： 

设表达式 

为在激光跟踪仪坐标系16下机器人BASE坐标系13的X轴的拟合曲线方程，需要拟合计算出各系数a₁、b₁、c₁、a₂、b₂、c₂的值。 

采用激光跟踪仪6测量m，m≥3个点都在该直线上，将这些点的坐标代入到该直线方程，构成m个方程组，写成矩阵形式： 

$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_m& y_m& z_m\end{array}\right]\·\left[\begin{array}{cc}{a}_1& a_2\\ b_1& b_2\\ c_1& c_2\end{array}\right]=\left[\begin{array}{cc}-1& -1\\ -1& -1\\ \·& \·\\ \·& \·\\ \·& \·\\ -1& -1\end{array}\right]---\left(6\right)$

式中，{x_i，y_i，z_i}为m，m≥3个点在激光跟踪仪坐标系(16)下的坐标值。 

即Q·K＝C        (7) 

其中 $Q=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_m& y_m& z_m\end{array}\right],$ $K=\left[\begin{array}{cc}{a}_1& a_2\\ b_1& b_2\\ c_1& c_2\end{array}\right],$ $C=\left[\begin{array}{cc}-1& -1\\ -1& -1\\ \·& \·\\ \·& \·\\ \·& \·\\ -1& -1\end{array}\right]$

式中，(x_i，y_i，z_i)为Q_i(i＝1，2，…，m)的坐标值。 

通过最小二乘法求解式(7)，得 

K＝(Q^TQ)^-1Q^TC    (8) 

通过求解出来的a₁，b₁，c₁，a₂，b₂，c₂即可得到该直线的一组方向数(l，m，n)，即该直线的方向矢量 

$l=\left|\begin{array}{cc}{b}_1& c_1\\ b_2& c_2\end{array}\right|,m=\left|\begin{array}{cc}{c}_1& a_1\\ c_2& a_2\end{array}\right|,n=\left|\begin{array}{cc}{a}_1& b_1\\ a_2& b_2\end{array}\right|$

归一化， 

(n_测，o_测，a_测)为该直线的归一化方向矢量，即机器人BASE坐标系13的X轴方向。 

步骤e)所述的通过机器人BASE坐标系13的X轴和Y轴叉乘的步骤为： 

如图6所示，设拟合出来坐标系的X、Y轴方向分别用矢量[n_x测 o_x测 a_x测]、 

表示。测量过程中X、Y轴不一定正交，需要进行正交化处理， 

则可 

拟合出来坐标系的X、Y、Z轴方向相对于激光跟踪仪坐标系16的旋转矩阵 

步骤h)所述的计算靶标在TOOL0坐标系中的位置的步骤为： 

机器人5各坐标系如图5所示。机器人5末端手腕坐标系T_h在机器人BASE坐标系T_b下的位姿机器人示教盒上得到，靶标工具坐标系在机器人末端手腕坐标系T_h下的位置为所求，固定不变。它们满足以下变换方程： 

^BT_H·^HT_R＝^BT_R    (9) 

式中，^BT_H是机器人末端手腕坐标系T_h相对于机器人BASE坐标系T_b的变换矩阵，^HT_R是靶标工具坐标系T_r相对于机器人末端手腕坐标系T_h的变换矩阵， ^BT_R是靶标工具坐标系T_r相对于机器人BASE坐标系T_b的变换矩阵。 

a)示教盒上得到机器人末端手腕坐标系T_h在机器人BASE坐标系T_b下的位姿以原点坐标X_hb、Y_hb、Z_hb及四元数q₁、q₂、q₃、q₄的形式表示时， 

$T_H^B=\left[\begin{array}{cccc}(q_1^2+q_2^2-q_3^2-q_4^2)& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)& X_{\mathrm{hb}}\\ 2(q_2{q}_3+q_1{q}_4)& (q_1^2-q_2^2+q_3^2-q_4^2)& 2(q_3{q}_4-q_1{q}_2)& Y_{\mathrm{hb}}\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_3{q}_4+q_1{q}_2)& (q_1^2-q_2^2-q_3^2+q_4^2)& Z_{\mathrm{hb}}\\ 0& 0& 0& 1\end{array}\right]$

b)示教盒上得到机器人末端手腕坐标系T_h在机器人BASE坐标系T_b下的位姿以原点坐标X_hb、Y_hb、Z_hb及欧拉角φ、θ、ψ的形式表示时。 

$T_H^B=\left[\begin{array}{cccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}& X_{\mathrm{hb}}\\ \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& Y_{\mathrm{hb}}\\ \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}& Z_{\mathrm{hb}}\\ 0& 0& 0& 1\end{array}\right]$

靶标工具坐标系T_r相对于机器人末端手腕坐标系T_h的变换矩阵^HT_R未知为所求，但位姿固定不变。 

$T_R^H=\left[\begin{array}{cccc}{n}_{\mathrm{xrh}}& o_{\mathrm{xrh}}& a_{\mathrm{xrh}}& X_{\mathrm{rh}}\\ n_{\mathrm{yrh}}& o_{\mathrm{yrh}}& a_{\mathrm{yrh}}& Y_{\mathrm{rh}}\\ n_{\mathrm{zrh}}& o_{\mathrm{zrh}}& a_{\mathrm{zrh}}& Z_{\mathrm{rh}}\\ 0& 0& 0& 1\end{array}\right]$

靶标工具坐标系T_r相对于机器人BASE坐标系T_b的变换矩阵未知，但位置固定不变、姿态变化。 

$T_R^B=\left[\begin{array}{cccc}{n}_{\mathrm{xrb}}& o_{\mathrm{xrb}}& a_{\mathrm{xrb}}& X_{\mathrm{rb}}\\ n_{\mathrm{yrb}}& o_{\mathrm{yrb}}& a_{\mathrm{yrb}}& Y_{\mathrm{rb}}\\ n_{\mathrm{zrb}}& o_{\mathrm{zrb}}& a_{\mathrm{zrb}}& Z_{\mathrm{rb}}\\ 0& 0& 0& 1\end{array}\right]$

取位置参数代入(9)式，得 

$T_H^B\·\left[\begin{array}{c}{X}_{\mathrm{rh}}\\ Y_{\mathrm{rh}}\\ Z_{\mathrm{rh}}\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{rb}}\\ Y_{\mathrm{rb}}\\ Z_{\mathrm{rb}}\end{array}\right]---\left(10\right)$

方程(10)中^BT_H已知，X_rh、T_rh、Z_rh、X_rb、Y_rb、Z_rb固定不变。至少取三个位姿可以计算出X_rh、Y_rh、Z_rh；取三个以上位姿可以采用基于奇异值分解的最小二乘估计计算方法拟合出X_rh、Y_rh、Z_rh。 

步骤k)所述的计算BASE坐标系13原点在世界坐标系11中的位置的步骤为： 

机器人5BASE坐标系13T_b相对于世界坐标系11T_w的旋转矩阵^WR_B为 

机器人5BASE坐标系13T_b相对于世界坐标系11T_w的姿态用四元数表示为(q1，q2，q3，q4) 

sign(q2)＝sign(a_y拟-o_z拟) 

sign(q3)＝sign(n_z拟-a_x测) 

sign(q4)＝sign(o_x测-n_y拟) 

机器人5BASE坐标系13、靶标工具坐标系12、世界坐标系11之间的关系如图6所示。^WT_B是机器人5BASE坐标系13T_b相对于世界坐标系11T_w的变换矩阵，^BT_R是靶标工具坐标系12T_r相对于机器人5BASE坐标系13T_b的变换矩阵，通过式(9)确定。它们满足以下变换方程： 

^WT_B·^BT_R＝^WT_R        (11) 

设机器人5靶标工具坐标系12相对于机器人5BASE坐标系13的坐标值为 ^BP_R(x_rb，y_rb，z_rb)，通过式(9)确定。机器人5BASE坐标系13相对于世界坐标系11的旋转变换矩阵为^WR_B，通过坐标轴方向的拟合计算已经得到。靶标3在世界坐标系11下的坐标值为^WP_R(x_rw，y_rw，z_rw)，通过激光跟踪仪6测得。机器人5BASE在世界坐标系13下的坐标值为^WP_B(x_bw，y_bw，z_bw)，为待求。 

$\left[\begin{array}{cc}R_B^W& P_B^W\\ O& 1\end{array}\right]\·\left[\begin{array}{cc}R_R^B& P_R^B\\ O& 1\end{array}\right]=\left[\begin{array}{cc}R_R^W& P_R^W\\ O& 1\end{array}\right]$

^WP_B·^BP_R+^WP_B·1＝^WP_R

^WP_B＝^WP_R-^WR_B·^BP_R

所述的上位机、机器人、激光跟踪仪之间通过RS485以太网连接实现Socket通讯，以字符串的形式发送数据、命令、请求，通过上位机完成测量数据和计算数据的备份和保存。其具体实现过程如图3所示，主要通过上位机PC相应 按钮发送相应指令来实现，具体步骤如下： 

(1)上位机PC发送“系统自检”指令给附加外轴机器人和激光跟踪仪，检验附加外轴机器人、激光跟踪仪全行程运动是否正常。 

(2)标定世界坐标系：上位机PC发送“连续测量公共观测点”指令给激光跟踪仪，激光跟踪仪将测得公共观测点在激光跟踪仪本机BASE坐标系下的坐标值反馈到上位机PC，上位机PC通过与数据库中保存的理论值进行姿态匹配运算，将激光跟踪仪本机BASE坐标系转站到世界坐标系下。 

(3)标定BASE坐标系坐标轴方向：上位机PC发送“机器人回零”指令给附加外轴机器人，机器人回零后反馈“机器人已回零”给上位机PC，上位机PC发送“机器人沿BASE坐标系X轴移动100mm”指令给附加外轴机器人，机器人移动后反馈“机器人已沿BASE坐标系X轴移动100mm”指令给激光跟踪仪，激光跟踪仪将测得靶标在世界坐标系下的坐标值反馈到上位机PC，...，反复测得若干点后上位机PC拟合计算出BASE坐标系X轴的方向。同样的方法可得出BASE坐标系Y、Z轴的方向。 

(4)标定靶标工具坐标系：上位机PC发送“转站至BASE坐标系方向”指令给激光跟踪仪，激光跟踪仪转站后反馈“转站完成”给上位机PC；上位机PC发送“机器人运动到靶标测量位置”指令给附加外轴机器人，机器人完成运动后反馈当前位姿下TCP相对于BASE坐标系的位姿给上位机PC，并发送“机器人已运动到靶标测量位置”给激光跟踪仪，激光跟踪仪测量当前位姿下靶标位置作为初值并发送“调整位姿(运动4、5、6轴)”指令给附加外轴机器人，激光跟踪仪通过比较当前位姿下靶标位置与初值，控制机器人以新的姿态运动到初值处，机器人完成运动后重复反馈当前位姿下TCP相对于BASE坐标系的位姿给上位机PC并通过激光跟踪仪调整新的姿态，...，反复测得若干姿态后上位机PC根据位姿转换矩阵拟合计算靶标在TCP下的位置。 

(5)标定BASE坐标系原点：上位机PC发送“转站至世界坐标系”指令给激光跟踪仪，激光跟踪仪转站后反馈“转站完成”给上位机PC；上位机PC发送“机器人回零”指令给附加外轴机器人，机器人回零后反馈“机器人已回零”给激光跟踪仪，激光跟踪仪将测得靶标在世界坐标系下的坐标值反馈到上位机PC，上位机PC根据位姿转换矩阵计算BASE坐标系原点在世界坐标系下的位置。 

(6)标定TRACK坐标系：上位机PC发送“机器人回零”指令给附加外轴机器人，机器人回零后反馈“机器人已回零”给上位机PC，上位机PC发送“机器人沿TRACK方向移动100mm”指令给附加外轴机器人，机器人移动后反馈 “机器人已沿TRACK方向移动100mm”指令给激光跟踪仪，激光跟踪仪将测得靶标在世界坐标系下的坐标值反馈到上位机PC，...，反复测得若干点后上位机PC拟合计算出TRACK方向。 

至此，已完全确立附加外轴机器人在世界坐标系下的位姿，实现了自动化标定过程。 

算例： 

标定世界坐标系11的步骤： 

实验过程中加工11个公共观测点时确定了其在世界坐标系下理论坐标值，每次移动激光跟踪仪后测量出这些点在本机BASE坐标系下的测量坐标值，采用基于奇异值分解的最小二乘估计计算方法，拟合出世界坐标系，如表1所示。 

表1激光跟踪仪测量坐标系移至世界坐标系转站实验数据 

标定靶标工具坐标系12的步骤： 

实验中，机器人从6个方位使靶标接近世界坐标系下固定虚点(3496.047，3794.223，1292.279)，经计算可得到靶标中心在手腕坐标系T_h下的位置为(-43.58395，76.56171，21.36438)，如表2所示，数据拟合过程中位置误差的均方差为0.0672089mm。 

表2激光跟踪仪靶标工具坐标系建立实验数据 

测量、计算数据	X         Y         Z
		世界坐标系下固定虚点位置	3496.047  3794.223  1292.279
手腕坐标系下靶标位置	-43.58395 76.56171  21.36438

标定BASE坐标系的步骤： 

实验中，机器人回零，通过机器人示教盒上可以读出，靶标工具坐标系相 对于机器人BASE坐标系的坐标值为^BP_R(1292.4，75.9，1530.8)、姿态用(q1，q2，q3，q4)表示为(0.50068，0，0.86568，0)；通过坐标轴方向的拟合计算，机器人BASE坐标系相对于世界坐标系的姿态用(q1，q2，q3，q4)表示为(0.70732，-0.000188，0.000231，0.70689)；通过激光跟踪仪测得靶标在世界坐标系下的坐标值为^WP_R(3411.396，4996.885，1367.396)。即可计算出机器人BASE在世界坐标系下的位置为^WP_B(3486.419，3703.533，-162.643)，如表3所示。 

表3机器人BASE坐标系建立实验数据 

标定TRACK坐标系14的步骤： 

实验中，锁住机器人6个关节，使机器人沿导轨移动，测得机器人在不同导轨位置靶标在世界坐标系下的坐标值，如表4所示，拟合计算出机器人导轨的方向。测量得到机器人导轨方向不在世界坐标系的XOY平面内，将其向世界坐标系的XOY平面的投影作为机器人TRACK坐标系的X轴方向，以世界坐标系的Z轴作为机器人TRACK坐标系的Z轴，拟合出机器人TRACK坐标系在世界坐标系下的姿态用四元数的形式表示为(0.70732，-0.000188，0.0002365，0.70689)。 

表4机器人导轨移动的测量数据 

激光跟踪仪测量值	X	Y	Z
				站位1(导轨1.4m处)	3411.357	3596.528	1367.738
站位2(导轨2.8m处)	3411.291	2196.507	1367.851
				站位3(导轨4.2m处)	3411.435	796.325	1368.016
站位4(导轨5.6m处)	3411.404	-603.633	1367.937

Claims (7)

1.一种基于激光跟踪测量的附加外轴机器人自动标定装置，其特征在于包括系统平台(1)、靶标法兰(2)、靶标安装孔(3)、机器人导轨(4)、机器人(5)、激光跟踪仪(6)、工件(7)、数控精加工机床(8)、三坐标数控定位器(9)、上位机(10)；系统平台(1)上安装有9个通过高精密数控机床加工出的靶标安装孔(3)、三坐标数控定位器(9)、机器人导轨(4)，在三坐标数控定位器(9)上固定有工件(7)，在机器人导轨(4)上安装有机器人(5)，机器人(5)上安装有靶标法兰(2)，靶标法兰(2)上有6个靶标安装孔(3)，在靶标安装孔(3)上设有靶标，系统平台(1)外侧设有数控精加工机床(8)、激光跟踪仪(6)、上位机(10)。

2.根据权利要求1所述的一种基于激光跟踪测量的附加外轴机器人自动标定装置，其特征在于所述的机器人(5)的定位精度为±0.692mm。

3.根据权利要求1所述的一种基于激光跟踪测量的附加外轴机器人自动标定装置，其特征在于所述的上位机(10)、机器人(5)、激光跟踪仪(6)之间通过RS485以太网连接实现Socket通讯，以字符串的形式发送数据、命令、请求，通过上位机(10)完成测量数据和计算数据的备份和保存。

4.一种使用如权利要求1所述装置的基于激光跟踪测量的附加外轴机器人自动标定方法，其特征在于包括标定世界坐标系(11)、标定靶标工具坐标系(12)、标定BASE坐标系(13)和标定TRACK坐标系(14)，标定BASE坐标系(13)包括标定BASE坐标系(13)各坐标轴方向和标定BASE坐标系(13)原点，其中，

标定世界坐标系(11)的步骤为：

a)测量9个靶标在激光跟踪仪坐标系(16)下的位置；

b)将靶标在激光跟踪仪坐标系(16)下的空间实际位置与理论位置进行匹配计算，获取激光跟踪仪坐标系(16)在世界坐标系(11)下的姿态；

c)根据激光跟踪仪坐标系(16)与世界坐标系(11)的位姿转换关系，将激光跟踪仪测量坐标系(16)转站到世界坐标系(11)下；

标定BASE坐标系(13)各坐标轴方向的步骤为：

d)使机器人(5)沿BASE坐标系(13)的X轴运动，连续测量靶标(3)的位置，拟合一条直线即机器人BASE坐标系(13)的X轴方向；使机器人沿BASE坐标系(13)的Y轴运动，连续测量靶标的位置，拟合一条直线即机器人BASE坐标系(13)Y轴的方向；

e)通过机器人BASE坐标系(13)的X轴和Y轴叉乘，计算出Z＝X×Y，即机器人BASE坐标系(13)Z轴的方向；

标定靶标工具坐标系(12)的步骤为：

f)激光跟踪仪(6)坐标系转站，使激光跟踪仪坐标系(16)的方向转站后与机器人BASE坐标系(13)的方向一致；

g)激光跟踪仪(6)控制机器人(5)以4种不同的姿态使靶标达到空间同一点，分别记录机器人(5)的工具中心点在BASE坐标系(13)下的位姿；

h)经过旋转矩阵变换，计算靶标在TOOL0坐标系中的位置；

标定BASE坐标系(13)原点的步骤为：

i)根据激光跟踪仪坐标系(16)与世界坐标系(11)的位姿转换关系，将激光跟踪仪测量坐标系(16)转站到世界坐标系(11)下；

j)上位机(10)控制机器人(5)回零，激光跟踪仪(6)测量靶标在世界坐标系(11)下的位置；

k)经过位姿矩阵变换，计算BASE坐标系(13)原点在世界坐标系(11)中的位置；

标定TRACK坐标系(14)的步骤为：

l)上位机(10)控制机器人(5)沿TRACK坐标系(14)X轴运动一段直线，每隔一段距离激光跟踪仪(6)测量靶标(3)在世界坐标系(11)下的位置；

m)将测量得到的一组数据进行直线拟合，得到TRACK坐标系(14)X轴在世界坐标系(11)下的姿态作为TRACK坐标系(14)的方向。

5.根据权利要求4所述的一种基于激光跟踪测量的附加外轴机器人自动标定方法，其特征在于步骤b)所述的将靶标在激光跟踪仪坐标系(16)下的空间实际位置与设定位置进行匹配计算，获取激光跟踪仪坐标系(16)在世界坐标系(11)下的姿态的步骤为：

记测量3～9个靶标的实际坐标为P_A、理论坐标为P_B，P_Ai，i＝1，2，...，m为空间固定靶标的实际坐标，P_Bi，i＝1，2，...，m为空间固定靶标的理论坐标；

姿态计算是实际测量坐标值到理论坐标值一个刚性转换^AT_B：

P_B＝^AT_B×P_A             (1)

该刚性变换^AT_B可表达为一个旋转变化R和一个平移变换t的组合，即：

P_B＝^AT_B×P_A＝R×P_A+t    (2)

由于测量误差的存在，变换后的结果不能完全等于理论的坐标值，即：

ζ＝P_Bi-(R×P_Ai+t)，ζ为误差矩阵。(3)

式中，{x_Ai’，y_Ai’，z_Ai’}为P_Ai经旋转变化和一个平移变换后的坐标值，{x_Ai，y_Ai，z_Ai}为P_Bi的坐标值。

令D_i，i＝1，2，...，m为靶标对应测量点转换后与理论点之间的距离，即

$D_i=\sqrt{{({x_{\mathrm{Ai}}}^{\′}-x_{\mathrm{Bi}})}^2+{({y_{\mathrm{Ai}}}^{\′}-y_{\mathrm{Bi}})}^2+{({z_{\mathrm{Ai}}}^{\′}-z_{\mathrm{Bi}})}^2}---\left(4\right)$

令导出相对应点的位置误差的均方差表达式为

${\mathrm{\σ}}_D=\sqrt{\frac{\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{(D_i-\stackrel{\‾}{D})}^2}{m-1}}---\left(5\right)$

以位置误差的均方差为目标函数，进行最小二乘拟合求解得到R和t，从而得到测量的实际坐标值到理论坐标值的位姿变换矩阵，即可将当前激光跟踪仪坐标系(16)转站到世界坐标系(11)。

6.根据权利要求4所述的一种基于激光跟踪测量的附加外轴机器人自动标定方法，其特征在于步骤d)所述的拟合一条直线即机器人BASE坐标系(13)的X轴方向的步骤为：

设表达式

为在激光跟踪仪坐标系(16)下机器人BASE坐标系(13)的X轴的拟合曲线方程，需要拟合计算出各系数a₁、b₁、c₁、a₂、b₂、c₂的值。

采用激光跟踪仪(6)测量m，m≥3个点都在该直线上，将这些点的坐标代入到该直线方程，构成m个方程组，写成矩阵形式：

$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_m& y_m& z_m\end{array}\right]\·\left[\begin{array}{cc}{a}_1& a_2\\ b_1& b_2\\ c_1& c_2\end{array}\right]=\left[\begin{array}{cc}-1& -1\\ -1& -1\\ \·& \·\\ \·& \·\\ \·& \·\\ -1& -1\end{array}\right]---\left(6\right)$

式中，{x_i，y_i，z_i}为m，m≥3个点在激光跟踪仪坐标系(16)下的坐标值，

即Q·K＝C    (7)

其中 $Q=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ x_m& y_m& z_m\end{array}\right],$ $K=\left[\begin{array}{cc}{a}_1& a_2\\ b_1& b_2\\ c_1& c_2\end{array}\right],$ $C=\left[\begin{array}{cc}-1& -1\\ -1& -1\\ \·& \·\\ \·& \·\\ \·& \·\\ -1& -1\end{array}\right]$

式中，(x_i，y_i，z_i)为Q_i(i＝1，2，…，m)的坐标值，

通过最小二乘法求解式(7)，得

K＝(Q^TQ)^-1Q^TC    (8)

通过求解出来的a₁，b₁，c₁，a₂，b₂，c₂即可得到该直线的一组方向数(l，m，n)，即该直线的方向矢量

$l=\left|\begin{array}{cc}{b}_1& c_1\\ b_2& c_2\end{array}\right|,m=\left|\begin{array}{cc}{c}_1& a_1\\ c_2& a_2\end{array}\right|,n=\left|\begin{array}{cc}{a}_1& b_1\\ a_2& b_2\end{array}\right|$

归一化，

(n_测，o_测，a_测)为该直线的归一化方向矢量，即机器人BASE坐标系(13)的X轴方向。

7.根据权利要求4所述的一种基于激光跟踪测量的附加外轴机器人自动标定方法，其特征在于步骤e)所述的通过机器人BASE坐标系(13)的X轴和Y轴叉乘的步骤为：

设拟合出来坐标系的X、Y轴方向分别用矢量[n_x测 o_x测 a_x测]、表示。测量过程中X、Y轴不一定正交，需要进行正交化处理，

则可

拟合出来坐标系的X、Y、Z轴方向相对于激光跟踪仪坐标系(16)的旋转矩阵

Images