激光测距仪和机械臂末端关系的正交平面标定方法及系统的制作方法
【技术领域】
[0001] 本发明设及视觉测量领域，具体设及一种激光测距仪和机械臂末端的空间变换关 系的正交平面标定方法及系统。
【背景技术】
[0002] 随着电子工业水平的提高，计算机技术和光学传感器技术的发展，使得机器视觉 在工业加工、航空、汽车、船舶、娱乐、医疗等领域都有广泛的应用。
[0003] 为了利用机械臂完成立体定向电极植入手术，需要将机械臂的坐标系和病人的Ξ 维空间坐标进行配准。而配准的基础是建立机械臂末端（比如爪手、法兰盘等)和传感器(相 机、结构光设备、激光测距仪等)之间的变换关系，求解运个变换关系的问题称为手眼标定 问题。
[0004] 对机械臂的手眼标定问题，已经有不少学者提出相应的解决办法，Zhuang将机械 臂和传感器一起建模，用于机械臂运动学参数校正，Tsai将手眼标定分为旋转和平移两个 部分分别求解，Ma采用相互正交的纯平移来线性化手眼矩阵的旋转部分，对旋转部分的求 解需要Ξ次有解的平移运动，Malm采用光流场的法向导数方法，SMu等人将手眼标定问题 归结为求解AX=XB的问题。
[000引 在文献[Theinternationaljournalofroboticsresearch, 1995,14(3): 195-210]中给出了通用的求解手眼标定方法，对它进行必要的修改可W适合相机、激光测距仪、 结构光等不同的传感器，但具体如何修改使得它适应不同的传感器，需要读者做一定创造 性探索。在文献[RoboticsandAutomation,IE邸Transactionson, 1998,14(3) :493-497]中给出一般情况下激光测距仪和机械臂的手眼标定方法，但具体如何从将在机械臂坐 标系下坐标的变换转换到激光测距仪坐标系下的变换，该文献[Roboticsand Automation,I邸ETransactionson, 1998,14(3) :493-497]中并没有明确给出，要想实现 它也需要读者做一定的创造性探索。并且当前的方法在理论上可W得到很高的精度，但是 在实践中由于约束条件不足，导致求解手眼标定矩阵的可重复性较差。
【发明内容】
[0006] 为了解决现有技术中存在的问题，本发明提出了一种激光测距仪和机械臂末端的 空间变换关系的正交平面标定方法及系统，使用固定在机械臂末端的激光测距仪来实现对 Ξ维空间中物体上点的重建，它具有测量精度高，价格相对低廉的优点。
[0007] 本发明的一个目的在于提出一种激光测距仪和机械臂末端的空间变换关系的正 交平面标定系统。
[0008] 本发明的激光测距仪和机械臂末端的空间变换关系的正交平面标定系统包括:机 械臂、机械臂末端、激光测距仪W及Ξ个相互正交的标定平面;其中，激光测距仪固定在机 械臂末端，二者相对位置不变;在空间中放置Ξ个相互正交的第一至第Ξ标定平面P1~P3， 标定平面相对于机械臂的位置固定，标定平面的表面平整;机械臂连接至机械臂的控制器， 从机械臂的控制器中直接读取出机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平 移列向量。
[0009]本发明的另一个目的在于提出一种激光测距仪和机械臂末端的空间变换关系的 正交平面标定方法。
[0010] 本发明的激光测距仪和机械臂末端的空间变换关系的正交平面标定方法，包括W 下步骤：
[0011] 1)建立空间变换关系：
[0012] 机械臂末端坐标系为Om，激光测距仪坐标系为化，激光测距仪坐标系相对于机械臂 末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系 下为V，机械臂末端坐标系下激光束照射到标定平面上的点为X，即机械臂末端坐标系的原 点到激光束照射到标定平面上的点表示为X，激光测距仪测量得到的距离为d，即在激光测 距仪坐标系下，激光测距仪坐标系的原点到激光束照射到标定平面的点X的距离为d，根据 机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点表示为:x=r+ dv，其中，r为平移向量，V为激光束的单位方向向量，二者未知，下面分别求得r和V;由第一 标定平面P1求得的激光束的单位方向向量为VI，平移向量为ri，由第二标定平面P2求得的激 光束的单位方向向量为V2,平移向量为K，由第Ξ标定平面P3求得的激光束的单位方向向量 为V3,平移向量为Γ3;
[0013] 2)通过控制机械臂运动，使得激光测距仪发出的激光束分别照射到第一至第Ξ标 定平面上，分别测量并求解得到VI和ri、V沸ηW及V3和η，其中，VI和η分别为由第一标定平 面Ρ1求得的激光束的单位方向向量和平移向量，V2和η分别为由第二标定平面Ρ2求得的激 光束的单位方向向量和平移向量，W及V3和η分别为由第Ξ标定平面Ρ3求得的激光束的单 位方向向量和平移向量；
[0014] 3)优化计算平移向量r和激光束的单位方向向量V:
[0015] 设定初始值、约束函数和目标函数，采用迭代算法，直至满足给定的收敛条件为 止，得到优化计算后的平移向量r和激光束的单位方向向量V，完成激光测距仪和机械臂末 端的空间变换关系的标定；
[0016] 4)对于激光束照射到Ξ维空间中物体的一个点，激光测距仪测量得到的距离为d，根据步骤3)得到的激光束的单位方向向量V和平移向量r，在机械臂末端坐标系下，该点表 示为X=r+dv，完成对Ξ维空间中物体上该点在机械臂末端坐标系下空间坐标的测量。
[0017]其中，在步骤2)中，由第一至第Ξ标定平面P1~P3,分别测量并求解得到VI和ri、V2 和Γ2W及V3和Γ3，包括W下步骤：
[0018] a)测量并求解由第一标定平面P1求得的激光束的单位方向向量VI:
[0019] i.控制机械臂运动，激光测距仪发出的激光束照射在第一标定平面P1上，保持机 械臂不动，记录运时的激光测距仪测量的距离瑞1，并且直接从机械臂的控制器中读取机械 臂末端坐标系相对于机械臂坐标系的旋转矩阵i?以和平移列向量巧。1，记为初始位姿点；
[0020] ii.保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，记录第j次平 移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末 端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量If，多次平移后测量得到第一 标定平面PI的第一组平动数据，在机械臂末端坐标系下初始位姿点的第一标定平面的单位 法向量端)满足：
[0021]
[0022] 得到第一标定平面P1的第一组约束，其中，j为含2的自然数，
[0023]iii.调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，且 激光测距仪发出的激光束照射在第一标定平面P1上，固定运时的位姿，记为第二位姿点，记 录运时的激光测距仪测量的距离诚2，并且直接从机械臂的控制器中读取机械臂末端坐标 系相对于机械臂坐标系的旋转矩阵每^和平移列向量瑞，在机械臂末端坐标系下第二位姿 点的第一标定平面的单位法向量
其中，S话为在机械臂末端坐标系下， 从原始位姿点到第二位姿点的旋转矩阵，并且
[0024]iv.保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，记录第j次平 移后的第j位置的激光测距仪测量的距离，并且直接从机械臂的控制器中读取旋转矩阵 巧;和平移列向量巧2,多次平移后测量得到第一标定平面P1的第二组平动数据，在机械臂 末端坐标系下第二位姿点的第一标定平面P1的单位法向量《5满足：
[0025]
[0026] 得到第一标定平面P1的第二组约束，其中，j为> 2的自然数
[0027] V.调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，且激 光测距仪发出的激光束照射在第一标定平面P1上，固定运时的位姿使其不同于初始位姿点 和第二位姿点，记为第Ξ位姿点，记录运时的激光测距仪测量的距离，并且直接从机械 臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵巧器和平移列向量 枯在机械臂末端坐标系下第Ξ位姿点的第一标定平面P1的单位法向量
其中，i?為为在机械臂末端坐标系下，从原始位姿点到第Ξ位姿点的旋转矩阵，并且
[0028] Vi.保持姿态不改变，机械臂末端相对于第Ξ位姿点只做平移运动，记录第j次平 移后的第j位置的激光测距仪测量的距离^3,并且直接从机械臂的控制器中读取旋转矩阵 時;'和平移列向量巧3,多次平移后测量得到第一标定平面PI的第Ξ组平动数据，在机械臂 末端坐标系下第Ξ位姿点的第一标定平面的单位法向量苗f。满足：
[0029]
[0030] 得到第一标定平面P1的第Ξ组约束，其中，j为> 2的自然数，.瑣/ =(媒Γ'(皆-域)，
[0031]vii.使用上面Ξ组第一标定平面P1的约束和单位向量模的约束I诚。If=1，求解出 城和pll、pl嘴pU的值；
[0032]viii.由
，求解出由第一标定平面P1 求得的激光束的单位方向向量VI;
[0033] b)测量并求解由第一标定平面P1求得的平移向量ri:
[0034] 控制机械臂相对于初始位姿点做旋转平移运动，至少旋转平移k次，且激光测距仪 发出的激光束照射在第一标定平面P1上，记录第j次旋转平移后的第j位置的激光测距仪测 量的距离并且直接从机械臂的控制器中读取旋转矩阵成,4和平移列向量3?4，满足约束 方程：
[0035]
[0036] 其中，1幻' ^，k为含3的自然数，端为在机械臂末端坐标系下初始位姿点的第一 标定平面P1的单位法向量，在步骤a)中已经得到，
对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第 一标定平面P1求得的平移向量ri;
[0037] C)测量并求解由第二标定平面P2求得的激光束的单位方向向量V2:
[0038] i.控制机械臂运动，激光测距仪发出的激光束照射在第二标定平面P2上，保持机 械臂不动，记录运时的激光测距仪测量的距离拆1，:并且直接从机械臂的控制器中读取机械 臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量记/，记为第二标定平面P2 的初始位姿点；
[0039] ii.保持姿态不改变，机械臂末端相对于第二标定平面P2的初始位姿点只做平移 运动，记录第j次平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制 器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵成/和平移列向量巧1，多次平 移后测量得到第二标定平面P2的第一组平动数据，在机械臂末端坐