基于红外立体视觉定位系统和机械臂的手眼标定方法与流程

本发明涉及机器人技术和光学传感器技术，具体地涉及基于红外立体视觉定位系统和机械臂的手眼标定方法。

背景技术

当今社会随着科学技术的发展，机器人技术和光学传感器技术在很多领域都有广泛的应用，特别是精准医疗领域。为了能利用机械臂帮助医生完成一些需要精准定位的手术，需要将机械臂所在的坐标系和病人所在的三维空间坐标系进行配准。这配准的基础是建立机械臂末端(如法兰盘，抓手等)和光学传感器设备(如相机、3d结构光定位器、激光测距仪等)之间的坐标系变换关系，求解这个变换关系的问题称为机器人的手眼标定问题。

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

传统的解决方法大多是结合激光测距仪和机械臂的手眼标定方法，这些方法着眼于以机械臂为机械变姿运动执行装置，以机械臂自身和激光传感器为测量装置，机械臂变姿带动激光传感器对若干空间固定点进行测量，分别记录机械臂的姿态和该空间点在机器人坐标系和激光传感器测量坐标系内的坐标值，通过一定算法求解传感器测量坐标系与机器人末端执行器坐标系之间的变换关系。相比于激光测距仪，由外部标记物和结构光传感器组成的红外立体视觉定位系统应用场景更丰富，应用起来更方便实用，精度也更高，多目标同时定位使得应用的手术场景更多。然而目前并没有解决方案能很好的解决由外部标记物和结构光传感器组成的红外立体视觉定位系统的机械臂手眼标定的问题，本发明弥补了这方面的空白。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种基于红外立体视觉定位系统和机械臂的手眼标定方法。

根据本发明提供的一种基于红外立体视觉定位系统和机械臂的手眼标定方法，利用红外立体视觉定位系统和机械臂进行手眼标定，其中，所述红外立体视觉定位系统包括外部标记物、红外传感器；外部标记物紧固安装在机械臂末端的法兰盘上。

优选地，所述外部标记物表面涂有反光材料，所述红外传感器内部集成有红外光发射接收装置、传感器。

优选地，所述机械臂包括控制器、示教器，控制器用于控制机械臂的机械变姿运动的执行。

根据本发明提供的一种基于红外立体视觉定位系统和机械臂的手眼标定方法，记机械臂坐标系、机械臂末端法兰盘坐标系、外部标记物的局部坐标系、红外立体视觉定位系统坐标系分别为{base}、{tcp}、{drf}、{tracker}，包括如下步骤：

步骤1：构建机械臂末端法兰盘坐标系{tcp}和机械臂坐标系{base}的变换关系

步骤2：构建机械臂末端法兰盘坐标系{tcp}与外部标记物的局部坐标系{drf}的变换关系

步骤3：构建机械臂末端法兰盘坐标系{tcp}和红外立体视觉定位系统坐标系{tracker}的变换关系

步骤4：利用和通过绝对定向方法求得机械臂坐标系{base}相对于红外立体视觉定位系统坐标系{tracker}的变换矩阵即实现基于红外立体视觉定位系统和机械臂的手眼标定；

其中，步骤1、步骤2、步骤3、步骤4依次执行，或者步骤2、步骤1、步骤3、步骤4依次执行。

优选地，所述步骤1具体为：

以机械臂基座的几何中心为原点obase，以机械臂正面前向为x轴正向，正面左向为y轴正向，正面垂直向上方向为z轴正向构建{base}坐标系；以机械臂末端的法兰盘几何中心点otcp为原点构建{tcp}坐标系；otcp在{base}坐标系中的位置描述为[x，y，z]，机械臂末端法兰盘坐标系{tcp}和机械臂坐标系{base}的变换关系用[x，y，z]来描述。

优选地，{tcp}坐标系的轴向的变化即机械臂末端的法兰盘朝向的变换描述为旋转矢量(rx，ry，rz)，所述步骤2包括如下步骤：

步骤2.1：控制机械臂以法兰盘几何中心点为原点进行绕轴旋转运动，即以法兰盘中心otcp为原点绕机械臂垂直轴向做绕轴旋转运动在{base}坐标系下可以描述为{tcp}坐标系的原点otcp不动，标记物绕过otcp点的平行于{base}坐标系的z轴的z′轴做圆锥状的运动；将机械臂绕轴旋转运动简化成做离散点的绕轴旋转运动，即离散点连起来组成一组绕轴旋转运动，但是每个离散点之间的运动并不要求是绕轴旋转运动；绕轴旋转运动用欧拉角(θz，θy，θx)来描述，具体欧拉角表示拆解为绕过otcp点的平行于{base}坐标系x轴的x′轴的横滚变换θx(r0ll)和绕过otcp点的平行于{base}坐标系y轴的y′轴的俯仰变换θy(pitch)的以及绕过otcp点的平行于{base}坐标系z轴的z′轴的首摇变换θz(yaw)组合，具体表达式如下：

θx(roll)＝sin(i*θ1)*θ2

θy(pitch)＝cos(i*θ1)*θ2

θz(yaw)＝0

其中，θ1表示每次转动的角度，i表示离散点的序号，i的取值范围为θ2表示与z′轴的夹角；

步骤2.2：通过罗德里格斯变换，将欧拉角(θz，θy，θx)转换成旋转矢量(rx，ry，rz)发送给机械臂控制器，控制机械臂移动到指定的离散点，红外立体视觉定位系统记录离散点的坐标数据；

步骤2.3：控制机械臂末端法兰盘绕固定点，即法兰盘几何中心点orcp的初始位置做旋转运动，采集一系列外部标记物动态参考系的刚体变换数据[ri，ti]i＝1：m，即外部标记物的局部坐标系{drf}的刚体变换数据，其中，ri表示i时刻的旋转变换，ti表示i时刻的平移变换，i∈[1，m]，m表示收集的旋转运动离散点的数目；

刚体变换数据[ri，ti]i＝1：m，外部标记物的局部坐标系{drf}原点到固定点的位移ｔ^drf，红外立体视觉定位系统坐标系{tracker}原点到固定点的位移ｔ^tracker三者之间的关系如下：

riｔ^drf+ti＝rjt^tracker+ｔj

其中，rj表示j时刻的旋转变换，ｔj表示j时刻的平移变换，j∈[1，m]，j≠i，t^drf通过下面的等式求出：

红外立体视觉定位系统坐标系{tracker}原点到固定点的位移t^tracker计算式如下：

t^tracker＝mean(riｔ^drf+ti)

其中，mean函数表示求平均数或者均值；

步骤2.4：机械臂末端法兰盘坐标系{tcp}与外部标记物的局部坐标系{drf}的变换关系求解方法如下所示：

简写为

其中，i表示3*3单位矩阵，ｔ^tracker[x]表示平移的具体变化，ｔ^tracker[0]表示x轴的变化，ｔ^tracker[1]表示y轴的变化，ｔ^tracker[2]表示z轴的变化。

优选地，所述步骤3具体为：

其中，由红外立体视觉定位系统得到。

优选地，所述步骤4包括如下步骤：

步骤4.1：通过随机数生成算法生成在机械臂坐标系{base}下空间中红外立体视觉定位系统可测范围内的由[xi，yi，zi]i＝1：n，(rx，ry，rz)i＝1：n描述的n个随机点，发送给控制器控制机械臂依次移动法兰盘中心点otcp到这n个点，然后从红外立体视觉定位系统中抓取出在该点处标记物在红外立体视觉定位系统的变换所以只取的位移部分

步骤4.2：将机械臂坐标系{base}下的位移即[xi，yi，zi]i＝1：n和红外立体视觉定位系统坐标系{tracker}下的位移通过绝对定向方法求得闭解式机械臂坐标系{base}相对于红外立体视觉定位系统坐标系{tracker}的变换矩阵即实现了基于红外立体视觉定位系统和机械臂的手眼标定。

优选地，一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现上述的方法的步骤。

与现有技术相比，本发明具有如下的有益效果：

1、本发明具有操作步骤简便、实用、灵活、精度高等特点。

2、本发明采用由外部标记物和红外光传感器组成的红外立体视觉定位系统，相较于激光测距仪应用场景更丰富，本发明配合多个外部标记物可实现多目标同时定位，再结合机械臂执行，实现了精准定位，在精准医疗领域方面有很大的应用前景。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的实施例的实施装置示意图。

图2为本发明坐标变换采集过程示意图。

图中示出：

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明是以机械臂为机械变姿运动执行装置，将红外立体视觉定位系统的外部标记物连接到机械臂末端的法兰盘，以红外立体视觉定位系统为测量装置，通过算法控制机械臂以法兰盘中心点为原点进行绕轴旋转运动带动标记物运动进行测量，记录标记物在红外立体视觉定位系统内的坐标值，通过一定算法求解出法兰盘中心点到标记物局部坐标系的变换，由此也就可以得到法兰盘中心点到红外立体视觉定位系统坐标系的变换。然后再通过算法控制机械臂移动法兰盘中心点在空间中一定范围内的随机点进行测量，分别记录该点在机器人坐标系和红外立体视觉定位系统坐标系内的坐标值，通过一定算法求解出机器人坐标系和红外立体视觉定位系统坐标系之间的变换关系，即进行由外部标记物和红外传感器组成的红外立体视觉定位系统与机械臂的手眼标定。

根据本发明提供的一种基于红外立体视觉定位系统和机械臂的手眼标定方法，尤其是利用由外部标记物和红外传感器组成的红外立体视觉定位系统与机械臂的基于红外立体视觉定位系统和机械臂的手眼标定方法，如图1所示，所述红外立体视觉定位系统包括外部标记物、红外传感器；外部标记物紧固安装在机械臂末端的法兰盘上；所述外部标记物表面涂有反光材料，所述红外传感器内部集成有红外光发射接收装置、传感器；所述机械臂包括控制器、示教器，控制器用于控制机械臂的机械变姿运动的执行。记机械臂坐标系、机械臂末端法兰盘坐标系、外部标记物的局部坐标系、红外立体视觉定位系统坐标系分别为{base}、{tcp}、{drf}、{tracker}，本发明包括如下步骤：

步骤1：构建机械臂末端法兰盘坐标系{tcp}和机械臂坐标系{base}的变换关系

步骤2：构建机械臂末端法兰盘坐标系{tcp}与外部标记物的局部坐标系{drf}的变换关系

步骤3：构建机械臂末端法兰盘坐标系{tcp}和红外立体视觉定位系统坐标系{tracker}的变换关系

步骤4：利用和通过绝对定向方法求得机械臂坐标系{base}相对于红外立体视觉定位系统坐标系{tracker}的变换矩阵即实现基于红外立体视觉定位系统和机械臂的手眼标定；

其中，步骤1、步骤2、步骤3、步骤4依次执行，或者步骤2、步骤1、步骤3、步骤4依次执行。

优选地，所述步骤1具体为：

以机械臂基座的几何中心为原点obase，以机械臂正面前向为x轴正向，正面左向为y轴正向，正面垂直向上方向为z轴正向构建{base}坐标系；以机械臂末端的法兰盘几何中心点otcp为原点构建{tcp}坐标系；otcp在{base}坐标系中的位置描述为[x，y，z]，机械臂末端法兰盘坐标系{tcp}和机械臂坐标系{base}的变换关系用[x，y，z]来描述。

优选地，{tcp}坐标系的轴向的变化即机械臂末端的法兰盘朝向的变换描述为旋转矢量(rx，ry，rz)，所述步骤2包括如下步骤：

步骤2.1：控制机械臂以法兰盘几何中心点为原点进行绕轴旋转运动，即以法兰盘中心otcp为原点绕机械臂垂直轴向做绕轴旋转运动在{base}坐标系下可以描述为{tcp}坐标系的原点otcp不动，标记物绕过otcp点的平行于{base}坐标系的z轴的z′轴做圆锥状的运动，也就是说以法兰盘中心otcp为原点绕机械臂垂直轴向做绕轴旋转运动在{base}坐标系下可以描述为{tcp}坐标系的原点otcp不动，绕过otcp平行于{base}坐标系的z轴的z′轴以圆锥状的旋转变换{tcp}坐标系的坐标轴；将机械臂绕轴旋转运动简化成做离散点的绕轴旋转运动，即离散点连起来组成一组绕轴旋转运动，但是每个离散点之间的运动并不要求是绕轴旋转运动；绕轴旋转运动用欧拉角(θz，θy，θx)来描述，具体欧拉角表示拆解为绕过otcp点的平行于{base}坐标系x轴的x′轴的横滚变换θx(roll)和绕过otcp点的平行于{base}坐标系y轴的y′轴的俯仰变换θy(pitch)的以及绕过otcp点的平行于{base}坐标系z轴的z′轴的首摇变换θz(yaw)组合，具体表达式如下：

θx(roll)＝sin(i*θ1)*θ2

θy(pitch)＝cos(i*θ1)*θ2

θz(yaw)＝0

其中，θ1表示每次转动的角度，i表示离散点的序号，i的取值范围为θ2表示与z′轴的夹角；

步骤2.2：通过罗德里格斯变换，将欧拉角(θz，θy，θx)转换成旋转矢量(rx，ry，rz)发送给机械臂控制器，控制机械臂移动到指定的离散点，红外立体视觉定位系统记录离散点的坐标数据；

步骤2.3：控制机械臂末端法兰盘绕固定点，即法兰盘几何中心点otcp的初始位置做旋转运动，采集一系列外部标记物动态参考系的刚体变换数据[ri，ti]i＝1：m，即外部标记物的局部坐标系{drf}的刚体变换数据，其中，ri表示i时刻的旋转变换，ti表示i时刻的平移变换，i∈[1，m]，m表示收集的旋转运动离散点的数目；

刚体变换数据[ri，ti]i＝1：m，外部标记物的局部坐标系{drf}原点到固定点的位移t^drf，红外立体视觉定位系统坐标系{tracker}原点到固定点的位移t^tracker三者之间的关系如下：

riｔ^drf+ti＝rj^ttracker+ｔj

其中，rj表示j时刻的旋转变换，ｔj表示j时刻的平移变换，j∈[1，m]，j≠i，ｔ^drf通过下面的等式求出：

红外立体视觉定位系统坐标系{tracker}原点到固定点的位移t^tracker计算式如下：

t^tracker＝mean(riｔ^drf+ti)

其中，mean函数表示求平均数或者均值；

步骤2.4：机械臂末端法兰盘坐标系{tcp}与外部标记物的局部坐标系{drf}的变换关系求解方法如下所示：

简写为

其中，i表示3*3单位矩阵，ｔ^tracker[x]表示平移的具体变化，ｔ^tracker[0]表示x轴的变化，ｔ^tracker[1]表示y轴的变化，ｔ^tracker[2]表示z轴的变化。

优选地，所述步骤3具体为：

其中，由红外立体视觉定位系统得到。

优选地，所述步骤4包括如下步骤：

步骤4.1：通过随机数生成算法生成在机械臂坐标系{base}下空间中红外立体视觉定位系统可测范围内的由[xi，yi，zi]i＝1：n，(rx，ry，rz)i＝1:n描述的n个随机点，优选地，n＝15，发送给控制器控制机械臂依次移动法兰盘中心点otcp到这n个点，然后从红外立体视觉定位系统中抓取出在该点处标记物在红外立体视觉定位系统的变换所以只取的位移部分

步骤4.2：将机械臂坐标系{base}下的位移即[xi，yi，zi]i＝1：n和红外立体视觉定位系统坐标系{tracker}下的位移通过绝对定向方法求得闭解式机械臂坐标系{base}相对于红外立体视觉定位系统坐标系{tracker}的变换矩阵即实现了基于红外立体视觉定位系统和机械臂的手眼标定。

优选地，一种存储有计算机程序的计算机可读存储介质，所述计算机程序被处理器执行时实现上述的方法的步骤。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。