CN104729407A - 机器人基坐标系与世界坐标系之间关系的自动确定方法 - Google Patents

Abstract

本发明公开了一种机器人基坐标系与世界坐标系之间关系的自动确定方法，包括以下步骤：安装指定工装；确定机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的齐次变换矩阵；确定机器人末端法兰坐标系{6}相对于靶球坐标系{Ba}的齐次变换矩阵；确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵；确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵；确定世界坐标系{W}相对于机器人基坐标系{Base}的齐次变换矩阵。本发明利用激光跟踪仪高精度测量的优点，通过读取机器人内部变量，应用坐标变换的方法，实现对机器人基坐标系与世界坐标系之间关系的自动确定，自动化程度高、速度快且精确度高。

Description

机器人基坐标系与世界坐标系之间关系的自动确定方法

技术领域

本发明属于机器人技术领域，具体涉及一种机器人基坐标系与世界坐标系之间关系的自动确定方法。

背景技术

在工业生产中，已经有越来越多的应用机器人代替人工进行生产，其生产率高，而且方便控制，自动化程度高，应用机器人已经成为现代工业生产中不可缺少的角色。

但在应用机器人对工件进行加工装配的场合，需要确定机器人与工件的相对位置关系，即机器人基坐标系与工件坐标系间的相对位置关系，然后通过对机器人进行编程的方式达到自动地对工件进行加工装配的目的。由于工件位置的频繁变化，导致需要经常地对机器人基坐标系进行测量，而机器人基坐标系的测量需要繁琐的操作与计算。世界坐标系通常由几个(至少三个)包容整个工作空间的固定点组成，机器人一旦落地(固定)，其基坐标系与世界坐标系间便具有固定的位置关系，一旦准确的测出这一固定关系，以后每次便可通过测量世界坐标系的几个固定点而间接获得机器人基坐标系，省去了每次因测量机器人基坐标系而带来的繁琐操作与计算过程。

发明内容

本发明提供了一种自动化程度高、速度快且精确度高的机器人基坐标系与世界坐标系之间关系的自动确定方法。

为实现上述目的，本发明提出如下技术方案：

一种机器人基坐标系与世界坐标系之间关系的自动确定方法，所述方法采用高精度测量，并通过读取机器人内部六自由度参数，应用坐标变换，实现对机器人基坐标系与世界坐标系之间关系的自动确定。

进一步的，所述方法包括如下步骤：

步骤一，安装指定工装；

步骤二，确定机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的齐次变换矩阵

步骤三，确定机器人末端法兰坐标系{6}相对于靶球坐标系{Ba}的齐次变换矩阵

步骤四，确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

步骤五，确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

步骤六，确定世界坐标系{W}相对于机器人基坐标系{Base}的齐次变换矩阵

进一步的，所述步骤一为将靶球放在带柱的靶球座上，靶球座上伸出的的圆柱插入机器人末端法兰盘上的控制孔中，用一环状磁铁将靶球座与末端法兰盘固定。

进一步的，所述步骤二中的算法为：

1)利用示教器控制机器人运动，使机器人末端法兰盘到达一个方便测量的方位；

2)将机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的内部6自由度参数(X、Y、Z、A、B、C)传到上位机，上位机通过计算得到

其中，X、Y、Z为{6}的原点相对于{Base}的位置，A、B、C分别表示按顺序将{6}绕{Base}的X轴旋转角度C，绕Y轴旋转角度B，绕Z轴旋转角度A后，所得新坐标系的方向与{Base}一致；

计算的公式为： $T_6^{\mathrm{Base}}=\left[\begin{array}{cccc}\mathrm{cAcB}& \mathrm{cAsBsC}& \mathrm{cAsBcC}+\mathrm{sAsC}& X\\ \mathrm{sAcB}& \mathrm{sAsBsC}+\mathrm{cAcC}& \mathrm{sAsBcC}-\mathrm{cAsC}& Y\\ -\mathrm{sB}& \mathrm{cBsC}& \mathrm{cBcC}& Z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}R_6^{\mathrm{Base}}& P_6^{\mathrm{Base}}\\ 0& 1\end{array}\right];$

其中：cA＝cosA，sA＝sinA， $R_6^{\mathrm{Base}}=\left[\begin{array}{ccc}\mathrm{cAcB}& \mathrm{cAsBsC}-\mathrm{sAcC}& \mathrm{cAsBcC}+\mathrm{sAsC}\\ \mathrm{sAcB}& \mathrm{sAsBsC}+\mathrm{cAcC}& \mathrm{sAsBcC}-\mathrm{cAsC}\\ -\mathrm{sB}& \mathrm{cBsC}& \mathrm{cBcC}\end{array}\right],$ $P_6^{\mathrm{Base}}={\left[\begin{array}{ccc}X& Y& Z\end{array}\right]}^T.$

进一步的，所述步骤三中的算法为：

1)用激光跟踪仪测量靶球，读出靶球中心在{JG}中的坐标，设为A＝(x_A,y_A,z_A)^T；

2)根据KUKA机器人手册得知机器人末端法兰盘控制孔中心在坐标系{6}中的齐次坐标为(-50,0,0,1)^T；

3)设靶球中心A在坐标系{6}中的齐次坐标为⁶A＝(-50,0,h,1)^T，其中h代表靶球中心到机器人末端法兰盘平面的距离；

4)确定靶球中心到机器人末端法兰盘平面的距离h；

用靶球在机器人法兰平面上测量涵盖整个平面的三点，设分别为点B＝(x_B,y_B,z_B)^T，点C＝(x_C,y_C,z_C)^T、点D＝(x_D,y_D,z_D)^T；

向量与叉乘得到： $\stackrel{\→}{n}=\stackrel{\→}{B}C\×\stackrel{\→}{C}D=\left[\begin{array}{c}(y_C-y_B)(z_D-z_C)-(y_D-y_C)(z_C-z_B)\\ (x_D-x_C)(z_C-z_B)-(x_C-x_B)(z_D-z_C)\\ (x_C-x_B)(y_D-y_C)-(x_D-x_C)(y_C-y_B)\end{array}\right]={(a,b,c)}^T;$

其中：向量是由B、C、D三点构成平面的法向量；

平面BCD的方程可写成a(x-x_B)+b(y-y_B)+c(z-z_B)＝0；

靶球中心A到平面BCD的距离为d，则：

靶球中心到法兰平面的距离h为：

其中：D_靶＝38.1mm为靶球直径；

5)靶球坐标系{Ba}相对于机器人末端法兰坐标系{6}的齐次变换矩阵为： $T_6^{\mathrm{Ba}}=\left[\begin{array}{cc}R_6^{\mathrm{Base}}& {-}^{6}A\\ 0& 1\end{array}\right].$

进一步的，所述步骤四中的算法为：

1)确定坐标系{Ba}的原点；

坐标系{Ba}的原点为：A＝(x_A,y_A,z_A)^T；

2)确定坐标系{Ba}的三个坐标轴；

控制示教器将机器人沿坐标系{Base}的X轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为(i＝1,2,...n)，然后按原路返回；

控制示教器将机器人沿坐标系{Base}的Y轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为(i＝1,2,...n)，然后按原路返回；

控制示教器将机器人沿坐标系{Base}的Y轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为(i＝1,2,...n)，然后按原路返回；

以求取坐标系{Ba}的X轴正方向为例，算法为：

设空间直线L1的方程为：

所述空间直线L1又可表示为： $\left\{\begin{array}{c}x=\frac{k_1}{k_3}(z-z_A)+x_A=a_{x}z+b_x\\ y=\frac{k_2}{k_3}(z-z_A)+y_A=c_{x}z+d_x\end{array}\right.;$

其中： $a_x=\frac{k_1}{k_3},b_x=-\frac{k_1}{k_3}{z}_A+x_A,c_x=\frac{k_2}{k_3},d_x=-\frac{k_2}{k_3}{z}_A+y_A;$

基于最小二乘法原理需满足： $\left\{\begin{array}{c}\min \mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{[x_{X_i}-(a_x{Z}_{X_i}+b_x)]}^2\\ \min \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{[y_{X_i}-(c_x{z}_{X_i}+d_x)]}^2\end{array}\right.;$

根据高等数学中的多元函数的极值定理，上式等价于：

$\left\{\begin{array}{c}\frac{\∂\mathrm{\Δx}}{\∂a_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(x_{X_i}{z}_{X_i}\right)+2a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{b}_x=0\\ \frac{\∂\mathrm{\Δx}}{\∂b_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}+2a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{b}_x=0\\ \frac{\∂\mathrm{\Δy}}{\∂c_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(y_{X_i}{z}_{X_i}\right)+2c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{d}_x=0\\ \frac{\∂\mathrm{\Δy}}{\∂d_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}+2c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{d}_x=0\end{array}\right.;$

求解得到： $a_x=\frac{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}{z}_{X_i}-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}^2-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}},b_x=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}-a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{2},$ $c_x=\frac{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}{z}_{X_i}-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}^2-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}},d_x=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}-c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{2};$

所建立的X轴正方向的方向向量为：n_x＝(a_x,c_x,1)；

其中：k₁:k₂:k₃＝a_x:c_x:1；

所建立的Y轴和Z轴正方向的方向向量与所述X轴正方向的方向向量确定方法相同，分别为：n_y＝(a_y,c_y,1)和n_z＝(a_z,c_z,1)；

所建立的方向向量n_x，n_y和n_z由于机器人定位误差和激光跟踪仪测量误差而不是理论上的相互垂直关系，因此不能直接作为坐标系{Ba}的三个坐标轴方向向量；

将方向向量n_x作为{Ba}的X轴正方向，则坐标系{Ba}的X轴正方向单位向量为： ${(\frac{a_x}{\sqrt{a_x^2+c_x^2+1}},\frac{c_x}{\sqrt{a_x^2+c_x^2+1}},\frac{1}{\sqrt{a_x^2+c_x^2+1}})}^T={(n_x^{\mathrm{Base}},n_y^{\mathrm{Base}},n_z^{\mathrm{Base}})}^T;$

将方向向量n_z与n_x的叉乘作为坐标系{Ba}的Y轴正方向，则{Ba}的Y轴正方向单位向量为： $\frac{1}{\sqrt{{(c_z-c_x)}^2+{(a_x-a_z)}^2+{(a_z{c}_x-a_x{c}_z)}^2}}{(c_z-c_x,a_x-a_z,a_z{c}_x-a_x{c}_z)}^T={(o_x^{\mathrm{Base}},o_y^{\mathrm{Base}},o_z^{\mathrm{Base}})}^T;$

将单位向量与叉乘得到{Ba}的Z轴正方向的单位向量为： $\frac{1}{m}{(c_x(a_z{c}_x-a_x{c}_z)-(a_x-a_z),(c_z-c_x)-a_x(a_z{c}_x-a_x{c}_z),a_x(a_x-a_z)-c_x(c_z-c_x))}^T={(a_x^{\mathrm{Base}},a_y^{\mathrm{Base}},a_z^{\mathrm{Base}})}^T;$

其中： $m=\sqrt{{[c_x(a_z{c}_x-a_x{c}_z)-(a_x-a_z)]}^2+{[(c_z-c_x)-a_x(a_z{c}_x-a_x{c}_z)]}^2+{[a_x(a_x-a_z)-c_x(c_z-c_x)]}^2};$

3)确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵为： $T_{\mathrm{Ba}}^{\mathrm{JG}}=\left[\begin{array}{cccc}{n}_x^{\mathrm{Base}}& o_x^{\mathrm{Base}}& a_x^{\mathrm{Base}}& x_A\\ n_y^{\mathrm{Base}}& o_y^{\mathrm{Base}}& a_y^{\mathrm{Base}}& y_A\\ n_z^{\mathrm{Base}}& o_z^{\mathrm{Base}}& a_z^{\mathrm{Base}}& z_A\\ 0& 0& 0& 1\end{array}\right].$

进一步的，所述步骤五中的算法为：

1)用激光跟踪仪测量世界坐标系{W}的三个靶球中心位置，分别设为P₁＝(x₁,y₁,z₁)^T、P₂＝(x₂,y₂,z₂)^T和P₃＝(x₃,y₃,z₃)^T；

2)确定世界坐标系{W}的原点；

以P₂为世界坐标系{W}的原点；

3)确定世界坐标系{W}的X轴正方向单位向量；

以向量为坐标系{W}的X轴正方向，则{W}的X轴正方向单位向量为： $\frac{1}{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2}{(x_2-x_1,y_2-y_1,z_2-z_1)}^T={(n_x^W,n_y^W,n_z^W)}^T;$

4)确定世界坐标系{W}的Z轴正方向单位向量；

以向量叉乘向量为{W}的Z轴正方向，则坐标系{W}的Z轴正方向单位向量为： $\frac{1}{a}\left[\begin{array}{c}(y_2-y_1)(z_3-z_2)-(y_3-y_2)(z_2-z_1)\\ (x_3-x_2)(z_2-z_1)-(x_2-x_1)(z_3-z_2)\\ (x_2-x_1)(y_3-y_2)-(x_3-x_2)(y_2-y_1)\end{array}\right]={(a_x^W,a_y^W,a_z^W)}^T;$

其中：

a＝[(y₂-y₁)(z₃-z₂)-(y₃-y₂)(z₂-z₁)]²+[(x₃-x₂)(z₂-z₁)-(x₂-x₁)(z₃-z₂)]²+[(x₂-x₁)(y₃-y₂)-(x₃-x₂)(y₂-y₁)]²；

5)确定世界坐标系{W}的Y轴正方向单位向量；

根据右手法则确定的Y轴正方向单位向量为：

${(a_x^W,a_y^W,a_z^W)}^T\×{(n_x^W,n_y^W,n_z^W)}^T={(o_x^W,o_y^W,o_z^W)}^T;$

6)确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵为： $T_W^{\mathrm{JG}}=\left[\begin{array}{cccc}{n}_x^W& o_x^W& a_x^W& x_2\\ n_y^W& o_y^W& a_y^W& y_2\\ n_z^W& o_z^W& a_z^W& z_2\\ 0& 0& 0& 1\end{array}\right].$

进一步的，所述步骤六中的算法为：

其中， $T_W^{\mathrm{Base}}=\left(T_6^{\mathrm{Base}}\right){\left(T_6^{\mathrm{Ba}}\right)}^{-1}{\left(T_{\mathrm{Ba}}^{\mathrm{JG}}\right)}^{-1}{\left(T_W^{\mathrm{JG}}\right)}^{-1};$

完成机器人基坐标系与世界坐标系之间关系的确定。

本发明的有益之处在于，利用激光跟踪仪高精度测量的优点，通过读取机器人内部变量，应用坐标变换的方法，实现对机器人基坐标系与世界坐标系之间关系的自动确定，自动化程度高、速度快且精确度高。

附图说明

图1为本发明所涉及机器人的轴测图；

图2为本发明所指定工装的安装示意图；

图3为本发明为上位机与激光跟踪仪、机器人通讯原理示意图。

图中：1.机器人基座，2.腰关节，3.大臂，4.小臂，5.肘关节，6.手腕，7.末端法兰盘，8.世界坐标系，9.机器人基坐标系，10.环状磁铁，11.带柱靶球座，12.靶球，13.靶球坐标系，14.激光跟踪仪坐标系，15.末端法兰坐标系，16.上位机，17.机器人控制器，18.激光跟踪仪控制器。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。下面结合附图与具体实施方式，对本发明进一步说明。

本发明是一种机器人基坐标系与世界坐标系之间关系的自动确定方法，采用安装有靶球的指定工装，指定工装包括环状磁铁、带柱靶球座和靶球；将指定工装安装于机器人末端法兰盘上；通过激光跟踪仪对靶球中心进行测量；激光跟踪仪具有激光跟踪仪坐标系，机器人具有机器人基坐标系，将激光跟踪仪测得数据和机器人内部六自由度参数(XYZABC)通过以太网(Ethernet)传递给上位机，上位机对数据进行处理，通过坐标变换的方式建立了机器人基坐标系与世界坐标系之间的关系。

本发明所涉及的坐标系包括：机器人基坐标系{Base}、世界坐标系{W}、激光跟踪仪坐标系{JG}、机器人末端法兰坐标系{6}和靶球坐标系{Ba}。

本发明所述的机器人基坐标系{Base}的原点位于其底座安装中心，其XOY平面与底座安装面重合，其Z轴正方向垂直于底座安装平面向上，第一至第六关节角度分别为0°，-90°，90°，0°，0°，0°时，垂直于末端法兰盘平面向外的方向即为其X轴正方向，Y轴正方向根据右手法则确定。

本发明所述的世界坐标系{W}是由三个固定的靶球中心(w1，w2，w3)构成，所述三个靶球中心(图中未示)靠磁力吸附于三个平底靶球座上，所述三个平底靶球座粘在包容整个机器人工作空间的地面上，以w1为世界坐标系{W}的原点，w2与w1连线为其X轴正方向，垂直于w1，w2，w3所构成的平面且过w2的直线为其Z轴正方向，由右手法则即可确定其Y轴正方向。

本发明所述的激光跟踪仪坐标系{JG}内置于激光跟踪仪控制器中。

本发明所述的机器人末端法兰坐标系{6}的原点位于法兰盘中心，其XOY平面与法兰平面重合，其Z轴正方向垂直于法兰平面向外，保持第一至第六关节角度分别为0°，-90°，90°，0°，0°，0°时，其X轴正方向竖直向下，Y轴正方向根据右手法则确定。

将靶球放在带柱靶球座上，带柱靶球座上伸出的直径为8mm的圆柱正好可以插入机器人末端法兰盘上直径为8mm的控制孔中，用一环状磁铁将带柱靶球座与末端法兰盘固定。本发明所述靶球坐标系{Ba}的原点位于靶球中心，三个坐标轴分别平行于机器人基坐标系{Base}的三个坐标轴。

下面结合附图对本发明作进一步详细说明。

如图1、图2、图3所示，图2位本发明所指定工装的安装示意图，包括：末端法兰盘7、环状磁铁10、带柱靶球座11、靶球12和激光跟踪仪14。

机器人基坐标系与世界坐标系之间关系的自动确定方法，具体步骤如下：

步骤一，安装指定工装；

所述步骤一中安装指定工装的安装方法为：将靶球12放在带柱靶球座11上，所述带柱靶球座11上伸出的直径为8mm的圆柱正好可以插入机器人末端法兰盘7上直径为8mm的控制孔中，用环状磁铁10将带柱靶球座11与末端法兰盘7固定。

步骤二，确定机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的齐次变换矩阵

所述步骤二中齐次变换矩阵的确定方法为：

1)利用示教器控制机器人运动，使机器人末端法兰盘7到达一个方便测量的方位；

2)将机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的内部6自由度参数(X、Y、Z、A、B、C)传到上位机，上位机通过计算得到

其中，X、Y、Z为{6}的原点相对于{Base}的位置，A、B、C分别表示按顺序将{6}绕{Base}的X轴旋转角度C，绕Y轴旋转角度B，绕Z轴旋转角度A后，所得新坐标系的方向与{Base}一致；

计算的公式为： $T_6^{\mathrm{Base}}=\left[\begin{array}{cccc}\mathrm{cAcB}& \mathrm{cAsBsC}& \mathrm{cAsBcC}+\mathrm{sAsC}& X\\ \mathrm{sAcB}& \mathrm{sAsBsC}+\mathrm{cAcC}& \mathrm{sAsBcC}-\mathrm{cAsC}& Y\\ -\mathrm{sB}& \mathrm{cBsC}& \mathrm{cBcC}& Z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}R_6^{\mathrm{Base}}& P_6^{\mathrm{Base}}\\ 0& 1\end{array}\right];$

其中：cA＝cosA，sA＝sinA， $R_6^{\mathrm{Base}}=\left[\begin{array}{ccc}\mathrm{cAcB}& \mathrm{cAsBsC}-\mathrm{sAcC}& \mathrm{cAsBcC}+\mathrm{sAsC}\\ \mathrm{sAcB}& \mathrm{sAsBsC}+\mathrm{cAcC}& \mathrm{sAsBcC}-\mathrm{cAsC}\\ -\mathrm{sB}& \mathrm{cBsC}& \mathrm{cBcC}\end{array}\right],$ $P_6^{\mathrm{Base}}={\left[\begin{array}{ccc}X& Y& Z\end{array}\right]}^T.$

步骤三，确定机器人末端法兰坐标系{6}相对于靶球坐标系{Ba}的齐次变换矩阵

所述步骤三中齐次变换矩阵的确定方法为：

1)用激光跟踪仪测量靶球，读出靶球中心在{JG}中的坐标，设为A＝(x_A,y_A,z_A)^T；

2)根据KUKA机器人手册得知机器人末端法兰盘7控制孔中心在坐标系{6}中的齐次坐标为(-50,0,0,1)^T；

3)设靶球中心A在坐标系{6}中的齐次坐标为⁶A＝(-50,0,h,1)^T，其中h代表靶球中心到机器人末端法兰盘7平面的距离；

4)确定靶球中心到机器人末端法兰盘7平面的距离h；

用靶球在机器人法兰平面上测量涵盖整个平面的三点，设分别为点B＝(x_B,y_B,z_B)^T，点C＝(x_C,y_C,z_C)^T、点D＝(x_D,y_D,z_D)^T；

向量与叉乘得到： $\stackrel{\→}{n}=\stackrel{\→}{B}C\×\stackrel{\→}{C}D=\left[\begin{array}{c}(y_C-y_B)(z_D-z_C)-(y_D-y_C)(z_C-z_B)\\ (x_D-x_C)(z_C-z_B)-(x_C-x_B)(z_D-z_C)\\ (x_C-x_B)(y_D-y_C)-(x_D-x_C)(y_C-y_B)\end{array}\right]={(a,b,c)}^T;$

其中：向量是由B、C、D三点构成平面的法向量；

平面BCD的方程可写成a(x-x_B)+b(y-y_B)+c(z-z_B)＝0；

靶球中心A到平面BCD的距离为d，则：

靶球中心到法兰平面的距离h为：

其中：D_靶＝38.1mm为靶球直径；

5)靶球坐标系{Ba}相对于机器人末端法兰坐标系{6}的齐次变换矩阵为： $T_6^{\mathrm{Ba}}=\left[\begin{array}{cc}R_6^{\mathrm{Base}}& {-}^{6}A\\ 0& 1\end{array}\right].$

步骤四，确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

所述步骤四中齐次变换矩阵的确定方法为；

1)确定坐标系{Ba}的原点；

坐标系{Ba}的原点为：A＝(x_A,y_A,z_A)^T；

2)确定坐标系{Ba}的三个坐标轴；

控制示教器将机器人沿坐标系{Base}的X轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为(i＝1,2,...n)，然后按原路返回；

控制示教器将机器人沿坐标系{Base}的Y轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为(i＝1,2,...n)，然后按原路返回；

控制示教器将机器人沿坐标系{Base}的Y轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为(i＝1,2,...n)，然后按原路返回；

以求取坐标系{Ba}的X轴正方向为例，算法为：

设空间直线L1的方程为：

所述空间直线L1又可表示为： $\left\{\begin{array}{c}x=\frac{k_1}{k_3}(z-z_A)+x_A=a_{x}z+b_x\\ y=\frac{k_2}{k_3}(z-z_A)+y_A=c_{x}z+d_x\end{array}\right.;$

其中： $a_x=\frac{k_1}{k_3},b_x=-\frac{k_1}{k_3}{z}_A+x_A,c_x=\frac{k_2}{k_3},d_x=-\frac{k_2}{k_3}{z}_A+y_A$

基于最小二乘法原理需满足： $\left\{\begin{array}{c}\min \mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{[x_{X_i}-(a_x{Z}_{X_i}+b_x)]}^2\\ \min \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{[y_{X_i}-(c_x{z}_{X_i}+d_x)]}^2\end{array}\right.$

根据高等数学中的多元函数的极值定理，上式等价于：

$\left\{\begin{array}{c}\frac{\∂\mathrm{\Δx}}{\∂a_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(x_{X_i}{z}_{X_i}\right)+2a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{b}_x=0\\ \frac{\∂\mathrm{\Δx}}{\∂b_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}+2a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{b}_x=0\\ \frac{\∂\mathrm{\Δy}}{\∂c_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(y_{X_i}{z}_{X_i}\right)+2c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{d}_x=0\\ \frac{\∂\mathrm{\Δy}}{\∂d_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}+2c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+2n{d}_x=0\end{array}\right.$

求解得到： $a_x=\frac{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}{z}_{X_i}-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}^2-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}},b_x=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}-a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{2},$ $c_x=\frac{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}{z}_{X_i}-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}^2-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}},d_x=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}-c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{2};$

所建立的X轴正方向的方向向量为：n_x＝(a_x,c_x,1)；

其中：k₁:k₂:k₃＝a_x:c_x:1；

所建立的Y轴和Z轴正方向的方向向量与所述X轴正方向的方向向量确定方法相同，分别为：n_y＝(a_y,c_y,1)和n_z＝(a_z,c_z,1)；

所建立的方向向量n_x，n_y和n_z由于机器人定位误差和激光跟踪仪测量误差而不是理论上的相互垂直关系，因此不能直接作为坐标系{Ba}的三个坐标轴方向向量；

将方向向量nx作为{Ba}的X轴正方向，则坐标系{Ba}的X轴正方向单位向量为： ${(\frac{a_x}{\sqrt{a_x^2+c_x^2+1}},\frac{c_x}{\sqrt{a_x^2+c_x^2+1}},\frac{1}{\sqrt{a_x^2+c_x^2+1}})}^T={(n_x^{\mathrm{Base}},n_y^{\mathrm{Base}},n_z^{\mathrm{Base}})}^T;$

将方向向量n_z与n_x的叉乘作为坐标系{Ba}的Y轴正方向，则{Ba}的Y轴正方向单位向量为： $\frac{1}{\sqrt{{(c_z-c_x)}^2+{(a_x-a_z)}^2+{(a_z{c}_x-a_x{c}_z)}^2}}{(c_z-c_x,a_x-a_z,a_z{c}_x-a_x{c}_z)}^T={(o_x^{\mathrm{Base}},o_y^{\mathrm{Base}},o_z^{\mathrm{Base}})}^T;$

将单位向量与叉乘得到{Ba}的Z轴正方向的单位向量为： $\frac{1}{m}{(c_x(a_z{c}_x-a_x{c}_z)-(a_x-a_z),(c_z-c_x)-a_x(a_z{c}_x-a_x{c}_z),a_x(a_x-a_z)-c_x(c_z-c_x))}^T={(a_x^{\mathrm{Base}},a_y^{\mathrm{Base}},a_z^{\mathrm{Base}})}^T;$

其中： $m=\sqrt{{[c_x(a_z{c}_x-a_x{c}_z)-(a_x-a_z)]}^2+{[(c_z-c_x)-a_x(a_z{c}_x-a_x{c}_z)]}^2+{[a_x(a_x-a_z)-c_x(c_z-c_x)]}^2};$

3)确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵为： $T_{\mathrm{Ba}}^{\mathrm{JG}}=\left[\begin{array}{cccc}{n}_x^{\mathrm{Base}}& o_x^{\mathrm{Base}}& a_x^{\mathrm{Base}}& x_A\\ n_y^{\mathrm{Base}}& o_y^{\mathrm{Base}}& a_y^{\mathrm{Base}}& y_A\\ n_z^{\mathrm{Base}}& o_z^{\mathrm{Base}}& a_z^{\mathrm{Base}}& z_A\\ 0& 0& 0& 1\end{array}\right].$

步骤五，确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

所述步骤五中齐次变换矩阵的确定方法为：

1)用激光跟踪仪测量世界坐标系{W}的三个靶球中心位置，分别设为P₁＝(x₁,y₁,z₁)^T、P₂＝(x₂,y₂,z₂)^T和P₃＝(x₃,y₃,z₃)^T；

2)确定世界坐标系{W}的原点；

以P₂为世界坐标系{W}的原点；

3)确定世界坐标系{W}的X轴正方向单位向量；

以向量为坐标系{W}的X轴正方向，则{W}的X轴正方向单位向量为： $\frac{1}{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2}{(x_2-x_1,y_2-y_1,z_2-z_1)}^T={(n_x^W,n_y^W,n_z^W)}^T;$

4)确定世界坐标系{W}的Z轴正方向单位向量；

以向量叉乘向量为{W}的Z轴正方向，则坐标系{W}的Z轴正方向单位向量为： $\frac{1}{a}\left[\begin{array}{c}(y_2-y_1)(z_3-z_2)-(y_3-y_2)(z_2-z_1)\\ (x_3-x_2)(z_2-z_1)-(x_2-x_1)(z_3-z_2)\\ (x_2-x_1)(y_3-y_2)-(x_3-x_2)(y_2-y_1)\end{array}\right]={(a_x^W,a_y^W,a_z^W)}^T;$

其中：

a＝[(y₂-y₁)(z₃-z₂)-(y₃-y₂)(z₂-z₁)]²+[(x₃-x₂)(z₂-z₁)-(x₂-x₁)(z₃-z₂)]²+[(x₂-x₁)(y₃-y₂)-(x₃-x₂)(y₂-y₁)]²；

5)确定世界坐标系{W}的Y轴正方向单位向量；

根据右手法则确定的Y轴正方向单位向量为：

6)确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵为： $T_W^{\mathrm{JG}}=\left[\begin{array}{cccc}{n}_x^W& o_x^W& a_x^W& x_2\\ n_y^W& o_y^W& a_y^W& y_2\\ n_z^W& o_z^W& a_z^W& z_2\\ 0& 0& 0& 1\end{array}\right].$

步骤六，确定世界坐标系{W}相对于机器人基坐标系{Base}的齐次变换矩阵

其中， $T_W^{\mathrm{Base}}=\left(T_6^{\mathrm{Base}}\right){\left(T_6^{\mathrm{Ba}}\right)}^{-1}{\left(T_{\mathrm{Ba}}^{\mathrm{JG}}\right)}^{-1}{\left(T_W^{\mathrm{JG}}\right)}^{-1}.$

至此，完成机器人基坐标系与世界坐标系之间关系的确定。

本发明的有益之处在于，利用激光跟踪仪高精度测量的优点，通过读取机器人内部变量，应用坐标变换，实现对机器人基坐标系与世界坐标系之间关系的自动确定，自动化程度高、速度快且精确度高。

Claims (8)

1.一种机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于，所述方法采用高精度测量，并通过读取机器人内部六自由度参数，应用坐标变换，实现对机器人基坐标系与世界坐标系之间关系的自动确定。

2.根据权利要求1所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于，所述方法包括如下步骤：

步骤一，安装指定工装；

步骤二，确定机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的齐次变换矩阵

步骤三，确定机器人末端法兰坐标系{6}相对于靶球坐标系{Ba}的齐次变换矩阵

步骤四，确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

步骤五，确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

步骤六，确定世界坐标系{W}相对于机器人基坐标系{Base}的齐次变换矩阵

3.根据权利要求2所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于，所述步骤一为将靶球放在带柱的靶球座上，靶球座上伸出的的圆柱插入机器人末端法兰盘上的控制孔中，用一环状磁铁将靶球座与末端法兰盘固定。

4.根据权利要求2所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于：所述步骤二中的算法为：

1)利用示教器控制机器人运动，使机器人末端法兰盘到达一个方便测量的方位；

2)将机器人末端法兰坐标系{6}相对于机器人基坐标系{Base}的内部6自由度参数(X、Y、Z、A、B、C)传到上位机，上位机通过计算得到

其中，X、Y、Z为{6}的原点相对于{Base}的位置，A、B、C分别表示按顺序将{6}绕{Base}的X轴旋转角度C，绕Y轴旋转角度B，绕Z轴旋转角度A后，所得新坐标系的方向与{Base}一致；

计算的公式为： $T_6^{\mathrm{Base}}=\left[\begin{array}{cccc}\mathrm{cAcB}& \mathrm{cAsBsC}-\mathrm{sAcC}& \mathrm{casBcC}+\mathrm{sAsC}& X\\ \mathrm{sAcB}& \mathrm{sAsBsC}+\mathrm{cAcC}& \mathrm{sAsBcC}-\mathrm{cAsC}& Y\\ -\mathrm{sB}& \mathrm{cBsC}& \mathrm{cBcC}& Z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}R_6^{\mathrm{Base}}& P_6^{\mathrm{Base}}\\ 0& 1\end{array}\right];$

其中：cA＝cosA，sA＝sinA， $R_6^{\mathrm{Base}}=\left[\begin{array}{ccc}\mathrm{cAcB}& \mathrm{cAsBsC}-\mathrm{sAcC}& \mathrm{cAsBcC}+\mathrm{sAsC}\\ \mathrm{sAcB}& \mathrm{sAsBsC}+\mathrm{cAcC}& \mathrm{sAsBcC}-\mathrm{cAsC}\\ -\mathrm{sB}& \mathrm{cBsC}& \mathrm{cBcC}\end{array}\right],$ $P_6^{\mathrm{Base}}={\left[\begin{array}{ccc}X& Y& Z\end{array}\right]}^T.$

5.根据权利要求2所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于：所述步骤三中的算法为：

1)用激光跟踪仪测量靶球，读出靶球中心在{JG}中的坐标，设为A＝(x_A,y_A,z_A)^T；

2)根据KUKA机器人手册得知机器人末端法兰盘控制孔中心在坐标系{6}中的齐次坐标为(-50,0,0,1)^T；

3)设靶球中心A在坐标系{6}中的齐次坐标为⁶A＝(-50,0,h,1)^T，其中h代表靶球中心到机器人末端法兰盘平面的距离；

4)确定靶球中心到机器人末端法兰盘平面的距离h；

用靶球在机器人法兰平面上测量涵盖整个平面的三点，设分别为点B＝(x_B,y_B,z_B)^T，点C＝(x_C,y_C,z_C)^T、点D＝(x_D,y_D,z_D)^T；

向量与叉乘得到： $\stackrel{\→}{n}=\stackrel{\→}{B}C\×\stackrel{\→}{C}D=\left[\begin{array}{c}(y_C-y_B)(z_D-z_C)-(y_D-y_C)(z_C-z_B)\\ (x_D-x_C)(z_C-z_B)-(x_C-x_B)(z_D-z_C)\\ (x_C-x_B)(y_D-y_C)-(x_D-x_C)(y_C-y_B)\end{array}\right]={(a,b,c)}^T;$

其中：向量是由B、C、D三点构成平面的法向量；

平面BCD的方程可写成a(x-x_B)+b(y-y_B)+c(z-z_B)＝0；

靶球中心A到平面BCD的距离为d，则： $d=\left|\frac{a(x_A-x_B)+b(y_A-y_B)+c(z_A-z_B)}{\sqrt{a^2+b^2+c^2}}\right|;$

靶球中心到法兰平面的距离h为：

其中：D_靶＝38.1mm为靶球直径；

5)靶球坐标系{Ba}相对于机器人末端法兰坐标系{6}的齐次变换矩阵为： $T_6^{\mathrm{Ba}}=\left[\begin{array}{cc}R_6^{\mathrm{Base}}& -_{6}A\\ 0& 1\end{array}\right].$

6.根据权利要求2所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于：所述步骤四中的算法为：

1)确定坐标系{Ba}的原点；

坐标系{Ba}的原点为：A＝(x_A,y_A,z_A)^T；

2)确定坐标系{Ba}的三个坐标轴；

控制示教器将机器人沿坐标系{Base}的X轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为然后按原路返回；

控制示教器将机器人沿坐标系{Base}的Y轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为然后按原路返回；

控制示教器将机器人沿坐标系{Base}的Y轴正方向移动，每移动一定距离测量一次靶球中心坐标，共移动n次(n一般取3～5)，将靶球中心位置读数设为然后按原路返回；

以求取坐标系{Ba}的X轴正方向为例，算法为：

设空间直线L1的方程为： $\frac{x-x_A}{k_1}=\frac{y-y_A}{k_2}=\frac{z-z_A}{k_3};$

所述空间直线L1又可表示为： $\left\{\begin{array}{c}x=\frac{k_1}{k_3}(z-z_A)+x_A=a_{x}z+b_x\\ y=\frac{k_2}{k_3}(z-z_A)+y_A=c_{x}z+d_x\end{array}\right.;$

其中： $a_x=\frac{k_1}{k_3},b_x=-\frac{k_1}{k_3}{z}_A+x_A,c_x=\frac{k_2}{k_3},d_x=-\frac{k_2}{k_3}{z}_A+y_A;$

基于最小二乘法原理需满足： $\left\{\begin{array}{c}\min \mathrm{\Δx}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{[x_{X_i}-(a_x{z}_{X_i}+b_x)]}^2\\ \min \mathrm{\Δy}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{[y_{X_i}-(c_x{z}_{X_i}+d_x)]}^2\end{array}\right.;$

根据高等数学中的多元函数的极值定理，上式等价于：

$\left\{\begin{array}{c}\frac{\∂\mathrm{\Δx}}{{\∂a}_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(x_{X_i}{z}_{X_i}\right)+{2a}_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+{2\mathrm{nb}}_x=0\\ \frac{\∂\mathrm{\Δx}}{\∂b_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}+{2a}_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+{2\mathrm{nb}}_x=0\\ \frac{\∂\mathrm{\Δy}}{{\∂c}_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(y_{X_i}{z}_{X_i}\right)+{2c}_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+{2\mathrm{nd}}_x=0\\ \frac{\∂\mathrm{\Δy}}{{\∂d}_x}=-2\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}+2c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}+{2\mathrm{nd}}_x=0\end{array}\right.;$

求解得到： $a_x=\frac{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}{z}_{X_i}-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}^2-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}},$ $b_x=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}-a_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{2},$ $c_x=\frac{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}{z}_{X_i}-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{Z}_{X_i}}{n\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}^2-\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}},$ $d_x=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{x}_{X_i}-c_x\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{z}_{X_i}}{2},$

所建立的X轴正方向的方向向量为：n_x＝(a_x,c_x,1)；

其中：k₁:k₂:k₃＝a_x:c_x:1；

所建立的Y轴和Z轴正方向的方向向量与所述X轴正方向的方向向量确定方法相同，分别为：n_y＝(a_y,c_y,1)和n_z＝(a_z,c_z,1)；

所建立的方向向量n_x，n_y和n_z由于机器人定位误差和激光跟踪仪测量误差而不是理论上的相互垂直关系，因此不能直接作为坐标系{Ba}的三个坐标轴方向向量；

将方向向量n_x作为{Ba}的X轴正方向，则坐标系{Ba}的X轴正方向单位向量为： ${(\frac{a_x}{\sqrt{a_x^2+c_x^2+1}},\frac{c_x}{\sqrt{a_x^2+c_x^2+1}},\frac{1}{\sqrt{a_x^2+c_x^2+1}})}^T={(n_x^{\mathrm{Base}},n_y^{\mathrm{Base}},n_z^{\mathrm{Base}})}^T;$

将方向向量n_z与n_x的叉乘作为坐标系{Ba}的Y轴正方向，则{Ba}的Y轴正方向单位向量为： $\frac{1}{\sqrt{{(c_z-c_x)}^2+{(a_x-a_z)}^2+{(a_z{c}_x-a_x{c}_z)}^2}}{(c_z-c_x,a_x-a_z,a_z{c}_x-a_x{c}_z)}^T={(o_x^{\mathrm{Base}},o_y^{\mathrm{Base}},o_z^{\mathrm{Base}})}^T;$

将单位向量与叉乘得到{Ba}的Z轴正方向的单位向量为： $\frac{1}{m}{(c_x(a_z{c}_x-a_x{c}_z)-(a_x-a_z),(c_z-c_x)-a_x(a_z{c}_x-a_x{c}_z),a_x(a_x-a_z)-c_x(c_z-c_x))}^T={(a_x^{\mathrm{Base}},a_y^{\mathrm{Base}},a_z^{\mathrm{Base}})}^T;$

其中： $m=\sqrt{{[c_x(a_z{c}_x-a_x{c}_z)-(a_x-a_z)]}^2+{\left[(c_z-c_x){-a}_x(a_z{c}_x-a_x{c}_z)\right]}^2+{[a_x(a_x-a_z)-c_x(c_z-c_x)]}^2};$

3)确定靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵

靶球坐标系{Ba}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵为： $T_{\mathrm{Ba}}^{\mathrm{JG}}=\left[\begin{array}{cccc}{n}_x^{\mathrm{Base}}& o_x^{\mathrm{Base}}& a_x^{\mathrm{Base}}& x_A\\ n_y^{\mathrm{Base}}& o_y^{\mathrm{Base}}& a_y^{\mathrm{Base}}& y_A\\ n_z^{\mathrm{Base}}& o_z^{\mathrm{Base}}& a_z^{\mathrm{Base}}& z_A\\ 0& 0& 0& 1\end{array}\right].$

7.根据权利要求2所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于：所述步骤五中的算法为：

1)用激光跟踪仪测量世界坐标系{W}的三个靶球中心位置，分别设为P₁＝(x₁,y₁,z₁)^T、P₂＝(x₂,y₂,z₂)^T和P₃＝(x₃,y₃,z₃)^T；

2)确定世界坐标系{W}的原点；

以P₂为世界坐标系{W}的原点；

3)确定世界坐标系{W}的X轴正方向单位向量；

以向量为坐标系{W}的X轴正方向，则{W}的X轴正方向单位向量为： $\frac{1}{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2}{(x_2-x_1,y_2-y_1,z_2-z_1)}^T={(n_x^W,n_y^W,n_z^W)}^T;$

4)确定世界坐标系{W}的Z轴正方向单位向量；

以向量叉乘向量为{W}的Z轴正方向，则坐标系{W}的Z轴正方向单位向量为： $\frac{1}{a}\left[\begin{array}{c}(y_2-y_1)(z_3-z_2)-(y_3-y_2)(z_2-z_1)\\ (x_3-x_2)(z_2-z_1)-(x_2-x_1)(z_3-z_2)\\ (x_2-x_1)(y_3-y_2)-(x_3-x_2)(y_2-y_1)\end{array}\right]={(a_x^W,a_y^W,a_z^W)}^T;$

其中：

a＝[(y₂-y₁)(z₃-z₂)-(y₃-y₂)(z₂-z₁)]²+[(x₃-x₂)(z₂-z₁)-(x₂-x₁)(z₃-z₂)]²+[(x₂-x₁)(y₃-y₂)-(x₃-x₂)(y₂-y₁)]²；

5)确定世界坐标系{W}的Y轴正方向单位向量；

根据右手法则确定的Y轴正方向单位向量为：

${(a_x^W,a_y^W,a_z^W)}^T\×{(n_x^W,n_y^W,n_z^W)}^T={(o_x^W,o_y^W,o_z^W)}^T;$

6)确定世界坐标系{W}相对于激光跟踪仪坐标系{JG}的齐次变换矩阵为： $T_W^{\mathrm{JG}}=\left[\begin{array}{cccc}{n}_x^W& o_x^W& a_x^W& x_2\\ n_y^W& o_y^W& a_y^W& y_2\\ n_z^W& o_z^W& a_z^W& z_2\\ 0& 0& 0& 1\end{array}\right].$

8.根据权利要求2所述的机器人基坐标系与世界坐标系之间关系的自动确定方法，其特征在于：所述步骤六中的算法为：

其中， $T_W^{\mathrm{Base}}=\left(T_6^{\mathrm{Base}}\right){\left(T_6^{\mathrm{Ba}}\right)}^{-1}{\left(T_{\mathrm{Ba}}^{\mathrm{JG}}\right)}^{-1}{\left(T_W^{\mathrm{JG}}\right)}^{-1};$

完成机器人基坐标系与世界坐标系之间关系的确定。