CN105318838A - 激光测距仪和机械臂末端关系的单平面标定方法及其系统 - Google Patents

Abstract

本发明公开了一种激光测距仪和机械臂末端关系的单平面标定方法及其系统。本发明采用激光测距仪固定在机械臂末端，并在空间中放置表面平整的标定平面，通过机械臂的旋转平移运动带动激光测距仪的位姿发生改变，使用激光测距仪测量的距离d，求解平移向量r和激光束的单位方向向量v，完成激光测距仪和机械臂末端的空间变换关系标定，根据x＝r+dv，可以实现对空间中物体上三维点x的重建，具有测量精度高，价格相对低廉的优点；本发明明确给出了机械臂末端坐标系和激光测距仪坐标系的变换关系；使用单平面作为标定平面和应用非线性优化方法求解空间变换关系的标定问题，从而简化标定工具、减少误差，降低噪声干扰，提高方法的鲁棒性和易实施性。

Description

激光测距仪和机械臂末端关系的单平面标定方法及其系统

技术领域

本发明涉及视觉测量领域，具体涉及激光测距仪和机械臂末端关系的单平面标定方法及其系统。

背景技术

随着电子工业水平的提高，计算机技术和光学传感器技术的发展，使得机器视觉在工业加工、航空、汽车、船舶、娱乐、医疗等领域都有广泛的应用。

为了利用机械臂完成立体定向电极植入手术，需要将机械臂的坐标系和病人的三维空间坐标进行配准。而配准的基础是建立机械臂末端(比如爪手、法兰盘等)和传感器(相机、结构光设备、激光测距仪等)之间的变换关系，求解这个变换关系的问题称为手眼标定问题。

对机械臂的手眼标定问题，已经有不少学者提出相应的解决办法，Zhuang将机械臂和传感器一起建模，用于机械臂运动学参数校正，Tsai将手眼标定分为旋转和平移两个部分分别求解，Ma采用相互正交的纯平移来线性化手眼矩阵的旋转部分，对旋转部分的求解需要三次有解的平移运动，Malm采用光流场的法向导数方法，Shiu等人将手眼标定问题归结为求解AX＝XB的问题。

归纳起来机械臂手眼标定问题的类型主要有三种：1、使用高精度的已知尺寸的加工件和测量仪器对机械臂和传感器直接测量；2、使用摄像机的手眼标定，基于特征点、基于运动、立体视觉等；3、使用结构光，如一维的激光测距仪、二维的线激光和相机、面结构光和相机等，进行手眼标定。

在文献[Theinternationaljournalofroboticsresearch,1995,14(3):195-210]中给出了通用的求解手眼标定方法，对它进行必要的修改可以适合相机、激光测距仪、结构光等不同的传感器，但具体如何修改使得它适应不同的传感器，需要读者做一定创造性探索。在文献[RoboticsandAutomation,IEEETransactionson,1998,14(3):493-497]中给出一般情况下激光测距仪和机械臂的手眼标定方法，但具体如何从将在机械臂坐标系下坐标的变换转换到激光测距仪坐标系下的变换，在该文献中并没有明确给出，要想实现它也需要读者做一定的创造性探索。并且当前的方法在理论上可以得到很高的精度，但是在实践中由于约束条件不足，导致求解手眼标定矩阵的可重复性较差。

发明内容

为了解决现有技术中存在的问题，本发明提出了一种激光测距仪和机械臂的空间变换关系的单平面标定方法，使用固定在机械臂末端的激光测距仪来实现对空间中物体上三维点的重建，具有测量精度高，价格相对低廉的优点。

本发明的一个目的在于提出一种激光测距仪和机械臂末端的空间变换关系的单平面标定系统。

本发明的激光测距仪和机械臂末端的空间变换关系的单平面标定系统包括：机械臂、机械臂末端、激光测距仪以及标定平面；其中，激光测距仪固定在机械臂末端，二者相对位置不变；在空间中放置标定平面，标定平面相对于机械臂的位置固定，标定平面的表面平整；机械臂连接至机械臂的控制器，从机械臂的控制器中直接读取出机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量。

本发明的另一个目的在于提出一种激光测距仪和机械臂末端的空间变换关系的单平面标定方法。

本发明的激光测距仪和机械臂末端的空间变换关系的单平面标定方法，包括以下步骤：

1)机械臂末端坐标系为O_m，激光测距仪坐标系为O_L，激光测距仪发出的激光束照射到标定平面上的一个点，激光测距仪坐标系相对于机械臂末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系为v，机械臂末端坐标系下激光束照射到标定平面上的点为x，激光测距仪测量得到的距离为d，即在激光测距仪坐标系下，激光测距仪坐标系的原点到激光束照射到标定平面的点的距离为d，根据机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点表示为：x＝r+dv，其中，r为平移向量，v为激光束的单位方向向量，二者未知，下面分别求得r和v；

2)测量并求解得到激光束的单位方向向量v：

a)控制机械臂运动，激光测距仪发出的激光束照射在标定平面上，保持机械臂不动，记录这时的激光测距仪测量的距离d₀，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R_b0和平移列向量T_b0，记为初始位姿点；

b)保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d_j，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R_bj和平移列向量T_bj，平移后测量得到第一组平动数据，在机械臂末端坐标系下初始位姿点对应的标定平面的单位方向向量n_m0满足：

$(d_j-d_0)p=-n_{m0}^T{T}_{m0j}$

得到第一组约束，其中，j为自然数，

c)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离d'₀，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R'_b0和平移列向量T′_b0，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量其中，R'_m0为在机械臂末端坐标系下，从初始位姿点到第二位姿点的旋转矩阵，并且 $R_{m0}^{\′}=R_{b0}^{-1}{R}_{b0}^{\′};$

d)保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d'_j，并且直接从机械臂的控制器中读取旋转矩阵R′_bj和平移列向量T′_bj，平移后测量得到第二组平动数据，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量n'_m0满足：

(d'_j-d'₀)p'＝-(n'_m0)^TT′_m0j

得到第二组约束，其中，T′_m0j＝(R′_b0)^-1(T′_bj-T′_b0)，p'＝(n'_m0)^Tv；

e)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离d″₀，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R″_b0和平移列向量T″_b0，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量其中，R″_m0为在机械臂末端坐标系下，从初始位姿点到第三位姿点的旋转矩阵，并且

f)保持姿态不改变，机械臂末端相对于第三位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d″_j，并且直接从机械臂的控制器中读取旋转矩阵R″_bj和平移列向量T″_bj，平移后测量得到第三组平动数据，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量n″_m0满足：

(d″_j-d″₀)p”＝-(n″_m0)^TT″_m0j

得到第三组约束，其中，T″_m0j＝(R″_b0)^-1(T″_bj-T″_b0)，p”＝(n″_m0)^Tv

g)使用上面三组约束和单位向量模的约束求解出n_m0和p值；

h)由求解出激光束的单位方向向量v；

3)测量并求解得到平移向量r：

控制机械臂相对于初始位姿点做旋转平移运动，旋转平移k次，分别记录第j次旋转平移后的第j位置的激光测距仪测量的距离d″′_j，并且直接从机械臂的控制器中读取旋转矩阵R″′_bj和平移列向量T″′_bj，满足约束方程：

$n_{m0}^T(R_{m0j}^{\′\′\′}-I)r+(d_j^{\′\′\′}{n}_{m0}^T{R}_{m0j}^{\′\′\′}-d_0{n}_{m0}^T)v+n_{m0}^T{T}_{m0j}^{\′\′\′}=0$

其中，1≤j≤k，k为≥3的自然数，n_m0为在机械臂末端坐标系下初始位姿点的标定平面的单位方向向量，步骤2)中已经得到，对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立求解得到平移向量r；

4)对于激光束照射到标定平面上的点，激光测距仪测量得到的距离为d，根据步骤2)和步骤3)得到的激光束的单位方向向量v和平移向量r，在机械臂末端坐标系下，该点表示为x＝r+dv，完成激光测距仪和机械臂末端的空间变换关系的标定。

其中，在步骤2)中，为了降低各种噪声和误差，增加采集数据，分别在初始位姿点、第二位姿点和第三位姿点，进行多次平移后测量得到第一组至第三组平动数据，分别对第一组至第三组约束使用最小二乘法计算，则在步骤b)、d)和f)中的第一组至第三组约束条件分别变为：

$n_{m0}=-{\[\underset{j}{\Σ}{T}_{m0j}{T}_{m0j}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}(d_j-d_0)\]p$

$n_{m0}^{\′}=-{\[\underset{j}{\Σ}{T}_{m0j}^{\′}{\left(T_{m0j}^{\′}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{\′}(d_j^{\′}-d_0^{\′})\]p^{\′}$

$n_{m0}^{\′\′}=-{\[\underset{j}{\Σ}{T}_{m0j}^{\′\′}{\left(T_{m0j}^{\′\′}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{\′\′}(d_j^{\′\′}-d_0^{\′\′})\]p^{\′\′}$

其中，j为≥2的自然数。

进一步，在空间变换关系的标定后，第j位置的标定平面上的点，激光测距仪测量得到距离d_j，以及这时的机械臂的控制器中的读出的机械臂坐标系的旋转矩阵和平移列向量R_bj和T_bj，由x_bj＝R_bj(r+d_jv)+T_bj，求得该点在机械臂坐标系下的空间坐标值x_bj，采集多个需要测量的三维空间点，从而完成三维空间点的测量。

本发明的优点：

本发明采用激光测距仪固定在机械臂末端，并在空间中放置表面平整的标定平面，通过机械臂的旋转平移运动带动激光测距仪的位姿发生改变，使用激光测距仪测量的距离d，求解平移向量r和激光束的单位方向向量v，完成激光测距仪和机械臂末端的空间变换关系标定，根据x＝r+dv，可以实现对空间中物体上三维点x的重建，具有测量精度高，价格相对低廉的优点；本发明明确给出了机械臂末端坐标系和激光测距仪坐标系的变换关系；使用单平面作为标定平面和应用非线性优化方法求解空间变换关系的标定问题，从而简化标定工具、减少误差，降低噪声干扰，提高方法的鲁棒性和易实施性。

附图说明

图1为本发明的激光测距仪和机械臂末端空间变换关系的示意图；

图2为本发明的激光测距仪和机械臂末端的空间变换关系的单平面标定方法的理论模型的示意图；

图3为本发明的激光测距仪和机械臂末端的空间变换关系的单平面标定方法的理论模型的示意图；

图4为发明的激光测距仪和机械臂末端的空间变换关系的单平面标定系统的示意图。

具体实施方式

下面结合附图，通过具体实施例，进一步阐述本发明。

机械臂末端坐标系为O_m，X_m、Y_m和Z_m分别为机械臂末端坐标系的三个坐标轴，激光测距仪坐标系为O_L，X_L、Y_L和Z_L分别为激光测距仪坐标系的三个坐标轴，激光测距仪坐标系相对于机械臂末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系为v，如图1所示。

下述空间点坐标和向量都用3*1列向量表示，单位方向向量为单位向量，激光束的单位方向向量也为单位向量。

如图2所示，在初始位置，标定平面的单位方向向量在机械臂末端坐标系下为n_m0，设在机械臂末端坐标系下激光测距仪发射出的激光束照射到标定平面上的点为x_m0，激光测距仪测量得到的距离为d₀，则在机械臂末端坐标系下标定平面的平面方程可表示为：

$n_{m0}^T{x}_{m0}+b_0=0---\left(1\right)$

其中，b₀为在初始位置，在机械臂末端坐标系下，机械臂末端坐标系的原点到标定平面的距离。

机械臂末端发生旋转和平移后，在第j位置，设机械臂末端坐标系下激光测距仪发射出的激光束照射到标定平面上的点为x_mj，激光测距仪测量得到的距离为d_j，这时在机械臂坐标系下的标定平面的单位方向向量为n_mj，同样对于标定平面在机械臂末端坐标系下有平面方程：

$n_{mj}^T{x}_{mj}+b_j=0---\left(2\right)$

其中，b_j为在第j位置，在机械臂末端坐标系下，机械臂末端坐标系的原点到标定平面的距离。

设在标定平面上的两点有如下关系：

x_m0＝R_m0jx_mj+T_m0j(3)

其中，R_m0j为在机械臂末端坐标系下，从第j位置到初始位置的旋转矩阵，T_m0j在机械臂末端坐标系下，从第j位置到初始位置的平移列向量。

则将(3)代入(1)中得到：

$n_{m0}^T{x}_{m0}+b_0=n_{m0}^T{R}_{m0j}{x}_{mj}+n_{m0}^T{T}_{m0j}+b_0=0---\left(4\right)$

比较(2)和(4)可以得到：

$n_{mj}^T=n_{m0}^T{R}_{m0j}---\left(5\right)$

$b_j=n_{m0}^T{T}_{m0j}+b_0---\left(6\right)$

在机械臂末端坐标系下，根据机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点可以表示成：

x_m0＝r+d₀v(7)

x_mj＝r+d_jv(8)

将(8)代入(4)中整理后得到：

$n_{m0}^T{R}_{m0j}r+d_j{n}_{m0}^T{R}_{m0j}v+n_{m0}^T{T}_{m0j}+b_0=0---\left(9\right)$

在(9)中的未知变量r和v是确定空间变换标定的关键变量，如果能够确定r和v则完成机械臂末端和激光测距仪的空间变换标定。在(9)中n_m0,R_m0j,T_m0j,b₀等变量也是待求解的，其中的R_m0j,T_m0j是无法直接得到，但是可以通过机械臂的旋转和平移情况得到，接下来就推导出它们和机械臂坐标系的旋转平移的关系。

如图3所示，标定平面相对于机械臂坐标系O_b是固定的，在机械臂坐标系下标定平面的单位单位方向向量为N，机械臂坐标系的原点到标定平面的距离为b，在实际操作中可以直接从机械臂的控制器中读取到在第j位置时，机械臂末端坐标系O_m到机械臂坐标系O_b的旋转矩阵R_bj和平移列向量T_bj。

在初始位置，标定平面的单位方向向量在机械臂末端坐标系下为n_m0，设在机械臂末端坐标系下激光束照射到标定平面上的点为x_m0，同时该点在机械臂坐标系下为x_b0，机械臂坐标系原点到标定平面的距离为b，激光测距仪测量得到的距离为d₀，从机械臂末端坐标系相对于机械臂坐标系的旋转矩阵为R_b0，平移列向量为T_b0，这时有从机械臂末端坐标系到机械臂坐标系的变换关系为：

$\left[\begin{array}{c}{x}_{b0}\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{b0}& T_{b0}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{m0}\\ 1\end{array}\right]---\left(10\right)$

对于标定平面，在机械臂末端坐标系下有平面方程(1)，在机械臂坐标系下有平面方程：

N^Tx_b0+b＝0(11)

机械臂末端发生旋转和平移后，在第j位置，机械臂末端坐标系下激光束照射到标定平面上的另外一点为x_mj，同时该点在机械臂坐标系下为x_bj，激光测距仪测量得到的距离为d_j，从机械臂末端坐标系相对于机械臂坐标系的旋转矩阵为R_bj，平移列向量为T_bj，这时有从机械臂末端坐标系到机械臂坐标系的变换关系为：

$\left[\begin{array}{c}{x}_{bj}\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{bj}& T_{bj}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{mj}\\ 1\end{array}\right]---\left(12\right)$

这时的标定平面，在第j位置，机械臂末端坐标系下的标定平面的单位方向向量为n_mj，同样对于标定平面，在机械臂末端坐标系下有平面方程(2)，在机械臂坐标系下有平面方程：

N^Tx_bj+b＝0(13)

其中，b为机械臂坐标系的原点到标定平面的距离。

将等式(10)代入(11)有：

N^Tx_b0+b＝N^T(R_b0x_m0+T_b0)+b＝0(14)

同理(12)代入(13)有：

N^Tx_bj+b＝N^T(R_bjx_mj+T_bj)+b＝0(15)

再由等式(14)和(15)，使用最小二乘可得：

$x_{m0}=R_{b0}^{-1}{R}_{bj}{x}_{mj}+R_{b0}^{-1}(T_{bj}-T_{b0})---\left(16\right)$

比较(3)和(16)可得：

$R_{m0j}-R_{b0}^{-1}{R}_{bj}---\left(17\right)$

$T_{m0j}=R_{b0}^{-1}(T_{bj}-T_{b0})---\left(18\right)$

在初始位置，从机械臂末端坐标系相对于机械臂坐标系的旋转矩阵为R_b0，平移列向量为T_b0，以及在第j位置，从机械臂末端坐标系相对于机械臂坐标系的旋转矩阵为R_bj，平移列向量为T_bj，都可以通过机械臂控制器直接读取，这样由(17)(18)两个等式可以得到R_m0j和T_m0j。

根据等式(9)，机械臂末端当只有平移运动时，即R_m0j＝I，代入(9)中得到：

$n_{m0}^{T}r+d_j{n}_{m0}^{T}v+n_{m0}^T{T}_{m0j}+b_0=0---\left(19\right)$

当j＝0时，即还没有产生相对的旋转和平移运动，这时的T_m0j＝[0；0；0]，则有：

$n_{m0}^{T}r+d_0{n}_{m0}^{T}v+b_0=0---\left(20\right)$

(19)-(20)有：

$(d_j-d_0)n_{m0}^{T}v=-n_{m0}^T{T}_{m0j}.---\left(21\right)$

接下来求解(21)式，把写成公式(22)

$n_{m0}^{T}v=p.---\left(22\right)$

则(21)式可表示成：

$(d_j-d_0)p=-n_{m0}^T{T}_{m0j}---\left(23\right)$

在(23)式中的p还是未知变量，但是n_m0为单位向量即有等式且z方向沿着机械臂末端坐标系的Z_m轴正方向，即n_m0z＞0。这样对于(23)式最少2次测量(即初始位置和第j位置)可以求出n_m0,且同时可以求得p。这样代回(22)式，可以得到v的一组约束。

接下来，控制机械臂进行旋转平移运动，在旋转平移后的机械臂末端坐标系下的标定平面的单位方向向量为n'_m0，从机械臂末端坐标系到机械臂坐标系的旋转矩阵为R'_b0，设在机械臂末端坐标系下，从初始位置到当前位置的旋转矩阵为R'_m0，并且由公式(5)可得到接着控制机械臂只进行平移运动，由公式(23)可以求得对应的p'，代入(22)得到另一组约束，再用同样的方法得到第三组约束，这样就可以解出v。

为了较少采集数据的各种噪声和误差，增加采集数据，对(23)使用最小二乘法计算可得：

$n_{m0}=-{\[\underset{j}{\Σ}{T}_{m0j}{T}_{m0j}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}(d_j-d_0)\]p---\left(24\right)$

其中，j≥2，即在每一个位置，需至少平移两次。

当完成对n_m0和v的求解后，再回到(9)式，将其重写如下：

$n_{m0}^T{R}_{m0j}r+d_j{n}_{m0}^T{R}_{m0j}v+n_{m0}^T{T}_{m0j}+b_0=0$

在(9)式中还有r和b₀是未知的，

重新写(20)：

$n_{m0}^{T}r+d_0{n}_{m0}^{T}v+b_0=0$

(9)-(20)得：

$n_{m0}^T(R_{m0j}-I)r+(d_j{n}_{m0}^T{R}_{m0j}-d_0{n}_{m0}^T)v+n_{m0}^T{T}_{m0j}=0---\left(25\right)$

在(25)式中只有r未知，由于每次测量只能求得一个约束方程，所以至少需要测量三组旋转平移数据，才能够将r解出。

至此，空间变换标定所需的r和v都已经解出。

在完成空间变换标定后，就可以根据(12)式得：

x_bj＝R_bjx_mj+T_bj＝R_bj(r+d_jv)+T_bj(26)

这样当使用激光测距仪测量空间中的某个点，在得到它的距离d_j和这时的机械臂的控制器中的读数R_bj和T_bj，就可以由(26)式求得该点在机械臂坐标系下的空间坐标值x_bj。

如图4所示，本实施例中，机械臂末端m挂载有激光测距仪L，在空间中放置有一个相对于机械臂基座B空间位置固定的标定平面Pi，要求标定平面的平整度尽可能好。

控制机械臂运动，使得激光测距仪发出的激光光斑照射在标定平面上，记录这时的激光测距仪测量的距离d₀，机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R_b0和平移列向量T_b0，记为初始位姿点。保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，测量每一次的距离d_j和旋转矩阵R_bj和平移列向量T_bj，一共测量15次数据得到第一组平动数据，代入(24)式得到n_m0和p的第一组约束。调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离d'₀及机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R'_b0和平移列向量T′_b0。保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，测量每一次的距离d'_j和旋转矩阵R′_bj和平移列向量T′_bj，一共测量15次数据得到第二组平动数据，代入(24)式得到n'_m0和p'的第二组约束。调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离d″₀及机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R″_b0和平移列向量T″_b0。保持姿态不改变，机械臂末端相对于第三位姿点只做平移运动，测量每一次的距离d″_j和旋转矩阵R″_bj和平移列向量T″_bj，一共测量15次数据得到第三组平动数据，代入(24)式得到n”_m0和p”的第三组约束。

使用上面三组约束和单位向量模的约束可以求解出每一组的标定平面的单位方向向量n_m0和p值，再根据(22)式，可以求解出激光束的单位方向向量v。

接下来控制机械臂相对于初始位姿点做旋转平移运动，测量每一次的距离d″′_j和旋转矩阵R″′_bj和平移列向量T″′_bj，一共测量15次数据得到旋转平移数据。将每一次得到的数据分别代入(25)式，列出15个约束方程，使用最小二乘法，就可以求出激光测距仪坐标系相对于机械臂末端坐标系的平移量r。

至此，空间变换关系的标定所需的r和v，都已经解出。

在完成空间变换关系的标定后，这样当使用激光测距仪测量空间中的第j位置的点，在得到它的距离d_j和这时的机械臂的控制器中的读数R_bj和T_bj，就可以由(26)式求得该点在机械臂坐标系下的空间坐标值x_bj，采集多个需要测量的三维空间点，就可以完成三维空间点的测量。

最后需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附的权利要求的精神和范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (4)

1.一种激光测距仪和机械臂末端的空间变换关系的单平面标定方法，其特征在于，所述标定方法，包括以下步骤：

1)机械臂末端坐标系为O_m，激光测距仪坐标系为O_L，激光测距仪发出的激光束照射到标定平面上的一个点，激光测距仪坐标系相对于机械臂末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系为v，机械臂末端坐标系下激光束照射到标定平面上的点为x，激光测距仪测量得到的距离为d，即在激光测距仪坐标系下，激光测距仪坐标系的原点到激光束照射到标定平面的点的距离为d，根据机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点表示为：x＝r+dv，其中，r为平移向量，v为激光束的单位方向向量，二者未知，下面分别求得r和v；

2)测量并求解得到激光束的单位方向向量v：

a)控制机械臂运动，激光测距仪发出的激光束照射在标定平面上，保持机械臂不动，记录这时的激光测距仪测量的距离d₀，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R_b0和平移列向量T_b0，记为初始位姿点；

b)保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d_j，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R_bj和平移列向量T_bj，平移后测量得到第一组平动数据，在机械臂末端坐标系下初始位姿点对应的标定平面的单位方向向量n_m0满足：

$(d_j-d_0)p=-n_{m0}^T{T}_{m0j}$

得到第一组约束，其中，j为自然数， $T_{m0j}=R_{b0}^{-1}(T_{bj}-T_{b0}),p=n_{m0}^{T}v;$

c)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离d'₀，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R'_b0和平移列向量T′_b0，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量其中，R'_m0为在机械臂末端坐标系下从初始位姿点到第二位姿点的旋转矩阵，并且 $R_{m0}^{\′}=R_{b0}^{-1}{R}_{b0}^{\′};$

d)保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d'_j，并且直接从机械臂的控制器中读取旋转矩阵R′_bj和平移列向量T′_bj，平移后测量得到第二组平动数据，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量n'_m0满足：

(d'_j-d'₀)p'＝-(n'_m0)^TT′_m0j

得到第二组约束，其中，T′_m0j＝(R′_b0)^-1(T′_bj-T′_b0)，p'＝(n'_m0)^Tv；

e)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离d″₀，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R″_b0和平移列向量T″_b0，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量其中，R″_m0为在机械臂末端坐标系下，从初始位姿点到第三位姿点的旋转矩阵，并且

f)保持姿态不改变，机械臂末端相对于第三位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d″_j，并且直接从机械臂的控制器中读取旋转矩阵R″_bj和平移列向量T″_bj，平移后测量得到第三组平动数据，在机械臂末端坐标系下第三位姿点对应的标定平面的单位方向向量n”_m0满足：

(d″_j-d″₀)p”＝-(n”_m0)^TT″_m0j

得到第三组约束，其中，T″_m0j＝(R″_b0)^-1(T″_bj-T″_b0)，p”＝(n″_m0)^Tv

g)使用上面三组约束和单位向量模的约束求解出n_m0和p值；

h)由求解出激光束的单位方向向量v；

3)测量并求解得到平移向量r：

控制机械臂相对于初始位姿点做旋转平移运动，旋转平移k次，分别记录第j次旋转平移后的第j位置的激光测距仪测量的距离d'”_j，并且直接从机械臂的控制器中读取旋转矩阵R”'_bj和平移列向量T”'_bj，满足约束方程：

$n_{m0}^T(R_{m0j}^{\′}-I)r+(d_j^{\′\′\′}{n}_{m0}^T{R}_{m0j}^{\′\′\′}-d_0{n}_{m0}^T)v+n_{m0}^T{T}_{m0j}^{\′\′\′}=0$

其中，1≤j≤k，k为≥3的自然数，n_m0为在机械臂末端坐标系下初始位姿点的标定平面的单位方向向量，步骤2)中已经得到，对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立求解得到平移向量r；

4)对于激光束照射到标定平面上的点，激光测距仪测量得到的距离为d，根据步骤2)和步骤3)得到的激光束的单位方向向量v和平移向量r，在机械臂末端坐标系下，该点表示为x＝r+dv，完成激光测距仪和机械臂末端的空间变换关系的标定。

2.如权利要求1所述的标定方法，其特征在于，在步骤2)中，增加采集数据，分别在初始位姿点、第二位姿点和第三位姿点，进行多次平移后测量得到第一组至第三组平动数据，分别对第一组至第三组约束使用最小二乘法计算，则在步骤b)、d)和f)中的第一组至第三组约束条件分别变为：

$n_{m0}=-{\[\underset{j}{\Σ}{T}_{m0j}{T}_{m0j}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}(d_j-d_0)\]p$

$n_{m0}^{\′}=-{\[\underset{j}{\Σ}{T}_{m0j}^{\′}{\left(T_{m0j}^{\′}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{\′}(d_j^{\′}-d_0^{\′})\]p^{\′}$

$n_{m0}^{\′\′}=-{\[\underset{j}{\Σ}{T}_{m0j}^{\′\′}{\left(T_{m0j}^{\′\′}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{\′\′}(d_j^{\′\′}-d_0^{\′\′})\]p^{\′\′}$

其中，j为≥2的自然数。

3.如权利要求1所述的标定方法，其特征在于，进一步，在完成空间变换关系的标定后，第j位置的标定平面上的点，激光测距仪测量得到距离d_j，以及这时的机械臂的控制器中的读出的机械臂坐标系的旋转矩阵和平移列向量R_bj和T_bj，由x_bj＝R_bj(r+d_jv)+T_bj，求得该点在机械臂坐标系下的空间坐标值x_bj，采集多个需要测量的三维空间点，从而完成三维空间点的测量。

4.一种激光测距仪和机械臂末端的空间变换关系的单平面标定系统，其特征在于，所述标定系统包括：机械臂、机械臂末端、激光测距仪以及标定平面；其中，所述激光测距仪固定在机械臂末端，二者相对位置不变；在空间中放置标定平面，标定平面相对于机械臂的位置固定，标定平面的表面平整；所述机械臂连接至机械臂的控制器，从机械臂的控制器中直接读取出机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量。