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

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

【技术实现步骤摘要】
激光测距仪和机械臂末端关系的单平面标定方法
本专利技术涉及视觉测量领域，具体涉及激光测距仪和机械臂末端关系的单平面标定方法。
技术介绍
随着电子工业水平的提高，计算机技术和光学传感器技术的发展，使得机器视觉在工业加工、航空、汽车、船舶、娱乐、医疗等领域都有广泛的应用。为了利用机械臂完成立体定向电极植入手术，需要将机械臂的坐标系和病人的三维空间坐标进行配准。而配准的基础是建立机械臂末端(比如爪手、法兰盘等)和传感器(相机、结构光设备、激光测距仪等)之间的变换关系，求解这个变换关系的问题称为手眼标定问题。对机械臂的手眼标定问题，已经有不少学者提出相应的解决办法，Zhuang将机械臂和传感器一起建模，用于机械臂运动学参数校正，Tsai将手眼标定分为旋转和平移两个部分分别求解，Ma采用相互正交的纯平移来线性化手眼矩阵的旋转部分，对旋转部分的求解需要三次有解的平移运动，Malm采用光流场的法向导数方法，Shiu等人将手眼标定问题归结为求解AX＝XB的问题。归纳起来机械臂手眼标定问题的类型主要有三种：1、使用高精度的已知尺寸的加工件和测量仪器对机械臂和传感器直接测量；2、使用摄像机的手眼标定，基于特征点、基于运动、立体视觉等；3、使用结构光，如一维的激光测距仪、二维的线激光和相机、面结构光和相机等，进行手眼标定。在文献[Theinternationaljournalofroboticsresearch,1995,14(3):195-210]中给出了通用的求解手眼标定方法，对它进行必要的修改可以适合相机、激光测距仪、结构光等不同的传感器，但具体如何修改使得它适应不同的传感器，需要读者做一定创造性探索。在文献[RoboticsandAutomation,IEEETransactionson,1998,14(3):493-497]中给出一般情况下激光测距仪和机械臂的手眼标定方法，但具体如何从将在机械臂坐标系下坐标的变换转换到激光测距仪坐标系下的变换，在该文献中并没有明确给出，要想实现它也需要读者做一定的创造性探索。并且当前的方法在理论上可以得到很高的精度，但是在实践中由于约束条件不足，导致求解手眼标定矩阵的可重复性较差。
技术实现思路
为了解决现有技术中存在的问题，本专利技术提出了一种激光测距仪和机械臂的空间变换关系的单平面标定方法，使用固定在机械臂末端的激光测距仪来实现对空间中物体上三维点的重建，具有测量精度高，价格相对低廉的优点。本专利技术的一个目的在于提出一种激光测距仪和机械臂末端的空间变换关系的单平面标定系统。本专利技术的激光测距仪和机械臂末端的空间变换关系的单平面标定系统包括：机械臂、机械臂末端、激光测距仪以及标定平面；其中，激光测距仪固定在机械臂末端，二者相对位置不变；在空间中放置标定平面，标定平面相对于机械臂的位置固定，标定平面的表面平整；机械臂连接至机械臂的控制器，从机械臂的控制器中直接读取出机械臂末端坐标系相对于机械臂坐标系的旋转矩阵和平移列向量。本专利技术的另一个目的在于提出一种激光测距仪和机械臂末端的空间变换关系的单平面标定方法。本专利技术的激光测距仪和机械臂末端的空间变换关系的单平面标定方法，包括以下步骤：1)机械臂末端坐标系为Om，激光测距仪坐标系为OL，激光测距仪发出的激光束照射到标定平面上的一个点，激光测距仪坐标系相对于机械臂末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系为v，机械臂末端坐标系下激光束照射到标定平面上的点为x，激光测距仪测量得到的距离为d，即在激光测距仪坐标系下，激光测距仪坐标系的原点到激光束照射到标定平面的点的距离为d，根据机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点表示为：x＝r+dv，其中，r为平移向量，v为激光束的单位方向向量，二者未知，下面分别求得r和v；2)测量并求解得到激光束的单位方向向量v：a)控制机械臂运动，激光测距仪发出的激光束照射在标定平面上，保持机械臂不动，记录这时的激光测距仪测量的距离d0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rb0和平移列向量Tb0，记为初始位姿点；b)保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离dj，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rbj和平移列向量Tbj，平移后测量得到第一组平动数据，在机械臂末端坐标系下初始位姿点对应的标定平面的单位方向向量nm0满足：得到第一组约束，其中，j为自然数，c)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离d'0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R'b0和平移列向量T'b0，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量其中，R'm0为在机械臂末端坐标系下，从初始位姿点到第二位姿点的旋转矩阵，并且d)保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d'j，并且直接从机械臂的控制器中读取旋转矩阵R'bj和平移列向量T'bj，平移后测量得到第二组平动数据，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量n'm0满足：(d'j-d'0)p'＝-(n'm0)TT'm0j得到第二组约束，其中，T'm0j＝(R'b0)-1(T'bj-T'b0)，p'＝(n'm0)Tv；e)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离d″0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R″b0和平移列向量T″b0，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量其中，R″m0为在机械臂末端坐标系下，从初始位姿点到第三位姿点的旋转矩阵，并且f)保持姿态不改变，机械臂末端相对于第三位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d″j，并且直接从机械臂的控制器中读取旋转矩阵R″bj和平移列向量T″bj，平移后测量得到第三组平动数据，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量n″m0满足：(d″j-d″0)p″＝-(n″m0)TT″m0j得到第三组约束，其中，T″m0j＝(R″b0)-1(T″bj-T″b0)，p″＝(n″m0)Tvg)使用上面三组约束和单位向量模的约束求解出nm0和p值；h)由求解出激光束的单位方向向量v；3)测量并求解得到平移向量r：控制机械臂相对于初始位姿点做旋转平移运动，旋转平移k次，分别记录第j次旋转平移后的第j位置的激光测距仪测量的距离d″′j，并且直接从机械臂的控制器中读取旋转矩阵R″′bj和平移列向量T″′bj，满足约束方程：其中，1≤j≤k，k为≥3的自然数，nm0为在机械臂末端坐标系下初始位姿点的标定平面的单位方向向量，步骤2)中已经得到，对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立求解得到平移向量r；4)对于激光束照射到标本文档来自技高网...

【技术保护点】
一种激光测距仪和机械臂末端的空间变换关系的单平面标定方法，其特征在于，所述标定方法，包括以下步骤：1)机械臂末端坐标系为Om，激光测距仪坐标系为OL，激光测距仪发出的激光束照射到标定平面上的一个点，激光测距仪坐标系相对于机械臂末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系为v，机械臂末端坐标系下激光束照射到标定平面上的点为x，激光测距仪测量得到的距离为d，即在激光测距仪坐标系下，激光测距仪坐标系的原点到激光束照射到标定平面的点的距离为d，根据机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点表示为：x＝r+dv，其中，r为平移向量，v为激光束的单位方向向量，二者未知，下面分别求得r和v；2)测量并求解得到激光束的单位方向向量v：a)控制机械臂运动，激光测距仪发出的激光束照射在标定平面上，保持机械臂不动，记录这时的激光测距仪测量的距离d0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rb0和平移列向量Tb0，记为初始位姿点；b)保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离dj，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rbj和平移列向量Tbj，平移后测量得到第一组平动数据，在机械臂末端坐标系下初始位姿点对应的标定平面的单位方向向量nm0满足：(dj-d0)p=-nm0TTm0j]]>得到第一组约束，其中，j为自然数，Tm0j=Rb0-1(Tbj-Tb0),p=nm0Tv;]]>c)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离d'0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R'b0和平移列向量T′b0，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量其中，R'm0为在机械臂末端坐标系下从初始位姿点到第二位姿点的旋转矩阵，并且Rm0′=Rb0-1Rb0′;]]>d)保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d'j，并且直接从机械臂的控制器中读取旋转矩阵R′bj和平移列向量T′bj，平移后测量得到第二组平动数据，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量n'm0满足：(d'j‑d'0)p'＝‑(n'm0)TT′m0j得到第二组约束，其中，T′m0j＝(R′b0)‑1(T′bj‑T′b0)，p'＝(n'm0)Tv；e)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离d″0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R″b0和平移列向量T″b0，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量其中，R″m0为在机械臂末端坐标系下，从初始位姿点到第三位姿点的旋转矩阵，并且f)保持姿态不改变，机械臂末端相对于第三位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d″j，并且直接从机械臂的控制器中读取旋转矩阵R″bj和平移列向量T″bj，平移后测量得到第三组平动数据，在机械臂末端坐标系下第三位姿点对应的标定平面的单位方向向量n”m0满足：(d″j‑d″0)p”＝‑(n”m0)TT″m0j得到第三组约束，其中，T″m0j＝(R″b0)‑1(T″bj‑T″b0)，p”＝(n″m0)Tvg)使用上面三组约束和单位向量模的约束求解出nm0和p值；h)由求解出激光束的单位方向向量v；3)测量并求解得到平移向量r：控制机械臂相对于初始位姿点做旋转平移运动，旋转平移k次，分别记录第j次旋转平移后的第j位置的激光测距仪测量的距离d'”j，并且直接从机械臂的控制器中读取旋转矩阵R”'bj和平移列向量T”'bj，满足约束方程：nm0T(Rm0j′-I)r+(dj′′′nm0TRm0j′′′-d0nm0T)v+nm0TTm0j′′′=0]]>其中，1≤j≤k，k为≥3的自然数，nm0为在机械臂末端坐标系下初始位姿点的标定平面的单位方向向量，步骤2)中已经得到，对于每一次的旋转平移，分别列出约束方程，一共k个约束方程，k个约束方程联立求解得到平移向量r；4)对于激光束照射到标定平面上的点，激光测距仪测量得到的距离为d，根据步骤2)和步骤3)得到的激光束的单位方向向量v和平移向量r，在机械臂末端坐标系下，该...

【技术特征摘要】
1.一种激光测距仪和机械臂末端的空间变换关系的单平面标定方法，其特征在于，所述标定方法，包括以下步骤：1)机械臂末端坐标系为Om，激光测距仪坐标系为OL，激光测距仪发出的激光束照射到标定平面上的一个点，激光测距仪坐标系相对于机械臂末端坐标系只发生平移r，激光测距仪发出的激光束的单位方向向量在激光测距仪坐标系为v，机械臂末端坐标系下激光束照射到标定平面上的点为x，激光测距仪测量得到的距离为d，即在激光测距仪坐标系下，激光测距仪坐标系的原点到激光束照射到标定平面的点的距离为d，根据机械臂末端坐标系和激光测距仪坐标系的关系，对于照射到标定平面上的点表示为：x＝r+dv，其中，r为平移向量，v为激光束的单位方向向量，二者未知，下面分别求得r和v；2)测量并求解得到激光束的单位方向向量v：a)控制机械臂运动，激光测距仪发出的激光束照射在标定平面上，保持机械臂不动，记录这时的激光测距仪测量的距离d0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rb0和平移列向量Tb0，记为初始位姿点；b)保持姿态不改变，机械臂末端相对于初始位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离dj，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵Rbj和平移列向量Tbj，平移后测量得到第一组平动数据，在机械臂末端坐标系下初始位姿点对应的标定平面的单位方向向量nm0满足：得到第一组约束，其中，j为自然数，c)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿，记为第二位姿点，记录这时的激光测距仪测量的距离d'0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R'b0和平移列向量T′b0，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量其中，R'm0为在机械臂末端坐标系下从初始位姿点到第二位姿点的旋转矩阵，并且d)保持姿态不改变，机械臂末端相对于第二位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d'j，并且直接从机械臂的控制器中读取旋转矩阵R′bj和平移列向量T′bj，平移后测量得到第二组平动数据，在机械臂末端坐标系下第二位姿点的标定平面的单位方向向量n'm0满足：(d'j-d'0)p'＝-(n'm0)TT′m0j得到第二组约束，其中，T′m0j＝(R′b0)-1(T′bj-T′b0)，p'＝(n'm0)Tv；e)调整机械臂末端的位姿，使其相对于初始位姿点既有平移又有旋转运动，固定这时的位姿使其不同于初始位姿点和第二位姿点，记为第三位姿点，记录这时的激光测距仪测量的距离d″0，并且直接从机械臂的控制器中读取机械臂末端坐标系相对于机械臂坐标系的旋转矩阵R″b0和平移列向量T″b0，在机械臂末端坐标系下第三位姿点的标定平面的单位方向向量其中，R″m0为在机械臂末端坐标系下，从初始位姿点到第三位姿点的旋转矩阵，并且f)保持姿态不改变，机械臂末端相对于第三位姿点只做平移运动，记录第j次平移后的第j位置的激光测距仪测量的距离d″j，并且直接从机械臂的控制器中读取旋转矩阵R″bj和平移列向量T″bj，平移后...

【专利技术属性】
技术研发人员：王广志，曾柏伟，丁辉，刘文博，吴迪，
申请(专利权)人：华科精准北京医疗科技有限公司，清华大学，
类型：发明
国别省市：北京;11