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

Abstract

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

Description

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

技术领域

本发明涉及视觉测量领域，具体涉及一种激光测距仪和机械臂末端的空间变换关系的正交平面标定方法及系统。

背景技术

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

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

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

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

发明内容

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

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

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

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

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

1)建立空间变换关系：

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

2)通过控制机械臂运动，使得激光测距仪发出的激光束分别照射到第一至第三标定平面上，分别测量并求解得到v₁和r₁、v₂和r₂以及v₃和r₃，其中，v₁和r₁分别为由第一标定平面P1求得的激光束的单位方向向量和平移向量，v₂和r₂分别为由第二标定平面P2求得的激光束的单位方向向量和平移向量，以及v₃和r₃分别为由第三标定平面P3求得的激光束的单位方向向量和平移向量；

3)优化计算平移向量r和激光束的单位方向向量v：

设定初始值、约束函数和目标函数，采用迭代算法，直至满足给定的收敛条件为止，得到优化计算后的平移向量r和激光束的单位方向向量v，完成激光测距仪和机械臂末端的空间变换关系的标定；

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

其中，在步骤2)中，由第一至第三标定平面P1～P3，分别测量并求解得到v₁和r₁、v₂和r₂以及v₃和r₃，包括以下步骤：

a)测量并求解由第一标定平面P1求得的激光束的单位方向向量v₁：

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

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

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

得到第一标定平面P1的第一组约束，其中，j为≥2的自然数， $p^{11}={\left(n_{m0}^{11}\right)}^T{v}_1;$

iii.调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第一标定平面P1上，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第二位姿点的第一标定平面的单位法向量其中，为在机械臂末端坐标系下，从原始位姿点到第二位姿点的旋转矩阵，并且

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

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

得到第一标定平面P1的第二组约束，其中，j为≥2的自然数， $p^{12}={\left(n_{m0}^{12}\right)}^T{v}_1;$

v.调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第一标定平面P1上，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第三位姿点的第一标定平面P1的单位法向量其中，为在机械臂末端坐标系下，从原始位姿点到第三位姿点的旋转矩阵，并且 $R_{m0}^{13}={\left(R_{b0}^{11}\right)}^{-1}{R}_{b0}^{13};$

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

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

得到第一标定平面P1的第三组约束，其中，j为≥2的自然数， $p^{13}={\left(n_{m0}^{13}\right)}^T{v}_1;$

vii.使用上面三组第一标定平面P1的约束和单位向量模的约束求解出和p¹¹、p¹²和p¹³的值；

viii.由 $\begin{array}{cc}{p}^{11}={\left(n_{m0}^{11}\right)}^T{v}_1,& p^{12}={\left(n_{m0}^{12}\right)}^T{v}_1\end{array}$ 和 $p^{13}={\left(n_{m0}^{13}\right)}^T{v}_1,$ 求解出由第一标定平面P1求得的激光束的单位方向向量v₁；

b)测量并求解由第一标定平面P1求得的平移向量r₁：

控制机械臂相对于初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪发出的激光束照射在第一标定平面P1上，记录第j次旋转平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取旋转矩阵和平移列向量满足约束方程：

${\left(n_{m0}^{11}\right)}^T(R_{m0j}^{14}-I)r_1+(d_j^{14}{\left(n_{m0}^{11}\right)}^T{R}_{m0j}^{14}-d_0^{11}{\left(n_{m0}^{11}\right)}^T)v_1+{\left(n_{m0}^{11}\right)}^T{T}_{m0j}^{14}=0$

其中，1≤j≤k，k为≥3的自然数，为在机械臂末端坐标系下初始位姿点的第一标定平面P1的单位法向量，在步骤a)中已经得到， $\begin{array}{cc}{T}_{m0j}^{14}={\left(R_{b0}^{11}\right)}^{-1}(T_{bj}^{14}-T_{b0}^{11}),& R_{m0j}^{14}={\left(R_{b0}^{11}\right)}^{-1}{R}_{bj}^{14}\end{array},$ 对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第一标定平面P1求得的平移向量r₁；

c)测量并求解由第二标定平面P2求得的激光束的单位方向向量v₂：

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

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

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

得到第二标定平面P2的第一组约束，其中，j为≥2的自然数， $p^{21}={\left(n_{m0}^{21}\right)}^T{v}_2;$

iii.调整机械臂末端的位姿，使其相对于第二标定平面P2的初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第二标定平面P2上，固定这时的位姿，记为第二标定平面P2的第二位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第二位姿点的第二标定平面P2的单位法向量其中，为在机械臂末端坐标系下，从第二标定平面P2的原始位姿点到第二位姿点的旋转矩阵，并且

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

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

得到第二标定平面P2的第二组约束，其中，j为≥2的自然数， $p^{22}={\left(n_{m0}^{22}\right)}^T{v}_2;$

v.调整机械臂末端的位姿，使其相对于第二标定平面P2的初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第二标定平面P2上，固定这时的位姿使其不同于第二标定平面P2的初始位姿点和第二位姿点，记为第二标定平面P2的第三位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第三位姿点的第二标定平面的单位法向量其中，为在机械臂末端坐标系下，从第二标定平面P2的原始位姿点到第三位姿点的旋转矩阵，并且 $R_{m0}^{23}={\left(R_{b0}^{21}\right)}^{-1}{R}_{b0}^{23};$

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

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

得到第二标定平面P2的第三组约束，其中，j为≥2的自然数， $p^{23}={\left(n_{m0}^{23}\right)}^T{v}_2$

vii.使用上面三组第二标定平面P2的约束和单位向量模的约束求解出和p²¹、p²²和p³³的值；

viii.由 $\begin{array}{cc}{p}^{21}={\left(n_{m0}^{21}\right)}^T{v}_2,& p^{22}={\left(n_{m0}^{22}\right)}^T{v}_2\end{array}$ 和 $p^{23}={\left(n_{m0}^{23}\right)}^T{v}_2,$ 求解出由第二标定平面P2求得的激光束的单位方向向量v₂；

d)测量并求解由第二标定平面P2求得的平移向量r₂：

控制机械臂相对于第二标定平面P2的初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪发出的激光束照射在第二标定平面P2上，记录第j次旋转平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取旋转矩阵和平移列向量满足约束方程：

${\left(n_{m0}^{21}\right)}^T(R_{m0j}^{24}-I)r_2+(d_j^{24}{\left(n_{m0}^{21}\right)}^T{R}_{m0j}^{24}-d_0^{21}{\left(n_{m0}^{21}\right)}^T)v_2+{\left(n_{m0}^{21}\right)}^T{T}_{m0j}^{24}=0$

其中，为在机械臂末端坐标系下初始位姿点的第二标定平面的单位法向量，在步骤c)中已经得到， $T_{m0j}^{24}={\left(R_{b0}^{21}\right)}^{-1}(T_{bj}^{24}-T_{b0}^{21}),R_{m0j}^{24}={\left(R_{b0}^{21}\right)}^{-1}{R}_{bj}^{24},$ 对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第二标定平面P2求得的平移向量r₂。

e)测量并求解由第三标定平面P3求得的激光束的单位方向向量v₃：

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

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

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

得到第三标定平面P3的第一组约束，其中，j为≥2自然数， $p^{31}={\left(n_{m0}^{31}\right)}^T{v}_3;$

iii.调整机械臂末端的位姿，使其相对于第三标定平面P3的初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第三标定平面P3上，固定这时的位姿且激光测距仪，记为第三标定平面P3的第二位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第二位姿点的第三标定平面P3的单位法向量其中，为在机械臂末端坐标系下，从第三标定平面P3的原始位姿点到第二位姿点的旋转矩阵，并且

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

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

得到第三标定平面P3的第二组约束，其中，j为≥2的自然数， $p^{32}={\left(n_{m0}^{32}\right)}^T{v}_3;$

v.调整机械臂末端的位姿，使其相对于第三标定平面P3的初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于第三标定平面P3的初始位姿点和第二位姿点，记为第三标定平面P3的第三位姿点，且激光测距仪发出的激光束照射在第三标定平面P3上，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第三位姿点的第三标定平面P3的单位法向量其中，为在机械臂末端坐标系下，从第三标定平面P3的原始位姿点到第三位姿点的旋转矩阵，并且 $R_{m0}^{33}={\left(R_{b0}^{31}\right)}^{-1}{R}_{b0}^{33};$

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

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

得到第三标定平面P3的第三组约束，其中，j为≥2的自然数， $p^{33}={\left(n_{m0}^{33}\right)}^T{v}_3$

vii.使用上面三组第三标定平面P3的约束和单位向量模的约束求解出和p³¹、p³²和p³³的值；

viii.由 $\begin{array}{cc}{p}^{31}={\left(n_{m0}^{31}\right)}^T{v}_3,& p^{32}={\left(n_{m0}^{32}\right)}^T{v}_3\end{array}$ 和 $p^{33}={\left(n_{m0}^{33}\right)}^T{v}_3,$ 求解出由第三标定平面P3求得的激光束的单位方向向量v₃；

f)测量并求解由第三标定平面P3求得的平移向量r₃：

控制机械臂相对于第三标定平面P3的初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪发出的激光束照射在第三标定平面P3上，记录第j次旋转平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取旋转矩阵和平移列向量满足约束方程：

${\left(n_{m0}^{31}\right)}^T(R_{m0j}^{34}-I)r_3+(d_j^{34}{\left(n_{m0}^{31}\right)}^T{R}_{m0j}^{34}-d_0^{31}{\left(n_{m0}^{31}\right)}^T)v_3+{\left(n_{m0}^{31}\right)}^T{T}_{m0j}^{34}=0$

其中，1≤j≤k，k为≥3的自然数，为在机械臂末端坐标系下初始位姿点的第三标定平面P3的单位法向量，在步骤e)中已经得到， $\begin{array}{cc}{T}_{m0j}^{34}={\left(R_{b0}^{31}\right)}^{-1}(T_{bj}^{34}-T_{b0}^{31}),& R_{m0j}^{34}={\left(R_{b0}^{31}\right)}^{-1}{R}_{bj}^{34}\end{array},$ 对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第三标定平面P3求得的平移向量r₃。

步骤3)中，优化计算平移向量r和激光束的单位方向向量v，具体包括以下步骤：

a)设定初始值：

对三个标定平面分别求得的平移向量取平均得到优化算法的平移向量r的初始值，即r的初始值为同理求得激光束的单位方向向量v的初始值，即v的初始值为

b)确定约束条件：

使用多个有约束的线性最小二乘法组成的非线性最小化算法，线性约束条件是非线性约束条件是标定平面的单位法向量的第三个分量大于零，即 $n_{m0}^{11}\left(3\right)>0,n_{m0}^{21}\left(3\right)>0,n_{m0}^{31}\left(3\right)>0;$

c)确定目标函数：

算法的目标函数为f＝f₁+f₂+f₃，其中f₁为与第一标定平面P1相关的约束函数，f₂为与第二标定平面P2相关的约束函数，f₃为与第三标定平面P3相关的约束函数；

d)迭代计算：

不断的迭代更新直至满足给定的收敛条件为止；

与第一标定平面P1相关的约束函数表示为f₁＝f₁₁+f₁₂+f₁₃+f₁₄，其中 $f_{11}=\left|\right|n_{m0}^{11}+{\[\underset{j}{\Σ}{T}_{m0j}^{11}{\left(T_{m0j}^{11}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{11}(d_j^{11}-d_0^{11})\]p^{11}|{|}_2^2+||X^{11}-X_{fit}^{11}|{|}_2^2,$ X¹¹为步骤2)的a)的ii)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{11}=\[x_{bj}^{11}\],& x_{bj}^{11}=R_{bj}^{11}(r+d_j^{11}v)+T_{bj}^{11}\end{array},$ 为X¹¹使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{12}=\left|\right|n_{m0}^{12}+{\[\underset{j}{\Σ}{T}_{m0j}^{12}{\left(T_{m0j}^{12}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{12}(d_j^{12}-d_0^{12})\]p^{12}|{|}_2^2+||X^{12}-X_{fit}^{12}|{|}_2^2,$ X¹²为步骤2)的a)的iv)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{12}=\[x_{bj}^{12}\],& x_{bj}^{12}=R_{bj}^{12}(r+d_j^{12}v)+T_{bj}^{12}\end{array},$ 为X¹²使用主成分分析方法得到的主平面上对应的点组成的矩阵；

X¹³为步骤2)的a)的vi)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{13}=\[x_{bj}^{13}\],& x_{bj}^{13}=R_{bj}^{13}(r+d_j^{13}v)+T_{bj}^{13}\end{array},$ 为X¹³使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{14}=\left|\right|\left[\begin{array}{c}{p}^{11}-{\left(n_{m0}^{11}\right)}^{T}v\\ p^{12}-{\left(n_{m0}^{12}\right)}^{T}v\\ p^{13}-{\left(n_{m0}^{13}\right)}^{T}v\end{array}\right]|{|}_2^2+||X^{14}-X_{fit}^{14}|{|}_2^2,$ X¹⁴为步骤2)的b)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{14}=\[x_{bj}^{14}\],& x_{bj}^{14}=R_{bj}^{14}(r+d_j^{14}v)+T_{bj}^{14}\end{array},$ 为X¹⁴使用主成分分析方法得到的主平面上对应的点组成的矩阵；

同样，与第二标定平面P2相关的函数f₂＝f₂₁+f₂₂+f₂₃+f₂₄，其中， $f_{21}=\left|\right|n_{m0}^{21}+{\[\underset{j}{\Σ}{T}_{m0j}^{21}{\left(T_{m0j}^{21}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{21}(d_j^{21}-d_0^{21})\]p^{21}|{|}_2^2+||X^{21}-X_{fit}^{21}|{|}_2^2,$ X²¹为步骤2)的c)的ii)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{21}=\[x_{bj}^{21}\],& x_{bj}^{21}=R_{bj}^{21}(r+d_j^{21}v)+T_{bj}^{21}\end{array},$ 为X²¹使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{22}=\left|\right|n_{m0}^{22}+{\[\underset{j}{\Σ}{T}_{m0j}^{22}{\left(T_{m0j}^{22}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{22}(d_j^{22}-d_0^{22})\]p^{22}|{|}_2^2+||X^{22}-X_{fit}^{22}|{|}_2^2,$ X²²为步骤2)的c)的iv)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{22}=\[x_{bj}^{22}\],& x_{bj}^{22}=R_{bj}^{22}(r+d_j^{22}v)+T_{bj}^{22}\end{array},$ 为X²²使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{23}=\left|\right|n_{m0}^{23}+{\[\underset{j}{\Σ}{T}_{m0j}^{23}{\left(T_{m0j}^{23}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{23}(d_j^{23}-d_0^{23})\]p^{23}|{|}_2^2+||X^{23}-X_{fit}^{23}|{|}_2^2,$ X²³为2)的c)的vi)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{23}=\[x_{bj}^{23}\],& x_{bj}^{23}=R_{bj}^{23}(r+d_j^{23}v)+T_{bj}^{23}\end{array},$ 为X²³使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{24}=\left|\right|\left[\begin{array}{c}{p}^{21}-{\left(n_{m0}^{21}\right)}^{T}v\\ p^{22}-{\left(n_{m0}^{22}\right)}^{T}v\\ p^{23}-{\left(n_{m0}^{23}\right)}^{T}v\end{array}\right]|{|}_2^2+||X^{24}-X_{fit}^{24}|{|}_2^2,$ X²⁴为步骤2)的d)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{24}=\[x_{bj}^{24}\],& x_{bj}^{24}=R_{bj}^{24}(r+d_j^{24}v)+T_{bj}^{24}\end{array},$ 为X²⁴使用主成分分析方法得到的主平面上对应的点组成的矩阵；

同样，与第三标定平面P3相关的函数f₃＝f₃₁+f₃₂+f₃₃+f₃₄，其中， $f_{31}=\left|\right|n_{m0}^{31}+{\[\underset{j}{\Σ}{T}_{m0j}^{31}{\left(T_{m0j}^{31}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{31}(d_j^{31}-d_0^{31})\]p^{31}|{|}_2^2+||X^{31}-X_{fit}^{31}|{|}_2^2,$ X³¹为步骤2)的e)的ii)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{31}=\[x_{bj}^{31}\],& x_{bj}^{301}=R_{bj}^{31}(r+d_j^{31}v)+T_{bj}^{31}\end{array},$ 为X³¹使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{32}=\left|\right|n_{m0}^{32}+{\[\underset{j}{\Σ}{T}_{m0j}^{32}{\left(T_{m0j}^{32}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{32}(d_j^{32}-d_0^{32})\]p^{32}|{|}_2^2+||X^{32}-X_{fit}^{32}|{|}_2^2,$ X³²为步骤2)的e)的iv)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{32}=\[x_{bj}^{32}\],& x_{bj}^{32}=R_{bj}^{32}(r+d_j^{32}v)+T_{bj}^{32}\end{array},$ 为X³²

使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{33}=\left|\right|n_{m0}^{33}+{\[\underset{j}{\Σ}{T}_{m0j}^{33}{\left(T_{m0j}^{33}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{33}(d_j^{33}-d_0^{33})\]p^{33}|{|}_2^2+||X^{33}-X_{fit}^{33}|{|}_2^2,$ X³³为步骤2)的e)的vi)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{33}=\[x_{bj}^{33}\],& x_{bj}^{33}=R_{bj}^{33}(r+d_j^{33}v)+T_{bj}^{33}\end{array},$ 为X³³使用主成分分析方法得到的主平面上对应的点组成的矩阵；

其中的 $f_{34}=\left|\right|\left[\begin{array}{c}{p}^{31}-{\left(n_{m0}^{31}\right)}^{T}v\\ p^{32}-{\left(n_{m0}^{32}\right)}^{T}v\\ p^{33}-{\left(n_{m0}^{33}\right)}^{T}v\end{array}\right]|{|}_2^2+||X^{34}-X_{fit}^{34}|{|}_2^2,$ X³⁴为步骤2)的f)中测量得到的点组成的矩阵，即 $\begin{array}{cc}{X}^{34}=\[x_{bj}^{34}\],& x_{bj}^{34}=R_{bj}^{34}(r+d_j^{34}v)+T_{bj}^{34}\end{array},$ 为X³⁴使用主成分分析方法得到的主平面上对应的点组成的矩阵；

当算法收敛停止迭代后，得到优化的激光束的单位方向向量v和平移向量r。

进一步，当完成空间变换关系标定后，激光测距仪测量空间中的第j位置的点，在得到它的距离d_j和这时的机械臂的控制器中的读出的机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R_bj和平移列向量T_bj，由x_bj＝R_bjx_mj+T_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)

将等式(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\]}^T\[\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^T{n}_{m0}v+b_0=0$

(9)-(20)得：

$n_{m0}^T(R_{m0j}-I)r+(d_j{n}_{m0}^T{R}_{m0j}-d_0^T{n}_{m0})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空间位置固定的互相正交的第一至第三标定平面P1～P3，要求标定平面的平整度尽可能好。

控制机械臂运动，使得激光测距仪发出的激光光斑照射在一个标定平面上，记录这时的激光测距仪测量的距离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，p'和p”值，再根据(22)式，可以求解出激光束的单位方向向量v。

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

按照上面的步骤，激光束依次照射到第一至第三标定平面，分别测量并求解得到v₁和r₁、v₂和r₂以及v₃和r₃，然后优化计算平移向量r和激光束的单位方向向量v。

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

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

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

Claims (5)

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

1)建立空间变换关系：

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

2)通过控制机械臂运动，使得激光测距仪发出的激光束分别照射到第一至第三标定平面上，分别测量并求解得到v₁和r₁、v₂和r₂以及v₃和r₃，其中，v₁和r₁分别为由第一标定平面P1求得的激光束的单位方向向量和平移向量，v₂和r₂分别为由第二标定平面P2求得的激光束的单位方向向量和平移向量，以及v₃和r₃分别为由第三标定平面P3求得的激光束的单位方向向量和平移向量；

3)优化计算平移向量r和激光束的单位方向向量v：

设定初始值、约束函数和目标函数，采用迭代算法，直至满足给定的收敛条件为止，得到优化计算后的平移向量r和激光束的单位方向向量v，完成激光测距仪和机械臂末端的空间变换关系的标定；

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

2.如权利要求1所述的标定方法，其特征在于，在步骤2)中，由第一至第三标定平面P1～P3，分别测量并求解得到v₁和r₁、v₂和r₂以及v₃和r₃，包括以下步骤：

a)测量并求解由第一标定平面P1求得的激光束的单位方向向量v₁：

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

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

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

得到第一标定平面P1的第一组约束，其中，j为≥2的自然数， $p^{11}={\left(n_{m0}^{11}\right)}^T{v}_1;$

iii.调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第一标定平面P1上，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第二位姿点的第一标定平面的单位法向量其中，为在机械臂末端坐标系下，从原始位姿点到第二位姿点的旋转矩阵，并且

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

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

得到第一标定平面P1的第二组约束，其中，j为≥2的自然数， $p^{12}={\left(n_{m0}^{12}\right)}^T{v}_1;$

v.调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第一标定平面P1上，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第三位姿点的第一标定平面P1的单位法向量其中，为在机械臂末端坐标系下，从原始位姿点到第三位姿点的旋转矩阵，并且 $R_{m0}^{13}={\left(R_{b0}^{11}\right)}^{-1}{R}_{b0}^{13};$

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

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

得到第一标定平面P1的第三组约束，其中，j为≥2的自然数， $p^{13}={\left(n_{m0}^{13}\right)}^T{v}_1;$

vii.使用上面三组第一标定平面P1的约束和单位向量模的约束求解出和p¹¹、p¹²和p¹³的值；

viii.由和求解出由第一标定平面P1求得的激光束的单位方向向量v₁；

b)测量并求解由第一标定平面P1求得的平移向量r₁：

控制机械臂相对于初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪发出的激光束照射在第一标定平面P1上，记录第j次旋转平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取旋转矩阵和平移列向量满足约束方程：

${\left(n_{m0}^{11}\right)}^T(R_{m0j}^{14}-I)r_1+(d_j^{14}{\left(n_{m0}^{11}\right)}^T{R}_{m0j}^{14}-d_0^{11}{\left(n_{m0}^{11}\right)}^T)v_1+{\left(n_{m0}^{11}\right)}^T{T}_{m0j}^{14}=0$

其中，1≤j≤k，k为≥3的自然数，为在机械臂末端坐标系下初始位姿点的第一标定平面P1的单位法向量，在步骤a)中已经得到， $T_{m0j}^{14}={\left(R_{b0}^{11}\right)}^{-1}(T_{bj}^{14}-T_{b0}^{11})R_{m0j}^{14}={\left(R_{b0}^{11}\right)}^{-1}{R}_{bj}^{14},$ 对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第一标定平面P1求得的平移向量r₁；

c)测量并求解由第二标定平面P2求得的激光束的单位方向向量v₂：

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

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

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

得到第二标定平面P2的第一组约束，其中，j为≥2的自然数， $p^{21}={\left(n_{m0}^{21}\right)}^T{v}_2;$

iii.调整机械臂末端的位姿，使其相对于第二标定平面P2的初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第二标定平面P2上，固定这时的位姿，记为第二标定平面P2的第二位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第二位姿点的第二标定平面P2的单位法向量其中，为在机械臂末端坐标系下，从第二标定平面P2的原始位姿点到第二位姿点的旋转矩阵，并且

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

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

得到第二标定平面P2的第二组约束，其中，j为≥2的自然数， $p^{22}={\left(n_{m0}^{22}\right)}^T{v}_2;$

v.调整机械臂末端的位姿，使其相对于第二标定平面P2的初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第二标定平面P2上，固定这时的位姿使其不同于第二标定平面P2的初始位姿点和第二位姿点，记为第二标定平面P2的第三位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第三位姿点的第二标定平面的单位法向量其中，为在机械臂末端坐标系下，从第二标定平面P2的原始位姿点到第三位姿点的旋转矩阵，并且 $R_{m0}^{23}={\left(R_{b0}^{21}\right)}^{-1}{R}_{b0}^{23};$

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

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

得到第二标定平面P2的第三组约束，其中，j为≥2的自然数， $p^{23}={\left(n_{m0}^{23}\right)}^T{v}_2$

vii.使用上面三组第二标定平面P2的约束和单位向量模的约束求解出和p²¹、p²²和p³³的值；

viii.由和求解出由第二标定平面P2求得的激光束的单位方向向量v₂；

d)测量并求解由第二标定平面P2求得的平移向量r₂：

控制机械臂相对于第二标定平面P2的初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪发出的激光束照射在第二标定平面P2上，记录第j次旋转平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取旋转矩阵和平移列向量满足约束方程：

${\left(n_{m0}^{21}\right)}^T(R_{m0j}^{24}-I)r_2+(d_j^{24}{\left(n_{m0}^{21}\right)}^T{R}_{m0j}^{24}-d_0^{21}{\left(n_{m0}^{21}\right)}^T)v_2+{\left(n_{m0}^{21}\right)}^T{T}_{m0j}^{24}=0$

其中，为在机械臂末端坐标系下初始位姿点的第二标定平面的单位法向量，在步骤c)中已经得到， $T_{m0j}^{24}={\left(R_{b0}^{21}\right)}^{-1}(T_{bj}^{24}-T_{b0}^{21}),R_{m0j}^{24}={\left(R_{b0}^{21}\right)}^{-1}{R}_{bj}^{24},$ 对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第二标定平面P2求得的平移向量r₂。

e)测量并求解由第三标定平面P3求得的激光束的单位方向向量v₃：

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

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

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

得到第三标定平面P3的第一组约束，其中，j为≥2自然数， $p^{31}={\left(n_{m0}^{31}\right)}^T{v}_3;$

iii.调整机械臂末端的位姿，使其相对于第三标定平面P3的初始位姿点既有平移又有旋转运动，且激光测距仪发出的激光束照射在第三标定平面P3上，固定这时的位姿且激光测距仪，记为第三标定平面P3的第二位姿点，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第二位姿点的第三标定平面P3的单位法向量其中，为在机械臂末端坐标系下，从第三标定平面P3的原始位姿点到第二位姿点的旋转矩阵，并且

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

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

得到第三标定平面P3的第二组约束，其中，j为≥2的自然数， $p^{32}={\left(n_{m0}^{32}\right)}^T{v}_3;$

v.调整机械臂末端的位姿，使其相对于第三标定平面P3的初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于第三标定平面P3的初始位姿点和第二位姿点，记为第三标定平面P3的第三位姿点，且激光测距仪发出的激光束照射在第三标定平面P3上，记录这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量在机械臂末端坐标系下第三位姿点的第三标定平面P3的单位法向量其中，为在机械臂末端坐标系下，从第三标定平面P3的原始位姿点到第三位姿点的旋转矩阵，并且 $R_{m0}^{33}={\left(R_{b0}^{31}\right)}^{-1}{R}_{b0}^{33};$

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

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

得到第三标定平面P3的第三组约束，其中，j为≥2的自然数， $p^{33}={\left(n_{m0}^{33}\right)}^T{v}_3$

vii.使用上面三组第三标定平面P3的约束和单位向量模的约束求解出和p³¹、p³²和p³³的值；

viii.由和求解出由第三标定平面P3求得的激光束的单位方向向量v₃；

f)测量并求解由第三标定平面P3求得的平移向量r₃：

控制机械臂相对于第三标定平面P3的初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪发出的激光束照射在第三标定平面P3上，记录第j次旋转平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取旋转矩阵和平移列向量满足约束方程：

${\left(n_{m0}^{31}\right)}^T(R_{m0j}^{34}-I)r_3+(d_j^{34}{\left(n_{m0}^{31}\right)}^T{R}_{m0j}^{34}-d_0^{31}{\left(n_{m0}^{31}\right)}^T)v_3+{\left(n_{m0}^{31}\right)}^T{T}_{m0j}^{34}=0$

其中，1≤j≤k，k为≥3的自然数，为在机械臂末端坐标系下初始位姿点的第三标定平面P3的单位法向量，在步骤e)中已经得到， $T_{m0j}^{34}={\left(R_{b0}^{31}\right)}^{-1}(T_{bj}^{34}-T_{b0}^{31})R_{m0j}^{34}={\left(R_{b0}^{31}\right)}^{-1}{R}_{bj}^{34},$ 对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第三标定平面P3求得的平移向量r₃。

3.如权利要求1所述的标定方法，其特征在于，步骤3)中，优化计算平移向量r和激光束的单位方向向量v，具体包括以下步骤：

a)设定初始值：

对三个标定平面分别求得的平移向量取平均得到优化算法的平移向量r的初始值，即r的初始值为同理求得激光束的单位方向向量v的初始值，即v的初始值为

b)确定约束条件：

使用多个有约束的线性最小二乘法组成的非线性最小化算法，线性约束条件是非线性约束条件是标定平面的单位法向量的第三个分量大于零，即 $n_{m0}^{11}\left(3\right)>0,n_{m0}^{21}\left(3\right)>0,n_{m0}^{31}\left(3\right)>0;$

c)确定目标函数：

算法的目标函数为f＝f₁+f₂+f₃，其中f₁为与第一标定平面P1相关的约束函数，f₂为与第二标定平面P2相关的约束函数，f₃为与第三标定平面P3相关的约束函数；

d)迭代计算：

不断的迭代更新直至满足给定的收敛条件为止；

与第一标定平面P1相关的约束函数表示为f₁＝f₁₁+f₁₂+f₁₃+f₁₄，其中

$f_{11}=\left|\right|n_{m0}^{11}+{\[\underset{j}{\Σ}{T}_{m0j}^{11}{\left(T_{m0j}^{11}\right)}^T\mathrm{\]}}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{11}(d_j^{11}-d_0^{11})\mathrm{\]}{p}^{11}|{|}_2^2+||X^{11}-X_{fit}^{11}|{|}_2^2,$ X¹¹为步骤2)的a)的ii)中测量得到的点组成的矩阵，即 为X¹¹使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{12}=\left|\right|n_{m0}^{12}+{\[\underset{j}{\Σ}{T}_{m0j}^{12}{\left(T_{m0j}^{12}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{12}(d_j^{12}-d_0^{12})\]p^{12}|{|}_2^2+||X^{11}-X_{fit}^{12}|{|}_2^2,$ X¹²为步骤2)的a)的iv)中测量得到的点组成的矩阵，即 为X¹²使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{13}=\left|\right|n_{m0}^{13}+{\[\underset{j}{\Σ}{T}_{m0j}^{13}{\left(T_{m0j}^{13}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{13}(d_j^{13}-d_0^{13})\]p^{13}|{|}_2^2+||X^{13}-X_{fit}^{13}|{|}_2^2,$ X¹³为步骤2)的a)的vi)中测量得到的点组成的矩阵，即 为X¹³使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{14}=\left|\right|\left[\begin{array}{c}{p}^{11}-{\left(n_{m0}^{11}\right)}^{T}v\\ p^{12}-{\left(n_{m0}^{12}\right)}^{T}v\\ p^{13}-{\left(n_{m0}^{13}\right)}^{T}v\end{array}\right]|{|}_2^2+||X^{14}-X_{fit}^{14}|{|}_2^2,$ X¹⁴为步骤2)的b)中测量得到的点组成的矩阵，即 为X¹⁴使用主成分分析方法得到的主平面上对应的点组成的矩阵；

同样，与第二标定平面P2相关的函数f₂＝f₂₁+f₂₂+f₂₃+f₂₄，其中， $f_{21}=\mathrm{||}{n}_{m0}^{21}+{\[\underset{j}{\Σ}{T}_{m0j}^{21}{\left(T_{m0j}^{21}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{21}(d_j^{21}-d_0^{21})\mathrm{\]}{p}^{21}|{|}_2^2+||X^{21}-X_{fit}^{21}|{|}_2^2,$ X²¹为步骤2)的c)的ii)中测量得到的点组成的矩阵，即 为X²¹使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{22}=\mathrm{||}{n}_{m0}^{22}+{\[\underset{j}{\Σ}{T}_{m0j}^{22}{\left(T_{m0j}^{22}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{22}(d_j^{22}-d_0^{22})\mathrm{\]}{p}^{22}|{|}_2^2+||X^{22}-X_{fit}^{22}|{|}_2^2,$ X²²为步骤2)的c)的iv)中测量得到的点组成的矩阵，即 为X²²使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{23}=\left|\right|n_{m0}^{23}+{\[\underset{j}{\Σ}{T}_{m0j}^{23}{\left(T_{m0j}^{23}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{23}(d_j^{23}-d_0^{23})\]p^{23}|{|}_2^2+||X^{23}-X_{fit}^{23}|{|}_2^2,$ X²³为步骤2)的c)的vi)中测量得到的点组成的矩阵，即 为X²³使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{24}=\left|\right|\left[\begin{array}{c}{p}^{21}-{\left(n_{m0}^{21}\right)}^{T}v\\ p^{22}-{\left(n_{m0}^{22}\right)}^{T}v\\ p^{23}-{\left(n_{m0}^{23}\right)}^{T}v\end{array}\right]|{|}_2^2+||X^{24}-X_{fit}^{24}|{|}_2^2,$ X²⁴为步骤2)的d)中测量得到的点组成的矩阵，即 为X²⁴使用主成分分析方法得到的主平面上对应的点组成的矩阵；

同样，与第三标定平面P3相关的函数f₃＝f₃₁+f₃₂+f₃₃+f₃₄，其中，

$f_{31}=\left|\right|n_{m0}^{31}+{\[\underset{j}{\Σ}{T}_{m0j}^{31}{\left(T_{m0j}^{31}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{31}(d_j^{31}-d_0^{31})\]p^{31}|{|}_2^2+||X^{31}-X_{fit}^{31}|{|}_2^2,$ X³¹为步骤2)的e)的ii)中测量得到的点组成的矩阵，即 为X³¹使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{32}=\mathrm{||}{n}_{m0}^{32}+{\[\underset{j}{\Σ}{T}_{m0j}^{32}{\left(T_{m0j}^{32}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{32}(d_j^{32}-d_0^{32})\mathrm{\]}{p}^{32}|{|}_2^2+||X^{32}-X_{fit}^{32}|{|}_2^2,$ X³²为步骤2)的e)的iv)中测量得到的点组成的矩阵，即 为X³²使用主成分分析方法得到的主平面上对应的点组成的矩阵；

$f_{33}=\mathrm{||}{n}_{m0}^{33}+{\[\underset{j}{\Σ}{T}_{m0j}^{33}{\left(T_{m0j}^{33}\right)}^T\]}^{-1}\[\underset{j}{\Σ}{T}_{m0j}^{33}(d_j^{33}-d_0^{33})\mathrm{\]}{p}^{33}|{|}_2^2+||X^{33}-X_{fit}^{33}|{|}_2^2,$ X³³为步骤2)的e)的vi)中测量得到的点组成的矩阵，即 为X³³使用主成分分析方法得到的主平面上对应的点组成的矩阵；

其中的 $f_{34}=\left|\right|\left[\begin{array}{c}{p}^{31}-{\left(n_{m0}^{31}\right)}^{T}v\\ p^{32}-{\left(n_{m0}^{32}\right)}^{T}v\\ p^{33}-{\left(n_{m0}^{33}\right)}^{T}v\end{array}\right]|{|}_2^2+||X^{34}-X_{fit}^{34}|{|}_2^2,$ X³⁴为步骤2)的f)中测量得到的点组成的矩阵，即 为X³⁴使用主成分分析方法得到的主平面上对应的点组成的矩阵；

当算法收敛停止迭代后，得到优化的激光束的单位方向向量v和平移向量r。

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

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