本發(fā)明涉及視覺slam定位與建圖，更具體地說，涉及一種改進(jìn)點(diǎn)線融合和關(guān)鍵幀選擇的雙目視覺slam方法，用于實(shí)現(xiàn)移動(dòng)機(jī)器人的自主定位與導(dǎo)航。

背景技術(shù)：

1、視覺slam是實(shí)現(xiàn)運(yùn)動(dòng)物體在未知場(chǎng)景下進(jìn)行高精度定位和導(dǎo)航的關(guān)鍵技術(shù)，該技術(shù)在無人駕駛、增強(qiáng)現(xiàn)實(shí)/虛擬現(xiàn)實(shí)、自動(dòng)引導(dǎo)車移動(dòng)小車、移動(dòng)機(jī)器人等領(lǐng)域廣泛應(yīng)用。視覺slam通?？煞譃橹苯臃ê吞卣鼽c(diǎn)法兩類。相較于直接法，特征點(diǎn)法能夠極大限度適應(yīng)場(chǎng)景的光照變化，對(duì)視覺相機(jī)的運(yùn)動(dòng)也不敏感，具有較高的魯棒性。但是當(dāng)場(chǎng)景中紋理信息不足時(shí)，特征點(diǎn)的數(shù)量會(huì)急劇減少，進(jìn)而導(dǎo)致算法的精度降低，給slam技術(shù)的實(shí)際應(yīng)用帶來了極大的障礙。視覺slam系統(tǒng)在估計(jì)相機(jī)位姿的過程中，為避免位姿估計(jì)的累積誤差，需要把所有幀的地圖點(diǎn)與位姿統(tǒng)一進(jìn)行非線性優(yōu)化以提升slam系統(tǒng)的定位精度，但是系統(tǒng)的計(jì)算量也隨之上升，為了實(shí)現(xiàn)計(jì)算機(jī)在有限的數(shù)據(jù)處理能力下也能對(duì)系統(tǒng)定位精度進(jìn)行有效的提升，降低數(shù)據(jù)冗余，優(yōu)化關(guān)鍵幀的選擇是該問題的有效解決方法之一。

2、目前，已經(jīng)存在基于半直接法使用點(diǎn)線特征的視覺里程計(jì)，之后又開源了基于雙目的點(diǎn)線特征slam方法，其在回環(huán)檢測(cè)模塊中充分考慮并權(quán)衡點(diǎn)線特征。但是，這些方法線特征提取的實(shí)時(shí)性、軌跡精度和魯棒性仍然較差。在關(guān)鍵幀選擇方面，相關(guān)研究人員提出啟發(fā)式閾值的關(guān)鍵幀篩選方法，該方法具有更高的魯棒性，盡管這種方法展示了較高的魯棒性，但在面對(duì)紋理特征較弱、光照強(qiáng)度不均或空間狹窄等特殊環(huán)境時(shí)，仍不足以準(zhǔn)確地選擇關(guān)鍵幀，從而影響整個(gè)系統(tǒng)的定位和地圖構(gòu)建性能。

技術(shù)實(shí)現(xiàn)思路

1、1.發(fā)明要解決的技術(shù)問題

2、本發(fā)明使用的點(diǎn)線特征融合方法能夠彌補(bǔ)僅點(diǎn)特征或僅線特征slam系統(tǒng)在復(fù)雜環(huán)境下無法獲取充足特征信息的缺陷；改進(jìn)點(diǎn)線融合后的誤差函數(shù)能夠有效剔除系統(tǒng)中的異常值，提升系統(tǒng)跟蹤匹配的準(zhǔn)確率；通過引入自適應(yīng)因子平衡線特征在光束平差過程中的參與程度，減少系統(tǒng)的計(jì)算量，提升系統(tǒng)的實(shí)時(shí)性；本發(fā)明設(shè)計(jì)的幾何映射模型，能夠有效減少冗余關(guān)鍵幀的生成，增強(qiáng)系統(tǒng)對(duì)復(fù)雜環(huán)境的適應(yīng)能力。

3、2.技術(shù)方案

4、為達(dá)到上述目的，本發(fā)明提供一種基于改進(jìn)點(diǎn)線融合和關(guān)鍵幀選擇的雙目視覺slam方法，在orb-slam3的基礎(chǔ)上線特征提取模塊，提升系統(tǒng)在復(fù)雜環(huán)境下的特征豐富度，利用融合線特征距離誤差和角度測(cè)量誤差，進(jìn)行異常值的有效剔除，提升系統(tǒng)的準(zhǔn)確性；在光束平差過程中，采用設(shè)置自適應(yīng)因子平衡線特征的參與度，動(dòng)態(tài)調(diào)節(jié)點(diǎn)、線特征的作用效果，提升系統(tǒng)的靈活度；在跟蹤線程決定是否插入關(guān)鍵幀的約束條件上，通過設(shè)置動(dòng)態(tài)閾值選擇最佳的關(guān)鍵幀，提高了系統(tǒng)的準(zhǔn)確性，實(shí)現(xiàn)slam系統(tǒng)的精準(zhǔn)定位。本發(fā)明公開了一種基于改進(jìn)點(diǎn)線融合和關(guān)鍵幀選擇的雙目視覺slam方法，包括如下步驟：

5、步驟1、選擇orb-salm3作為主體框架，增加線特征提取線程，彌補(bǔ)復(fù)雜場(chǎng)景中特征信息不足的缺陷，利用融合加權(quán)線特征的距離誤差和角度測(cè)量誤差剔除異常值；

6、步驟2、光束平差過程中引入自適應(yīng)因子平衡線特征的參與程度；

7、步驟3、改進(jìn)關(guān)鍵幀的選擇策略；

8、步驟4、進(jìn)行位姿誤差估計(jì)。

9、更進(jìn)一步地，步驟1所述線特征的檢測(cè)和描述使用lsd檢測(cè)器和lbd描述符(linebanddiscriptor)，在光束平差(bundle?adjustment，ba)優(yōu)化過程中，由于特征提取、匹配或環(huán)境場(chǎng)景的光照變化等原因?qū)е聅lam系統(tǒng)存在少量的離群點(diǎn)，本文使用兩種誤差函數(shù)，一種是傳統(tǒng)的最小化從重投影的端點(diǎn)到觀測(cè)線段的距離，在此基礎(chǔ)上，增加角度測(cè)量誤差函數(shù)進(jìn)一步約束slam系統(tǒng)對(duì)于離群值的剔除。

10、更進(jìn)一步地，步驟1所述的距離誤差函數(shù)使用重投影的三維線段的端點(diǎn)與其在圖像平面上對(duì)應(yīng)的直線端點(diǎn)之間的歐幾里得距離，距離誤差函數(shù)表示為：

11、

12、更進(jìn)一步地，步驟1所述的角度距離誤差函數(shù)采用重投影線段和觀測(cè)線段之間夾角的余弦值，角度測(cè)量誤差函數(shù)表示為：

13、

14、更進(jìn)一步地，步驟2中所述的光束平差是跟蹤線程中的僅運(yùn)動(dòng)光束平差，只對(duì)相機(jī)參數(shù)(運(yùn)動(dòng)參數(shù))進(jìn)行優(yōu)化，對(duì)3d結(jié)構(gòu)(地標(biāo))保持不變；

15、更進(jìn)一步地，步驟2中引入的自適應(yīng)因子根據(jù)特征點(diǎn)的集群數(shù)目的總和值動(dòng)態(tài)地調(diào)整，在實(shí)際應(yīng)用中，通過觀測(cè)初始觀測(cè)幀中參與匹配關(guān)聯(lián)特征的數(shù)量，根據(jù)slam系統(tǒng)特征提取的一般性能情況，自適應(yīng)因子表示為：

16、

17、更進(jìn)一步地，步驟3中所述改進(jìn)關(guān)鍵幀的選擇策略通過在幾何和時(shí)間上進(jìn)行約束，選擇那些能最大限度增加環(huán)境理解的關(guān)鍵幀還可以減少冗余計(jì)算。

18、更進(jìn)一步地，步驟3中的幾何約束通過基于特征點(diǎn)在不同幀間的數(shù)量變化建立幾何映射模型。當(dāng)幀從上一個(gè)關(guān)鍵幀移動(dòng)到當(dāng)前幀時(shí)，矢量在每個(gè)區(qū)域內(nèi)對(duì)應(yīng)的特征點(diǎn)數(shù)量也發(fā)生變化，基于這些變化點(diǎn)的數(shù)量，確定動(dòng)態(tài)閾值，以決定是否插入新的關(guān)鍵幀。動(dòng)態(tài)閾值表示為：

19、t＝(1+εθ+α-β)·tinitial

20、其中，tinitial代表關(guān)鍵幀選擇策略中的初始閾值，定義為：

21、

22、nc表示當(dāng)前幀與上一關(guān)鍵幀對(duì)應(yīng)特征點(diǎn)的數(shù)量，nr表示參考幀與上一關(guān)鍵幀對(duì)應(yīng)特征點(diǎn)的數(shù)量，bc表示當(dāng)前幀中所有特征點(diǎn)的數(shù)量，br表示參考幀中所有特征點(diǎn)的數(shù)量，cc表示當(dāng)前幀與上一關(guān)鍵幀的映射區(qū)域中發(fā)生特征點(diǎn)變化的數(shù)量，cr表示參考幀與上一關(guān)鍵幀的幾何映射平面中發(fā)生特征點(diǎn)變化的數(shù)量。

23、θ系數(shù)定義為：

24、

25、ε系數(shù)為：

26、

27、系數(shù)α：

28、

29、β定義為：

30、

31、更進(jìn)一步地，步驟4中本發(fā)明使用euroc數(shù)據(jù)集進(jìn)行改進(jìn)點(diǎn)線融合和關(guān)鍵幀選擇后的位姿誤差估計(jì)，測(cè)試中分別對(duì)orbs-slam3、pl-slam進(jìn)行對(duì)照實(shí)驗(yàn)，通過測(cè)評(píng)軌跡與真值的貼合程度及ate值來評(píng)估算法的優(yōu)越性。

32、更進(jìn)一步地，步驟4中為了全面評(píng)估本發(fā)明的性能，選擇具有代表性的弱紋理會(huì)議室場(chǎng)景作為真實(shí)測(cè)試環(huán)境，以回到起點(diǎn)的距離作為誤差判定實(shí)驗(yàn)結(jié)果，以檢驗(yàn)系統(tǒng)的抗干擾能力。

33、3.有益效果

34、采用本發(fā)明提供的技術(shù)方案，與已有的公知技術(shù)相比，具有如下顯著效果：

35、(1)本發(fā)明的一種基于改進(jìn)點(diǎn)線融合和關(guān)鍵幀選擇的雙目視覺slam方法，在前端特征提取模塊增加特征線提取模塊，有效彌補(bǔ)了在弱紋理環(huán)境下特征信息提取不足的缺陷，同時(shí)根據(jù)運(yùn)動(dòng)估計(jì)方法，對(duì)線特征的匹配進(jìn)行約束，篩選出符合條件的線段；在線段重投影誤差部分通過融合歐幾里得距離和角度距離兩種方法的誤差函數(shù)，并對(duì)其加權(quán)處理，實(shí)現(xiàn)異常值的剔除；在此基礎(chǔ)上，針對(duì)光束平差法中的點(diǎn)線特征選擇問題，引入自適應(yīng)因子衡量線段在平差過程中的參與程度。在關(guān)鍵幀選擇方面，本發(fā)明依據(jù)幾何和攝影測(cè)量原理設(shè)計(jì)了一種關(guān)鍵幀選擇方法，并依據(jù)相機(jī)的幀率設(shè)定動(dòng)態(tài)閾值，以避免插入冗余關(guān)鍵幀，從而減少對(duì)slam系統(tǒng)魯棒性和實(shí)時(shí)性的影響。

36、(2)視覺slam依賴于從圖像中提取的特征來估計(jì)相機(jī)的位置和姿態(tài)。異常值，如錯(cuò)誤的特征匹配或由動(dòng)態(tài)物體引起的誤差，可以導(dǎo)致誤導(dǎo)性的位姿估計(jì)。剔除這些異常值有助于提供更準(zhǔn)確的位置數(shù)據(jù)，確保slam系統(tǒng)正確地理解和反映其環(huán)境。對(duì)于線特征，通過傳統(tǒng)方法，對(duì)其端點(diǎn)進(jìn)行距離誤差判定并剔除異常值的方式存在局限性，本發(fā)明通過增加角度誤差約束，能夠解決由相機(jī)運(yùn)動(dòng)產(chǎn)生位移，出現(xiàn)線段間遮擋的情況，導(dǎo)致線段端點(diǎn)存在不穩(wěn)定性的不情況。

37、(3)slam系統(tǒng)中，點(diǎn)特征相較于線特征具有更簡(jiǎn)單的幾何結(jié)構(gòu)、較少的遮擋問題以及更快的匹配速度，本發(fā)明通過設(shè)定一種自適應(yīng)因子來根據(jù)當(dāng)前場(chǎng)景的特征分布(如點(diǎn)的豐富程度)動(dòng)態(tài)平衡點(diǎn)特征與線特征的貢獻(xiàn)，盡可能減少線特征的參與，提高整個(gè)slam系統(tǒng)的準(zhǔn)確性。

38、(4)slam系統(tǒng)需要實(shí)時(shí)運(yùn)行，而地圖的創(chuàng)建和更新通常涉及大量的計(jì)算，本發(fā)明通過合理選擇關(guān)鍵幀，減少需要處理的數(shù)據(jù)量，減輕計(jì)算負(fù)擔(dān)。在復(fù)雜環(huán)境中，基于幾何約束選擇關(guān)鍵幀的方法可以確保所選幀在視角和位置上有足夠的變化，選擇那些具有明顯特征和穩(wěn)定幾何結(jié)構(gòu)的幀，可以增加系統(tǒng)對(duì)處理視角變化和存在遮擋的幀的魯棒性。