基于一點(diǎn)ransac和fast算法的移動(dòng)機(jī)器人導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于移動(dòng)機(jī)器人自主定位和導(dǎo)航領(lǐng)域，具體設(shè)及基于圖像匹配的移動(dòng)機(jī)器 人定位與導(dǎo)航。
【背景技術(shù)】
[0002] 隨著計(jì)算機(jī)技術(shù)的飛速發(fā)展和社會(huì)的不斷進(jìn)步，移動(dòng)機(jī)器人的應(yīng)用得到了不斷擴(kuò) 展，人工智能水平也得到了不斷提高。和傳統(tǒng)的工業(yè)機(jī)器人相比較而言，移動(dòng)機(jī)器人集成了 更多的先進(jìn)科技，正逐步成為一個(gè)集任務(wù)規(guī)劃、環(huán)境創(chuàng)建與自主定位、路徑規(guī)劃、實(shí)時(shí)導(dǎo)航、 目標(biāo)識(shí)別等多項(xiàng)功能于一體的綜合性系統(tǒng)。其應(yīng)用領(lǐng)域也從工業(yè)逐漸擴(kuò)展到了軍事、醫(yī)療、 家用服務(wù)、緊急救援、海洋開(kāi)發(fā)、太空探索等各個(gè)方面，應(yīng)用領(lǐng)域不斷擴(kuò)展的同時(shí)也對(duì)機(jī)器 人的自主定位與導(dǎo)航能力提出了更高的要求。
[0003] 近20年來(lái)，基于各種傳感器的導(dǎo)航技術(shù)研究獲得了巨大的發(fā)展，而基于視覺(jué)傳感 器的同時(shí)定位與地圖創(chuàng)建技術(shù)（SimultaneousLocalizationAndMapping,SLAM問(wèn)題）受 到了最為廣泛的重視。移動(dòng)機(jī)器人在完全未知的環(huán)境中利用自身傳感器獲得周?chē)h(huán)境信息 進(jìn)行實(shí)時(shí)定位和地圖創(chuàng)建SLAM是移動(dòng)機(jī)器人需要解決的關(guān)鍵問(wèn)題。SLAM是指機(jī)器人在一 個(gè)未知的環(huán)境中，從一個(gè)未知的位置開(kāi)始，通過(guò)對(duì)環(huán)境的觀測(cè)，遞增地構(gòu)建環(huán)境地圖，并同 時(shí)運(yùn)用環(huán)境地圖實(shí)現(xiàn)機(jī)器人定位的一個(gè)過(guò)程?；谝曈X(jué)傳感器的導(dǎo)航技術(shù)根據(jù)視覺(jué)攝像機(jī) 數(shù)目的不同，可W分為全向視覺(jué)定位與導(dǎo)航、雙目視覺(jué)定位與導(dǎo)航、單目視覺(jué)定位與導(dǎo)航。 其中基于單目視覺(jué)的導(dǎo)航方法操作方便、模型簡(jiǎn)單，其實(shí)時(shí)性也較高，而且單目視覺(jué)成像設(shè) 備如數(shù)碼相機(jī)、監(jiān)控?cái)z像頭、拍照手機(jī)等，其數(shù)量多，體積小，結(jié)構(gòu)簡(jiǎn)單，更重要的是該些設(shè) 備更加符合人們的使用習(xí)慣。因此基于單目視覺(jué)SLAM的移動(dòng)機(jī)器人導(dǎo)航技術(shù)最有發(fā)展前 景的一種機(jī)器人自主導(dǎo)航技術(shù)。
[0004] 另外，盡管在簡(jiǎn)單的結(jié)構(gòu)化環(huán)境W及地圖信息已知（具有人工路標(biāo)）的環(huán)境中 進(jìn)行視覺(jué)導(dǎo)航已經(jīng)有了很大進(jìn)展，但在完全未知的環(huán)境中進(jìn)行定位與導(dǎo)航還存在諸多問(wèn) 題。造成該一現(xiàn)象的根本原因在于如何在未知環(huán)境中獲取自然特征。所謂自然特征，是 指環(huán)境中已有的、非人工設(shè)置的、能夠用W標(biāo)識(shí)不同環(huán)境場(chǎng)景的特征對(duì)象。通過(guò)提取一 些具備局部不變性的角點(diǎn)作為環(huán)境的自然特征，該方法選取的自然路標(biāo)有非常好的穩(wěn)定 性，因此角點(diǎn)檢測(cè)也是機(jī)器人導(dǎo)航的熱點(diǎn)問(wèn)題之一，也出現(xiàn)了一些較好的角點(diǎn)檢測(cè)方法。 FAST(feagresfromacceleratedsegmenttest)是一種運(yùn)算簡(jiǎn)單直觀的角點(diǎn)檢測(cè)方 法，相關(guān)測(cè)試表明，F(xiàn)AST在計(jì)算速度上優(yōu)于SIFT、SURF和Harries等角點(diǎn)檢測(cè)方法。文獻(xiàn) (基于FAST改進(jìn)的快速角點(diǎn)探測(cè)算法，紅外與激光工程，2009 (6) : 1104-1108)采用改進(jìn) 的FAST算法來(lái)提取角點(diǎn)，剔除了大部分邊緣點(diǎn)和局部非極大值點(diǎn)；另一方面，針對(duì)誤匹配 剔除，文獻(xiàn)（STRASDAH，M0NTIELJMM,DAVISONAJ.Real-timemonocularSLAM:why filter[C],ProceedingsoftheIEEEInternationalConferenceonroboticsand automation. 2010:2657-2664. )RANSAC算法是一種應(yīng)用廣泛的誤匹配剔除算法，但該算法 隨著匹配點(diǎn)個(gè)數(shù)增加，算法迭代次數(shù)會(huì)迅速上升，降低了算法的計(jì)算速度。

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

[0005] 本發(fā)明針對(duì)現(xiàn)有技術(shù)中存在的上述技術(shù)問(wèn)題，采用一點(diǎn)RANSAC(RandomSample Consensus,隨機(jī)抽樣一致性）算法可W充分利用EKF預(yù)測(cè)階段得到的先驗(yàn)信息，在確 保計(jì)算精度的同時(shí)，有效降低了算法迭代次數(shù)。機(jī)器人通過(guò)自身攝像頭，采用改進(jìn)的 FAST(FeaUiresfromAcceleratedSegmentTest,加速段檢測(cè)特征點(diǎn)）角點(diǎn)提取算法采集 周?chē)h(huán)境信息，把一點(diǎn)RANSAC算法和FAST算法結(jié)合起來(lái)，共同用于移動(dòng)機(jī)器人自主導(dǎo)航中 的地圖構(gòu)建和定位。并結(jié)合EKF的一點(diǎn)RANSAC算法對(duì)誤匹配點(diǎn)進(jìn)一步剔除，獲得魯椿性更 強(qiáng)的特征點(diǎn)，從而構(gòu)建環(huán)境的地圖信息，最后用得到的匹配點(diǎn)進(jìn)行=維環(huán)境重建和機(jī)器人 定位。
[0006] 本發(fā)明解決上述技術(shù)問(wèn)題的技術(shù)方案是，提出一種基于一點(diǎn)RANSAC(Random SampleConsensus,隨機(jī)抽樣一致性）和FAST(Fea1:uresfromAcceleratedSegment Test,加速段檢測(cè)特征點(diǎn)）算法的移動(dòng)機(jī)器人導(dǎo)航方法，該方法包括步驟；機(jī)器人運(yùn)動(dòng)過(guò)程 中攝像頭提取FAST特征點(diǎn)，根據(jù)特征點(diǎn)進(jìn)行相鄰帖的匹配，如果匹配點(diǎn)個(gè)數(shù)大于闊值，進(jìn) 行一點(diǎn)RANSAC誤匹配剔除，建立擬合模型獲取移動(dòng)機(jī)器人路徑，重建=維環(huán)境，根據(jù)提取 的FAST特征點(diǎn)進(jìn)行KLT特征跟蹤，完成機(jī)器人導(dǎo)航。
[0007] 具體包括，機(jī)器人運(yùn)動(dòng)過(guò)程中攝像頭采集圖像信息，從采集的圖像信息視頻帖中 提取特征點(diǎn)，根據(jù)特征點(diǎn)對(duì)相鄰兩個(gè)視頻帖的同名特征點(diǎn)進(jìn)行粗匹配，對(duì)同名特征點(diǎn)進(jìn)行 精確匹配獲得圖像的匹配特征點(diǎn)，根據(jù)匹配特征點(diǎn)進(jìn)行環(huán)境模型擬合，規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng) 路徑，驅(qū)動(dòng)移動(dòng)機(jī)器人進(jìn)行逐步探索，獲得運(yùn)動(dòng)路徑的環(huán)境信息建立環(huán)境=維模型，根據(jù)= 維模型進(jìn)行特征跟蹤控制機(jī)器人自主導(dǎo)航。
[0008] 同名特征點(diǎn)進(jìn)行粗匹配具體包括；從提取的特征點(diǎn)中截取特征片元，從中選出穩(wěn) 定點(diǎn)集，穩(wěn)定點(diǎn)集中的每個(gè)穩(wěn)定點(diǎn)片元變換得到訓(xùn)練片元；隨機(jī)生成隨機(jī)藤叢和藤判斷節(jié) 點(diǎn)屬性參數(shù)，將訓(xùn)練片元投入隨機(jī)藤叢，得到二值串劃分，統(tǒng)計(jì)各個(gè)二值串劃分的類(lèi)型分 布，輸出一個(gè)完整的隨機(jī)藤叢結(jié)構(gòu)和訓(xùn)練樣本的分類(lèi)統(tǒng)計(jì)；檢測(cè)當(dāng)前帖的自然特征并截取 形成待匹配特征片元，將所有待匹配特征片元投入完整的隨機(jī)藤叢得到二值串劃分，分類(lèi) 器集成各二值串劃分的訓(xùn)練統(tǒng)計(jì)結(jié)果，作為同名特征點(diǎn)的粗匹配結(jié)果。
[0009] 提取特征點(diǎn)進(jìn)一步包括；產(chǎn)生穩(wěn)定的內(nèi)點(diǎn)集ziuniisu，根據(jù)卡爾曼濾波增益矩陣 Kk，預(yù)測(cè)誤差均方矩陣Pk|k_i，預(yù)測(cè)模型評(píng)估的雅可比矩陣Hk，調(diào)用公式：
[0010]
獲得穩(wěn)定的內(nèi)點(diǎn)集ziunlis"的特征向量 V;
[0011] 對(duì)其余的不穩(wěn)定內(nèi)點(diǎn)zhunlku進(jìn)行補(bǔ)救，根據(jù)設(shè)定的補(bǔ)救條件，重新得到卡爾曼濾 波增益矩陣K'k和預(yù)測(cè)模型評(píng)估的雅可比矩陣H'k，并調(diào)用公式：
[001引 ，獲得不穩(wěn)定內(nèi)點(diǎn)zhUnliKs的特征向量是D
[0013]W任一像素點(diǎn)X為圓屯、，作一個(gè)半徑為3個(gè)像素點(diǎn)的圓，圓周上所有像素點(diǎn)按順時(shí) 針依次編號(hào)，根據(jù)公式二二將每個(gè)像素點(diǎn)分別與中屯、像素X 求灰度差，將灰度差值作為特征判斷條件，如果圓周上像素至少有連續(xù)n個(gè)像素灰度 差在預(yù)定闊值范圍內(nèi)，則中屯、像素X被視為圖像的匹配特征點(diǎn)FAST-n。
[0014] 通常遠(yuǎn)距離點(diǎn)可用來(lái)估計(jì)相機(jī)角度變換，而近距離點(diǎn)用來(lái)估計(jì)相機(jī)尺度變換。在 RANSAC假設(shè)中，一個(gè)遠(yuǎn)距離點(diǎn)可W產(chǎn)生一個(gè)對(duì)角度變換較敏感的點(diǎn)，而對(duì)尺度變換不是很 精確。在該種情況下，遠(yuǎn)距離點(diǎn)支持?jǐn)M合模型，但是由于尺度變化的精度不高，附近的點(diǎn)即 使是內(nèi)點(diǎn)也可能表現(xiàn)出較高的不確定性。所W在確定了擬合模型之后，需要從不穩(wěn)定的點(diǎn) 集中"補(bǔ)救"一些內(nèi)點(diǎn)，W提高系統(tǒng)的穩(wěn)定性。在采用可靠?jī)?nèi)點(diǎn)進(jìn)行局部狀態(tài)和協(xié)方差更新 后，一些不穩(wěn)定的內(nèi)點(diǎn)會(huì)被挽救。
[0015] 使用改進(jìn)的FAST角點(diǎn)提取算法相對(duì)于經(jīng)典FAST算法，既可W保持環(huán)境模型的各 向同性，又具有便于計(jì)算機(jī)高效處理的超高速度。采用FAST-9角點(diǎn)檢測(cè)具有最高的可再現(xiàn) 性，依次檢測(cè)圓周像素Pi，Pe，Pg，Pi3,當(dāng)其中連續(xù)S個(gè)點(diǎn)經(jīng)過(guò)灰度差值判斷，滿足灰度差值條 件，就進(jìn)一步檢測(cè)該=個(gè)點(diǎn)所覆蓋圓弧的圓周像素點(diǎn)，若該圓周像素點(diǎn)也滿足灰度差值條 件，就可W判定該點(diǎn)為特征點(diǎn)，否則該點(diǎn)就不為特征點(diǎn)。也可采用本領(lǐng)域技術(shù)人員熟知的方 法其他方法實(shí)現(xiàn)，W下WFAST-9角點(diǎn)檢測(cè)為例對(duì)其實(shí)現(xiàn)方法進(jìn)一步具體說(shuō)明。W像素點(diǎn) X為圓屯、，作一個(gè)半徑為3個(gè)像素點(diǎn)的圓，對(duì)圓周上所有像素點(diǎn)按順時(shí)針依次編號(hào)，將每個(gè) 像素Pi，iG 分別與中屯、像素X求灰度差，灰度差值作為特征判斷條件： Mr氣x=IprIx，Ip,=Ip,，,md、b.〇
[001引圓周像素Pi經(jīng)灰度差值判斷后的狀態(tài)為為像素點(diǎn)X的狀態(tài)。如果 圓周上1-16號(hào)像素中至少有連續(xù)n個(gè)像素點(diǎn)灰度差都大于闊值t或都小于闊值-t，則中屯、 像素點(diǎn)X被視為匹配特征點(diǎn)FAST-n。
[0017] 如公式所示：
[001引使用W上算法遍歷檢測(cè)所有像素，可得到當(dāng)前帖的FAST角點(diǎn)。通過(guò)改變闊值t可 W控制圖像帖的FAST角點(diǎn)個(gè)數(shù)。
[0020] 本發(fā)明采用的改進(jìn)FAST(加速段檢測(cè)特征點(diǎn)）算法，可W剔除經(jīng)典FAST算法在 提取角點(diǎn)時(shí)存在的大部分邊緣點(diǎn)和局部非極大值點(diǎn)；同時(shí)采用一點(diǎn)RANSAC(隨機(jī)抽樣一致 性）算法來(lái)充分利用EKF(擴(kuò)展卡爾曼濾波）預(yù)測(cè)階段W得到先驗(yàn)信息，在確保計(jì)算精度的 同時(shí)有效降低了算法迭代次數(shù)。本發(fā)明提供的移動(dòng)機(jī)器人導(dǎo)航方法能有效地提高特征匹配 的正確率，將匹配過(guò)程中出現(xiàn)的錯(cuò)誤匹配剔除，并能有效提高算法實(shí)時(shí)性。
【附圖說(shuō)明】
[0021] 圖1為移動(dòng)機(jī)器人導(dǎo)航方法流程圖。
【具體實(shí)施方式】
[0022] 圖1所示為基于一點(diǎn)RANSAC和FAST算法的移動(dòng)機(jī)器人導(dǎo)航方法流程圖，其具體 內(nèi)容如下：
[0023] (1)啟動(dòng)移動(dòng)機(jī)器人控制系統(tǒng)，進(jìn)行攝像頭參數(shù)標(biāo)定。
[0024] (2)移動(dòng)機(jī)器人在伺服驅(qū)動(dòng)器和伺服電機(jī)驅(qū)動(dòng)下在環(huán)境中探索行進(jìn)，同時(shí)攝像頭 不停地采集環(huán)境圖像信息。
[0025] (3)從采集的視頻帖中提取特征點(diǎn)。如采用如下方法提?。?br>[0026] 對(duì)相鄰兩個(gè)視頻帖的同名特征點(diǎn)進(jìn)行粗匹配。提取關(guān)鍵帖的自然特征點(diǎn)截取特征 片元，經(jīng)仿射變換選出穩(wěn)定點(diǎn)集，穩(wěn)定點(diǎn)集中的每個(gè)穩(wěn)定點(diǎn)片元經(jīng)仿射變換得到訓(xùn)練片