CN107170345A - 面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置 - Google Patents

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

Info

Publication number
CN107170345A
CN107170345A CN201710232670.XA CN201710232670A CN107170345A CN 107170345 A CN107170345 A CN 107170345A CN 201710232670 A CN201710232670 A CN 201710232670A CN 107170345 A CN107170345 A CN 107170345A
Authority
CN
China
Prior art keywords
peony
bead
gray value
sequence
described image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710232670.XA
Other languages
English (en)
Other versions
CN107170345B (zh
Inventor
陈新度
罗坚铭
吴磊
熊勋
李延年
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
GUANGZHOU ZSROBOT INTELLIGENT EQUIPMENT Co.,Ltd.
Original Assignee
Guangdong University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN201710232670.XA priority Critical patent/CN107170345B/zh
Publication of CN107170345A publication Critical patent/CN107170345A/zh
Application granted granted Critical
Publication of CN107170345B publication Critical patent/CN107170345B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G09—EDUCATION; CRYPTOGRAPHY; DISPLAY; ADVERTISING; SEALS
    • G09B—EDUCATIONAL OR DEMONSTRATION APPLIANCES; APPLIANCES FOR TEACHING, OR COMMUNICATING WITH, THE BLIND, DEAF OR MUTE; MODELS; PLANETARIA; GLOBES; MAPS; DIAGRAMS
    • G09B25/00—Models for purposes not provided for in G09B23/00, e.g. full-sized devices for demonstration purposes
    • G09B25/02—Models for purposes not provided for in G09B23/00, e.g. full-sized devices for demonstration purposes of industrial processes; of machinery
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Geometry (AREA)
  • Business, Economics & Management (AREA)
  • Educational Administration (AREA)
  • Educational Technology (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种面向工业机器人的基于机器视觉和陀螺仪的示教方法和装置。通过获取工业机器人末端执行器上背景颜色不同的深红色小球中心的陀螺仪的姿态角度数值序列{θRt}，{θPt}，{θYt}，以及深红色小球的坐标序列{(Xtc,Ytc,Ztc)}，并通过最小二乘法来对所获取的序列进行曲线拟合，并将拟合曲线的系数作为示教的动轨迹以及姿态轨迹的参数结果，从而避免单纯基于图像的解析算法的精度不高，计算复杂而且计算量大的问题，并且大大降低计算复杂度的同时也减少计算量，达到实时处理，提高***的鲁棒性。

Description

面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置
技术领域
本发明涉及工业机器人示教领域，特别涉及面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置。
背景技术
随着机器人技术的快速发展，机器人的机械结构变得越来越小型化、轻巧化以及柔性化，并且机器人的驱动***控制***以及编程语言也在不断优化，因此机器人越来越稳定性可靠、工作效率以及工艺精度也越来越高。此外，机器人在危险度高、环境恶劣的工作环境中表现出越来越不可代替的优越性。特别是机器人能够在打磨、焊接、装配、喷涂釉漆等传统的劳动密集型的工作环境之中长时间工作，并且无惧恶劣环境，所以机器人技术的应用对此类产业的转型升级发挥了关键作用。在工业应用领域，一般采用人工示教的方式使机器人重复再现动作从而完成指定的工作内容。所谓的人工示教的过程是指一种机器人执行通过示教编程存储起来的作业程序使其重复再现示教过程。而所述的示教编程是指由人工导引机器人末端执行器(安装于机器人关节结构末端的夹持器、工具、焊枪、喷枪等)，或由人工操作导引机械模拟装置，或用示教盒来使机器人完成预期的动作并存储为机器人可执行程序段。由于在机器人应用的大多数领域都需要借助人工示教的方式来指导和训练机器人，因此人工示教非常重要，是机器人在工业应用领域的一个关键步骤。一般人工示教是通过示教盒或示教臂方式来实施的，而这些方式的示教在操作上比较复杂，并且相关设备相对来说比较笨重，示教操作麻烦。其次，示教器械只能在有限空间进行示教，不能翻转到其对称面进行示教。最关键的是每一种机器人的机械臂长短不一，因此这些示教并不通用。此外，在示教末端体型大或产品曲面结构复杂(如在马桶等容器内壁喷涂)时，人工示教方式难以实施。另外一种较常用的人工示教方式是拖动柔顺示教。虽然这种示教方便，只需人手拖着末端执行器即可记录机器人移动轨迹，但是此类拖动示教***复杂、需要高性能的力传感器配合。由于高性能传感器的价格以及整体***开发成本相对于精度要求不是很高的喷釉、喷漆等喷涂作业而言过于高昂。为了解决上述方式所存在的问题，采用实时视觉位置姿态跟踪方式的示教***通过采用对输入视频进行图像处理获取指定的定位点的三维坐标，并提取各个关节节点的控制信号从而现实对机器人进行人工示教。但是这种方式所采用的对输入视频的图像处理的计算复杂度高，需要的计算量较大，具有一定的延迟性，很难达到实时姿态跟踪，并且整个***的鲁棒性容易受到工作环境的各种因素的影响。
发明内容
本发明的主要目的在于提供一种面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置，旨在解决采用实时视觉位置姿态跟踪方式的示教***对输入视频的图像处理的计算复杂度高，需要的计算量较大，具有一定的延迟性，很难达到实时姿态跟踪，并且整个***的鲁棒性容易受到工作环境的各种因素的影响等，现有技术所存在的问题。
为实现上述目的，本发明提供的一种面向工业机器人的基于机器视觉和陀螺仪的示教方法，包括：
步骤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)2+(Ytc-Yt1)2+(Ztc-Zt1)2＝R2
(Xtc-Xt2)2+(Ytc-Yt2)2+(Ztc-Zt2)2＝R2
(Xtc-Xt3)2+(Ytc-Yt3)2+(Ztc-Zt3)2＝R2
(Xtc-Xt4)2+(Ytc-Yt4)2+(Ztc-Zt4)2＝R2
其中，所述R为所述深红色小球的半径，
步骤S50、将从所述T0时刻至所述Tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(Xtc,Ytc,Ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}之后，依次采用二次多项式
Xc(t)＝a0+a1*t+a2*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
对所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}进行拟合，依次采用三次多项式
θR(t)＝d0+d1*t+d2*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
对所述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)2+(Ytc-Yt1)2+(Ztc-Zt1)2＝R2
(Xtc-Xt2)2+(Ytc-Yt2)2+(Ztc-Zt2)2＝R2
(Xtc-Xt3)2+(Ytc-Yt3)2+(Ztc-Zt3)2＝R2
(Xtc-Xt4)2+(Ytc-Yt4)2+(Ztc-Zt4)2＝R2
其中，所述R为所述深红色小球的半径，
曲线拟合模块、用于将从所述T0时刻至所述Tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(Xtc,Ytc,Ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}之后，依次采用二次多项式
Xc(t)＝a0+a1*t+a2*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
对所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}进行拟合，依次采用三次多项式
θR(t)＝d0+d1*t+d2*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
对所述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+Xbox One Kinect的开发套件已经被广泛应用到各种视觉场景。其原理也是众所周知，因此在此处不再详细叙述。
步骤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)2+(Ytc-Yt1)2+(Ztc-Zt1)2＝R2
(Xtc-Xt2)2+(Ytc-Yt2)2+(Ztc-Zt2)2＝R2
(Xtc-Xt3)2+(Ytc-Yt3)2+(Ztc-Zt3)2＝R2
(Xtc-Xt4)2+(Ytc-Yt4)2+(Ztc-Zt4)2＝R2
其中，所述R为所述深红色小球的半径。
步骤S50、曲线拟合。
即将从所述T0时刻至所述Tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(Xtc,Ytc,Ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}之后，依次采用二次多项式
Xc(t)＝a0+a1*t+a2*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
对所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}进行曲线拟合，依次采用三次多项式
θR(t)＝d0+d1*t+d2*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
对所述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*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
来近似。然后依次将从T0时刻至Tf时刻的时间段之内所有的采样时间t带入上述二次多项式，从而获得的坐标序列{(Xc(t),Yc(t),Zc(t))}，再与实际的获得的坐标序列{(Xtc,Ytc,Ztc)}与的差值平方和最小，即求解
S＝Σt(Xc(t)-Xtc)2+Σt(Yc(t)-Ytc)2+Σt(Zc(t)-Ztc)2
的最小值，也就是分别将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*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
来近似。然后依次将从T0时刻至Tf时刻的时间段之内所有的采样时间t带入上述三次多项式，从而分别获得角度序列{θR(t)},{θP(t)},{θY(t)}，再与实际的获得的角度序列{θRt}，{θPt}，{θYt}差值平方和最小，即求解
L＝Σt(θR(t)-θRt)2+Σt(θP(t)-θPt)2+Σt(θY(t)-θYt)2
的最小值，也就是分别将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+Xbox One Kinect的开发套件已经被广泛应用到各种视觉场景。其原理也是众所周知，因此在此处不再详细叙述。
输入模块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)2+(Ytc-Yt1)2+(Ztc-Zt1)2＝R2
(Xtc-Xt2)2+(Ytc-Yt2)2+(Ztc-Zt2)2＝R2
(Xtc-Xt3)2+(Ytc-Yt3)2+(Ztc-Zt3)2＝R2
(Xtc-Xt4)2+(Ytc-Yt4)2+(Ztc-Zt4)2＝R2
其中，所述R为所述深红色小球的半径。
曲线拟合模块50、用于将从所述T0时刻至所述Tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(Xtc,Ytc,Ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}之后，依次采用二次多项式
Xc(t)＝a0+a1*t+a2*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
对所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}进行曲线拟合，依次采用三次多项式
θR(t)＝d0+d1*t+d2*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
对所述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*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
来近似。然后依次将从T0时刻至Tf时刻的时间段之内所有的采样时间t带入上述二次多项式，从而获得的坐标序列{(Xc(t),Yc(t),Zc(t))}，再与实际的获得的坐标序列{(Xtc,Ytc,Ztc)}与的差值平方和最小，即求解
S＝Σt(Xc(t)-Xtc)2+Σt(Yc(t)-Ytc)2+Σt(Zc(t)-Ztc)2
的最小值，也就是分别将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*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
来近似。然后依次将从T0时刻至Tf时刻的时间段之内所有的采样时间t带入上述三次多项式，从而分别获得角度序列{θR(t)},{θP(t)},{θY(t)}，再与实际的获得的角度序列{θRt}，{θPt}，{θYt}差值平方和最小，即求解
L＝Σt(θR(t)-θRt)2+Σt(θP(t)-θPt)2+Σt(θY(t)-θYt)2
的最小值，也就是分别将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、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

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对应像素位置，
步骤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)2+(Ytc-Yt1)2+(Ztc-Zt1)2＝R2
(Xtc-Xt2)2+(Ytc-Yt2)2+(Ztc-Zt2)2＝R2
(Xtc-Xt3)2+(Ytc-Yt3)2+(Ztc-Zt3)2＝R2
(Xtc-Xt4)2+(Ytc-Yt4)2+(Ztc-Zt4)2＝R2
其中，所述R为所述深红色小球的半径，
步骤S50、将从所述T0时刻至所述Tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(Xtc,Ytc,Ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}之后，依次采用二次多项式
Xc(t)＝a0+a1*t+a2*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
对所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}进行拟合，依次采用三次多项式
θR(t)＝d0+d1*t+d2*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
对所述RPY姿态角度数值序列{θRt}，{θPt}，{θYt}进行拟合。
2.如权利要求1所述的面向工业机器人的基于机器视觉和陀螺仪的示教方法，其特征在于，在所述步骤S10设定的所述时间间隔Td为40ms。
3.如权利要求1或2所述的面向工业机器人的基于机器视觉和陀螺仪的示教方法，其特征在于，所述步骤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。
4.如权利要求3所述的面向工业机器人的基于机器视觉和陀螺仪的示教方法，其特征在于，在所述步骤S20设定的所述阈值K为80。
5.如权利要求1或2所述的面向工业机器人的基于机器视觉和陀螺仪的示教方法，其特征在于，在所述步骤S50所进行的所述拟合方法是最小二乘法。
6.一种面向工业机器人的基于机器视觉和陀螺仪的示教装置，其特征在于，包括：
输入模块、用于依据设定的时间间隔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)2+(Ytc-Yt1)2+(Ztc-Zt1)2＝R2
(Xtc-Xt2)2+(Ytc-Yt2)2+(Ztc-Zt2)2＝R2
(Xtc-Xt3)2+(Ytc-Yt3)2+(Ztc-Zt3)2＝R2
(Xtc-Xt4)2+(Ytc-Yt4)2+(Ztc-Zt4)2＝R2
其中，所述R为所述深红色小球的半径，
曲线拟合模块、用于将从所述T0时刻至所述Tf时刻的时间段之内计算所得的所述深红色小球的球心坐标(Xtc,Ytc,Ztc)按所述采样时间t组合为所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}之后，依次采用二次多项式Xc(t)＝a0+a1*t+a2*t2
Yc(t)＝b0+b1*t+b2*t2
Zc(t)＝c0+c1*t+c2*t2
对所述深红色小球的球心坐标序列{(Xtc,Ytc,Ztc)}进行拟合，依次采用三次多项式
θR(t)＝d0+d1*t+d2*t2+d3*t3
θP(t)＝e0+e1*t+e2*t2+e3*t3
θY(t)＝f0+f1*t+f2*t2+f3*t3
对所述RPY姿态角度数值序列{θRt}，{θPt}，{θYt}进行拟合。
7.如权利要求6所述的面向工业机器人的基于机器视觉和陀螺仪的示教装置，其特征在于，在所述输入模块设定的所述时间间隔Td设定为40ms。
8.如权利要求6或7所述的面向工业机器人的基于机器视觉和陀螺仪的示教装置，其特征在于，所述联通区域计算模块包括：
颜色模式转换单元、用于将所述图像序列{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。
9.如权利要求6或7所述的面向工业机器人的基于机器视觉和陀螺仪的示教装置，其特征在于，所述联通区域计算模块设定的所述阈值K为80。
10.如权利要求6或7所述的面向工业机器人的基于机器视觉和陀螺仪的示教装置，其特征在于，在所述曲线拟合模块所进行的所述拟合方法是最小二乘法。
CN201710232670.XA 2017-04-11 2017-04-11 面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置 Active CN107170345B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710232670.XA CN107170345B (zh) 2017-04-11 2017-04-11 面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710232670.XA CN107170345B (zh) 2017-04-11 2017-04-11 面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置

Publications (2)

Publication Number Publication Date
CN107170345A true CN107170345A (zh) 2017-09-15
CN107170345B CN107170345B (zh) 2019-07-19

Family

ID=59849174

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710232670.XA Active CN107170345B (zh) 2017-04-11 2017-04-11 面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置

Country Status (1)

Country Link
CN (1) CN107170345B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109558188A (zh) * 2017-09-27 2019-04-02 卡西欧计算机株式会社 显示控制方法、控制方法和存储介质
CN109754434A (zh) * 2018-12-27 2019-05-14 歌尔科技有限公司 相机标定方法、装置、用户设备及存储介质
CN111971955A (zh) * 2018-04-19 2020-11-20 索尼公司 接收装置、接收方法、发送装置和发送方法
CN116851930A (zh) * 2023-04-28 2023-10-10 华南智能机器人创新研究院 一种三维五轴曲面激光切割轨迹生成方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE202008014481U1 (de) * 2007-10-31 2009-01-15 Abb As Tragbares Roboterkontrollgerät zum Kontrollieren einer Bewegung eines Roboters
CN103959012A (zh) * 2011-12-06 2014-07-30 赫克斯冈技术中心 6自由度位置和取向确定
CN104924313A (zh) * 2015-05-13 2015-09-23 北京工业大学 具有模仿学习机制的手把手示教机械臂***及方法
CN105345823A (zh) * 2015-10-29 2016-02-24 广东工业大学 一种基于空间力信息的工业机器人自由驱动示教方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE202008014481U1 (de) * 2007-10-31 2009-01-15 Abb As Tragbares Roboterkontrollgerät zum Kontrollieren einer Bewegung eines Roboters
CN103959012A (zh) * 2011-12-06 2014-07-30 赫克斯冈技术中心 6自由度位置和取向确定
CN104924313A (zh) * 2015-05-13 2015-09-23 北京工业大学 具有模仿学习机制的手把手示教机械臂***及方法
CN105345823A (zh) * 2015-10-29 2016-02-24 广东工业大学 一种基于空间力信息的工业机器人自由驱动示教方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109558188A (zh) * 2017-09-27 2019-04-02 卡西欧计算机株式会社 显示控制方法、控制方法和存储介质
CN109558188B (zh) * 2017-09-27 2022-04-05 卡西欧计算机株式会社 显示控制方法、控制方法和存储介质
CN111971955A (zh) * 2018-04-19 2020-11-20 索尼公司 接收装置、接收方法、发送装置和发送方法
CN109754434A (zh) * 2018-12-27 2019-05-14 歌尔科技有限公司 相机标定方法、装置、用户设备及存储介质
CN109754434B (zh) * 2018-12-27 2023-08-29 歌尔科技有限公司 相机标定方法、装置、用户设备及存储介质
CN116851930A (zh) * 2023-04-28 2023-10-10 华南智能机器人创新研究院 一种三维五轴曲面激光切割轨迹生成方法及装置

Also Published As

Publication number Publication date
CN107170345B (zh) 2019-07-19

Similar Documents

Publication Publication Date Title
CN107170345A (zh) 面向工业机器人的基于机器视觉和陀螺仪的示教方法及装置
Lobos-Tsunekawa et al. Visual navigation for biped humanoid robots using deep reinforcement learning
CN106650630B (zh) 一种目标跟踪方法及电子设备
Skaar et al. Camera-space manipulation
Li Human–robot interaction based on gesture and movement recognition
Courbon et al. Autonomous navigation of vehicles from a visual memory using a generic camera model
CN109544629A (zh) 摄像头位姿确定方法和装置以及电子设备
CN110298854B (zh) 基于在线自适应与单目视觉的飞行蛇形臂协同定位方法
Dang et al. Vision based ground object tracking using AR. Drone quadrotor
CN103914855B (zh) 一种运动目标的定位方法及装置
CN113103235B (zh) 一种基于rgb-d图像对柜体表面设备进行垂直操作的方法
CN110722533B (zh) 轮式移动机器人外参数无标定视觉伺服跟踪
CN113284192A (zh) 运动捕捉方法、装置、电子设备以及机械臂控制***
CN107831791A (zh) 一种无人机的操控方法、装置、操控设备及存储介质
Nahar et al. Autonomous UAV forced graffiti detection and removal system based on machine learning
CN113858217A (zh) 多机器人交互的三维视觉位姿感知方法及***
Dong et al. Simultaneous pose and correspondence determination combining softassign and orthogonal iteration
CN107066654A (zh) 面向点云模型的包容盒面的轨迹控制点拾取方法和装置
CN109531578B (zh) 仿人机械手臂体感控制方法及装置
JP7487118B2 (ja) １つ以上のロボット相互参照パラグラフの自然ロボットタスクのためのシステム及び方法
CN114964268B (zh) 一种无人机导航方法及装置
Piponidis et al. Towards a Fully Autonomous UAV Controller for Moving Platform Detection and Landing
CN110134234A (zh) 一种三维物体定位的方法及装置
JPH1151611A (ja) 認識対象物体の位置姿勢認識装置および位置姿勢認識方法
Tian et al. A real-time football goalkeeper robot system based on fuzzy logic control

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20201209

Address after: Room 101, No.6 Fangda Road, YUNPU Industrial Zone, Huangpu District, Guangzhou City, Guangdong Province 510000

Patentee after: GUANGZHOU ZSROBOT INTELLIGENT EQUIPMENT Co.,Ltd.

Address before: 510006 No. 100 West Ring Road, Guangzhou University, Guangzhou, Guangdong, Panyu District

Patentee before: GUANGDONG University OF TECHNOLOGY

TR01 Transfer of patent right