專利名稱：一種球形機器人視覺定位系統(tǒng)及視覺定位方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機器人定位導(dǎo)航領(lǐng)域，特別涉及一種球形機器人視覺定位系統(tǒng)及視覺定位方法
背景技術(shù)：
球形機器人是指一類將運動執(zhí)行機構(gòu)、傳感器、控制器、能源裝置都內(nèi)置于一個球形殼體內(nèi)部、利用球形外殼作為滾動行走裝置的系統(tǒng)的總稱。與輪式，足式和履帶式的機器人相比，具有結(jié)構(gòu)新穎，運動靈活和效率高的特定，在軍事、民用和工業(yè)領(lǐng)域有廣泛的應(yīng)用前景。球形機器人的定位問題是實現(xiàn)球形機器人智能化的一個關(guān)鍵問題。目前球形機器人主要采用慣性導(dǎo)航系統(tǒng)、里程計、GPS等方式進行定位，但是慣性導(dǎo)航系統(tǒng)需要通過積分來得到位移和角度，存在時間漂移誤差；里程計由于球面滑動，跳躍等因素，將產(chǎn)生累計誤差，同時球形機器人平臺不穩(wěn)定，難以找到合適的搭載里程計的位置；GPS精度不高，室內(nèi)無法使用。目前視覺定位是研究熱點，是未來機器人導(dǎo)航定位的一個方向，其具有信息量大、 感應(yīng)時間短的特點，可以通過獲取周圍環(huán)境位置的變化，得到機器人精確的機器人運動和姿態(tài)。球形機器人利用視覺定位還可以克服由于球面滑動，跳躍等采用內(nèi)部傳感器定位造成的累計誤差。但是目前在球形機器人上采用視覺定位的研究成果以及在球形機器人上搭載完整的視覺定位系統(tǒng)基本沒有，主要是球形機器上采用視覺定位存在不少問題1、視覺定位算法復(fù)雜、計算量大，球形機器人的處理系統(tǒng)難以進行實時處理。2、球形機器人外面是封閉球殼，相機透過球殼采集圖像質(zhì)量很差甚至無法采集圖像。3、相機安裝困難，通常的安裝位置會使相機隨球360度轉(zhuǎn)動4、球形機器人平臺不穩(wěn)定，相機坐標(biāo)系相對與機器人坐標(biāo)系會不斷變化，導(dǎo)致定位不準(zhǔn)。

發(fā)明內(nèi)容
本發(fā)明的目的是為了克服現(xiàn)有球形機器人導(dǎo)航的技術(shù)不足，提出一種精度高、實時性好的球形機器人視覺定位系統(tǒng)及視覺定位方法。本發(fā)明的采用如下技術(shù)方案一種球形機器人視覺定位系統(tǒng)，包括雙目視覺系統(tǒng)、一維陀螺儀、球形機器人本體、核心控制系統(tǒng)、無線通信模塊。本發(fā)明中，所述的視覺定位系統(tǒng)的球形機器人本體設(shè)計成左右兩個半球殼，左右半球獨立驅(qū)動，同時左右半球之間開縫設(shè)計。所述的視覺定位系統(tǒng)的雙目視覺系統(tǒng)包括兩個型號相同的CCD相機，其主光軸平行放置，雙目相機搭載到球形機器人與重擺相連的框架上，保證球靜止時，相機光軸與地面平行，并指向正前方；所述的陀螺儀安裝在雙目相機的正上方。定位系統(tǒng)通過相機和陀螺儀配合進行定位，定位算法在核心控制系統(tǒng)中在線實現(xiàn)，定位的數(shù)據(jù)通過無線通信模塊進行遠(yuǎn)程傳輸，用戶可以遠(yuǎn)程實時監(jiān)控機器人的位置。一種球形機器人視覺定位方法，包括以下步驟步驟1 對相機和陀螺儀距相機中心的位置進行標(biāo)定。步驟2 開啟雙目相機，進行左右圖像連續(xù)采集，開啟陀螺儀，對相機姿態(tài)進行測量。步驟3 如果圖像是第一幀，對采集的左右圖像提取Sii-Tomasi特征，并利用SIFT 描述符進行描述，通過立體視覺坐標(biāo)轉(zhuǎn)化原理，將二維圖像坐標(biāo)轉(zhuǎn)換為三維空間坐標(biāo)，利用陀螺儀對特征點的位置進行補償，初始化旋轉(zhuǎn)矩陣R、平移矩陣T，R設(shè)為單位矩陣，T元素全置零，并設(shè)當(dāng)前位置為原點，重新進入步驟3 ；否則直接進入步驟4。步驟4 利用KLT算法對前后兩幀左圖和前后兩幀右像分別進行特征的跟蹤， 同時對前后兩幀圖像中左右圖像利用極線約束條件去除誤跟蹤特征對，將如果跟蹤特征點總數(shù)小于閾值，重新對前后幀圖像進行Sii-Tomasi提取并匹配，將匹配特征點二維圖像坐標(biāo)轉(zhuǎn)換為三維空間坐標(biāo)，得到前后兩幀圖像的三維特征點集，并利用陀螺儀對特征點的位置進行補償；步驟5 利用前后兩幀的三維特征點集，采用解析法并結(jié)合RANSAC算法進行運動估計，得到R和T ；其中的詳細(xì)步驟如下5-1.對前后幀三維特征點集進行篩選，選出匹配成功的特征點，組成新的特征點集。5-2.對于前后幀新的特征點集，隨機選擇3對三維特征點，通過解析法，進行一次運動估計，得到一個R和T。5-3.判斷當(dāng)前R和T，能使多少對特征點成為局內(nèi)點，記錄局內(nèi)點對數(shù)。5-4.針對5-2、5_3，進行N次迭代，得到局內(nèi)特征對數(shù)最多時候的局內(nèi)特征對。5-5.將這些特征對，重新根據(jù)解析法，進行運動估計，得到更優(yōu)的R和T。步驟6 利用卡爾曼濾波算法對R，T進行更新，并得到當(dāng)前機器人在世界坐標(biāo)系中的位置，返回步驟3，進行下一次計算。本發(fā)明的優(yōu)點有1)本發(fā)明采用雙目視覺結(jié)合陀螺儀的方式進行定位，降低了球形機器人不穩(wěn)定帶來的定位誤差，提高了定位精度。2)本發(fā)明中球形機器人本體采用左右半球之間開縫設(shè)計，外部傳感器可以擺脫球殼的約束，更好的獲取外部的環(huán)境信息；3)本發(fā)明視覺系統(tǒng)搭載到相對穩(wěn)定的主框上，是其保證一直指向球形機器人正前方位置，并只有小范圍的偏轉(zhuǎn)，保證采集圖像的穩(wěn)定性。3)本發(fā)明中視覺定位算法，使用Sii-Tomasi特征和SIFT描述符相結(jié)合的特征提取方法，既滿足了特征點提取的實時性，又提高特征點匹配的魯棒性。4)本發(fā)明中視覺定位算法采用KLT算法進行特征跟蹤，不用每兩幀圖像進行特征重新提取和匹配，計算量大為減少。5)本發(fā)明中視覺定位算法，使用解析法和RANSAC算法進行運動估計，保證了定位
4精度和實時性。


圖1是本發(fā)明提出的球形機器人視覺導(dǎo)航系統(tǒng)結(jié)構(gòu)圖。圖2是本發(fā)明中所定義的各個坐標(biāo)系。圖3是本發(fā)明中機器人運動時，球形機器人坐標(biāo)系和攝像機坐標(biāo)系變化示意圖。圖4是本發(fā)明提出的球形機器人視覺導(dǎo)航算法流程圖。圖5是本發(fā)明提出的球形機器人視覺導(dǎo)航算法中運動估計算法結(jié)合RANSAC算法流程圖
具體實施例方式下面結(jié)合附圖對本發(fā)明進行進一步說明如圖1所示為本發(fā)明所述的球形機器人視覺導(dǎo)航系統(tǒng)。該系統(tǒng)包括球形機器人本體1、雙目視覺系統(tǒng)2、陀螺儀3、核心控制系統(tǒng)4、無線通信模塊5。定位系統(tǒng)通過雙目定位系統(tǒng)2和陀螺儀3配合進行定位，定位算法在核心控制系統(tǒng)4中在線實現(xiàn)，定位的數(shù)據(jù)通過無線通信模塊5進行遠(yuǎn)程傳輸，用戶可以遠(yuǎn)程實時監(jiān)控球形機器人本體1的位置。球形機器人本體1由左右兩個半球殼構(gòu)成球形外殼，中間開縫；在球殼內(nèi)裝有主框架，與重擺固連，重擺設(shè)置在豎直平面上，主框架兩側(cè)與球殼通過支撐架滑動連接。球形機器人本體1兩個半球殼進行分別驅(qū)動控制。球形機器人本體1包括機械結(jié)構(gòu)和底層電機驅(qū)動電路。雙目視覺系統(tǒng)2由兩個同型號的C⑶黑白攝像頭構(gòu)成，事先進行標(biāo)定，已獲得其內(nèi)參數(shù)和畸變參數(shù)。雙目視覺系統(tǒng)2搭載到球形機器人與重擺相連的框架上，保證球靜止時， 相機光軸與地面平行，并指向正前方。雙目視覺系統(tǒng)2與核心控制系統(tǒng)4通過1394數(shù)據(jù)線相連，核心控制系統(tǒng)4不斷接收雙目視覺系統(tǒng)2的圖像，并進行處理。陀螺儀3為一維陀螺儀，可以測量指定方向的角度變化。其安裝在雙目視覺系統(tǒng)上方，保證其可以測量重擺擺起的角度。陀螺儀3與核心控制系統(tǒng)4通過串口相連，心控制系統(tǒng)4不斷接收陀螺儀3反饋的角度進行處理。核心控制系統(tǒng)4上運行球形機器人視覺導(dǎo)航算法，核心控制系統(tǒng)4采用MINI-ATX 主板、13 3. IGHZ處理器、30G固態(tài)硬盤、4G內(nèi)存，DC-ATX電源模塊，極大的提高了系統(tǒng)的數(shù)據(jù)處理能力和存儲能力，同時兼顧了系統(tǒng)穩(wěn)定性、功率和小尺寸的要求。核心控制系統(tǒng)4運行穩(wěn)定性、安全性好的Linux操作系統(tǒng)，通過配備的DC12V鋰電池，可以持續(xù)工作兩個小時。無線通信模塊5為寬帶寬、大功率的無線網(wǎng)卡，實現(xiàn)與遠(yuǎn)程主機進行通信，將定位的結(jié)果不斷發(fā)送給遠(yuǎn)程客戶端，實現(xiàn)實時監(jiān)控。同時無線通信模塊也可以將采集的圖像遠(yuǎn)程傳輸，，實現(xiàn)圖像遠(yuǎn)程監(jiān)控。無線通信模塊5采用USB接口與核心控制系統(tǒng)4相連。參照圖2，對本發(fā)明球形機器人視覺定位算法中用到的幾種不同的坐標(biāo)系和坐標(biāo)系間轉(zhuǎn)換作簡單介紹(1)世界坐標(biāo)系{A :Ow-xwywzw}用來表示球形機器人和特征點的絕對位置，在空間中垂直地面為Z軸，設(shè)單位為米。
5
(2)球形機器人本體坐標(biāo)系{B :Or-xryrzr}坐標(biāo)原點位于球形機器人球心，ζ軸指向機器人運動方向，y軸指向地面，χ軸與y 軸、ζ軸構(gòu)成右手坐標(biāo)系，單位為米。(3)攝像機坐標(biāo)系{C :oc-xcyczc}坐標(biāo)原點位于立體視覺系統(tǒng)左相機光心，ζ軸與立體視覺系統(tǒng)左相機光軸重合，χ 軸指向右相機光心，y軸與χ、ζ軸構(gòu)成右手坐標(biāo)系。單位為米。(4)圖像坐標(biāo)系{D Ou-XuyJ在立體視覺系統(tǒng)左相機成像平面內(nèi)，以左上角為坐標(biāo)原點，水平方向為χ軸，豎直方向為y軸，單位為像素。由于攝像機位于球形機器人本體上不穩(wěn)定平臺上，如圖3，當(dāng)機器人重擺擺動，球形機器人本體坐標(biāo)系和攝像機坐標(biāo)系關(guān)系將發(fā)生變化，球形機器人本體坐標(biāo)系和攝像機坐標(biāo)系轉(zhuǎn)化需要知道重擺擺動的角度θ。θ由陀螺儀測出，由幾何關(guān)系，可得球形機器人本體和攝像機坐標(biāo)系轉(zhuǎn)化關(guān)系
權(quán)利要求
1.一種球形機器人視覺定位系統(tǒng)，其特征在于，包括雙目視覺系統(tǒng)、一維陀螺儀、球形機器人本體、核心控制系統(tǒng)和無線通信模塊等。定位系統(tǒng)通過相機和陀螺儀配合進行定位， 定位算法在核心控制系統(tǒng)中在線實現(xiàn)，定位的數(shù)據(jù)通過無線通信模塊進行遠(yuǎn)程傳輸，用戶可以遠(yuǎn)程實時監(jiān)控機器人的位置。
2.權(quán)利要求1所述的一種球形機器人視覺定位系統(tǒng)，其特征在于，球形機器人本體設(shè)計成左右兩個半球殼，左右半球獨立驅(qū)動，同時左右半球之間開縫設(shè)計。雙目視覺定位系統(tǒng)搭載到球形機器人與重擺相連的框架上，通過左右球殼之間的縫隙，伸出球殼外部，保證機器人靜止時，相機光軸與地面平行，并指向正前方。陀螺儀安裝在雙目相機的正上方。
3.一種球形機器人視覺定位方法，其特征在于，包含以下步驟步驟1 對相機和陀螺儀距相機中心的位置進行標(biāo)定。步驟2 開啟雙目相機，進行左右圖像連續(xù)采集，開啟陀螺儀，對相機姿態(tài)進行測量。步驟3 如果圖像是第一幀，對采集的左右圖像提取Sii-Tomasi特征，并利用SIFT描述符進行描述，通過立體視覺三角測量法將特征點的二維圖像坐標(biāo)轉(zhuǎn)換為三維空間坐標(biāo)， 利用陀螺儀對特征點的位置進行補償，初始化旋轉(zhuǎn)矩陣R、平移矩陣T，R設(shè)為單位矩陣，T元素全置零，并設(shè)當(dāng)前位置為原點，重新進入步驟3 ；如果圖像不是第一幀，直接進入步驟4。步驟4 利用KLT算法對前后兩幀左圖和前后兩幀右像分別進行特征的跟蹤，同時對前后兩幀圖像中左右圖像利用極線約束條件去除誤跟蹤對，得到特征匹配對，如果跟蹤特征對總數(shù)小于閾值，重新對前后幀圖像進行Sii-Tomasi提取并匹配，將新匹配的點加至特征匹配對，通過立體視覺三角測量法將匹配特征對二維圖像坐標(biāo)轉(zhuǎn)換為三維空間坐標(biāo)， 得到前后兩幀圖像的三維特征點集，并利用陀螺儀對特征點的位置進行補償；步驟5 利用前后兩幀的三維特征點集，采用解析法并結(jié)合RANSAC算法進行運動估計， 得到R和T ；步驟6 利用卡爾曼濾波算法對R，T進行更新，并得到當(dāng)前機器人在世界坐標(biāo)系中的位置。返回步驟3，進行下一次計算。
4.權(quán)利要求4所述的一種球形機器人視覺定位方法，其特征在于，步驟5具體步驟如下(1).對前后幀三維特征點集進行篩選，選出匹配成功的點，組成新的特征點集。(2).對于前后幀新的特征點集，隨機選擇3對三維特征點，通過解析法，進行一次運動估計，得到一個R和T。(3).判斷當(dāng)前R和T，能使多少對特征點成為局內(nèi)點，記錄局內(nèi)點對數(shù)。(4).針對0)、(3)，進行N次迭代，得到局內(nèi)特征對數(shù)最多時候的局內(nèi)特征對。(5).將這些特征對，重新根據(jù)解析法，進行運動估計，得到更優(yōu)的R和T。
全文摘要
本發(fā)明公開了一種球形機器人視覺定位系統(tǒng)及視覺定位方法。視覺定位系統(tǒng)包括雙目視覺系統(tǒng)、一維陀螺儀、球形機器人本體、核心控制系統(tǒng)和無線通信模塊組成。定位系統(tǒng)通過相機和陀螺儀配合進行定位，定位算法在核心控制系統(tǒng)中在線實現(xiàn)，定位的數(shù)據(jù)通過無線通信模塊進行遠(yuǎn)程傳輸，用戶可以遠(yuǎn)程實時監(jiān)控機器人的位置。定位方法是通過雙目相機實時采集左右圖像序列，通過雙目視覺里程計算法，計算出球形機器人姿態(tài)和位置的變化，通過陀螺儀對球形機器人平臺不穩(wěn)定造成的特征定位誤差進行補償，最后采用卡爾曼濾波方法進行狀態(tài)更新。整個過程不斷迭代，實現(xiàn)實時對機器人的姿態(tài)和位置進行計算。
文檔編號G01C11/04GK102435172SQ20111025896
公開日2012年5月2日 申請日期2011年9月2日 優(yōu)先權(quán)日2011年9月2日
發(fā)明者葉平, 孫漢旭, 張?zhí)焓? 李自亮, 王軒, 賈慶軒 申請人:北京郵電大學(xué)