(19)國(guó)家知識(shí)產(chǎn)權(quán)局(12)發(fā)明專(zhuān)利381號(hào)限公司44245GO6V20/10(2022.01)本發(fā)明公開(kāi)了一種面向動(dòng)態(tài)環(huán)境的單目視21.一種面向動(dòng)態(tài)環(huán)境的單目視覺(jué)SLAM方法，其特征在于，包括以下步驟：1)初始化，包括讀取圖像序列的前兩幀、提取并匹配ORB特征點(diǎn)、建立世界坐標(biāo)系、設(shè)置單目尺度信息、建立初始地圖、構(gòu)建關(guān)鍵幀序列以及關(guān)鍵幀滑動(dòng)窗口；2)讀取圖像序列作為當(dāng)前幀，將上一幀設(shè)為當(dāng)前幀的參考幀，完成當(dāng)前幀與參考幀的3)根據(jù)步驟2)中的相對(duì)位姿，比較當(dāng)前幀與參考幀匹配特征點(diǎn)對(duì)的幾何信息一致性和光度信息一致性，剔除不一致的匹配特征點(diǎn)對(duì)以及對(duì)應(yīng)的地圖點(diǎn)，并根據(jù)剩余的匹配特征點(diǎn)對(duì)，通過(guò)最小化重投影誤差函數(shù)優(yōu)化當(dāng)前幀與參考幀的相對(duì)位姿；4)若步驟3)中剩余的匹配特征點(diǎn)對(duì)數(shù)小于設(shè)置的閾值T,或者當(dāng)前幀與當(dāng)前關(guān)鍵幀間隔幀數(shù)超過(guò)閾值T,則根據(jù)當(dāng)前幀構(gòu)造新的關(guān)鍵幀；5)若步驟4)構(gòu)造了新的關(guān)鍵幀，則將新的關(guān)鍵幀插入關(guān)鍵幀序列和滑動(dòng)窗口，將新的關(guān)鍵幀設(shè)為當(dāng)前關(guān)鍵幀，將上一關(guān)鍵幀設(shè)為當(dāng)前關(guān)鍵幀的參考關(guān)鍵幀；然后根據(jù)步驟3)優(yōu)化后的相機(jī)位姿，將當(dāng)前關(guān)鍵幀與參考關(guān)鍵幀進(jìn)行特征點(diǎn)匹配，其中，對(duì)于未初始化地圖點(diǎn)的匹配特征點(diǎn)對(duì)，則通過(guò)三角化方法構(gòu)造新的地圖點(diǎn)并插入地圖M;6)根據(jù)幾何信息一致性，識(shí)別并剔除滑動(dòng)窗口上的動(dòng)態(tài)地圖點(diǎn)，然后在滑動(dòng)窗口上進(jìn)行局部光束平差優(yōu)化；7)若滑動(dòng)窗口中關(guān)鍵幀個(gè)數(shù)達(dá)到最大值T,并且當(dāng)前關(guān)鍵幀位姿T,與參考關(guān)鍵幀位姿,則剔除滑動(dòng)窗口中的參考關(guān)鍵幀，,則剔除滑動(dòng)窗全部關(guān)鍵幀及地圖點(diǎn)，若關(guān)鍵幀A所觀測(cè)到地圖點(diǎn)的90%以上都被至少三個(gè)其它關(guān)鍵幀所觀測(cè)，則剔除該關(guān)鍵幀A,若地圖點(diǎn)B被觀測(cè)次數(shù)少于三次，則剔除該地圖點(diǎn)B;8)完成步驟7)后，新開(kāi)啟一條線程進(jìn)行一次全局光束平差優(yōu)化；重復(fù)步驟2)至步驟8),直到序列中的圖像全部處理完。2.根據(jù)權(quán)利要求1所述的一種面向動(dòng)態(tài)環(huán)境的單目視覺(jué)SLAM方法，其特征在于，在步驟1)中，將讀取的RGB圖像轉(zhuǎn)化為灰度圖，將灰度圖連續(xù)降采樣構(gòu)造圖像金字塔，根據(jù)每層圖像分辨率大小提取相應(yīng)數(shù)量的FAST角點(diǎn)，并計(jì)算全部角點(diǎn)的BRIEF描述子，根據(jù)描述子之間的漢明距離，在兩幀之間進(jìn)行特征點(diǎn)匹配；隨后將世界坐標(biāo)系設(shè)為第一幀的位姿，并通過(guò)隨機(jī)采樣一致性計(jì)算第二幀的位姿，具體包括以下步驟：1.1)隨機(jī)選取8對(duì)匹配的特征點(diǎn)，利用八點(diǎn)法計(jì)算本質(zhì)矩陣E1?;1.2)根據(jù)步驟1.1)的結(jié)果，檢驗(yàn)全部匹配特征點(diǎn)對(duì)(q?1,qi?)的對(duì)極約束偏差ea=|9:2K-"E??K-19|,若ea>T,則將匹配特征點(diǎn)對(duì)標(biāo)記為離群值，否則標(biāo)記為內(nèi)圍值，其1.3)根據(jù)步驟1.2)的結(jié)果，若內(nèi)圍值匹配特征點(diǎn)對(duì)數(shù)占總匹配特征點(diǎn)對(duì)數(shù)比例超過(guò)閾值T,0.7<T.≤1,則對(duì)步驟1.1)的本質(zhì)矩陣進(jìn)行奇異值分解，并得到第一幀與第二幀的相對(duì)位姿T??,其中，令位姿中的平移向量為單位向量，即確定了尺度信息；若內(nèi)圍值比例低于T,則重復(fù)步驟1.1)至1.3)直到內(nèi)圍值比例不低于T;若全部組合選取完，仍未找到滿(mǎn)足內(nèi)圍值比例大于T的組合，則選擇內(nèi)圍值比例最大的組合對(duì)應(yīng)的本質(zhì)矩陣進(jìn)行奇異值分解，并得到T?2;31.4)根據(jù)步驟1.3)的結(jié)果，將全部?jī)?nèi)圍值進(jìn)行三角化計(jì)算得到三維地圖點(diǎn)，并構(gòu)建初前關(guān)鍵幀。征點(diǎn)通過(guò)BRIEF描述子在參考幀中尋找最佳特征點(diǎn)匹配，若最佳特征點(diǎn)匹配的漢明距大于T;若選取完全部組合，仍無(wú)法滿(mǎn)足條件，則選擇內(nèi)圍值比例最高的位姿作為估計(jì)位3.1)根據(jù)步驟2)中標(biāo)記為內(nèi)圍值的特征點(diǎn)對(duì)，計(jì)征點(diǎn)對(duì)的區(qū)域像素值差值大于閾值Tg,則將相應(yīng)的匹配特征點(diǎn)對(duì)標(biāo)記為動(dòng)態(tài)特征點(diǎn)并移除匹配特征點(diǎn)對(duì)及其對(duì)應(yīng)的地圖點(diǎn)；3.2)對(duì)于經(jīng)過(guò)步驟3.1)檢驗(yàn)并具有三維地圖點(diǎn)的3.3)根據(jù)步驟3.2)過(guò)濾后的匹配特征點(diǎn)對(duì)，通過(guò)求解重投影誤差函數(shù)優(yōu)化相對(duì)位姿，4以下步驟：5.1)根據(jù)步驟3)優(yōu)化后的相對(duì)位姿，通過(guò)累乘的5.2)根據(jù)步驟5.1)的本質(zhì)矩陣，將當(dāng)前關(guān)鍵幀中的特征點(diǎn)二維像素坐標(biāo)通過(guò)對(duì)極約束計(jì)算得到參考關(guān)鍵幀圖像上對(duì)應(yīng)的二維坐標(biāo)點(diǎn)，并在計(jì)算得到的二維坐標(biāo)點(diǎn)附近通過(guò)6.1)將滑動(dòng)窗口上的全部地圖點(diǎn)投影到被觀測(cè)到的關(guān)鍵幀上獲得二維投影坐標(biāo)，構(gòu)造個(gè)地圖點(diǎn)與第j個(gè)關(guān)鍵幀構(gòu)造的重投影誤差，具體的表達(dá)式為if關(guān)鍵幀j觀測(cè)到地圖點(diǎn)P”,T,?為世界坐標(biāo)系到第j個(gè)關(guān)鍵幀的相對(duì)位姿，P"為世界坐6.2)將步驟6.1)的誤差函數(shù)作為最小化的目標(biāo)函數(shù)，迭56.3)通過(guò)最小化重投影誤差函數(shù)來(lái)優(yōu)化滑動(dòng)窗口上的關(guān)表示為：表示地圖中地圖點(diǎn)的索引號(hào)，b=1,2,…,Na,Tg,表示世界坐標(biāo)系到第8個(gè)關(guān)鍵幀的相對(duì)位重投影誤差，具體表達(dá)式為if關(guān)鍵幀g觀測(cè)到地圖點(diǎn)P”,π(·)為去6技術(shù)領(lǐng)域[0001]本發(fā)明涉及視覺(jué)同時(shí)定位與建圖的技術(shù)領(lǐng)域，尤其是指一種面向動(dòng)態(tài)環(huán)境的單目背景技術(shù)[0002]視覺(jué)同時(shí)定位與建圖(VisualSimultaneousLocalizationandMapping,簡(jiǎn)稱(chēng)視覺(jué)SLAM)是無(wú)人自主系統(tǒng)的關(guān)鍵技術(shù)之一，其作用是使得智能機(jī)器人等在陌生環(huán)境中通過(guò)可見(jiàn)光相機(jī)感知周邊環(huán)境信息而不依賴(lài)于外接設(shè)備，即可完成自身位姿的精確測(cè)量和環(huán)[0003]當(dāng)前，關(guān)于視覺(jué)SLAM研究的主要成果是基于靜態(tài)環(huán)境的假設(shè)，這些方法在靜態(tài)環(huán)境中可以獲取良好的工作效果。但是，實(shí)際應(yīng)用中不可避免地會(huì)出現(xiàn)運(yùn)動(dòng)物體，例如無(wú)人駕駛汽車(chē)不可能行駛在完全靜態(tài)的高速公路上，智能機(jī)器人在執(zhí)行任務(wù)過(guò)程中經(jīng)常遇到行人、動(dòng)物、車(chē)輛等動(dòng)態(tài)物體。環(huán)境中出現(xiàn)的動(dòng)態(tài)物體會(huì)導(dǎo)致現(xiàn)有基于靜態(tài)假設(shè)的主流視覺(jué)SLAM方法性能下降，甚至無(wú)法工作。因此，缺乏對(duì)運(yùn)動(dòng)物體的考慮限制了當(dāng)前主流基于靜態(tài)[0004]綜合以上論述，發(fā)明一種可工作于動(dòng)態(tài)環(huán)境下的視覺(jué)SLAM方法，具有較高的實(shí)際應(yīng)用價(jià)值。發(fā)明內(nèi)容[0005]本發(fā)明的目的在于克服現(xiàn)有技術(shù)的缺點(diǎn)與不足，提出了一種面向動(dòng)態(tài)環(huán)境的單目視覺(jué)SLAM方法，根據(jù)運(yùn)動(dòng)一致性的原理進(jìn)行動(dòng)態(tài)特征點(diǎn)識(shí)別與剔除，進(jìn)而通過(guò)靜態(tài)特征點(diǎn)估計(jì)相機(jī)位姿并構(gòu)建稀疏地圖。該方法分為兩個(gè)線程執(zhí)行：追蹤線程和建圖線程。在追蹤線核心環(huán)節(jié)是動(dòng)態(tài)特征點(diǎn)過(guò)濾，通過(guò)比較當(dāng)前幀與上一幀匹配特征點(diǎn)的幾何信息和光度信息一致性進(jìn)行快速運(yùn)動(dòng)物體的特征點(diǎn)識(shí)別與剔除。建圖線程是在追蹤線程插入關(guān)鍵幀之后執(zhí)行，主要完成動(dòng)態(tài)地圖點(diǎn)剔除、靜態(tài)地圖構(gòu)建和全局光束平差優(yōu)化，其中的核心環(huán)節(jié)是動(dòng)態(tài)地圖點(diǎn)剔除，通過(guò)比較滑動(dòng)窗口中關(guān)鍵幀上地圖點(diǎn)的幾何信息一致性進(jìn)行緩慢運(yùn)動(dòng)物體上的動(dòng)態(tài)地圖點(diǎn)識(shí)別與剔除。通過(guò)以上兩處的動(dòng)態(tài)點(diǎn)識(shí)別與剔除操作，有效減少了動(dòng)態(tài)特征點(diǎn)對(duì)視覺(jué)SLAM方法的影響，并且僅利用靜態(tài)特征點(diǎn)信息完成了位姿估計(jì)和稀疏地圖構(gòu)建，提升了視覺(jué)SLAM方法在動(dòng)態(tài)環(huán)境中的魯棒性及精度。[0006]為實(shí)現(xiàn)上述目的，本發(fā)明所提供的技術(shù)方案為：一種面向動(dòng)態(tài)環(huán)境的單目視覺(jué)設(shè)置單目尺度信息、建立初始地圖、構(gòu)建關(guān)鍵幀序列以及關(guān)鍵幀滑動(dòng)窗口；[0008]2)讀取圖像序列作為當(dāng)前幀，將上一幀設(shè)為當(dāng)前幀的參考幀，完成當(dāng)前幀與參考7幀的特征匹配并估計(jì)相對(duì)位姿Tk-1,k,其中，當(dāng)前幀為[0009]3)根據(jù)步驟2)中的相對(duì)位姿，比較當(dāng)前幀與參考幀匹配特征點(diǎn)對(duì)的幾何信息一致性和光度信息一致性，剔除不一致的匹配特征點(diǎn)對(duì)以及對(duì)應(yīng)的地圖點(diǎn)，并根據(jù)剩余的匹配特征點(diǎn)對(duì)，通過(guò)最小化重投影誤差函數(shù)優(yōu)化當(dāng)前幀與參考幀的相對(duì)位姿；[0010]4)若步驟3)中剩余的匹配特征點(diǎn)對(duì)數(shù)小于設(shè)置的閾值T,或者當(dāng)前幀與當(dāng)前關(guān)鍵幀間隔幀數(shù)超過(guò)閾值T,則根據(jù)當(dāng)前幀構(gòu)造新的關(guān)鍵幀；[0011]5)若步驟4)構(gòu)造了新的關(guān)鍵幀，則將新的關(guān)鍵幀插入關(guān)鍵幀序列和滑動(dòng)窗口，將新的關(guān)鍵幀設(shè)為當(dāng)前關(guān)鍵幀，將上一關(guān)鍵幀設(shè)為當(dāng)前關(guān)鍵幀的參考關(guān)鍵幀；然后根據(jù)步驟3)優(yōu)化后的相機(jī)位姿，將當(dāng)前關(guān)鍵幀與參考關(guān)鍵幀進(jìn)行特征點(diǎn)匹配，其中，對(duì)于未初始化地圖點(diǎn)的匹配特征點(diǎn)對(duì)，則通過(guò)三角化方法構(gòu)造新的地圖點(diǎn)并插入地圖M;[0012]6)根據(jù)幾何信息一致性，識(shí)別并剔除滑動(dòng)窗口上的動(dòng)態(tài)地圖點(diǎn)，然后在滑動(dòng)窗上進(jìn)行局部光束平差優(yōu)化；[0013]7)若滑動(dòng)窗口中關(guān)鍵幀個(gè)數(shù)達(dá)到最大值T,并且當(dāng)前關(guān)鍵幀位姿Tn,w與參考關(guān)鍵后遍歷全部關(guān)鍵幀及地圖點(diǎn)，若關(guān)鍵幀所觀測(cè)到地圖點(diǎn)的90%以上都被至少三個(gè)其它關(guān)鍵[0014]8)完成步驟7)后，新開(kāi)啟一條線程進(jìn)行一次全局光束平差優(yōu)化；重復(fù)步驟2)至步驟8),直到序列中的圖像全部處理完。[0015]進(jìn)一步，在步驟1)中，將讀取的RGB圖像轉(zhuǎn)化為灰度圖，將灰度圖連續(xù)降采樣構(gòu)造圖像金字塔，根據(jù)每層圖像分辨率大小提取相應(yīng)數(shù)量的FAST角點(diǎn)，并計(jì)算全部角點(diǎn)的BRIEF描述子，根據(jù)描述子之間的漢明距離，在兩幀之間進(jìn)行特征點(diǎn)匹配；隨后將世界坐標(biāo)系設(shè)為第一幀的位姿，并通過(guò)隨機(jī)采樣一致性計(jì)算第二幀的位姿，具體包括以下步驟：[0016]1.1)隨機(jī)選取8對(duì)匹配的特征點(diǎn)，利用八點(diǎn)法計(jì)算本質(zhì)矩陣E12;[0017]1.2)根據(jù)步驟1.1)的結(jié)果，檢驗(yàn)全部匹配特征點(diǎn)對(duì)(q;1,q;2)的對(duì)極約束偏差則將匹配特征點(diǎn)對(duì)標(biāo)記為離群值(outlier),否則標(biāo)記為內(nèi)圍值(inlier),其中，q;1和q;2分別是第一幀和第二幀特征點(diǎn)的齊次坐標(biāo)，K是相機(jī)內(nèi)參矩[0018]1.3)根據(jù)步驟1.2)的結(jié)果，若內(nèi)圍值匹配特征點(diǎn)對(duì)數(shù)占總匹配特征點(diǎn)對(duì)數(shù)比例超過(guò)閾值T,0.7<T.≤1,則對(duì)步驟1.1)的本質(zhì)矩陣進(jìn)行奇異值分解(SVD),并得到第一幀與第二幀的相對(duì)位姿T1?,其中，令位姿中的平移向量為單位向量，即確定了尺度信息；若內(nèi)圍值比例低于T,則重復(fù)步驟1.1)至1.3)直到內(nèi)圍值比例不低于T;若全部組合選取完，仍未找到滿(mǎn)足內(nèi)圍值比例大于T的組合，則選擇內(nèi)圍值比例最大的組合對(duì)應(yīng)的本質(zhì)矩陣進(jìn)行奇異[0019]1.4)根據(jù)步驟1.3)的結(jié)果，將全部?jī)?nèi)圍值進(jìn)行三角化(Triangulation)計(jì)算得到三維地圖點(diǎn)，并構(gòu)建初始地圖M;根據(jù)第一幀構(gòu)造關(guān)鍵幀KF?,插入到關(guān)鍵幀序列和滑動(dòng)窗[0020]進(jìn)一步，在步驟2)中，首先將讀取的RGB圖轉(zhuǎn)換為灰度圖，連續(xù)降采樣構(gòu)造圖像金8若特征點(diǎn)對(duì)的區(qū)域像素值差值大于閾值Tg,則將相應(yīng)的匹配特征點(diǎn)對(duì)標(biāo)記為動(dòng)態(tài)特征點(diǎn)并之后地圖點(diǎn)的索引號(hào)，1=1,2,...,N′,argmin(·)表示函數(shù)取最小值時(shí)的參數(shù)取值，π內(nèi)參；9[0038]6.1)將滑動(dòng)窗口上的全部地圖點(diǎn)投影到被觀測(cè)到的關(guān)鍵幀上獲得二維投影坐,if關(guān)鍵幀觀測(cè)到地圖點(diǎn)P",Tj,w為世界坐標(biāo)系到第j個(gè)關(guān)鍵幀的相[0041]6.2)將步驟6.1)的誤差函數(shù)作為最小化的目標(biāo)函數(shù)，迭代優(yōu)化三維地圖點(diǎn)坐[0044]6.3)通過(guò)最小化重投影誤差函數(shù)來(lái)優(yōu)化滑動(dòng)窗口上的關(guān)鍵幀位姿和地圖點(diǎn)坐[0047]進(jìn)一步，在步驟8)中，開(kāi)啟一條新的線程對(duì)全部關(guān)鍵幀位姿和地圖點(diǎn)位置進(jìn)行優(yōu)Nsw,b表示地圖中地圖點(diǎn)的索引號(hào)，b=1,2,...,Naw,Tg,表示世界坐標(biāo)系到第8個(gè)關(guān)鍵幀的相對(duì)位姿，P“表示地圖點(diǎn)坐標(biāo)，argmin(·)表示函數(shù)取最小值時(shí)的參數(shù)取值，e′是待優(yōu)化的重投影誤差函數(shù)，其具體表達(dá)式為是地圖點(diǎn)P“與關(guān)鍵幀Tg,構(gòu)造的重投影誤差，具體表達(dá)式為if關(guān)鍵幀g觀測(cè)到地圖點(diǎn)?,[0050]本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)與有益效果：[0051]1、本發(fā)明是適用于動(dòng)態(tài)環(huán)境下的單目視覺(jué)SLAM方法。當(dāng)前主流的視覺(jué)SLAM方法都是基于靜態(tài)環(huán)境的假設(shè)，較少關(guān)注動(dòng)態(tài)物體造成的影響。[0052]2、本發(fā)明使用單目相機(jī)作為環(huán)境感知設(shè)備，在室內(nèi)外環(huán)境都能良好工作。目前很多針對(duì)于動(dòng)態(tài)環(huán)境的視覺(jué)SLAM方法的技術(shù)發(fā)明都是基于RGB-D相機(jī)，因?yàn)檫@些技術(shù)必須用到相機(jī)提供的深度圖像。與這類(lèi)技術(shù)相比，本發(fā)明的新穎之處在于動(dòng)態(tài)特征點(diǎn)的識(shí)別策略不同。本發(fā)明通過(guò)比較特征點(diǎn)對(duì)的光度值差異以及對(duì)應(yīng)地圖點(diǎn)在優(yōu)化過(guò)程中的較大波動(dòng)性，來(lái)識(shí)別動(dòng)態(tài)特征點(diǎn)，而其它的方法經(jīng)常需要直接比較深度值的絕對(duì)差異。[0054]4、本發(fā)明在追蹤線程和建圖線程設(shè)計(jì)了不同的動(dòng)態(tài)地圖點(diǎn)識(shí)別和剔除策略，綜合附圖說(shuō)明[0055]圖1為本發(fā)明方法實(shí)施例的流程框圖。具體實(shí)施方式[0056]下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述，但本發(fā)明的實(shí)施方式不限于此。[0057]本實(shí)施例公開(kāi)了一種面向動(dòng)態(tài)環(huán)境的單目視覺(jué)SLAM方法，如圖1所示，該方法分為征提取與匹配、動(dòng)態(tài)特征點(diǎn)過(guò)濾、初始位姿估計(jì)和關(guān)鍵幀決策，其中的核心環(huán)節(jié)是動(dòng)態(tài)特征點(diǎn)過(guò)濾，通過(guò)比較當(dāng)前幀與上一幀匹配特征點(diǎn)的幾何信息和光度信息一致性進(jìn)行快速運(yùn)動(dòng)物體的特征點(diǎn)識(shí)別與剔除。建圖線程是在追蹤線程插入關(guān)鍵幀之后執(zhí)行，主要完成動(dòng)態(tài)地較滑動(dòng)窗口中關(guān)鍵幀上地圖點(diǎn)的幾何信息一致性進(jìn)行緩慢運(yùn)動(dòng)物體上的動(dòng)態(tài)地圖點(diǎn)識(shí)別圍值(inlier),其中，q?1和q:2分別是第一幀和第過(guò)閾值T,0.7<T?≤1(優(yōu)選0.8),則對(duì)步驟1.1)的本質(zhì)矩陣進(jìn)行奇異值分解(SVD),并得到第一幀與第二幀的相對(duì)位姿T??,其中，令位姿中的平移向量為單位向量，即確定了尺息；若內(nèi)圍值比例低于T,則重復(fù)步驟1.1)至1.3)直到內(nèi)圍值比例不低于T;若全部組合選取完，仍未找到滿(mǎn)足內(nèi)圍值比例大于T的組合，則選擇內(nèi)圍值比例最大的組合對(duì)應(yīng)的本質(zhì)三維地圖點(diǎn)，并構(gòu)建初始地圖M;根據(jù)第一幀構(gòu)造關(guān)鍵幀KF?,插入到關(guān)鍵幀序列和滑動(dòng)窗口