專利名稱：一種光學(xué)成像式輪式移動機器人方位的檢測方法及其裝置的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種光學(xué)成像式輪式移動機器人方位的檢測方法及其裝置，屬于運動定位技術(shù)領(lǐng)域。
背景技術(shù)：
隨著社會發(fā)展和科技進步，機器人在當(dāng)前生產(chǎn)生活中得到了越來越廣泛的應(yīng)用。 輪式移動機器人由于其具有自重輕、承載大、機構(gòu)簡單、驅(qū)動和控制相對方便、行走速度快、 機動靈活、工作效率高等優(yōu)點，被大量應(yīng)用于工業(yè)、農(nóng)業(yè)、軍事、醫(yī)院、家庭、空間探測等領(lǐng)域。
輪式移動機器人要完成指定的任務(wù)，首先要在移動過程中實時感知當(dāng)前的位置和方位，并不斷與目標(biāo)位置和方位比較，控制移動機構(gòu)進行相應(yīng)的調(diào)整，最終到達目標(biāo)位置， 從而有效完成任務(wù)。檢測移動機器人位置和方位的方法分為兩類自主檢測法和參照物檢測法。參照物檢測方法要依賴磁條、路標(biāo)、信標(biāo)(如GPS信號)等外部參照物或參照信號來確定位置和方位，需要建立和維護參照物或參照信號；自主檢測方法不需要在外部設(shè)立參照物或參照信號，利用機器人相對地球表面的運動來確定位置和方位，檢測裝置全部安裝在機器人內(nèi)部，工作時不依賴外部信息，因此不易收到外界干擾的影響。
常用自主檢測法主要有兩種
(I)基于慣性傳感器的測量方法。依據(jù)牛頓慣性原理，利用安裝在移動機器人內(nèi)部的加速度計和陀螺，分別測量機器人本身相對地球表面的線加速度和角加速度，對測量結(jié)果進行積分，從而計算出機器人移動的距離及方位變化。其缺點是由于加速度的常值誤差將引起與時間平方成正比的測量誤差，因此任何小的常值誤差都會隨時間而無限增大，長期精度很差；每次使用之前需要較長的初始對準(zhǔn)時間；慣性測量設(shè)備的結(jié)構(gòu)復(fù)雜，價格較曰蟲印貝ο
(2)基于編碼器的測量方法。利用安裝在輪式移動機器人驅(qū)動系統(tǒng)中的編碼器分別測量左、右驅(qū)動輪的轉(zhuǎn)動角度，然后根據(jù)左、右驅(qū)動輪的轉(zhuǎn)動角度推算出左、右驅(qū)動輪走過的距離，最后估算出機器人的位置和方位。基于編碼器的測量方法結(jié)構(gòu)相對簡單、成本較低，但由于左、右驅(qū)動輪的實際直徑不完全相等、有效輪距也在隨時變化，因此基于編碼器的測量方法的常值誤差和測量誤差也會隨時間不斷積累，長期精度也很差。如果出現(xiàn)驅(qū)動輪打滑現(xiàn)象，基于編碼器的測量方法完全失效。
上述兩種位置和方位自主測量方法獲得的測量結(jié)果是間接測量值?；趹T性傳感器的測量結(jié)果是根據(jù)慣性傳感器的加速度直接測量值間接計算出來的，測量結(jié)果不僅與慣性傳感器加速度有關(guān)，而且還與時間的平方成正比；基于編碼器的測量結(jié)果是根據(jù)編碼器角位移直接測量值間接計算出來的，測量結(jié)果不僅與編碼器角位移有關(guān)，而且還與驅(qū)動系統(tǒng)傳動比、驅(qū)動輪直徑及輪間距等參數(shù)有關(guān)。因此，時間、驅(qū)動系統(tǒng)傳動比、驅(qū)動輪直徑及輪間距等參數(shù)的估算誤差都會傳遞到位置和方位的測量結(jié)果中，這些估算誤差會隨時間不斷積累，導(dǎo)致上述兩種測量方法的測量結(jié)果誤差越來越大，長期精度變低。

發(fā)明內(nèi)容
本發(fā)明的目的是提出一種光學(xué)成像式輪式移動機器人方位的檢測方法及其裝置，以克服現(xiàn)有技術(shù)中存在的定位測量誤差隨時間累計而無限增大導(dǎo)致的長期測量后定位精度變低的問題，以提高輪式移動機器人位置和方位的長期檢測精度。
本發(fā)明提出的光學(xué)成像式輪式移動機器人方位的檢測方法，包括以下步驟(I)在輪式移動機器人車體的底部安裝一個攝像機，用于按設(shè)定的采樣周期拍攝地面圖像，并將拍攝的圖像傳輸給圖像處理器；(2)設(shè)定一個地面坐標(biāo)系xoy,設(shè)定一個移動機器人的坐標(biāo)系Χ0Υ,初始位置時兩個坐標(biāo)系重合；(3)圖像處理器連續(xù)接收攝像機輸出的地面圖像，在第一幀地面圖像中設(shè)定兩個特征點M和N，則兩個特征點M和N在坐標(biāo)系XOY中的坐標(biāo)分別為(XmCI，Ym0)和(XnCI，Yn(l)，兩個特征點M和N在第二幀圖像中的坐標(biāo)系XOY中的坐標(biāo)分別為(Xml，Yml)和(Xnl，Ynl)，通過計算，得到機器人相對地面坐標(biāo)系的旋轉(zhuǎn)角度Λ θ1Δ Θ j=arc tan [ ( | tan a -tan β | ) / (1+tan α X tan β )],其中，α為點(XmQ，Ym0)和點(XnQ，Yn0)之間的連線與坐標(biāo)系XOY的X軸的夾角，β為點(Xml，Yml)和點(Xnl，Ym)之間的連線與坐標(biāo)系XOY的Y軸的夾角，tan α = (Yn0-Ym0) /(Xn0-Xmtl), tani3 = (Ynl-Yml) / (Xnl-Xml)；(4)根據(jù)上述旋轉(zhuǎn)角度Λ Θ i，得到機器人在地面坐標(biāo)系中沿X軸方向的移動增量Δ gl和沿y軸方向的移動增量Λ Ii1 Δ gi=- Δ X1X cos θ + Δ Y1X sin Θ j+ (Xm0X sin θ ^Ym0X cos θ j) X Δ Q1Ah1=-AX1XsinQ1—Δ Y1Xcos θ j+ (Xm0Xcos θ j—Ym0Xsin Q1) X A θ 17_8]其中Λ X1=Xffll-Xffl0, Δ Y1=Yml-Ym0 ；(5)重復(fù)步驟(3)和(4)，連續(xù)接收攝像機輸出的地面圖像，得到機器人沿地面坐標(biāo)系的X軸方向的移動量g、沿I軸方向的移動量h以及相對地面坐標(biāo)系xoy旋轉(zhuǎn)Θ角g= Δ gj+ Δ g2+... + Agi
h= Δ Ii1+ Δ h2+... + Ahiθ = Λ θ Λ θ 2+…+ Δ θ j ο本發(fā)明提出的光學(xué)成像式輪式移動機器人方位的檢測裝置，包括光源，用于地面照明，所述的光源固定在輪式移動機器人的車體上；攝像機，用于拍攝地面圖像，并將圖像信號傳送給圖像處理器，所述的攝像機固定在輪式移動機器人的車體上；圖像處理器，用于接收攝像機輸出的圖像信號，并進行預(yù)處理，測量相鄰兩幀圖像的相對位置和方向的變化值，然后換算為移動機器人相對地面的位置和方向的變化值，最后將所有位置和方向變化值分別累加，獲得移動機器人相對起始點的位置和方向，所述的圖像處理器與攝像機相連接。本發(fā)明提出的光學(xué)成像式輪式移動機器人方位的檢測方法及其裝置，其優(yōu)點是利用本發(fā)明方法得到的測量結(jié)果，僅與機器人相對地面的相對位置和方向變化有關(guān)，與測量時間或移動機器人的結(jié)構(gòu)參數(shù)估值無關(guān)，因此從理論上消除了眾多誤差因素的影響；移動機器人位置和方位的測量結(jié)果是多次直接測量值的累加，由于直接測量值的測量誤差為隨機誤差，因此隨著機器人移動距離的增大，直接測量值的數(shù)量也不斷增加，測量誤差的累加值將趨于零，因此本發(fā)明測量方法具有較高的長期測量精度，解決了現(xiàn)有檢測技術(shù)測量誤差隨時間累計增大而導(dǎo)致的長期精度變低的問題。而且本發(fā)明的輪式移動機器人位置和方向的檢測裝置，結(jié)構(gòu)簡單、使用方便、價格低廉，在原理上不受時間或機器人結(jié)構(gòu)參數(shù)估值的影響，有很高的長期精度，具有實際推廣應(yīng)用價值。


圖1是本發(fā)明檢測裝置的結(jié)構(gòu)原理示意圖。
圖2是本發(fā)明檢測方法的坐標(biāo)系與地面坐標(biāo)系關(guān)系示意圖。
圖3是本發(fā)明測量裝置中攝像機采集的圖像，其中(a)是一個采樣間隔中的第一幀圖像，(b)是一個采樣間隔的第二幀圖像。
圖1中，I是輪式移動機器人車體、2是圖像處理器、3是攝像機、4是光源。
具體實施方式
本發(fā)明提出的光學(xué)成像式輪式移動機器人方位的檢測方法，包括以下步驟
(I)在輪式移動機器人車體I的底部安裝一個攝像機3，用于按設(shè)定的采樣周期拍攝地面圖像，并將拍攝的圖像傳輸給圖像處理器。
(2)設(shè)定一個地面坐標(biāo)系xoy,設(shè)定一個移動機器人的坐標(biāo)系Χ0Υ,初始位置時兩個坐標(biāo)系重合，如圖2中所示，其中xoy直角坐標(biāo)系為地面坐標(biāo)系，XOY為輪式移動機器人檢測裝置坐標(biāo)系，g是XOY坐標(biāo)系(車體)沿X軸平移的距離，h是XOY坐標(biāo)系(車體)沿y軸平移的距離，Θ是XOY坐標(biāo)系(車體)相對xoy坐標(biāo)系的轉(zhuǎn)動角度。
(3)圖像處理器2連續(xù)接收攝像機3輸出的地面圖像，如圖3所示，在第一幀地面圖像中設(shè)定兩個特征點M和N，則兩個特征點M和N在坐標(biāo)系XOY中的坐標(biāo)分別為(XmCI，Ym0) 和(XnCI，Yntl),兩個特征點M和N在第二幀圖像中的坐標(biāo)系XOY中的坐標(biāo)分別為(Xml，Yml)和 (Xnl, Ynl)，通過計算，得到機器人相對地面坐標(biāo)系的旋轉(zhuǎn)角度Λ Θ1:
Δ Θ j=arc tan [ ( | tan a -tan β | ) / (1+tan α X tan β )],
其中，α為點(XmQ，Ym0)和點(XnQ，Yn0)之間的連線與坐標(biāo)系XOY的X軸的夾角，β 為點(Xml，Yml)和點(Xnl，Ym)之間的連線與坐標(biāo)系XOY的Y軸的夾角，tan a = (Yn0-Ym0) / (Xn0-Xmtl), tani3 = (Ynl-Yml) / (Xnl-Xml)；
(4)根據(jù)上述旋轉(zhuǎn)角度Λ Θ i，得到機器人在地面坐標(biāo)系中沿X軸方向的移動增量 Δ gl和沿y軸方向的移動增量Λ Ii1
Δ gi=- Δ X1X cos θ + Δ Y1X sin Θ j+ (Xm0X sin θ ^Ym0X cos θ j) X Δ Q1
Ah1=-AX1XsinQ1—AY1XcosQA(Xm0XcosQfYm0XsinQ1)XA θ 17
其中ΛX1=Xffll-Xffl0, Δ Y1=Yffll-Ym0 ；
(5)重復(fù)步驟(3)和(4)，連續(xù)接收攝像機輸出的地面圖像，得到機器人沿地面坐標(biāo)系的X軸方向的移動量g、沿I軸方向的移動量h以及相對地面坐標(biāo)系xoy旋轉(zhuǎn)Θ角
g= Δ gj+ Δ g2+... + Agi
h= Δ Ii1+ Δ h2+... + Ahi
θ = Δ θ J+ Δ θ2+…+Λ θ i0本發(fā)明提出的光學(xué)成像式輪式移動機器人方位的檢測裝置，其結(jié)構(gòu)如圖1所示，包括光源4，用于地面照明，所述的光源固定在輪式移動機器人的車體I上；攝像機3，用于拍攝地面圖像，并將圖像信號傳送給圖像處理器，所述的攝像機固定在輪式移動機器人的車體上；圖像處理器2，用于接收攝像機3輸出的圖像信號，并進行預(yù)處理，測量相鄰兩幀圖像的相對位置和方向的變化值，然后換算為移動機器人相對地面的位置和方向的變化值，最后將所有位置和方向變化值分別累加，獲得移動機器人相對起始點的位置和方向，所述的圖像處理器與攝像機相連接。本發(fā)明提出的光學(xué)成像式輪式移動機器人方位的檢測方法中，圖像處理器接收攝像機輸出的地面圖像，在第一幀地面圖像中設(shè)定兩個特征點M和N，并測量M和N在第一幀圖像中的坐標(biāo)值(XmQ，Ym0)和(XnQ，YnQ)，再測量M和N在第二幀圖像中的坐標(biāo)值(Xml，Yml)和(Xnl，Ynl)，計算直線[(Xm0，Ym0)，(Xn0，Yn0)]和直線[(Xml, Yml), (Xnl, Ynl)]之間的夾角Λ Θ 1，以及車體相對地面M點的位移Λ X2=Xml-XmO和Λ Y2=Yml-YmO,最后可根據(jù)Λ θ ΚΔ Xl和Λ Υ1，計算出移動機器人在第二幀圖像時車體相對地面的平移距離增量Λ gl和Ahl，最后將連續(xù)i個采樣周期的移動機器人平移距離增量Λ gi和Λ hi,以及車體相對地面的轉(zhuǎn)動角度變化Λ Θ i分別累加，可得到第i個采樣時刻時車體相對地面坐標(biāo)系原點的位置g= Λ gl+Λ g2+…+ Λ gi 和 h= Λ hi+Λ h2+…+ Λ hi,以及方位角 Θ = Α θ 1+ Δ θ 2+...+ Λ θ i。車體平移距離g和h的增量Λ g和Λ h與地面M點在XOY坐標(biāo)系中前一時刻的位置X、Y及位置變化Λ Χ、Λ Y，以及車體XOY坐標(biāo)系相對地面xoy坐標(biāo)系前一時刻的轉(zhuǎn)動角度Θ和轉(zhuǎn)動角度變化Λ Θ有關(guān)。圖3中，Ca)是一個采樣間隔的第一幀圖像，(b)是一個采樣間隔的第二幀圖像。其中的M、N是地面上的兩個特征點，在相鄰的前后兩幀數(shù)字圖像中，M、N點的坐標(biāo)值分別從(Xm0, Ym0)和(XnQ，Yn0)變化為(Xml，Yml)和(Χη1，Υη1)。α為MN兩點連線在第一幀圖像中的傾角，β為MN兩點連線在第二幀圖像中的傾角，MN兩點連線在車體XOY坐標(biāo)系中的偏轉(zhuǎn)角度Δ Θ i就是車體相對地面的轉(zhuǎn)動角度變化Λ θ 10 Δ Θ i可利用兩直線夾角計算公式計算Δ Θ j=arc tan [ ( | tan a -tan β | ) / (1+tan a *tan β )]其中tan α = (Yn0-Ym0) / (Xn0-Xni0), tan β = (Ynl-Yml) / (Xnl-XnilX在一個采樣間隔內(nèi)，M點的X軸坐標(biāo)變化為Λ X1=Xml-Xm0, Y軸坐標(biāo)變化為Δ Y1=Yffll-Ym0o設(shè)初始位置輪式移動機器人車體XOY坐標(biāo)系與地面xoy坐標(biāo)系重合，圖像處理器根據(jù)ΧΠ(Ι、ΛYii^AY1和厶Q1計算出車體平移距離的增量Ag1和Λ 4。連續(xù)測量并計算Λ gi和Λ Iii (i=l、2、3…)，則第i個采樣時刻時的輪式移動機器人車體位置及方位為g= Δ gj+ Δ g2+... + Agih= Δ Ii1+ Δ h2+... + Ahiθ = Δ θ j+ Δ θ 2+…+Aei本發(fā)明的輪式移動機器人位置和方向的檢測裝置中，照明光源4采用威世半導(dǎo)體(Vishay Semiconductors)公司的TLHG520紅外線發(fā)光二極管,攝像機3采用分辨率為2048X 1536的CCD (Charge Coupled Device)數(shù)字?jǐn)z像機，圖像處理器2采用TI公司的 TMS320數(shù)字信號處理器，圖像采樣速率設(shè)為6幀/秒。
本發(fā)明輪式移動機器人位置和方向檢測裝置的工作原理是
照明光源4對地面照明，突出地面紋理特征，增強地面細(xì)節(jié)。
由于攝像機3的圖像位移變化與真實地面上的位移變化 存在一個比例關(guān)系，因此首先對攝像機3進行標(biāo)定。在地面上標(biāo)識5個已知坐標(biāo)值的參考點，使5個參考點位于攝像機3視場的四個角及中央；圖像處理器2通過圖像處理提取各參考點在攝像機3坐標(biāo)系中的坐標(biāo)值，與參考點在地面坐標(biāo)系中的坐標(biāo)值對比，獲得攝像機3坐標(biāo)系與地面坐標(biāo)系的比例關(guān)系。
攝像機3以6幀/秒的速率拍攝地面圖像，并將數(shù)字圖像信號傳送給圖像處理器 2。圖像處理器2首先對數(shù)字圖像進行濾波處理，去除圖像中的噪聲，對數(shù)字?jǐn)?shù)字圖像進行增強處理，突出地面上的特征點；然后設(shè)定地面特征點，測量前后兩幀圖像中相同特征點的位置變化Λ Xi和Λ Yi,以及車體相對地面坐標(biāo)系的方位角變化Λ θ i，計算車體相對地面平移距離增量Agi和Λ h”分別將AgpAhi和Λ Qi累加，可獲得當(dāng)前移動機器人相對地面坐標(biāo)系原點的位置(g, h)和方位Θ。
權(quán)利要求
1.一種光學(xué)成像式輪式移動機器人方位的檢測方法，其特征在于該檢測方法包括以下步驟 (1)在輪式移動機器人車體的底部安裝一個攝像機，用于按設(shè)定的采樣周期拍攝地面圖像，并將拍攝的圖像傳輸給圖像處理器； (2)設(shè)定一個地面坐標(biāo)系xoy,設(shè)定一個移動機器人的坐標(biāo)系XOY,初始位置時兩個坐標(biāo)系重合； (3)圖像處理器連續(xù)接收攝像機輸出的地面圖像，在第一幀地面圖像中設(shè)定兩個特征點M和N，則兩個特征點M和N在坐標(biāo)系XOY中的坐標(biāo)分別為(XmCI，Ym0)和(XnCI，Yn(l)，兩個特征點M和N在第二幀圖像中的坐標(biāo)系XOY中的坐標(biāo)分別為(Xml，Yml)和(Χη1，Υη1)，通過計算，得到機器人相對地面坐標(biāo)系的旋轉(zhuǎn)角度Λ Θi :Δ Θ =arc tan [ ( | tan a -tan β | ) / (1+tan α X tan β )]， 其中，α為點(XmQ，Ym0)和點(XnQ，Yn0)之間的連線與坐標(biāo)系XOY的X軸的夾角，β為點(Xml，Yml)和點(Xnl，Ynl)之間的連線與坐標(biāo)系XOY的Y軸的夾角，tan a = (Yn0-Ym0)/ (Xn0-Xm0),tan3 = (Ynl-Ynil) / (Xnl-Xnil); (4)根據(jù)上述旋轉(zhuǎn)角度ΛΘ i，得到機器人在地面坐標(biāo)系中沿X軸方向的移動增量Λ gl和沿y軸方向的移動增量Ah1:Δ g!=- Λ X1Xcos θ + Δ Y1X sin θ x+ (Xm0X sin θ JYm0Xcos θ χ) X Δ Q1Δ Ii1=- Δ X1X sin θ χ—Δ Y1X cos θ χ+ (Xm0X cos θ χ—Ym0X sin θ χ) X Δ θ 1 其中Λ X1=Xml-Xmtl, Λ Y1=Yml-Ymtl; (5)重復(fù)步驟(3)和(4)，連續(xù)接收攝像機輸出的地面圖像，得到機器人沿地面坐標(biāo)系的X軸方向的移動量g、沿I軸方向的移動量h以及相對地面坐標(biāo)系xoy旋轉(zhuǎn)Θ角g= Δ gi+ Δ g2+··· + Δ gih= Δ Ii1+ Δ h2+··· + Δ hiθ = Λ θ ^ Λ θ2+…+ A θ i 0
2.一種光學(xué)成像式輪式移動機器人方位的檢測裝置，其特征在于該裝置包括 光源，用于地面照明，所述的光源固定在輪式移動機器人的車體上； 攝像機，用于拍攝地面圖像，并將圖像信號傳送給圖像處理器，所述的攝像機固定在輪式移動機器人的車體上； 圖像處理器，用于接收攝像機輸出的圖像信號，并進行預(yù)處理，測量相鄰兩幀圖像的相對位置和方向的變化值，然后換算為移動機器人相對地面的位置和方向的變化值，最后將所有位置和方向變化值分別累加，獲得移動機器人相對起始點的位置和方向，所述的圖像處理器與攝像機相連接。
全文摘要
本發(fā)明涉及一種光學(xué)成像式輪式移動機器人方位的檢測方法及其裝置，屬于運動定位技術(shù)領(lǐng)域。本發(fā)明在輪式移動機器人車體安裝光源、攝像機和圖像處理器。攝像機拍攝地面圖像，并將拍攝的圖像傳輸給圖像處理器，圖像處理器接收攝像機輸出的地面圖像，測量相鄰兩幀圖像的相對位置和方向的變化值，換算為移動機器人相對地面的位置和方向的變化值，最后將所有變化值分別累加，獲得移動機器人相對起始點的位置和方向。本發(fā)明的檢測方法和裝置結(jié)構(gòu)簡單、使用方便、價格低廉，在原理上不受時間或機器人結(jié)構(gòu)參數(shù)估值的影響，具有很高的長期精度，具有實際推廣應(yīng)用價值。
文檔編號G01B11/26GK102980555SQ20121052175
公開日2013年3月20日 申請日期2012年12月6日 優(yōu)先權(quán)日2012年12月6日
發(fā)明者高宏, 王慶 申請人:紫光股份有限公司