陣列式高速視覺里程計(jì)及其實(shí)現(xiàn)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于機(jī)器人自主定位導(dǎo)航領(lǐng)域，涉及數(shù)字圖像處理、計(jì)算機(jī)視覺和機(jī)器人 學(xué)等技術(shù)，為一種機(jī)器人高精度實(shí)時(shí)定位定姿方法，具體為一種陣列式高速視覺里程計(jì)及 其實(shí)現(xiàn)方法。
【背景技術(shù)】
[0002] 在機(jī)器人自主定位導(dǎo)航領(lǐng)域中，傳統(tǒng)的機(jī)器人定位定姿方法主要包括；W碼盤式 里程計(jì)和慣導(dǎo)等設(shè)備W及相應(yīng)的航跡推算算法（Dead-Reckoning)實(shí)現(xiàn)的相對定位方法； W固定路標(biāo)和GI^S技術(shù)為代表的絕對定位方法。其中，相對定位方法通過碼盤式里程計(jì)實(shí) 現(xiàn)定位，但由于碼盤式里程計(jì)存在車輪半徑測量誤差帶來的系統(tǒng)誤差和車輪滑動造成的非 系統(tǒng)誤差，在高滑動地形條件下誤差累積的速度非?？欤虼送c慣導(dǎo)組合使用，利用慣 導(dǎo)實(shí)時(shí)估計(jì)機(jī)器人的姿態(tài)變化并推算其位置變化，通過數(shù)據(jù)融合相互校正。然而慣導(dǎo)存在 W時(shí)間次方速率產(chǎn)生漂移的問題，當(dāng)機(jī)器人進(jìn)行長時(shí)間、大范圍作業(yè)時(shí)，仍需要進(jìn)行周期性 校正。在人工環(huán)境下，該個(gè)問題可W通過設(shè)置固定路標(biāo)或使用GI^S解決，但在不存在固定路 標(biāo)的野外環(huán)境等非人工環(huán)境下，該種相對定位方法對GPS的依賴就十分嚴(yán)重了。但是由于 GI^S技術(shù)的敏感性(主要由美國掌握)、不確定性(受到干擾或衛(wèi)星故障)、定位精度和應(yīng)用空 間的局限性，導(dǎo)致其在野外環(huán)境中的應(yīng)用受限甚至無法應(yīng)用，在該種情況下，傳統(tǒng)技術(shù)方法 的缺點(diǎn)更加突出，已明顯無法滿足機(jī)器人的技術(shù)要求。
[0003] 為了滿足機(jī)器人在非結(jié)構(gòu)化環(huán)境下長時(shí)間、長距離作業(yè)時(shí)的精確自主導(dǎo)航技術(shù)要 求，上世紀(jì)80年代中期W來，出現(xiàn)了被稱為視覺里程計(jì)（VisualOdometry)的機(jī)器人實(shí)時(shí) 定位定姿方法，是計(jì)算機(jī)視覺理論在機(jī)器人領(lǐng)域的一個(gè)重要應(yīng)用。視覺里程計(jì)僅利用視覺 系統(tǒng)獲得的環(huán)境圖像，對機(jī)器人運(yùn)動過程中的姿態(tài)和位置變化進(jìn)行精確估計(jì)，屬于被動非 接觸式測量方法，具有精度高、隱蔽性好、不依賴GI^S校正W及不受環(huán)境地表特性和應(yīng)用空 間限制等突出的優(yōu)點(diǎn)。雖然視覺里程計(jì)仍是一種相對定位方法，但已有的研究表明，視覺里 程計(jì)的誤差呈非單應(yīng)性，即使機(jī)器人在高滑動地形條件(滑動比>100%)下進(jìn)行長距離（作 業(yè)距離〉lkm)導(dǎo)航時(shí)，仍能有效將累積誤差控制在一定范圍內(nèi)，并且當(dāng)與測角儀和慣導(dǎo)等組 合使用時(shí)，累積誤差的范圍更小。2004年美國國家航空和宇宙航行局（NASA)將視覺里程計(jì) 應(yīng)用于火星探測并取得了巨大的成功，充分說明了視覺里程計(jì)方法的有效性W及在某些特 定環(huán)境下的不可替代性。
[0004] 目前，現(xiàn)有的視覺里程計(jì)方法基本上都采用了 "特征一特征H維位置一運(yùn)動估計(jì)" 的方法框架，只是在結(jié)構(gòu)上有單目和雙目之分，W及具體實(shí)現(xiàn)方法的差異?，F(xiàn)有的方法都存 在實(shí)時(shí)性差的瓶頸問題(處理速度<1甜Z)。該主要是由于現(xiàn)有的該些方法對環(huán)境圖像的處 理過程采用的都是逐峽逐點(diǎn)的計(jì)算方式，該就造成當(dāng)圖像尺寸較大時(shí)，待處理的數(shù)據(jù)量很 大，而計(jì)算機(jī)的處理能力與之相比就明顯不夠。而通過減小圖像大小，降低數(shù)據(jù)量，將導(dǎo)致 視覺里程計(jì)精度的大幅下降。此外，在非結(jié)構(gòu)化環(huán)境下，存在各種導(dǎo)致視覺里程計(jì)失效的不 確定性因素。現(xiàn)有的視覺里程計(jì)方法僅采用單目或雙目結(jié)構(gòu)，遇到相機(jī)或線路損壞等情況 時(shí)，整個(gè)視覺里程計(jì)就完全失效，魯棒性差。

【發(fā)明內(nèi)容】

[0005] 針對現(xiàn)有技術(shù)中存在的上述不足之處，本發(fā)明要解決的技術(shù)問題是提供一種解決 機(jī)器人作業(yè)過程中實(shí)時(shí)精確定位定姿問題的高速魯棒H維視覺里程計(jì)及其實(shí)現(xiàn)方法。
[0006] 本發(fā)明為實(shí)現(xiàn)上述目的所采用的技術(shù)方案是；一種陣列式高速視覺里程計(jì)，包括
[0007] 若干個(gè)視覺測量單元，呈平面或空間的幾何陣列分布，用于觀測機(jī)器人的姿態(tài)和 位置參數(shù)，并行計(jì)算，獨(dú)立輸出，多個(gè)視覺測量單元設(shè)置圖像各個(gè)方向上不同的長寬比，所 提取的特征點(diǎn)在圖像中均勻分布；
[0008] 數(shù)據(jù)采集電路板，連接各個(gè)視覺測量單元，用于采集所述視覺測量單元監(jiān)測到的 圖像數(shù)據(jù)并對其進(jìn)行數(shù)據(jù)校驗(yàn)和故障診斷，然后傳送給工控機(jī)；
[0009] 工控機(jī)，用于將多個(gè)視覺測量單元間的數(shù)據(jù)進(jìn)行融合，輸出最終的機(jī)器人的姿態(tài) 和位置。
[0010] 所述視覺測量單元包括：
[0011] 相機(jī)，用于拍攝機(jī)器人圖像；
[0012] 固化算法的專用電路板，連接相機(jī)，用于計(jì)算圖像特征點(diǎn)的空間H維位置。
[0013] 所述相機(jī)采集的圖像大小不大于10000像素，數(shù)據(jù)傳輸速率不低于2Mbps。
[0014] 所述數(shù)據(jù)采集電路板還用于關(guān)閉失效的視覺測量單元通道。
[0015] 一種陣列式高速視覺里程計(jì)的實(shí)現(xiàn)方法，包括W下步驟：
[0016] 利用的單個(gè)視覺測量單元并行計(jì)算特征點(diǎn)的H維位置；
[0017] 根據(jù)視覺測量單元的空間位置變換關(guān)系和空間運(yùn)動一致性，對各視覺測量單元輸 出的特征點(diǎn)的H維位置數(shù)據(jù)進(jìn)行校驗(yàn)；
[0018] 利用多個(gè)視覺測量單元輸出的特征點(diǎn)的H維位置數(shù)據(jù)，通過無色卡爾曼濾波方法 進(jìn)行數(shù)據(jù)融合；并根據(jù)視覺測量單元的空間位置變換關(guān)系，通過非線性優(yōu)化方法，精確估計(jì) 機(jī)器人的位姿變化量。
[0019] 所述單個(gè)視覺測量單元并行計(jì)算特征點(diǎn)的H維位置，包括W下步驟：
[0020] 步驟1，當(dāng)i= 1時(shí)，從ty和ti時(shí)刻立體視覺圖像對/jy和中，采用化rris算子 提取特征點(diǎn)；當(dāng)i> 1時(shí)，僅從ti時(shí)刻立體視覺圖像中，提取特征點(diǎn)；
[002U步驟2,當(dāng)i= 1時(shí)，分別對ty和ti時(shí)刻立體視覺圖像對，和中提取的特征點(diǎn) 進(jìn)行匹配；當(dāng)i> 1時(shí)，僅對ti時(shí)刻立體視覺圖像沖提取的特征點(diǎn)進(jìn)行匹配；
[0022] 步驟3,對立體視覺圖像對人，和中提取和匹配的特征點(diǎn)，在左右圖像序列中分 別進(jìn)行跟蹤，
[0023] 步驟4,對匹配和跟蹤的特征點(diǎn)，進(jìn)行最小二乘意義下的H維重建；
[0024] 步驟5,將中提取和匹配的特征點(diǎn)作為，中提取和匹配的特征點(diǎn)，進(jìn)入下一時(shí)刻 的特征點(diǎn)匹配與跟蹤。
[00巧]所述對特征點(diǎn)進(jìn)行匹配采用改進(jìn)的MNCC算法計(jì)算特征點(diǎn)在立體視覺圖像對之間 的匹配相關(guān)性，采用外極線校正和視差約束減小搜索范圍；通過雙向一致性約束和唯一性 約束濾除誤匹配特征點(diǎn)。
[0026] 所述步驟3采用改進(jìn)的MNCC算法計(jì)算特征點(diǎn)之間的相關(guān)性，相關(guān)性最大的特征點(diǎn) 將作為跟蹤的特征點(diǎn)；僅采用極線約束條件，通過Hartley八點(diǎn)法估計(jì)基礎(chǔ)矩陣，利用約束 條件濾除誤跟蹤的特征點(diǎn)。
[0027] 所述對各視覺測量單元輸出的特征點(diǎn)的H維位置數(shù)據(jù)進(jìn)行校驗(yàn)，包括W下步驟：
[0028] 利用采集的視覺測量數(shù)據(jù)，通過加權(quán)最小二乘法估計(jì)機(jī)器人運(yùn)動初值M0 ;
[0029] 設(shè)定一個(gè)閥，通過RANSAC法去除無效數(shù)據(jù)點(diǎn)值t'，當(dāng)根據(jù)特征點(diǎn)計(jì)算得到的機(jī) 器人運(yùn)動與最優(yōu)值之間的誤差<>/'時(shí)，當(dāng)前的跟蹤特征點(diǎn)分別為離群值，反之為有效特征 占. ;、、、?
[0030] 根據(jù)測量單元坐標(biāo)變換關(guān)系，進(jìn)行數(shù)據(jù)有效性的二次判斷；
[0031] 如果視覺測量單元的輸出數(shù)據(jù)過少或輸出的有效數(shù)據(jù)過少時(shí)，將認(rèn)為視覺測量單 元出現(xiàn)故障，關(guān)閉該視覺單元。
[0032] 所述利用多個(gè)視覺測量單元輸出的特征點(diǎn)的H維位置數(shù)據(jù)，通過無色卡爾曼濾波 方法進(jìn)行數(shù)據(jù)融合；并根據(jù)視覺測量單元的空間位置變換關(guān)系，通過非線性優(yōu)化方法，精確 估計(jì)機(jī)器人的位姿變化量，包括W下步驟：
[0033] 根據(jù)數(shù)據(jù)校驗(yàn)算法計(jì)算得到的機(jī)器人位姿估計(jì)機(jī)器人運(yùn)動初值M。和全部視覺測 量單元中匹配和跟蹤的特征點(diǎn)，計(jì)算逆投影誤差，與設(shè)定的誤差闊值進(jìn)行比較，濾除可能存 在的無效數(shù)據(jù)；
[0034] 利用計(jì)算得到的有效數(shù)據(jù)，根據(jù)加權(quán)最小二乘法重新計(jì)算機(jī)器人位姿估計(jì)初值 M;，并將其作為機(jī)器人位姿優(yōu)化估計(jì)的初始值；
[00巧]根據(jù)稀疏LM算法計(jì)算機(jī)器人的位姿變化