面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置与流程

本发明涉及工业机器人示教领域，特别涉及面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置。

背景技术：

随着机器人技术的快速发展，机器人的机械结构变得越来越小型化、轻巧化以及柔性化，并且机器人的驱动系统控制系统以及编程语言也在不断优化，因此机器人越来越稳定性可靠、工作效率以及工艺精度也越来越高。此外，机器人在危险度高、环境恶劣的工作环境中表现出越来越不可代替的优越性。特别是机器人能够在打磨、焊接、装配、喷涂釉漆等传统的劳动密集型的工作环境之中长时间工作，并且无惧恶劣环境，所以机器人技术的应用对此类产业的转型升级发挥了关键作用。在工业应用领域，一般采用人工示教的方式使机器人重复再现动作从而完成指定的工作内容。所谓的人工示教的过程是指一种机器人执行通过示教编程存储起来的作业程序使其重复再现示教过程。而所述的示教编程是指由人工导引机器人末端执行器(安装于机器人关节结构末端的夹持器、工具、焊枪、喷枪等)，或由人工操作导引机械模拟装置，或用示教盒来使机器人完成预期的动作并存储为机器人可执行程序段。由于在机器人应用的大多数领域都需要借助人工示教的方式来指导和训练机器人，因此人工示教非常重要，是机器人在工业应用领域的一个关键步骤。一般人工示教是通过示教盒或示教臂方式来实施的，而这些方式的示教在操作上比较复杂，并且相关设备相对来说比较笨重，示教操作麻烦。其次，示教器械只能在有限空间进行示教，不能翻转到其对称面进行示教。最关键的是每一种机器人的机械臂长短不一，因此这些示教并不通用。此外，在示教末端体型大或产品曲面结构复杂(如在马桶等容器内壁喷涂)时，人工示教方式难以实施。另外一种较常用的人工示教方式是拖动柔顺示教。虽然这种示教方便，只需人手拖着末端执行器即可记录机器人移动轨迹，但是此类拖动示教系统复杂、需要高性能的力传感器配合。由于高性能传感器的价格以及整体系统开发成本相对于精度要求不是很高的喷釉、喷漆等喷涂作业而言过于高昂。为了解决上述方式所存在的问题，采用实时视觉位置姿态跟踪方式的示教系统通过采用对输入视频进行图像处理获取指定的定位点的三维坐标，并提取各个关节节点的控制信号从而现实对机器人进行人工示教。但是这种方式所采用的对输入视频的图像处理的计算复杂度高，需要的计算量较大，具有一定的延迟性，很难达到实时姿态跟踪，并且整个系统的鲁棒性容易受到工作环境的各种因素的影响。

技术实现要素：

本发明的主要目的在于提供一种面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置，旨在解决采用实时视觉位置姿态跟踪方式的示教系统对输入视频的图像处理的计算复杂度高，需要的计算量较大，具有一定的延迟性，很难达到实时姿态跟踪，并且整个系统的鲁棒性容易受到工作环境的各种因素的影响等，现有技术所存在的问题。

为实现上述目的，本发明提供的一种面向工业机器人的基于机器视觉和陀螺仪的示教方法，包括：

步骤s10、依据设定的时间间隔td从t0时刻至tf时刻的时间段之内获取：设置在工业机器人末端执行器上深红色小球中心的陀螺仪所采集的rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}，与所述图像序列{bt}对应的深度图像序列{zt}，所述下标t为采样时间，所述下标r代表roll轴,所述下标p代表pitch轴,所述下标y代表yaw轴，所述深度图像序列{zt}的每一幅深度图像zt的像素值zt(i,j)为对应所述图像序列{bt}的图像bt的像素bt(i,j)的深度值，所述像素bt(i,j)为包含rgb三通道灰度值的向量[brt(i,j),bgt(i,j),bbt(i,j)]，所述brt(i,j)为r通道的灰度值，所述bgt(i,j)为g通道的灰度值，所述bbt(i,j)为b通道的灰度值，所述下标(i,j)为所述像素bt(i,j)在所述图像bt对应像素位置，

步骤s20、将所述图像序列{bt}的每一幅所述图像bt进行二值化处理，从而获取所述图像bt内对应深红色小球的连通区域dt，

步骤s30、计算所述连通区域dt的中心像素的位置(ipt,jpt)，通过公式xtip＝(ipt-cx+δx)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的x轴坐标值xtip，通过公式ytjp＝(jpt-cy+δy)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的y轴坐标值ytjp，其中，所述cx和所述cy为所述相机的主点偏移量，所述δx和所述δy为径向畸变和切向畸变的补偿量，所述cx和所述cy，所述δx和所述δy由张氏标定法标定获得，将所述深红色小球的表面中心点的深度值zt(ipt,jpt)设置为所述深红色小球的表面中心点的z轴坐标值，所述f为所述相机的焦距，

步骤s40、根据所述深红色小球的表面中心点的所述位置坐标(xtip,ytjp，zt(ipt,jpt))随机选取所述深红色小球表面的不共线的四个点，所述四个点的位置坐标分别为：(xt1,yt1,zt1),(xt2,yt2,zt2),(xt3,yt3,zt3),(xt4,yt4,zt4)，通过克拉默法则求解下述线性方程组，从而获得所述深红色小球的球心坐标(xtc,ytc,ztc),

(xtc-xt1)²+(ytc-yt1)²+(ztc-zt1)²＝r²

(xtc-xt2)²+(ytc-yt2)²+(ztc-zt2)²＝r²

(xtc-xt3)²+(ytc-yt3)²+(ztc-zt3)²＝r²

(xtc-xt4)²+(ytc-yt4)²+(ztc-zt4)²＝r²

其中，所述r为所述深红色小球的半径，

步骤s50、将从所述t0时刻至所述tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(xtc,ytc,ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}之后，依次采用二次多项式

xc(t)＝a0+a1*t+a2*t²

yc(t)＝b0+b1*t+b2*t²

zc(t)＝c0+c1*t+c2*t²

对所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}进行拟合，依次采用三次多项式

θr(t)＝d0+d1*t+d2*t²+d3*t³

θp(t)＝e0+e1*t+e2*t²+e3*t³

θy(t)＝f0+f1*t+f2*t²+f3*t³

对所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}进行拟合。

优选地，在所述步骤s10设定的所述时间间隔td为40ms。

优选地，所述步骤s20包括：

步骤s21、将所述图像序列{bt}的每一幅所述图像bt转换为yuv颜色模式的图像ct，所述图像ct之内像素ct(i,j)为包含yuv三通道灰度值的向量[cyt(i,j),cut(i,j),cvt(i,j)]，所述cyt(i,j)为y通道的灰度值,所述cut(i,j)为u通道的灰度值,所述cvt(i,j)为v通道的灰度值，

步骤s22、当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)大于设定阈值k，设定所述灰度值brt(i,j)为255，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0，

步骤s23、当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)不大于所述设定阈值k，设定所述灰度值brt(i,j)为0，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0，

步骤s24、将全部所述灰度值brt(i,j)为255的像素归入所述深红色小球的连通区域dt。

优选地，在所述步骤s20设定的所述阈值k为80。

优选地，在所述步骤s50所进行的所述拟合方法是最小二乘法。

本发明进一步提供一种面向工业机器人的基于机器视觉和陀螺仪的示教装置，包括：

输入模块、用于依据设定的时间间隔td从t0时刻至tf时刻的时间段之内获取：设置在工业机器人末端执行器上深红色小球中心的陀螺仪所采集的rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}，与所述图像序列{bt}对应的深度图像序列{zt}，所述下标t为采样时间，所述下标r代表roll轴,所述下标p代表pitch轴,所述下标y代表yaw轴，所述深度图像序列{zt}的每一幅深度图像zt的像素值zt(i,j)为对应所述图像序列{bt}的图像bt的像素bt(i,j)的深度值，所述像素bt(i,j)为包含rgb三通道灰度值的向量[brt(i,j),bgt(i,j),bbt(i,j)]，所述brt(i,j)为r通道的灰度值，所述bgt(i,j)为g通道的灰度值，所述bbt(i,j)为b通道的灰度值，所述下标(i,j)为所述像素bt(i,j)在所述图像bt对应像素位置，

联通区域计算模块、用于将所述图像序列{bt}的每一幅所述图像bt进行二值化处理，从而获取所述图像bt内对应深红色小球的连通区域dt，

球表面中心点计算模块、用于计算所述连通区域dt的中心像素的位置(ipt,jpt)，通过公式xtip＝(ipt-cx+δx)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的x轴坐标值xtip，通过公式ytjp＝(jpt-cy+δy)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的y轴坐标值ytjp，其中，所述cx和所述cy为所述相机的主点偏移量，所述δx和所述δy为径向畸变和切向畸变的补偿量，所述cx和所述cy，所述δx和所述δy由张氏标定法标定获得，将所述深红色小球的表面中心点的深度值zt(ipt,jpt)设置为所述深红色小球的表面中心点的z轴坐标值，所述f为所述相机的焦距，

球心坐标计算模块、用于根据所述深红色小球的表面中心点的所述位置坐标(xtip,ytjp，zt(ipt,jpt))随机选取所述深红色小球表面的不共线的四个点，所述四个点的位置坐标分别为：(xt1,yt1,zt1),(xt2,yt2,zt2),(xt3,yt3,zt3),(xt4,yt4,zt4)，通过克拉默法则求解下述线性方程组，从而获得所述深红色小球的球心坐标(xtc,ytc,ztc),

(xtc-xt1)²+(ytc-yt1)²+(ztc-zt1)²＝r²

(xtc-xt2)²+(ytc-yt2)²+(ztc-zt2)²＝r²

(xtc-xt3)²+(ytc-yt3)²+(ztc-zt3)²＝r²

(xtc-xt4)²+(ytc-yt4)²+(ztc-zt4)²＝r²

其中，所述r为所述深红色小球的半径，

曲线拟合模块、用于将从所述t0时刻至所述tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(xtc,ytc,ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}之后，依次采用二次多项式

xc(t)＝a0+a1*t+a2*t²

yc(t)＝b0+b1*t+b2*t²

zc(t)＝c0+c1*t+c2*t²

对所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}进行拟合，依次采用三次多项式

θr(t)＝d0+d1*t+d2*t²+d3*t³

θp(t)＝e0+e1*t+e2*t²+e3*t³

θy(t)＝f0+f1*t+f2*t²+f3*t³

对所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}进行拟合。

优选地，在所述输入模块设定的所述时间间隔td设定为40ms。

优选地，所述联通区域计算模块包括：

颜色模式转换单元、用于将所述图像序列{bt}的每一幅所述图像bt转换为yuv颜色模式的图像ct，所述图像ct之内像素ct(i,j)为包含yuv三通道灰度值的向量[cyt(i,j),cut(i,j),cvt(i,j)]，所述cyt(i,j)为y通道的灰度值,所述cut(i,j)为u通道的灰度值,所述cvt(i,j)为v通道的灰度值，

第1二值化处理单元、当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)大于设定阈值k，设定所述灰度值brt(i,j)为255，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0，

第2二值化处理单元、当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)不大于所述设定阈值k，设定所述灰度值brt(i,j)为0，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0，

联通区域单元、将全部所述灰度值brt(i,j)为255的像素归入所述深红色小球的连通区域dt。

优选地，所述联通区域计算模块设定的所述阈值k为80。

优选地，在所述曲线拟合模块所进行的所述拟合方法是最小二乘法。

本发明通过设置在工业机器人末端执行器上深红色小球中心的陀螺仪获取所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，并通过作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}；与所述图像序列{bt}对应的深度图像序列{zt}，从而获取所述深红色小球中心的球心坐标序列{(xtc,ytc,ztc)}，并通过最小二乘法来对所获取的所述姿态角度数值序列和球心坐标序列进行曲线拟合，从而降低计算复杂度，减少计算量，达到实时处理以及姿态跟踪，并且大大提高整个系统的鲁棒性。

附图说明

图1为本发明方法第一实施例的流程示意图；

图2为本发明方法第二实施例的流程示意图；

图3为本发明装置第一实施例的功能模块示意图；

图4为本发明装置第二实施例的功能模块示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参照图1，图1为本发明第一实施例的流程示意图。所述方法包括：

步骤s10、输入处理。

即依据设定的时间间隔td从t0时刻至tf时刻的时间段之内获取：设置在工业机器人末端执行器上深红色小球中心的陀螺仪所采集的rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}，与所述图像序列{bt}对应的深度图像序列{zt}，所述下标t为采样时间，所述下标r代表roll轴,所述下标p代表pitch轴,所述下标y代表yaw轴，所述深度图像序列{zt}的每一幅深度图像zt的像素值zt(i,j)为对应所述图像序列{bt}的图像bt的像素bt(i,j)的深度值，所述像素bt(i,j)为包含rgb三通道灰度值的向量[brt(i,j),bgt(i,j),bbt(i,j)]，所述brt(i,j)为r通道的灰度值，所述bgt(i,j)为g通道的灰度值，所述bbt(i,j)为b通道的灰度值，所述下标(i,j)为所述像素bt(i,j)在所述图像bt对应像素位置。

其中，通过usb将陀螺仪的串口模块链接到pc并安装好驱动，并测试好陀螺仪模块的数据输出是否正常，并对其进行姿态校准。根据陀螺仪欧拉角的计算方法：角度＝高8位<<8|第8位(结果为实际角度乘以100)。因此通过读取陀螺仪模块所传输的每帧11字节数据，通过公式

θrt＝(bytet4*256+bytet5)/100.0

θpt＝(bytet6*256+bytet7)/100.0

θyt＝(bytet8*256+bytet9)/100.0

可获取rpy姿态角度，每个字节为一个8位二进制数据，其中，bytet0～bytet10为所述陀螺仪模块所传输的每帧11字节数据，bytet0～bytet3，以及bytet10分别为标志位、数据类型以及数据大小，在这里我们不做处理；通过上述公式，根据bytet4～bytet5、bytet6～bytet7、bytet8～bytet9可以分别计算出r、p、y为姿态角度。

由于采用三角测量原理的红外深度相机是目前工业界常用的进行深度信息测量的设备，例如目前流行的kinect2.0+xboxonekinect的开发套件已经被广泛应用到各种视觉场景。其原理也是众所周知，因此在此处不再详细叙述。

步骤s20、计算联通区域。

即将所述图像序列{bt}的每一幅所述图像bt进行二值化处理，从而获取所述图像bt内对应深红色小球的连通区域dt。在这里的二值化处理方法采用图像视觉处理常规的二值化算法，可根据光照强度，颜色参数来进行二值化处理。

步骤s30、计算球表面中心点。

即计算所述连通区域dt的中心像素的位置(ipt,jpt)，通过公式xtip＝(ipt-cx+δx)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的x轴坐标值xtip，通过公式ytjp＝(jpt-cy+δy)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的y轴坐标值ytjp，其中，所述cx和所述cy为所述相机的主点偏移量，所述δx和所述δy为径向畸变和切向畸变的补偿量，所述cx和所述cy，所述δx和所述δy由张氏标定法标定获得，将所述深红色小球的表面中心点的深度值zt(ipt,jpt)设置为所述深红色小球的表面中心点的z轴坐标值，所述f为所述相机的焦距。

步骤s40、计算球心坐标。

即根据所述深红色小球的表面中心点的所述位置坐标(xtip,ytjp，zt(ipt,jpt))随机选取所述深红色小球表面的不共线的四个点，所述四个点的位置坐标分别为：(xt1,yt1,zt1),(xt2,yt2,zt2),(xt3,yt3,zt3),(xt4,yt4,zt4)，通过克拉默法则求解下述线性方程组，从而获得所述深红色小球的球心坐标(xtc,ytc,ztc),

(xtc-xt1)²+(ytc-yt1)²+(ztc-zt1)²＝r²

(xtc-xt2)²+(ytc-yt2)²+(ztc-zt2)²＝r²

(xtc-xt3)²+(ytc-yt3)²+(ztc-zt3)²＝r²

(xtc-xt4)²+(ytc-yt4)²+(ztc-zt4)²＝r²

其中，所述r为所述深红色小球的半径。

步骤s50、曲线拟合。

即将从所述t0时刻至所述tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(xtc,ytc,ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}之后，依次采用二次多项式

xc(t)＝a0+a1*t+a2*t²

yc(t)＝b0+b1*t+b2*t²

zc(t)＝c0+c1*t+c2*t²

对所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}进行曲线拟合，依次采用三次多项式

θr(t)＝d0+d1*t+d2*t²+d3*t³

θp(t)＝e0+e1*t+e2*t²+e3*t³

θy(t)＝f0+f1*t+f2*t²+f3*t³

对所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}进行曲线拟合。

通过设置在工业机器人末端执行器上深红色小球中心的陀螺仪获取所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，并通过作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}；与所述图像序列{bt}对应的深度图像序列{zt}，从而获取所述深红色小球中心的球心坐标序列{(xtc,ytc,ztc)}，并通过最小二乘法来对所获取的所述姿态角度数值序列和球心坐标序列进行曲线拟合，从而不需要通过对图像进行解析捕获取姿态，从而避免基于图像的解析算法的精度不高，计算复杂而且计算量大的问题，并且采用与背景颜色不同的深红色小球来捕获运动轨迹，大大降低计算复杂度的同时也减少计算量，达到实时处理，并且大大提高系统的鲁棒性。同时采用曲线拟合的方法，可以采用拟合曲线的函数系数作为动轨迹以及姿态轨迹的参数，即降低了需要保存的轨迹数据量，同时也能在后续的应用过程使系统对运动轨迹以及姿态轨迹控制更加的简单和精确。

进一步，基于上述图1的实施例，在所述步骤s10设定的所述时间间隔td为40ms。

设定采样的时间间隔td为40ms既可以确保能对深红色小球进行实时轨迹和姿态跟踪，同时也能减少后续步骤的计算量，从而降低该方法对计算资源的最低要求，从而降低该方法的应用门槛，降低用户的应用成本。

参照图2，图2为本发明方法的第二实施例的流程示意图。如图2所示，基于上述图1的实施例，所述面向工业机器人的基于机器视觉和陀螺仪的示教方法的所述步骤s20包括：

步骤s21、颜色模式转换。

即将所述图像序列{bt}的每一幅所述图像bt转换为yuv颜色模式的图像ct，所述图像ct之内像素ct(i,j)为包含yuv三通道灰度值的向量[cyt(i,j),cut(i,j),cvt(i,j)]，所述cyt(i,j)为y通道的灰度值,所述cut(i,j)为u通道的灰度值,所述cvt(i,j)为v通道的灰度值。

步骤s25、判断所述图像ct的所述v通道的所述灰度值cvt(i,j)是否大于设定阈值k。当所述灰度值cvt(i,j)大于设定阈值k，执行步骤s22，否则执行步骤s23。

步骤s22、设定brt(i,j)为255，设定bgt(i,j)和bbt(i,j)为0。

即当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)大于设定阈值k，设定所述灰度值brt(i,j)为255，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0。步骤s23、设定brt(i,j)为255，设定bgt(i,j)和bbt(i,j)为0。

即当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)不大于所述设定阈值k，设定所述灰度值brt(i,j)为0，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0。步骤s24、将brt(i,j)为255的像素纳入连通区域dt。

即将全部所述灰度值brt(i,j)为255的像素归入所述深红色小球的连通区域dt。

由于rgb颜色空间对光强的敏感度较高，单纯通过对rgb通道的灰度值进行过滤来提取深红色小球的连通区域的效果不佳，而且鲁棒性不高。同时由于深红色在yuv三通道的v通道的敏感度最高，因此单独对v通道的灰度值设定阈值，再进行二值化处理，可以得到深红色小球的连通区域。综上所述，先将所述图像bt的rgb颜色空间转到yuv颜色空间的图像ct，然后通过设定的阈值k与所述图像ct的所述v通道的所述灰度值cvt(i,j)进行对比，从而将所述图像bt进行二值化处理，再通过遍历所述二值化图像的每一个像素，于是得到深红色小球的连通区域图dt。因此在降低了计算的复杂度的同时，并且提高了精度和鲁棒性。此外，还可以通过对先前计算的连通区域图dt-1的中心点为中心的150*150像素区域为搜索范围来计算连通区域图dt的中心点，从而节省计算时间。

进一步，基于上述图1或图2的实施例，在所述步骤s20设定的所述阈值k为80。经过实验验证，当所述阈值k为80时，可以得到最好的性能结果。

进一步，基于上述图1或图2的实施例，在所述步骤s50所进行的所述拟合方法是最小二乘法。

即将球心在x轴，y轴和z轴的球心坐标用以采样时间t为变量的二次多项式：

xc(t)＝a0+a1*t+a2*t²

yc(t)＝b0+b1*t+b2*t²

zc(t)＝c0+c1*t+c2*t²

来近似。然后依次将从t0时刻至tf时刻的时间段之内所有的采样时间t带入上述二次多项式，从而获得的坐标序列{(xc(t),yc(t),zc(t))}，再与实际的获得的坐标序列{(xtc,ytc,ztc)}与的差值平方和最小，即求解

s＝σt(xc(t)-xtc)²+σt(yc(t)-ytc)²+σt(zc(t)-ztc)²

的最小值，也就是分别将s与系数a0、a1、a2、b0、b1、b2、c0、c1、c2求偏导，并将偏导值设为零，即

δs/δa0＝0

δs/δa1＝0

δs/δa2＝0

δs/δb0＝0

δs/δb1＝0

δs/δb2＝0

δs/δc0＝0

δs/δc1＝0

δs/δc2＝0

通过求解上述公式，既可以得到所述二次多项式的系数a0、a1、a2、b0、b1、b2、c0、c1、c2。

同理，将深红色小球的rpy姿态角度用以采样时间t为变量的三次次多项式：

rpy姿态角度

θr(t)＝d0+d1*t+d2*t²+d3*t³

θp(t)＝e0+e1*t+e2*t²+e3*t³

θy(t)＝f0+f1*t+f2*t²+f3*t³

来近似。然后依次将从t0时刻至tf时刻的时间段之内所有的采样时间t带入上述三次多项式，从而分别获得角度序列{θr(t)},{θp(t)},{θy(t)}，再与实际的获得的角度序列{θrt}，{θpt}，{θyt}差值平方和最小，即求解

l＝σt(θr(t)-θrt)²+σt(θp(t)-θpt)²+σt(θy(t)-θyt)²

的最小值，也就是分别将l与系数d0、d1、d2、d3、e0、e1、e2、e3、f0、f1、f2、f3求偏导，并将偏导值设为零，即

δl/δd0＝0

δl/δd1＝0

δl/δd2＝0

δl/δd3＝0

δl/δe0＝0

δl/δe1＝0

δl/δe2＝0

δl/δe3＝0

δl/δf0＝0

δl/δf1＝0

δl/δf2＝0

δl/δf3＝0

通过求解上述公式，既可以得到所述二次多项式的系数d0、d1、d2、d3、e0、e1、e2、e3、f0、f1、f2、f3。

可以将上一个时间段的最后一个采样时刻可作为紧接着的下一时间段的第一个采样时刻，从而实现分段拟合。所述t0时刻至tf时刻的时间段设定为1秒，即每一秒更新上述所有系数。

采用上述最小二乘法来进行曲线拟合，具有计算简单，计算量不大的优点，而且拟合的效果好。并且所采用拟合曲线的多项式函数系数作为动轨迹以及姿态轨迹的参数，即降低了需要保存的轨迹数据量，同时也能在后续的应用过程使系统对运动轨迹以及姿态轨迹控制更加的简单和精确。

上述本发明面向工业机器人的基于机器视觉和陀螺仪的示教方法的第一实施例中的面向工业机器人的基于机器视觉和陀螺仪的示教方法可以由本发明面向工业机器人的基于机器视觉和陀螺仪的示教装置的第一实施例所提供的面向工业机器人的基于机器视觉和陀螺仪的示教装置来实现。

参照图3，图3为本发明面向工业机器人的基于机器视觉和陀螺仪的示教装置100的第一实施例提供一种面向工业机器人的基于机器视觉和陀螺仪的示教装置的功能模块示意图。如图4所示，所述面向工业机器人的基于机器视觉和陀螺仪的示教装置100包括：

输入模块10、用于依据设定的时间间隔td从t0时刻至tf时刻的时间段之内获取：设置在工业机器人末端执行器上深红色小球中心的陀螺仪所采集的rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}，与所述图像序列{bt}对应的深度图像序列{zt}，所述下标t为采样时间，所述下标r代表roll轴,所述下标p代表pitch轴,所述下标y代表yaw轴，所述深度图像序列{zt}的每一幅深度图像zt的像素值zt(i,j)为对应所述图像序列{bt}的图像bt的像素bt(i,j)的深度值，所述像素bt(i,j)为包含rgb三通道灰度值的向量[brt(i,j),bgt(i,j),bbt(i,j)]，所述brt(i,j)为r通道的灰度值，所述bgt(i,j)为g通道的灰度值，所述bbt(i,j)为b通道的灰度值，所述下标(i,j)为所述像素bt(i,j)在所述图像bt对应像素位置。其中，通过usb将陀螺仪的串口模块链接到pc并安装好驱动，并测试好陀螺仪模块的数据输出是否正常，并对其进行姿态校准。根据陀螺仪欧拉角的计算方法：角度＝高8位<<8|第8位(结果为实际角度乘以100)。因此通过读取陀螺仪模块所传输的每帧11字节数据，通过公式

θrt＝(bytet4*256+bytet5)/100.0

θpt＝(bytet6*256+bytet7)/100.0

θyt＝(bytet8*256+bytet9)/100.0

可获取rpy姿态角度，每个字节为一个8位二进制数据，其中，bytet0～bytet10为所述陀螺仪模块所传输的每帧11字节数据，bytet0～bytet3，以及bytet10分别为标志位、数据类型以及数据大小，在这里我们不做处理；通过上述公式，根据bytet4～bytet5、bytet6～bytet7、bytet8～bytet9可以分别计算出r、p、y为姿态角度。

由于采用三角测量原理的红外深度相机是目前工业界常用的进行深度信息测量的设备，例如目前流行的kinect2.0+xboxonekinect的开发套件已经被广泛应用到各种视觉场景。其原理也是众所周知，因此在此处不再详细叙述。

输入模块20、用于将所述图像序列{bt}的每一幅所述图像bt进行二值化处理，从而获取所述图像bt内对应深红色小球的连通区域dt。在这里的二值化处理方法采用图像视觉处理常规的二值化算法，可根据光照强度，颜色参数来进行二值化处理。

球表面中心点计算模块30、用于计算所述连通区域dt的中心像素的位置(ipt,jpt)，通过公式xtip＝(ipt-cx+δx)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的x轴坐标值xtip，通过公式ytjp＝(jpt-cy+δy)*zt(ipt,jpt)/f计算所述深红色小球的表面中心点的y轴坐标值ytjp，其中，所述cx和所述cy为所述相机的主点偏移量，所述δx和所述δy为径向畸变和切向畸变的补偿量，所述cx和所述cy，所述δx和所述δy由张氏标定法标定获得，将所述深红色小球的表面中心点的深度值zt(ipt,jpt)设置为所述深红色小球的表面中心点的z轴坐标值，所述f为所述相机的焦距。

球心坐标计算模块40、用于根据所述深红色小球的表面中心点的所述位置坐标(xtip,ytjp，zt(ipt,jpt))随机选取所述深红色小球表面的不共线的四个点，所述四个点的位置坐标分别为：(xt1,yt1,zt1),(xt2,yt2,zt2),(xt3,yt3,zt3),(xt4,yt4,zt4)，通过克拉默法则求解下述线性方程组，从而获得所述深红色小球的球心坐标(xtc,ytc,ztc),

(xtc-xt1)²+(ytc-yt1)²+(ztc-zt1)²＝r²

(xtc-xt2)²+(ytc-yt2)²+(ztc-zt2)²＝r²

(xtc-xt3)²+(ytc-yt3)²+(ztc-zt3)²＝r²

(xtc-xt4)²+(ytc-yt4)²+(ztc-zt4)²＝r²

其中，所述r为所述深红色小球的半径。

曲线拟合模块50、用于将从所述t0时刻至所述tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(xtc,ytc,ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}之后，依次采用二次多项式

xc(t)＝a0+a1*t+a2*t²

yc(t)＝b0+b1*t+b2*t²

zc(t)＝c0+c1*t+c2*t²

对所述深红色小球的球心坐标序列{(xtc,ytc,ztc)}进行曲线拟合，依次采用三次多项式

θr(t)＝d0+d1*t+d2*t²+d3*t³

θp(t)＝e0+e1*t+e2*t²+e3*t³

θy(t)＝f0+f1*t+f2*t²+f3*t³

对所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}进行曲线拟合。

通过设置在工业机器人末端执行器上深红色小球中心的陀螺仪获取所述rpy姿态角度数值序列{θrt}，{θpt}，{θyt}，并通过作为视频输入的相机所采集的包含所述深红色小球的图像序列{bt}；与所述图像序列{bt}对应的深度图像序列{zt}，从而获取所述深红色小球中心的球心坐标序列{(xtc,ytc,ztc)}，并通过最小二乘法来对所获取的所述姿态角度数值序列和球心坐标序列进行曲线拟合，从而不需要通过对图像进行解析捕获取姿态，从而避免基于图像的解析算法的精度不高，计算复杂而且计算量大的问题，并且采用与背景颜色不同的深红色小球来捕获运动轨迹，大大降低计算复杂度的同时也减少计算量，达到实时处理，并且大大提高系统的鲁棒性。同时采用曲线拟合的方法，可以采用拟合曲线的函数系数作为动轨迹以及姿态轨迹的参数，即降低了需要保存的轨迹数据量，同时也能在后续的应用过程使系统对运动轨迹以及姿态轨迹控制更加的简单和精确。

进一步，基于上述图3的实施例，在所述输入模块10设定的所述时间间隔td设定为40ms。

设定采样的时间间隔td为40ms既可以确保能对深红色小球进行实时轨迹和姿态跟踪，同时也能减少后续步骤的计算量，从而降低该方法对计算资源的最低要求，从而降低该方法的应用门槛，降低用户的应用成本。

上述本发明面向工业机器人的基于机器视觉和陀螺仪的示教方法的第二实施例中的面向工业机器人的基于机器视觉和陀螺仪的示教方法可以由本发明面向工业机器人的基于机器视觉和陀螺仪的示教装置的第二实施例所提供的面向工业机器人的基于机器视觉和陀螺仪的示教装置来实现。

参照图4，图4为本发明面向工业机器人的基于机器视觉和陀螺仪的示教装置的第二实施例提供一种面向工业机器人的基于机器视觉和陀螺仪的示教装置的功能模块示意图。如图4所示，基于上述图4所示的实施例，所述面向工业机器人的基于机器视觉和陀螺仪的示教装置的所述联通区域计算模块20包括还包括：

颜色模式转换单元21、用于将所述图像序列{bt}的每一幅所述图像bt转换为yuv颜色模式的图像ct，所述图像ct之内像素ct(i,j)为包含yuv三通道灰度值的向量[cyt(i,j),cut(i,j),cvt(i,j)]，所述cyt(i,j)为y通道的灰度值,所述cut(i,j)为u通道的灰度值,所述cvt(i,j)为v通道的灰度值。

第1二值化处理单元22用于当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)大于设定阈值k，设定所述灰度值brt(i,j)为255，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0。

第2二值化处理单元23、用于当判断所述图像ct的所述v通道的所述灰度值cvt(i,j)不大于所述设定阈值k，设定所述灰度值brt(i,j)为0，设定所述灰度值bgt(i,j)和所述灰度值bbt(i,j)为0。

联通区域单元24、用于将全部所述灰度值brt(i,j)为255的像素归入所述深红色小球的连通区域dt。

由于rgb颜色空间对光强的敏感度较高，单纯通过对rgb通道的灰度值进行过滤来提取深红色小球的连通区域的效果不佳，而且鲁棒性不高。同时由于深红色在yuv三通道的v通道的敏感度最高，因此单独对v通道的灰度值设定阈值，再进行二值化处理，可以得到深红色小球的连通区域。综上所述，先将所述图像bt的rgb颜色空间转到yuv颜色空间的图像ct，然后通过设定的阈值k与所述图像ct的所述v通道的所述灰度值cvt(i,j)进行对比，从而将所述图像bt进行二值化处理，再通过遍历所述二值化图像的每一个像素，于是得到深红色小球的连通区域图dt。因此在降低了计算的复杂度的同时，并且提高了精度和鲁棒性。此外，还可以通过对先前计算的连通区域图dt-1的中心点为中心的150*150像素区域为搜索范围来计算连通区域图dt的中心点，从而节省计算时间。

进一步，基于上述图3或图4的实施例，在所述联通区域计算模块20设定的所述阈值k为80。经过实验验证，当所述阈值k为80时，可以得到最好的性能结果。

进一步，基于上述图3或图4的实施例，在所述曲线拟合模块50所进行的所述拟合方法是最小二乘法。

即将球心在x轴，y轴和z轴的球心坐标用以采样时间t为变量的二次多项式：

xc(t)＝a0+a1*t+a2*t²

yc(t)＝b0+b1*t+b2*t²

zc(t)＝c0+c1*t+c2*t²

来近似。然后依次将从t0时刻至tf时刻的时间段之内所有的采样时间t带入上述二次多项式，从而获得的坐标序列{(xc(t),yc(t),zc(t))}，再与实际的获得的坐标序列{(xtc,ytc,ztc)}与的差值平方和最小，即求解

s＝σt(xc(t)-xtc)²+σt(yc(t)-ytc)²+σt(zc(t)-ztc)²

的最小值，也就是分别将s与系数a0、a1、a2、b0、b1、b2、c0、c1、c2求偏导，并将偏导值设为零，即

δs/δa0＝0

δs/δa1＝0

δs/δa2＝0

δs/δb0＝0

δs/δb1＝0

δs/δb2＝0

δs/δc0＝0

δs/δc1＝0

δs/δc2＝0

通过求解上述公式，既可以得到所述二次多项式的系数a0、a1、a2、b0、b1、b2、c0、c1、c2。

同理，将深红色小球的rpy姿态角度用以采样时间t为变量的三次次多项式：rpy姿态角度

θr(t)＝d0+d1*t+d2*t²+d3*t³

θp(t)＝e0+e1*t+e2*t²+e3*t³

θy(t)＝f0+f1*t+f2*t²+f3*t³

来近似。然后依次将从t0时刻至tf时刻的时间段之内所有的采样时间t带入上述三次多项式，从而分别获得角度序列{θr(t)},{θp(t)},{θy(t)}，再与实际的获得的角度序列{θrt}，{θpt}，{θyt}差值平方和最小，即求解

l＝σt(θr(t)-θrt)²+σt(θp(t)-θpt)²+σt(θy(t)-θyt)²

的最小值，也就是分别将l与系数d0、d1、d2、d3、e0、e1、e2、e3、f0、f1、f2、f3求偏导，并将偏导值设为零，即

δl/δd0＝0

δl/δd1＝0

δl/δd2＝0

δl/δd3＝0

δl/δe0＝0

δl/δe1＝0

δl/δe2＝0

δl/δe3＝0

δl/δf0＝0

δl/δf1＝0

δl/δf2＝0

δl/δf3＝0

通过求解上述公式，既可以得到所述二次多项式的系数d0、d1、d2、d3、e0、e1、e2、e3、f0、f1、f2、f3。

可以将上一个时间段的最后一个采样时刻可作为紧接着的下一时间段的第一个采样时刻，从而实现分段拟合。所述t0时刻至tf时刻的时间段设定为1秒，即每一秒更新上述所有系数。

采用上述最小二乘法来进行曲线拟合，具有计算简单，计算量不大的优点，而且拟合的效果好。并且所采用拟合曲线的多项式函数系数作为动轨迹以及姿态轨迹的参数，即降低了需要保存的轨迹数据量，同时也能在后续的应用过程使系统对运动轨迹以及姿态轨迹控制更加的简单和精确。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

显然，本领域的技术人员应该明白，上述的本发明的各模块单元或各步骤可以用通用的计算装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如rom/ram、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。