面向工業機器人的基于機器視覺和陀螺儀的示教方法及裝置與流程

本發明涉及工業機器人示教領域，特別涉及面向工業機器人的基于機器視覺和陀螺儀的示教方法及裝置。

背景技術：

隨著機器人技術的快速發展，機器人的機械結構變得越來越小型化、輕巧化以及柔性化，并且機器人的驅動系統控制系統以及編程語言也在不斷優化，因此機器人越來越穩定性可靠、工作效率以及工藝精度也越來越高。此外，機器人在危險度高、環境惡劣的工作環境中表現出越來越不可代替的優越性。特別是機器人能夠在打磨、焊接、裝配、噴涂釉漆等傳統的勞動密集型的工作環境之中長時間工作，并且無懼惡劣環境，所以機器人技術的應用對此類產業的轉型升級發揮了關鍵作用。在工業應用領域，一般采用人工示教的方式使機器人重復再現動作從而完成指定的工作內容。所謂的人工示教的過程是指一種機器人執行通過示教編程存儲起來的作業程序使其重復再現示教過程。而所述的示教編程是指由人工導引機器人末端執行器(安裝于機器人關節結構末端的夾持器、工具、焊槍、噴槍等)，或由人工操作導引機械模擬裝置，或用示教盒來使機器人完成預期的動作并存儲為機器人可執行程序段。由于在機器人應用的大多數領域都需要借助人工示教的方式來指導和訓練機器人，因此人工示教非常重要，是機器人在工業應用領域的一個關鍵步驟。一般人工示教是通過示教盒或示教臂方式來實施的，而這些方式的示教在操作上比較復雜，并且相關設備相對來說比較笨重，示教操作麻煩。其次，示教器械只能在有限空間進行示教，不能翻轉到其對稱面進行示教。最關鍵的是每一種機器人的機械臂長短不一，因此這些示教并不通用。此外，在示教末端體型大或產品曲面結構復雜(如在馬桶等容器內壁噴涂)時，人工示教方式難以實施。另外一種較常用的人工示教方式是拖動柔順示教。雖然這種示教方便，只需人手拖著末端執行器即可記錄機器人移動軌跡，但是此類拖動示教系統復雜、需要高性能的力傳感器配合。由于高性能傳感器的價格以及整體系統開發成本相對于精度要求不是很高的噴釉、噴漆等噴涂作業而言過于高昂。為了解決上述方式所存在的問題，采用實時視覺位置姿態跟蹤方式的示教系統通過采用對輸入視頻進行圖像處理獲取指定的定位點的三維坐標，并提取各個關節節點的控制信號從而現實對機器人進行人工示教。但是這種方式所采用的對輸入視頻的圖像處理的計算復雜度高，需要的計算量較大，具有一定的延遲性，很難達到實時姿態跟蹤，并且整個系統的魯棒性容易受到工作環境的各種因素的影響。

技術實現要素：

本發明的主要目的在于提供一種面向工業機器人的基于機器視覺和陀螺儀的示教方法及裝置，旨在解決采用實時視覺位置姿態跟蹤方式的示教系統對輸入視頻的圖像處理的計算復雜度高，需要的計算量較大，具有一定的延遲性，很難達到實時姿態跟蹤，并且整個系統的魯棒性容易受到工作環境的各種因素的影響等，現有技術所存在的問題。

為實現上述目的，本發明提供的一種面向工業機器人的基于機器視覺和陀螺儀的示教方法，包括：

步驟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、磁碟、光盤)中，包括若干指令用以使得一臺終端設備(可以是手機，計算機，服務器，空調器，或者網絡設備等)執行本發明各個實施例所述的方法。

以上僅為本發明的優選實施例，并非因此限制本發明的專利范圍，凡是利用本發明說明書及附圖內容所作的等效結構或等效流程變換，或直接或間接運用在其他相關的技術領域，均同理包括在本發明的專利保護范圍內。