一種基于語義光束平差法的slam方法
【專利摘要】本發(fā)明提供了一種基于語義光束平差法的SLAM方法，屬于移動(dòng)機(jī)器人同步定位與地圖創(chuàng)建(SLAM)領(lǐng)域。其特征在于：本發(fā)明所述的方法通過新的語義上的全局的優(yōu)化來聯(lián)合6DOF物體和相機(jī)姿態(tài)，可以在2D或者3D傳感器下工作。由于添加了語義上的信息，目標(biāo)檢測(cè)通道無需外設(shè)，就可以無縫的整合到任何基于BA的SLAM系統(tǒng)的BA類型優(yōu)化系統(tǒng)中。本發(fā)明所述的方法簡單、易實(shí)現(xiàn)，并且具有很強(qiáng)的實(shí)用性，SLAM約束可以被用于健壯的目標(biāo)檢測(cè)，可以適應(yīng)更復(fù)雜的環(huán)境。
【專利說明】
-種基于語義光束平差法的SLAM方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及一種基于語義光束平差法SLAM(simultaneous localization and mapping)方法，屬于移動(dòng)機(jī)器人同步定位與地圖創(chuàng)建（SLAM)領(lǐng)域。
【背景技術(shù)】
[0002] 視覺的SLAM(同時(shí)定位和地圖構(gòu)建）問題設(shè)及到逐漸的重構(gòu)地圖和同時(shí)只根據(jù)視 覺線索定位傳感裝置的能力。在過去十年里，隨著如AR(增強(qiáng)現(xiàn)實(shí)技術(shù)）和機(jī)器導(dǎo)航和構(gòu) 圖等更有效的工具的應(yīng)用，運(yùn)個(gè)領(lǐng)域有著卓越的進(jìn)步。
[0003] 傳統(tǒng)SLAM問題的解決方式是基于如卡爾曼濾波器一樣的濾波技術(shù)，運(yùn)用運(yùn)種方 式，視覺的特征是根據(jù)帖和它們的估計(jì)出的3D位置和不確定的攝像機(jī)位置來追蹤的。由于 只有很小部分的圖像像素被追蹤，所W運(yùn)個(gè)方法只能創(chuàng)建稀疏地圖。作為一種選擇，視覺的 SLAM問題可W通過優(yōu)化攜帶一系列選定的帖的子集的BA(光束平差法）來解決。近年來， 各種各樣的基于BA模式的方法相繼問世。一個(gè)成功的BA模式是PTAM(并行的跟蹤和地圖 構(gòu)建）。運(yùn)種方式提出了一種分開SLAM問題為2個(gè)并行的相關(guān)的任務(wù)：一個(gè)任務(wù)是根據(jù)當(dāng) 前評(píng)估的標(biāo)志位置來跟蹤攝像機(jī)；另一個(gè)負(fù)責(zé)管理選定的關(guān)鍵帖的全局優(yōu)化。由于復(fù)雜性 隨著環(huán)境中提取出來的特征的數(shù)量而迅猛增加，PTAM只能夠在很小的空間中使用。為了克 服運(yùn)個(gè)缺陷，對(duì)運(yùn)個(gè)方案進(jìn)行優(yōu)化，不考慮整個(gè)的過去的追蹤的關(guān)鍵帖，而是其中的一小部 分，從而使算法達(dá)到一個(gè)常量的時(shí)間復(fù)雜度。
[0004] 由于跟蹤和構(gòu)圖任務(wù)已經(jīng)達(dá)到一定程度的成熟，在視覺SLAM過程中，沒有一個(gè)曾 經(jīng)的方法能夠無縫的解決和提取語義上的信息。目前已經(jīng)有了一些基于SLAM框架的算法， 大多數(shù)是在一個(gè)視角上進(jìn)行目標(biāo)檢測(cè)，并不檢測(cè)多視圖的一致性。有些方案中盡管檢測(cè)方 案和SLAM框架精密結(jié)合，但并沒有試圖估算出目標(biāo)物體的實(shí)際位置；其余的利用幾何信息 來連續(xù)的目標(biāo)檢測(cè)，但是沒有有效的利用原來的探測(cè)出的物體來構(gòu)建已知地圖；還有一些 算法，使用標(biāo)準(zhǔn)的基于特征的管道來檢測(cè)物體并使用估計(jì)相對(duì)姿勢(shì)來做位置表述，它們運(yùn) 用激光和測(cè)程數(shù)據(jù)來探測(cè)地圖上的物體，但是運(yùn)種算法仍然沒有把物體與攝像機(jī)的位置進(jìn) 行合并評(píng)估；有些算法雖然使用了聯(lián)合檢測(cè)結(jié)果和重建的方式，但是要么是僅僅限于汽車 和行人并假定了環(huán)境和攝像機(jī)的位置，要么是提出了需要校準(zhǔn)相機(jī)小基線的共同像素標(biāo)簽 和密集的立體重現(xiàn)方法；一種基于運(yùn)動(dòng)技巧的語義上的結(jié)構(gòu)，評(píng)估了攝像頭姿勢(shì)的問題并 從一系列的圖像序列中識(shí)別物體，但是當(dāng)算法算延伸SFM檢測(cè)器并立即創(chuàng)建所有帖時(shí)，必 須借助一個(gè)外部的物體探測(cè)器來創(chuàng)建假說和度量。

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

[0005] 本發(fā)明的目的在于針對(duì)上述技術(shù)存在的缺陷，提出了一種基于語義光束平差法 SLAM方法，本方法在進(jìn)行低締度抽象時(shí)，度量方法是評(píng)估輸人帖和一個(gè)特征庫的特征一致 性。更重要的是，由于添加了語義上的信息，目標(biāo)檢測(cè)通道無需外設(shè)，就可W無縫的整合到 任何基于BA的SLAM系統(tǒng)的BA類型優(yōu)化系統(tǒng)中。
[0006] 本發(fā)明是通過如下方式實(shí)現(xiàn)的：一種基于語義光束平差法SLAM方法，包括w下步 驟：
[0007] 步驟1、確定檢測(cè)目標(biāo)的一系列特征，對(duì)每個(gè)檢測(cè)目標(biāo)建立模型數(shù)據(jù)庫。
[0008] 步驟2、隨著一個(gè)新帖的可用，對(duì)新帖提取描述特征并與模型數(shù)據(jù)庫相匹配，然后 為每個(gè)給定的檢測(cè)目標(biāo)創(chuàng)建確定圖。
[0009] 步驟3、驗(yàn)證步驟2中的確定圖，去除錯(cuò)誤邊緣，保留正確邊緣；依賴一個(gè)上一次全 局優(yōu)化得到的全局的加權(quán)平均差I(lǐng)作為在下一個(gè)目標(biāo)檢測(cè)過程中的驗(yàn)證闊值：
[0010]
W11] 假設(shè)，權(quán)重Wi，的表達(dá)式可W被分為所有的邊緣；
[0012] 步驟3. 1、對(duì)于2D特征匹配方案，從帖頂點(diǎn)到標(biāo)志頂點(diǎn)與闊值比較，滿足W下公 式，則去掉邊緣：
[0013]
[0014] h(o，n)表示第η個(gè)特征點(diǎn)在第0個(gè)物體上有關(guān)的標(biāo)志頂點(diǎn)的索引，α是一個(gè)給定 的參數(shù)，范圍在4-9之間；如果帖到標(biāo)志邊緣的移動(dòng)離開了只依附在物體上的標(biāo)志頂點(diǎn)，需 要?jiǎng)h除物體到標(biāo)志的邊緣；
[0015] 步驟3. 2、對(duì)于3D特征匹配，每個(gè)帖到物體的邊緣與闊值比較，滿足如下公式，貝U 去掉邊緣：
[0016]
[0017] 步驟4、在確定正確的語義邊緣后，根據(jù)邊緣的闊值評(píng)估是否將確定圖加入全局 圖。
[0018] 步驟5、查看是否有新帖出現(xiàn)，若無新帖出現(xiàn)，則完成全局圖優(yōu)化；若有新帖出現(xiàn)， 則返回步驟2,重新執(zhí)行步驟2到步驟5。
[0019] 其中，邊緣的闊值評(píng)估方法如下：
[0020] 運(yùn)個(gè)過程可W使確定圖處于3個(gè)不同的狀態(tài)，通過語義邊緣的最終數(shù)字成。和2個(gè) 闊值rif和η t(nf<nt)比較而定義；
[0021] 如果成。< η f，目標(biāo)被定為錯(cuò)誤檢測(cè)，運(yùn)個(gè)確定圖被刪除并且將檢測(cè)目標(biāo)從全局圖 移除；
[00巧如果Nw<nt，目標(biāo)檢測(cè)是模棱兩可的，確定圖被保存等待更多的視覺線索， 但是該檢測(cè)目標(biāo)被移除出全局圖； 陽02引如果成。> η t，目標(biāo)被檢測(cè)并添加到全局圖。
[0024] 本發(fā)明有益效果是： 陽0巧]1、本發(fā)明所述的方法通過新的語義上的全局的優(yōu)化來聯(lián)合抓OF化個(gè)方向自由 度，即沿立個(gè)方向的平動(dòng)W及繞立個(gè)軸的轉(zhuǎn)動(dòng)）物體和相機(jī)姿態(tài)，由于W上特點(diǎn)，該方法可 W在2D或者3D傳感器下工作。
[0026] 2、由于添加了語義上的信息，目標(biāo)檢測(cè)通道無需外設(shè)，就可W無縫的整合到任何 基于光束平差法的SLAM系統(tǒng)光束平差法類型優(yōu)化系統(tǒng)中。
[0027] 3、本發(fā)明所述的方法簡單、易實(shí)現(xiàn)，并且具有很強(qiáng)的實(shí)用性，SLAM約束可W被用于 健壯的目標(biāo)檢測(cè)，可W適應(yīng)更復(fù)雜的環(huán)境。
【附圖說明】
[0028] 圖1是本發(fā)明所述的基于語義光束平差法的SLAM方法流程圖
【具體實(shí)施方式】
[0029] 下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的描述。
[0030] 本發(fā)明是通過如下方式實(shí)現(xiàn)的：一種基于語義光束平差法SLAM方法，包括W下步 驟：
[0031] 步驟1、確定檢測(cè)目標(biāo)的一系列特征，對(duì)每個(gè)檢測(cè)目標(biāo)建立模型數(shù)據(jù)庫：如果整個(gè) 3D的模型是可得到的，3D關(guān)鍵點(diǎn)檢測(cè)子和描述子就可W使用；否則，該模型需要使用一堆 標(biāo)準(zhǔn)化的圖片和提供需要特征的2D關(guān)鍵點(diǎn)檢測(cè)子和描述子；特征描述子被保存為未來的 匹配；然后，對(duì)于特征位置，對(duì)于前者保存3D坐標(biāo)，對(duì)于后者保存2D圖片坐標(biāo)和它們相關(guān)的 視角姿勢(shì)。
[0032] 步驟2、隨著一個(gè)新帖的可用，對(duì)新帖提取描述特征并與模型數(shù)據(jù)庫相匹配，然后 為每個(gè)給定的檢測(cè)目標(biāo)創(chuàng)建確定圖；對(duì)于模型和帖兩者，通過內(nèi)置的形狀信號(hào)檢測(cè)器在3 個(gè)不同的尺度提取3D關(guān)鍵點(diǎn)，然后由旋轉(zhuǎn)圖像描述運(yùn)些關(guān)鍵點(diǎn)；對(duì)于每一個(gè)尺度，在該尺 度上創(chuàng)建一個(gè)包含所有模型的描述符的索引，在該尺度上執(zhí)行一個(gè)基于歐式距離的k鄰近 捜索；最終計(jì)算加權(quán)平均差。
[0033] 步驟2. 1、如果2D特征匹配，則在運(yùn)個(gè)代價(jià)函數(shù)中創(chuàng)建一個(gè)新的代表運(yùn)個(gè)路標(biāo)未 知3D位置和一系列的邊緣來包含他的投影誤差，比如考慮到某個(gè)對(duì)應(yīng)位置x4,第η個(gè)目標(biāo) 特征的路標(biāo)位置，相關(guān)的約束條件如W下公式：
[0034]
[00對(duì)其中，織表示從第0個(gè)物體上學(xué)習(xí)到的第η個(gè)特征點(diǎn)，pjf'表示第i個(gè)帖在可 能性sjK下匹配的2D特征點(diǎn)，Vi Μ旋轉(zhuǎn)和平移r e R3來應(yīng)用當(dāng)前的姿勢(shì)估算第i個(gè)頂點(diǎn)。
[0036] 步驟2. 2、當(dāng)步驟2. 1中所述的Vi關(guān)系到一個(gè)物體姿勢(shì)，運(yùn)個(gè)圖片平面是訓(xùn)練時(shí)獲 取的物體的標(biāo)準(zhǔn)視圖的一個(gè)，所W重投影通過已知物體參考帖和視圖參考帖之間的剛性變 換所約束，運(yùn)時(shí)，步驟2. 1)中公式的約束可W如下公式表示：
[0040] 步驟2. 3、當(dāng)3D特化可用時(shí)，直接連接攝像頭帖和更多物體，因此，如果Γη; j是代表 i特征和j特征相匹配，已知Pr (mj = Sik和Pr (m .jk) = s.jk，假定niik和m .jk是獨(dú)立的并且滿 足如下公式：
[0041 ]
[00創(chuàng)那么Pr (m。）= Sik*s，k,現(xiàn)在雜和接if代表了孤特征。
[0043] 步驟3、驗(yàn)證步驟2中的確定圖，去除錯(cuò)誤邊緣，保留正確邊緣；依賴一個(gè)上一次全 局優(yōu)化得到的全局的加權(quán)平均差f作為在下一個(gè)目標(biāo)檢測(cè)過程中的驗(yàn)證闊值：
[0044]
[0045] 假設(shè)，權(quán)重Wi,的表達(dá)式可W被分為所有的邊緣；
[0046] 步驟3. 1、對(duì)于2D特征匹配方案，從帖頂點(diǎn)到標(biāo)志頂點(diǎn)去掉邊緣，滿足公式如下：
[0047]
W48] h(o, η)表示第η個(gè)特征點(diǎn)在第0個(gè)物體上有關(guān)的標(biāo)志頂點(diǎn)的索引，α為7 ;如果 帖到標(biāo)志邊緣的移動(dòng)離開了只依附在物體上的標(biāo)志頂點(diǎn)，需要?jiǎng)h除物體到標(biāo)志的邊緣。 [0049] 步驟3. 2、對(duì)于3D特征匹配方案，與闊值比較每個(gè)帖到物體的邊緣，邊緣將被抹 去，依據(jù)如下公式： 陽化0]
[0051] 步驟4、在確定正確的語義邊緣后，根據(jù)邊緣的闊值評(píng)估是否將確定圖加入全局 圖：首先使確定圖處于3個(gè)不同的狀態(tài)，通過語義邊緣的最終數(shù)字成。和2個(gè)闊值η f和 nt(nf<nt)比較而定義。
[0052] 如果成。< η f，目標(biāo)被定為錯(cuò)誤檢測(cè)，運(yùn)個(gè)確定圖被刪除并且將檢測(cè)目標(biāo)從全局圖 移除；
[005引如果Nw<nt，目標(biāo)檢測(cè)是模棱兩可的，確定圖被保存等待更多的視覺線索， 但是該檢測(cè)目標(biāo)被移除出全局圖； 陽054] 如果成。> η t，目標(biāo)被檢測(cè)并添加到全局圖。
[0055] 其中，2個(gè)闊值rif和η t的值不是特定的，因?yàn)樵诿刻碌募僬f的驗(yàn)證受到前面 提取的特征匹配的約束，而且最終的評(píng)估只保持最好的邊界，因此，一個(gè)更高的η f可能會(huì) 跳過只在幾個(gè)視圖中的匹配的高度阻擋的物體，而差δ = nt- rif是關(guān)于運(yùn)個(gè)驗(yàn)證程序的 健壯性：δ越高表明不同帖的一致性越要被檢測(cè)，但是最少的錯(cuò)誤會(huì)被傳遞到全局圖并且 可能被在下面的帖中刪除。
[0056] 其中，步驟4中所述的全局圖為語義上的全局圖，包括：① SLAM引擎中得到的帶有 帖到帖約束的所有攝像機(jī)的姿勢(shì)頂點(diǎn)；②驗(yàn)證過程成功的所有物體的姿勢(shì)頂點(diǎn)；③所有帖 到標(biāo)志和物體到標(biāo)志的約束，運(yùn)些約束，在2D特征匹配上，或者是帖到物體和虛帖到帖的 約束，在3D的特征匹配，則是來自于刪除的物體的驗(yàn)證圖。
[0057] W上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用W限制本發(fā)明，凡在本發(fā)明的精 神和原則之內(nèi)的所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于語義光束平差法的SLAM方法，其特征在于，包括W下步驟： 步驟1、確定檢測(cè)目標(biāo)的一系列特征，對(duì)每個(gè)檢測(cè)目標(biāo)建立模型數(shù)據(jù)庫； 步驟2、隨著一個(gè)新帖的可用，對(duì)新帖提取描述特征并與模型數(shù)據(jù)庫相匹配，然后為每 個(gè)給定的檢測(cè)目標(biāo)創(chuàng)建確定圖； 步驟3、驗(yàn)證步驟2中的確定圖，去除錯(cuò)誤邊緣，保留正確邊緣；依賴一個(gè)上一次全局優(yōu) 化得到的全局的加權(quán)平均差§作為在下一個(gè)目標(biāo)檢測(cè)過程中的驗(yàn)證闊值：假設(shè)，權(quán)重Wu的表達(dá)式可W被分為所有的邊緣； 步驟3. 1、對(duì)于2D特征匹配方案，從帖頂點(diǎn)到標(biāo)志頂點(diǎn)與闊值比較，滿足W下公式，貝U 去掉邊緣：h(o，n)表示第n個(gè)特征點(diǎn)在第O個(gè)物體上有關(guān)的標(biāo)志頂點(diǎn)的索引，a是一個(gè)給定的參 數(shù)，范圍在4-9之間；如果帖到標(biāo)志邊緣的移動(dòng)離開了只依附在物體上的標(biāo)志頂點(diǎn)，需要?jiǎng)h 除物體到標(biāo)志的邊緣； 步驟3. 2、對(duì)于3D特征匹配，每個(gè)帖到物體的邊緣與闊值比較，滿足如下公式，則去掉 邊緣：步驟4、在確定正確的語義邊緣后，根據(jù)邊緣的闊值評(píng)估是否將確定圖加入全局圖。 步驟5、查看是否有新帖出現(xiàn)，若無新帖出現(xiàn)，則完成全局圖；若有新帖出現(xiàn)，則返回步 驟2,重新執(zhí)行步驟2到步驟5。2. 根據(jù)權(quán)利要求1所述的基于語義光束平差法的SLAM方法，其特征在于：所述步驟4 中邊緣的闊值評(píng)估方法如下： 首先使確定圖處于3個(gè)不同的狀態(tài)，通過語義邊緣的最終數(shù)字Nw和2個(gè)闊值n f和 nt(nf<nt)比較而定義； 如果成。< n f，目標(biāo)被定為錯(cuò)誤檢測(cè)，運(yùn)個(gè)確定圖被刪除并且將檢測(cè)目標(biāo)從全局圖移 除； 如果n N w< n t，目標(biāo)檢測(cè)是模棱兩可的，確定圖被保存等待更多的視覺線索，但是 該檢測(cè)目標(biāo)被移除出全局圖； 如果成。> n t，目標(biāo)被檢測(cè)并添加到全局圖。
【文檔編號(hào)】G06T7/00GK105989586SQ201510050181
【公開日】2016年10月5日
【申請(qǐng)日】2015年3月4日
【發(fā)明人】廖鴻宇, 孫放
【申請(qǐng)人】北京雷動(dòng)云合智能技術(shù)有限公司