定平面P2上，固定这时的位姿使其不同 于第二标定平面P2的初始位姿点和第二位姿点，记为第二标定平面P2的第三位姿点，记录 这时的激光测距仪测量的距离，并且直接从机械臂的控制器中读取机械臂末端坐标系 相对于机械臂坐标系的旋转矩阵和平移列向量rj，在机械臂末端坐标系下第三位姿点 的第二标定平面的单位法向量，其中，为在机械臂末端坐标系下，从 第二标定平面P2的原始位姿点到第三位姿点的旋转矩阵，并且vi .保持姿态不改变，机械臂末端相对于第二标定平面P2的第三位姿点只做平移运动， 记录第j次平移后的第j位置的激光测距仪测量的距离并且直接从机械臂的控制器中 读取旋转矩阵Λ:和平移列向量?，多次平移后测量得到第二标定平面P2的第三组平动数 据，在机械臂末端坐标系下第三位姿点的第二标定平面P2的单位法向量满足：得到第二标定平面P2的第三组约束，其中，j为2 2的自然数，P2i =(nfin)Tv2 vi i .使用上面三组第二标定平面P2的约束和单位向量模的约束，求解出π二和 p21、p22 和 P33 的值；vii i.由1求解出由第二标定平面P2求得 的激光束的单位方向向量v2; d) 测量并求解由第二标定平面P2求得的平移向量r2: 控制机械臂相对于第二标定平面P2的初始位姿点做旋转平移运动，至少旋转平移k次， 且激光测距仪发出的激光束照射在第二标定平面P2上，记录第j次旋转平移后的第j位置的 激光测距仪测量的距离rff，并且直接从机械臂的控制器中读取旋转矩阵和平移列向量 ?；f，满足约束方程：#中，为$机械臂末端4标系、下权^台姿点』勺第二标定￥胃白勺单法步马聚 c)中已经得到对于每一次的旋转平移，分别 列出约束方程，一共k个约束方程，k个约束方程联立得到由第二标定平面P2求得的平移向 量Γ2〇 e) 测量并求解由第三标定平面Ρ3求得的激光束的单位方向向量V3: i.控制机械臂运动，激光测距仪发出的激光束照射在第三标定平面P3上，记录这时的 激光测距仪测量的距离431，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于 机械臂坐标系的旋转矩阵和平移列向量，记为第三标定平面P3的初始位姿点； i i .保持姿态不改变，机械臂末端相对于第三标定平面P3的初始位姿点只做平移运动， 记录第j次平移后的第j位置的激光测距仪测量的距离f1，并且直接从机械臂的控制器中 读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵^?1.和平移列向量If，多次平移后 测量得到第三标定平面P3的第一组平动数据，在机械臂末端坐标系下初始位姿点的第三标 定平面的单位法向量^^满足：得到第三标定平面P3的第一组约束，其中，j为2 2自然数，iii.调整机械臂末端的位姿，使其相对于第三标定平面P3的初始位姿点既有平移又有 旋转运动，且激光测距仪发出的激光束照射在第三标定平面P3上，固定这时的位姿且激光 测距仪，记为第三标定平面P3的第二位姿点，记录这时的激光测距仪测量的距离df·，并且 直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平 移列向量拉卜在机械臂末端坐标系下第二位姿点的第三标定平面P3的单位法向量，其中为在机械臂末端坐标系下，从第三标定平面P3的原始位姿点 到第二位姿点的旋转矩阵，并且iv. 保持姿态不改变，机械臂末端相对于第三标定平面P3的第二位姿点只做平移运动， 记录第j次平移后的第j位置的激光测距仪测量的距离^并且直接从机械臂的控制器中 读取旋转矩阵和平移列向量，多次平移后测量得到第三标定平面P3的第二组平动数 据，在机械臂末端坐标系下第二位姿点的第三标定平面P3的单位法向量满足：得到第三标定平面P3的第二组约束，其中，j为2 2的自然数v. 调整机械臂末端的位姿，使其相对于第三标定平面P3的初始位姿点既有平移又有旋 转运动，固定这时的位姿使其不同于第三标定平面P3的初始位姿点和第二位姿点，记为第 三标定平面P3的第三位姿点，且激光测距仪发出的激光束照射在第三标定平面P3上，记录 这时的激光测距仪测量的距离并且直接从机械臂的控制器中读取机械臂末端坐标系 相对于机械臂坐标系的旋转矩阵和平移列向量Τ/θ3，在机械臂末端坐标系下第三位姿 点的第三标定平面Ρ3的单位法向量其中，及^为在机械臂末端坐标系 下，从第三标定平面Ρ3的原始位姿点到第三位姿点的旋转矩阵，并且vi .保持姿态不改变，机械臂末端相对于第三标定平面Ρ3的第三位姿点只做平移运动， 记录第j次平移后的第j位置的激光测距仪测量的距离^3，并且直接从机械臂的控制器中 读取旋转矩阵C和平移列向量If，多次平移后测量得到平移后测量的第三组平动数据， 在机械臂末端坐标系下第三位姿点的第三标定平面P3的单位法向量满足：得到第三标定平面P3的第三组约束，其中，j为2 2的自然数，Vii .使用上面三组第三标定平面P3的约束和单位向量模的约束 ，求解出c和p31、p32和p33的值； viii .由，求解出由第三标定平面P3求得 的激光束的单位方向向量v3; f)测量并求解由第三标定平面P3求得的平移向量r3: 控制机械臂相对于第三标定平面P3的初始位姿点做旋转平移运动，至少旋转平移k次， 且激光测距仪发出的激光束照射在第三标定平面P3上，记录第j次旋转平移后的第j位置的 激光测距仪测量的距离，并且直接从机械臂的控制器中读取旋转矩阵^^夂和平移列向 量?？，满足约束方程：其中，1 < j < k，k为2 3的自然数，^为在机械臂末端坐标系下初始位姿点的第三标定 平面P3的单位法向量，在步骤e)中已经得到对 于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立得到由第三 标定平面P3求得的平移向量r3。3.如权利要求1所述的标定方法，其特征在于，步骤3)中，优化计算平移向量r和激光束 的单位方向向量v，具体包括以下步骤： a) 设定初始值： 对三个标定平面分别求得的平移向量取平均得到优化算法的平移向量r的初始值，即r 的初始值为，同理求得激光束的单位方向向量v的初始值，即v的初始值为b) 确定约束条件： 使用多个有约束的线性最小二乘法组成的非线性最小化算法，线性约束条件是非线性约束条件是标定平面的单位法向量的第三个分 量大于零，ΕΦc) 确定目标函数： 算法的目标函数为fifi+fdft，其中fi为与第一标定平面P1相关的约束函数，f2为与第 二标定平面P2相关的约束函数，f3为与第三标定平面P3相关的约束函数； d) 迭代计算： 不断的迭代更新V,直至满足给定的收敛条件为止； 与第一标定平面P1相关的约束函数表示为fl = fll+fl2+fl3+fl4,其中X11为步骤2)的a)的 ii)中测量得到的点组成的矩阵，即为X11使用主成 分分析方法得到的主平面上对应的点组成的矩阵；，X12为步骤2)的a)的 iv)中测量得到的点组成的矩阵，即.为X12使用主 成分分析方法得到的主平面上对应的点组成的矩阵；.X13为步骤2)的a)的 vi)中测量得到的点组成的矩阵，即，夏^为#3使用主成 分分析方法得到的主平面上对应的点组成的矩阵；X14为步骤2)的b)中测量得到的点组成的矩阵，即1?:为X14使用主成分分析方法得到的主平面上对应 的点组成的矩阵； 同样，与第二标定平面P2相关的函数f2 = f21 + f22 + f23 + f24,其中，X21为步骤2)的C)的ii) 中测量得到的点组成的矩阵，即:为X21使用主成分 分析方法得到的主平面上对应的点组成的矩阵；的iv)中测量得到的点组成的矩阵，g卩，为X22使用 主成分分析方法得到的主平面上对应的点组成的矩阵；，x23为步骤2)的c) 的vi)中测量得到的点组成的矩阵，gp、^)为乂23使用 主成分分析方法得到的主平面上对应的点组成的矩阵；,Χ24为步骤2)的d)中测量得到的点组成的矩阵，即，X&为X24使用主成分分析方法得到的主平面上对应 的点组成的矩阵； 同样，与第三标定平面P3相关的函数f3 = f31+f32+f33+f34,其中，的ii)中测量得到的点组成的矩阵，即使用 主成分分析方法得到的主平面上对应的点组成的矩阵；的i v)中测量得到的点组成的矩阵，g卩为X32使用 主成分分析方法得到的主平面上对应的点组成的矩阵；^33为步骤2)的6)的 vi)中测量得到的点组成的矩阵，即为X33使用主 成分分析方法得到的主平面上对应的点组成的矩阵； 其中的，X34为步骤2)的f)中测量得到的点组成的 矩阵，即为X34使用主成分分析方法得到的主平面 上对应的点组成的矩阵； 当算法收敛停止迭代后，得到优化的激光束的单位方向向量V和平移向量r。4. 如权利要求1所述的标定方法，其特征在于，进一步，在完成空间变换关系标定后，激 光测距仪测量空间中的第j位置的点，在得到它的距离山和这时的机械臂的控制器中的读 出的机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rbj和平移列向量Tbj，由Xbj = RbjXmj +Tbj = Rbj(r+djv)+Tbj，求得该点在机械臂坐标系下的空间坐标值xbj，采集多个需要测量的 三维空间点，从而完成三维空间点的测量。5. -种激光测距仪和机械臂末端的空间变换关系的正交平面标定系统，其特征在于， 所述标定系统包括:机械臂、机械臂末端、激光测距仪以及三个相互正交的标定平面;其中， 激光测距仪固定在机械臂末端，二者相对位置不变;在空间中放置三个相互正交的第一至 第三标定平面P1~P3,标定平面相对于机械臂的位置固定，标定平面的表面平整;机械臂连 接至机械臂的控制器，从机械臂的控制器中直接读取出机械臂末端坐标系相对于机械臂坐 标系的旋转矩阵和平移列向量。
【专利摘要】本发明公开了一种激光测距仪和机械臂末端关系的正交平面标定方法及系统。本发明采用激光测距仪固定在机械臂末端，并在空间中放置相互正交的标定平面，通过机械臂的旋转平移运动带动激光测距仪的位姿改变，使用激光测距仪测量的距离d，求解平移向量r和激光束的单位方向向量v，完成激光测距仪和机械臂末端的空间变换关系标定，根据x＝r+dv，实现对三维空间中物体上点x的重建；明确给出了机械臂末端坐标系和激光测距仪坐标系的变换关系；使用正交平面作为标定平面和应用非线性优化方法充分利用点的共面性作为约束条件来求解空间变换关系的标定问题，从而增加约束条件减少误差，降低噪声干扰，提高方法的鲁棒性和易实施性，并提高空间变换关系的标定精度。
【IPC分类】G01B11/00
【公开号】CN105444672
【申请号】CN201510953856
【发明人】王广志, 曾柏伟, 丁辉, 刘文博, 吴迪
【申请人】华科精准（北京）医疗科技有限公司, 清华大学
【公开日】2016年3月30日
【申请日】2015年12月17日