一種融合車載多傳感器的slam裝置的制造方法
【專利摘要】本實(shí)用新型涉及融合車載多傳感器的三輪全向移動(dòng)機(jī)器人的同步定位與建圖方法的技術(shù)領(lǐng)域，更具體地，涉及一種融合車載多傳感器的SLAM裝置。一種融合車載多傳感器的SLAM裝置，其中，包括三輪全向移動(dòng)機(jī)器人車體，所述的機(jī)器人車體上設(shè)有車載傳感器、車載編碼器、車載慣性測(cè)量單元、車載控制器、上位機(jī)；所述的車載編碼器和車載慣性測(cè)量單元連接車載控制器，所述的車載控制器連接上位機(jī)；所述的車載傳感器與上位機(jī)連接。能夠提高機(jī)器人定位和建圖的效果，解決單純依靠RGB?D傳感器進(jìn)行SLAM的深度值缺失或特征點(diǎn)稀少帶來的定位和建圖的誤差問題，從而提高SLAM的魯棒性和準(zhǔn)確性。
【專利說明】
一種融合車載多傳感器的SLAM裝置
技術(shù)領(lǐng)域
[0001] 本實(shí)用新型涉及融合車載多傳感器的三輪全向移動(dòng)機(jī)器人的同步定位與建圖方 法的技術(shù)領(lǐng)域，更具體地，涉及一種融合車載多傳感器的SLAM裝置。
【背景技術(shù)】
[0002] 全向移動(dòng)機(jī)器人能實(shí)現(xiàn)任意方向的運(yùn)動(dòng)，可廣泛應(yīng)用于軍事、工業(yè)、家用服務(wù)等領(lǐng) 域。移動(dòng)機(jī)器人的同時(shí)定位和地圖創(chuàng)建（SLAM，Simultaneous Localization And Mapping) 是機(jī)器人領(lǐng)域的熱點(diǎn)研究問題，它是移動(dòng)機(jī)器人自助任務(wù)規(guī)劃和路徑規(guī)劃的前提和基礎(chǔ)。 機(jī)器人的SLAM問題，簡(jiǎn)單的說就是在一個(gè)未知的環(huán)境中，移動(dòng)機(jī)器人需要建立環(huán)境地圖，并 在地圖上的同時(shí)定位自身。這個(gè)過程類似于人走到一個(gè)完全陌生的環(huán)境中，在未攜帶任何 能夠確定位置和方向的設(shè)備情況下，只能根據(jù)對(duì)周圍環(huán)境的觀察和對(duì)自身運(yùn)動(dòng)的估計(jì)認(rèn)識(shí) 環(huán)境并確定自己的位置。
[0003] SLAM本質(zhì)上是一個(gè)系統(tǒng)狀態(tài)(包括機(jī)器人當(dāng)前位姿以及所有地圖特征位置等）估 計(jì)問題。從這一角度，其求解方法可大致分為基于Kalman濾波器的方法、基于粒子濾波器的 方法、基于圖優(yōu)化的方法3類?；贙alman濾波(KF)和粒子濾波(PF)的方法主要依據(jù)遞歸貝 葉斯?fàn)顟B(tài)估計(jì)理論。而基于圖優(yōu)化的增量式SLAM算法框架，主要包括順序數(shù)據(jù)關(guān)聯(lián)、環(huán)形閉 合檢測(cè)以及圖優(yōu)化3大部分。
[0004] 從傳感器的角度分析，在室外環(huán)境中，定位的問題通?？梢杂蒅PS(Global Positioning System)來完成，因?yàn)镚PS為機(jī)器人所承擔(dān)的任務(wù)提供了很好的準(zhǔn)確性。然而， 當(dāng)在室內(nèi)移動(dòng)時(shí)，GPS數(shù)據(jù)不可用，難以準(zhǔn)確估計(jì)機(jī)器人的位置，因此必須尋找其它解決方 案。以前大部分的機(jī)器人地圖構(gòu)建只能提供環(huán)境的二維信息，因?yàn)楂@得高質(zhì)量的三維數(shù)據(jù) 非常昂貴或者對(duì)于機(jī)器人的運(yùn)動(dòng)有很多的約束。這一階段的研究使用的數(shù)據(jù)源一般為激光 測(cè)距儀，雙目攝像頭和單目攝像頭。激光測(cè)距儀的定位精度雖然高，但價(jià)格昂貴，三維激光 掃描儀的價(jià)格更是讓人難以承擔(dān)。而基于雙目或單目的SLAM依賴于計(jì)算機(jī)視覺和圖像處理 技術(shù)，缺點(diǎn)在于缺乏直觀的場(chǎng)景深度信息，三維地圖重建工作變得復(fù)雜。最近幾年，隨著微 軟公司的Kinect、華碩公司的Xtion Pro Live等便宜且高效的RGB-D攝像機(jī)的出現(xiàn)，采用 RGB-D傳感器進(jìn)行移動(dòng)機(jī)器人定位或SLAM的研究迅速發(fā)展起來。
[0005] RGB-D傳感器能夠同時(shí)提供彩色(RGB)和深度(Depth)圖像信息，相比于激光測(cè)距 儀，它能夠更廉價(jià)地獲得深度信息，而相比于普通相機(jī)，它連續(xù)性好，環(huán)境信息更加豐富，因 此定位的精度更高，且更容易進(jìn)行三維地圖重建。而采用RGB-D攝像機(jī)對(duì)環(huán)境進(jìn)行SLAM的最 新研究中，則有相當(dāng)一部分都采用了基于圖優(yōu)化的SLAM方法。
[0006] 但RGB-D傳感器也存在可獲得的深度值有效范圍較小的問題，對(duì)于一些空曠且較 大的場(chǎng)景，會(huì)出現(xiàn)深度值缺失或特征點(diǎn)缺失的問題，影響運(yùn)動(dòng)估計(jì)的準(zhǔn)確性，甚至導(dǎo)致定位 和建圖出現(xiàn)嚴(yán)重偏差。 【實(shí)用新型內(nèi)容】
[0007] 本實(shí)用新型為克服上述現(xiàn)有技術(shù)所述的至少一種缺陷，提供一種融合車載多傳感 器的SLAM裝置，基于圖優(yōu)化的SLAM方法，考慮RGB-D傳感器的優(yōu)點(diǎn)并針對(duì)其局限性，引入車 載的編碼器和慣性測(cè)量單元兩類傳感器，提出融合車載多傳感器的SLAM方法，通過融合多 傳感器的數(shù)據(jù)，計(jì)算得到全向移動(dòng)機(jī)器人的位姿變換，減小由于深度值缺失以及特征點(diǎn)稀 少所造成的誤差，使獲得的機(jī)器人運(yùn)動(dòng)軌跡更為平滑準(zhǔn)確，同時(shí)也提高建圖的準(zhǔn)確度。
[0008] 為解決上述技術(shù)問題，本實(shí)用新型采用的技術(shù)方案是：一種融合車載多傳感器的 SLAM裝置，其中，包括三輪全向移動(dòng)機(jī)器人車體，所述的機(jī)器人車體上設(shè)有車載傳感器、車 載編碼器、車載慣性測(cè)量單元、車載控制器、上位機(jī);所述的車載編碼器和車載慣性測(cè)量單 元連接車載控制器，所述的車載控制器連接上位機(jī);所述的車載傳感器與上位機(jī)連接。所述 的車載傳感器為Kinect體感傳感器。
[0009] 本實(shí)用新型中，在未知環(huán)境下，三輪全向移動(dòng)機(jī)器人在運(yùn)動(dòng)的過程中通過車載 RGB-D傳感器（即Kinect體感傳感器)獲取周圍環(huán)境的彩色圖像和深度圖像，從圖像變化解 算出自身的運(yùn)動(dòng)變換，同時(shí)還通過車載編碼器和車載慣性測(cè)量單元獲得機(jī)器人自身的運(yùn)動(dòng) 數(shù)據(jù)，對(duì)這些數(shù)據(jù)進(jìn)行融合處理，以獲得機(jī)器人自身所處的位置并構(gòu)建出周圍環(huán)境的三維 地圖，實(shí)現(xiàn)機(jī)器人的同步定位與建圖（SLAM)。
[0010] 具體的，所述的機(jī)器人車體為正三角三輪機(jī)構(gòu)。所述的機(jī)器人車體尺寸為長(zhǎng)85cm， 寬78cm，高50cm。所述的三個(gè)輪的旋轉(zhuǎn)軸互成120度。
[0011] 與現(xiàn)有技術(shù)相比，有益效果是:本實(shí)用新型在未知的環(huán)境中，全向移動(dòng)機(jī)器人采用 車載傳感器獲取周圍環(huán)境的信息，進(jìn)行同步定位和建圖，我們所考慮的車載傳感器系統(tǒng)包 括RGB-D傳感器、編碼器和慣性測(cè)量單元。由于Kinect存在深度范圍有限的問題，且基于圖 像匹配估計(jì)運(yùn)動(dòng)的方式容易受到特征點(diǎn)缺失的影響，導(dǎo)致估計(jì)結(jié)果誤差較大，因此我們考 慮融合Kinect、編碼器和慣性測(cè)量單元的數(shù)據(jù)，優(yōu)化位姿估計(jì)?；谒岢龅亩鄠鞲衅骷訖?quán) 融合算法，本方法能夠提高機(jī)器人定位和建圖的效果，解決單純依靠 RGB-D傳感器進(jìn)行SLAM 的深度值缺失或特征點(diǎn)稀少帶來的定位和建圖的誤差問題，從而提高SLAM的魯棒性和準(zhǔn)確 性。
[0012] 融合車載編碼器和慣性測(cè)量單元所估計(jì)的數(shù)據(jù)，較好地解決了RGB-D SLAM中存在 的深度范圍有限、深度值缺失導(dǎo)致的定位和建圖的偏差，提高SLAM方法的魯棒性；
[0013] 采用加權(quán)平均的方式融合多傳感器解算得到的位姿信息，提高了位姿的準(zhǔn)確性， 從而提高SLAM的效果和準(zhǔn)確性；
[0014] 把多傳感器融合估計(jì)位姿的方法應(yīng)用于SLAM系統(tǒng)，并提供一個(gè)較為完整的SLAM解 決方案。
【附圖說明】
[0015] 圖1是融合多傳感器的SLAM系統(tǒng)結(jié)構(gòu)圖。
[0016] 圖2是三輪全向機(jī)器人機(jī)械結(jié)構(gòu)示意圖。
[0017] 圖3是世界坐標(biāo)系圖。
[0018]圖4是機(jī)器人坐標(biāo)系圖。
[0019] 圖5是合速度分解圖。
[0020] 圖6是多傳感器數(shù)據(jù)融合方法流程框圖。
[0021 ]圖7是融合多傳感器的SLAM方法流程框圖。
[0022] 圖8是機(jī)器人X軸上的軌跡跟蹤效果對(duì)比圖。
[0023] 圖9是機(jī)器人y軸上的軌跡跟蹤效果對(duì)比圖。
[0024]圖10是只使用Kinect的建圖效果圖。
[0025]圖11是使用多傳感器加權(quán)融合方法的建圖效果圖。
[0026]圖12是機(jī)器人X軸上的軌跡跟蹤效果對(duì)比圖。
[0027]圖13是機(jī)器人y軸上的軌跡跟蹤效果對(duì)比圖。
[0028]圖14是只使用Kinect的建圖效果圖。
[0029] 圖15是使用多傳感器加權(quán)融合方法的建圖效果圖。
【具體實(shí)施方式】
[0030] 附圖僅用于示例性說明，不能理解為對(duì)本專利的限制;為了更好說明本實(shí)施例，附 圖某些部件會(huì)有省略、放大或縮小，并不代表實(shí)際產(chǎn)品的尺寸;對(duì)于本領(lǐng)域技術(shù)人員來說， 附圖中某些公知結(jié)構(gòu)及其說明可能省略是可以理解的。附圖中描述位置關(guān)系僅用于示例性 說明，不能理解為對(duì)本專利的限制。
[0031] 如圖1所示，一種融合車載多傳感器的SLAM裝置，其中，包括三輪全向移動(dòng)機(jī)器人 車體，所述的機(jī)器人車體上設(shè)有車載傳感器、車載編碼器、車載慣性測(cè)量單元、車載控制器、 上位機(jī);所述的車載編碼器和車載慣性測(cè)量單元連接車載控制器，所述的車載控制器連接 上位機(jī);所述的車載傳感器與上位機(jī)連接。所述的車載傳感器為Kinect體感傳感器。
[0032]本實(shí)用新型中，在未知環(huán)境下，三輪全向移動(dòng)機(jī)器人在運(yùn)動(dòng)的過程中通過車載 RGB-D傳感器（即Kinect體感傳感器)獲取周圍環(huán)境的彩色圖像和深度圖像，從圖像變化解 算出自身的運(yùn)動(dòng)變換，同時(shí)還通過車載編碼器和車載慣性測(cè)量單元獲得機(jī)器人自身的運(yùn)動(dòng) 數(shù)據(jù)，對(duì)這些數(shù)據(jù)進(jìn)行融合處理，以獲得機(jī)器人自身所處的位置并構(gòu)建出周圍環(huán)境的三維 地圖，實(shí)現(xiàn)機(jī)器人的同步定位與建圖（SLAM)。
[0033](一)基于RGB-D傳感器的位姿估計(jì)
[0034] RGB-D傳感器采用微軟公司的Kinect體感傳感器。Kinect有一個(gè)彩色攝像頭，用來 獲取640*480的彩色圖像，每秒最多獲取30幀圖像;彩色攝像頭兩邊分別是紅外發(fā)射器和紅 外接收器，，它們共同組成景深攝像頭，用以檢測(cè)物體距離Kinect的深度信息。
[0035] 1、特征提取與匹配
[0036] 常用的圖像特征提取算法有SIFT(Scale_invariant feature transform)，SURF (Speeded Up Robust Features)等算法。SIFT算法基于尺度空間，能夠不受圖像的縮放、轉(zhuǎn) 動(dòng)以及仿射變換的影響。SURF算法是由Herbert Bay提出的一種基于二維Hessian矩陣的算 法，該算法是在SIFT算法上做的改進(jìn)，提尚了算法的執(zhí)行效率。本方法米用SURF算法對(duì) Kinect獲取的彩色圖像進(jìn)行特征點(diǎn)檢測(cè)和描述符提取，采用FLANN的特征匹配方法進(jìn)行特 征匹配。
[0037] (1)特征點(diǎn)檢測(cè)：首先對(duì)原圖像構(gòu)建尺度空間金字塔，然后用Hessian矩陣求出極 值，在3X3X3的立體鄰域內(nèi)進(jìn)行非極大值抑制，找出候選特征點(diǎn)，再在尺度空間和圖像空 間中進(jìn)行插值運(yùn)算，得到穩(wěn)定的特征點(diǎn)位置及所在的尺度值，最后利用像素的Haar小波響 應(yīng)，通過統(tǒng)計(jì)60°范圍內(nèi)像素點(diǎn)的水平和垂直Haar小波響應(yīng)來指定特征點(diǎn)的方向；
[0038] (2)描述符提取：以特征點(diǎn)為中心，首先將坐標(biāo)軸旋轉(zhuǎn)到主方向，按照主方向選取 邊長(zhǎng)為20s的正方形區(qū)域，將該窗口區(qū)域劃分成4X4的子區(qū)域，在每一個(gè)子區(qū)域內(nèi)，計(jì)算5s X 5s (采樣步長(zhǎng)取s)范圍內(nèi)的小波響應(yīng)，相對(duì)于主方向的水平、垂直方向的Haar小波響應(yīng)分 另Ij記做dx、d y，同樣賦予響應(yīng)值以權(quán)值系數(shù)，以增加對(duì)幾何變換的魯棒性;然后將每個(gè)子區(qū)域 的響應(yīng)以及響應(yīng)的絕對(duì)值相加形成Σ(1 Χ，Σ4, |dx|，Σ |dy|。這樣，在每個(gè)子區(qū)域形成四維 分量的矢量Vsub=( Σ(1Χ, Σ |dx|，Sdy, Σ |dy| )，因此，對(duì)每一特征點(diǎn)，形成4Χ(4Χ4)=64維 的描述向量，再進(jìn)行向量的歸一化，從而對(duì)光照具有一定的魯棒性。
[0039] (3)特征點(diǎn)匹配：使用FLANN(Fast Library for Approximate Nearest Neighbors)進(jìn)行特征點(diǎn)匹配。FLANN是在高維空間執(zhí)行快速近似最近鄰搜索的一個(gè)庫，它包 含一組優(yōu)秀的最近鄰搜索算法和一個(gè)自動(dòng)選擇最優(yōu)算法并且根據(jù)數(shù)據(jù)集對(duì)算法參數(shù)進(jìn)行 優(yōu)化的系統(tǒng);對(duì)于高維空間中的最近鄰搜索問題，F(xiàn)LANN采用基于分層k-均值樹的優(yōu)先級(jí)搜 索算法或采用多重隨機(jī)k-d樹時(shí)具有最好的性能；相對(duì)于其他傳統(tǒng)最近鄰搜索算法，基于 FLANN的近似快速最近鄰搜索的速度提高了 一個(gè)數(shù)量級(jí)。
[0040] 2、幀間變換估計(jì)
[0041] 得到兩幀彩色圖像之間的一組匹配特征點(diǎn)后，就可結(jié)合對(duì)應(yīng)的深度圖像的信息計(jì) 算兩個(gè)圖像間的轉(zhuǎn)換關(guān)系。設(shè)兩個(gè)幀分別為FdPF 2,則一組一一對(duì)應(yīng)的匹配特征點(diǎn)為：
[0042] P={pi,p2, . . . ,pn} ^Fi, (I)
[0043] Q={qi，q2, · · ·，qN} EF2, (2)
[0044] 其中pi和qi都是R2中的點(diǎn)，i = I，…，N。
[0045] 三維空間點(diǎn)[X，y，z ]和它在圖像中的像素坐標(biāo)[u，V，d] (d指深度數(shù)據(jù))的對(duì)應(yīng)關(guān)系 為：
[0046；
(3)
[0047]其中，fx、fy指相機(jī)在兩個(gè)軸上的焦距，Cx、c y指相機(jī)的光圈中心，s指深度圖的縮放 因子。利用此公式，將幀F(xiàn)2中的特征點(diǎn)映射到當(dāng)前幀的相機(jī)坐標(biāo)系中，可得：
[0048]
[0049] 其中，qci 是 R3 中的點(diǎn)，i = l，. . .，N。
[0050] Ul丨丨亦步MfrnTC .
[0051]
(5)
[0052]其中，f是投影矩陣。通過最小化重投影誤差的方法，可以獲得比直接采用三維點(diǎn) 對(duì)進(jìn)行計(jì)算并最小化空間距離更好的效果。
[0053] RANSAC(Random Sample Consensus)是一種迭代算法，用于去除圖像特征點(diǎn)中的 一些離群點(diǎn)，提高匹配精度。RANSAC算法是一種比最小二乘法更優(yōu)的求取模型參數(shù)的算法， 對(duì)于一組包含有噪聲點(diǎn)的數(shù)據(jù)集，除了可以找到最佳參數(shù)，還能夠剔除這些噪聲點(diǎn)。本方法 結(jié)合RANSAC算法剔除錯(cuò)誤匹配，以提高變換矩陣的準(zhǔn)確性。通過使用RANSAC算法多次迭代 求解，從而獲得更準(zhǔn)確的變換估計(jì)。
[0054](二)基于編碼器和慣性測(cè)量單元的位姿估計(jì)
[0055]本方法考慮的三輪全向移動(dòng)機(jī)器人采用了正三角的三輪結(jié)構(gòu)，長(zhǎng)85cm，寬78cm，高 50cm，采取全向輪，可實(shí)現(xiàn)任意方向的移動(dòng)。機(jī)器人上搭載了編碼器、慣性測(cè)量單元，可解算 出機(jī)器人的位移和姿態(tài)變化。
[0056]本方法采用雙向光電編碼器，轉(zhuǎn)動(dòng)軸與電機(jī)通過聯(lián)軸器連接，能夠在電機(jī)轉(zhuǎn)動(dòng)時(shí) 產(chǎn)生三路方波，稱作A相、B相和Z相。
[0057 ] 三輪全向移動(dòng)機(jī)械結(jié)構(gòu)如圖2，其中Li = L2 = L3 = L (L表示驅(qū)動(dòng)輪中心到機(jī)器人中 心的距離），三個(gè)驅(qū)動(dòng)輪的旋轉(zhuǎn)軸互成120°，％、％、V3表示驅(qū)動(dòng)輪的運(yùn)動(dòng)方向。
[0058]三輪全向機(jī)器人的工作環(huán)境為一平面空間，建立世界坐標(biāo)系X-Y，如圖3所示；以全 向輪小車的中心點(diǎn)為原點(diǎn)，L2所在直線方向?yàn)閥軸，建立機(jī)器人的體坐標(biāo)系xa_ya，如圖4所 示;在世界坐標(biāo)系中對(duì)機(jī)器人的整體速度進(jìn)行X軸和y軸方向上的分解，如圖5。
[0059] 圖3中的Θ。為世界坐標(biāo)X軸與機(jī)器人坐#Xa軸的夾角，假設(shè)機(jī)器人在零狀態(tài)時(shí)其夾 角為零，即世界坐標(biāo)軸與機(jī)器人起始點(diǎn)的坐標(biāo)軸重合，則Θ。表示機(jī)器人運(yùn)動(dòng)時(shí)產(chǎn)生的自旋 角度。圖5中的β表示合速度V與世界坐標(biāo)的X軸的夾角，V分解為V x和Vy。K =[匕％之J表 示小車的姿態(tài)變量。三輪全向移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型為：
[0060]
(6)
[0061] 將編碼器輸出的A相和B相輸入到測(cè)速電路模塊中，可以得到機(jī)器人三個(gè)輪子的速 度，即VhV2、V3。根據(jù)等式(6)，可計(jì)算出機(jī)器人的水平速度Vx和縱向速度V y，積分即可獲得三 輪全向機(jī)器人相對(duì)于起始點(diǎn)位置的相對(duì)位移X和y。
[0062] 慣性測(cè)量單元包括加速度計(jì)和陀螺儀，加速度計(jì)用于測(cè)量在機(jī)器人坐標(biāo)下的三個(gè) 坐標(biāo)軸方向上的加速度，陀螺儀用于測(cè)量機(jī)器人體坐標(biāo)下的三個(gè)坐標(biāo)軸方向上的角速度， 通過積分可以得到機(jī)器人旋轉(zhuǎn)角度。通過數(shù)據(jù)融合算法融合慣性測(cè)量單元中的加速度與角 速度信息，可以估計(jì)得到Φ、Θ、P三個(gè)姿態(tài)角度，分別對(duì)應(yīng)機(jī)器人繞Z軸、Y軸和X軸的旋轉(zhuǎn)角 度，即機(jī)器人的位姿信息。
[0063](三)融合多傳感器的機(jī)器人同步定位和建圖方法
[0064] 本方法對(duì)Kinect、編碼器和慣性測(cè)量單元的數(shù)據(jù)進(jìn)行融合估計(jì)位姿，使用了關(guān)鍵 幀減少幾余圖像幀以提尚SLAM效率，并對(duì)關(guān)鍵幀進(jìn)彳丁閉環(huán)檢測(cè)以增加幀間關(guān)聯(lián)，同時(shí)利用 幀間關(guān)聯(lián)進(jìn)行位姿圖優(yōu)化以減小累積誤差，通過以上方法提高定位和建圖的準(zhǔn)確度。
[0065] 1、加權(quán)平均的多傳感器數(shù)據(jù)融合方法
[0066]本方法采用加權(quán)平均的方法對(duì)Kinect、編碼器和慣性測(cè)量單元的數(shù)據(jù)進(jìn)行融合， 優(yōu)化位姿估計(jì)。
[0067]用p表示用RANSAC算法求解幀間變換時(shí)的局內(nèi)點(diǎn)對(duì)數(shù)目和用于求解變換的點(diǎn)對(duì)總 數(shù)目的比值，O^P^l;則當(dāng)P越大，該算法求解的變換矩陣越可信，P的比值由求解變換的每 兩幀圖像確定。此外，用q作為衡量編碼器數(shù)據(jù)準(zhǔn)確性的因子，用r作為衡量慣性測(cè)量單元數(shù) 據(jù)準(zhǔn)確性的因子，q和r的值越大則對(duì)應(yīng)的傳感器數(shù)據(jù)的準(zhǔn)確度越高。當(dāng)采用不同精確度的 傳感器時(shí)，所對(duì)應(yīng)的描述準(zhǔn)確度的因子亦不同。在對(duì)多傳感器數(shù)據(jù)進(jìn)行融合時(shí)，可以通過實(shí) 驗(yàn)的方法選擇適宜的q和:r的值。
[0068] 利用Kinect獲取的圖像信息可解算出幀間的平移向量和旋轉(zhuǎn)向量，由車載編碼器 可算出幀間的平移向量，而由車載慣性測(cè)量單元可估計(jì)得到幀間的旋轉(zhuǎn)向量。
[0069] 本方法采用如下規(guī)則對(duì)多傳感器的數(shù)據(jù)進(jìn)行融合：
[0070] (1)獲取位移變換時(shí)，將由Kinect獲取的圖像信息解算獲得的平移向量與由編碼 器的測(cè)量信息估計(jì)得到的幀間位移差值以4:1的比值進(jìn)行融合，從而獲得當(dāng)前幀與前一幀 機(jī)器人的位移變換。機(jī)器人在二維平面運(yùn)動(dòng)，只有X和Y方向的位移變化，因此位移變換融合 公式如下：
[0071]
(7)
[0072] 其中，△ Xk和A yi^v別表示Kinect解得的機(jī)器人沿X方向和沿y方向的幀間位移，Δ X。和△ y。表示編碼器解得的幀間位移，△ X和△ y表示加權(quán)融合后的幀間位移。
[0073] (2)獲取姿態(tài)變換時(shí)，將由Kinect獲取的圖像信息解算獲得的旋轉(zhuǎn)向量與由慣性 測(cè)量單元的測(cè)量信息估計(jì)得到的幀間姿態(tài)差值以的比值進(jìn)行融合，從而獲得當(dāng)前幀與 r 前一關(guān)鍵幀機(jī)器人的姿態(tài)變換。機(jī)器人在二維平面運(yùn)動(dòng)，只有繞Z軸旋轉(zhuǎn)的角度Φ的變化，因 此姿態(tài)變換融合公式如下：
[0074]
(8)
[0075]其中，Δ ik表示Kined解得的機(jī)器人繞Z軸的幀間姿態(tài)角變化，Δ 表示慣性測(cè)量 單元解得的幀間姿態(tài)角變化，Αφ表示加權(quán)融合后的幀間姿態(tài)角變化。
[0076] (3)因?yàn)橛糜谇蠼庾钚』赝队罢`差的點(diǎn)對(duì)至少需要4組，當(dāng)利用Kinect獲取的圖 像信息求解的局內(nèi)點(diǎn)數(shù)小于5組時(shí)，解算出的變換矩陣誤差很大。在這種情況下舍棄由 Kinect獲取的圖像數(shù)據(jù)的估計(jì)值，而使用由編碼器和慣性測(cè)量單元所獲得的解算結(jié)果。 [0077]多傳感器數(shù)據(jù)加權(quán)融合方法流程圖如圖6所示。
[0078] 2、關(guān)鍵幀檢測(cè)與閉環(huán)檢測(cè)
[0079] Kinect每秒鐘可以獲取30幀的圖像，因此幀與幀之間距離很近，如果把每一幀都 用于構(gòu)建地圖，會(huì)導(dǎo)致地圖頻繁更新，消耗計(jì)算時(shí)間與儲(chǔ)存空間。因此我們采取了提取關(guān)鍵 幀的方法，剔除冗余幀以提高效率。算出兩幀之間的運(yùn)動(dòng)變換矩陣f的二范數(shù)e，若e小于設(shè) 定的閾值E min，則兩幀之間變換太小，為冗余幀，丟棄當(dāng)前幀;若e大于閾值Emin，則取為關(guān)鍵 幀。E min太小會(huì)導(dǎo)致剔除的冗余幀過少，從而降低效率，Emin過大則相鄰關(guān)鍵幀的圖像相差較 多，運(yùn)動(dòng)估計(jì)容易出錯(cuò)。E min的選取可根據(jù)不同的數(shù)據(jù)集進(jìn)行測(cè)試，選取適宜的值。本方法選 取的Emin為0.1。
[0080]對(duì)提取的當(dāng)如關(guān)鍵幀進(jìn)行閉環(huán)檢測(cè)，用關(guān)鍵幀分別與肖LlN個(gè)關(guān)鍵幀和整個(gè)關(guān)鍵幀 序列中的隨機(jī)M幀進(jìn)行幀間變換估計(jì)，算出兩幀之間的運(yùn)動(dòng)變換矩陣T'類似于關(guān)鍵幀檢 測(cè)，計(jì)算f的二范數(shù)e，當(dāng)e大于Emin且小于Emax時(shí)，判定這兩幀是機(jī)器人在相近的位置取得， 即機(jī)器人又回到曾到過的位置，則將算得的變換矩陣f作為新的約束關(guān)系加入位姿圖中， 以便進(jìn)行位姿圖優(yōu)化時(shí)能更好地消減誤差。E max太小，則閉環(huán)檢測(cè)成功率低，Emax太大，則容 易出現(xiàn)錯(cuò)誤的閉環(huán)匹配，導(dǎo)致位姿圖優(yōu)化時(shí)誤差更大。E max的選取也可根據(jù)不同的數(shù)據(jù)集進(jìn) 行測(cè)試，選取適宜的值。本方法選取的Emax為1.0 。
[0081] 3、位姿圖優(yōu)化
[0082] 經(jīng)過位姿融合、關(guān)鍵幀提取、閉環(huán)檢測(cè)等步驟后，會(huì)得到一系列的位姿點(diǎn)之間的變 換關(guān)系。因此，對(duì)位姿進(jìn)行全局一致優(yōu)化的問題可用圖來表示，也就是目前視覺SLAM里常用 的姿態(tài)圖(pose graph)。圖中的每個(gè)節(jié)點(diǎn)表示一個(gè)狀態(tài)變量，即相機(jī)的各個(gè)姿態(tài)Vi:
[0083] Vi= [x,y,z ,qx,qy,qz,qw]i, (9)
[0084] 其中，x、y、z表示相機(jī)的位置，qx、qy、qz、qw用四元數(shù)表示相機(jī)的姿態(tài)角。
[0085] 每條邊Eu表示連接兩個(gè)相鄰狀態(tài)變量vdPw之間的測(cè)量值，即兩幀之間的變換矩 陣 Ti，j:
[0086] (10)
[0087] 其中，R3*3表示旋轉(zhuǎn)矩陣，可由旋轉(zhuǎn)向量通過羅格里德斯變換獲得，表示平移向 量。
[0088] 四元數(shù)的表示形式可與旋轉(zhuǎn)矩陣的形式相互轉(zhuǎn)換，所以節(jié)點(diǎn)V1的狀態(tài)變量也可用 變換矩_的形―才V」豐G .
[0089] (11)
[0090] 理想的情況下，應(yīng)有如下變換關(guān)系：
[0091] Xi = TijXj (12)
[0092] 但由于邊中存在誤差，使得所有的邊給出的數(shù)據(jù)并不一致，因此轉(zhuǎn)化成求解誤差 函數(shù)的最小值的問題：
[0093]
(丄。）
[0094] 其中，表示優(yōu)化變量Xi的估計(jì)值。
[0095] 本方法采用g2o(General Graph Optimization)算法，即通用圖優(yōu)化算法，來進(jìn)行 位姿圖的優(yōu)化，求解出最優(yōu)位姿f的組合，讓誤差E收斂到一個(gè)極小值。g2o是一個(gè)算法集， 根據(jù)具體的問題選用合適的算法求解優(yōu)化問題。
[0096]優(yōu)化后的位姿圖包含了各個(gè)關(guān)鍵幀對(duì)應(yīng)的相機(jī)位置和姿態(tài)，可據(jù)此繪出機(jī)器人的 運(yùn)動(dòng)軌跡。
[0097] 4、拼接點(diǎn)云建圖
[0098]利用RGB-D傳感器獲取的彩色信息和深度信息將每幀圖像轉(zhuǎn)化為點(diǎn)云。利用公式 (3)可以將每個(gè)圖像點(diǎn)的二維坐標(biāo)映射到當(dāng)前幀相機(jī)坐標(biāo)系的三維坐標(biāo)，再加入彩色RGB信 息，就可獲得點(diǎn)云pc=[x y z r g b]。
[0099]點(diǎn)云的拼接，實(shí)質(zhì)上是對(duì)點(diǎn)云做變換的過程。這個(gè)變換往往是用變換矩陣來描述 的，即公式(10)，變換矩陣T結(jié)合了旋轉(zhuǎn)和縮放，可對(duì)點(diǎn)進(jìn)行齊次變換：
[0100]
(14)
[0101] 本方法采用了 PCL庫里提供的點(diǎn)云變換函數(shù)，給定變換矩陣，就可對(duì)整個(gè)點(diǎn)云進(jìn)行 移動(dòng)，完成點(diǎn)Ζ5Γ的拼接。
[0102] 本方法提供了一個(gè)較為完整的SLAM解決方案，包括了特征的提取與匹配、運(yùn)動(dòng)估 計(jì)、關(guān)鍵幀檢測(cè)、閉環(huán)檢測(cè)、地圖優(yōu)化、建圖與定位等步驟，并將多傳感器加權(quán)融合的方法應(yīng) 用于位姿估計(jì)，提高了 SLAM的準(zhǔn)確度。本方法提供的SLAM方案的具體流程如圖7所示。
[0103] 經(jīng)過多次實(shí)驗(yàn)測(cè)量，本方法使用的編碼器因子q設(shè)置為0.2,慣性測(cè)量單元因子r設(shè) 置為0.3。
[0104] 圖8至11為針對(duì)同一數(shù)據(jù)集進(jìn)行的SLAM方法的比較，包括只使用Kinect數(shù)據(jù)以及 融合了多傳感器數(shù)據(jù)兩種方法的即時(shí)建圖和定位對(duì)比圖。
[0105] 圖8、9中方框中上部線條表示只使用車載Kinect獲取的圖像進(jìn)行機(jī)器人自身運(yùn)動(dòng) 估計(jì)時(shí)算出的機(jī)器人的運(yùn)動(dòng)軌跡，方框中下部線條表示融合車載Kinect、編碼器和慣性測(cè) 量單元獲取的數(shù)據(jù)進(jìn)行運(yùn)動(dòng)估計(jì)時(shí)算出的運(yùn)動(dòng)軌跡。當(dāng)局內(nèi)點(diǎn)對(duì)數(shù)目和用于RANSAC求解的 點(diǎn)對(duì)總數(shù)目的比值P較大時(shí)，Kinect的解算結(jié)果可信度較高，融合多傳感器方法獲得的位置 與Kinect解算出的位置相對(duì)較近；當(dāng)p較小時(shí)，Kinect的解算結(jié)果可信度較低，融合多傳感 器方法獲得的位置與Kinert解算出的位置相對(duì)較遠(yuǎn)。
[0106] 可以看出，采取了加權(quán)融合的多傳感器融合方法后，只使用Kinect時(shí)出現(xiàn)的明顯 的運(yùn)動(dòng)估計(jì)錯(cuò)誤(即尖刺)得到了修正，提高了機(jī)器人定位的效果和魯棒性。
[0107] 圖10為只使用Kinect的圖像信息時(shí)的建圖效果，圖11為使用多傳感器加權(quán)融合方 法的建圖效果。由圖中可以看到，圖10的圈內(nèi)是因運(yùn)動(dòng)估計(jì)誤差較大而導(dǎo)致的建圖偏差;而 在圖11中，由于使用了多傳感器加權(quán)融合方法，這個(gè)問題得到了修正。
[0108] 圖12至15為使用另一數(shù)據(jù)集進(jìn)行的SLAM方法的比較，包括只使用Kinect數(shù)據(jù)以及 融合了多傳感器數(shù)據(jù)兩種方法的即時(shí)建圖和定位對(duì)比圖。
[0109] 類似的，圖12、13中方框中上部線條表示只使用車載Kinect獲取的圖像信息進(jìn)行 機(jī)器人自身運(yùn)動(dòng)估計(jì)時(shí)算出的機(jī)器人的運(yùn)動(dòng)軌跡，方框中下部線條表示融合車載Kinect、 編碼器和慣性測(cè)量單元獲取的數(shù)據(jù)進(jìn)行運(yùn)動(dòng)估計(jì)時(shí)算出的運(yùn)動(dòng)軌跡。
[0110]在圖中可以看到，只使用Kinect的圖像信息時(shí)，機(jī)器人的運(yùn)動(dòng)軌跡跟蹤到中間處 丟失了，這是由于Kinect的深度測(cè)量范圍有限，只能探測(cè)到4米內(nèi)的物體，當(dāng)機(jī)器人運(yùn)動(dòng)到 較空曠的位置時(shí)，Kinect無法獲得足夠的深度值去解算幀間的運(yùn)動(dòng)變換，只能丟棄當(dāng)前幀； 在連續(xù)丟棄多幀后，即使機(jī)器人再次回到深度值豐富的地方，獲得的當(dāng)前幀與丟幀之前的 最后一個(gè)關(guān)鍵幀也因相差太遠(yuǎn)而難以匹配，最終導(dǎo)致機(jī)器人的定位失敗；而采用多傳感器 加權(quán)融合方法后，機(jī)器人在深度值缺失的情況下可以采用編碼器和慣性測(cè)量單元繼續(xù)估計(jì) 自身運(yùn)動(dòng)，很好地彌補(bǔ)了Kinect深度測(cè)量范圍有限的問題。
[0111]圖14為只使用Kinect的圖像信息時(shí)的建圖效果，圖15為使用多傳感器加權(quán)融合方 法的建圖效果。由圖中可以看到，圖14的圈內(nèi)是因?yàn)闄C(jī)器人定位失敗而缺失的部分，而圖15 中這個(gè)缺失的部分得到彌補(bǔ)，改善了建圖的效果。
[0112]顯然，本實(shí)用新型的上述實(shí)施例僅僅是為清楚地說明本實(shí)用新型所作的舉例，而 并非是對(duì)本實(shí)用新型的實(shí)施方式的限定。對(duì)于所屬領(lǐng)域的普通技術(shù)人員來說，在上述說明 的基礎(chǔ)上還可以做出其它不同形式的變化或變動(dòng)。這里無需也無法對(duì)所有的實(shí)施方式予以 窮舉。凡在本實(shí)用新型的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在 本實(shí)用新型權(quán)利要求的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種融合車載多傳感器的SLAM裝置，其特征在于，包括三輪全向移動(dòng)機(jī)器人車體，所 述的機(jī)器人車體上設(shè)有車載傳感器、車載編碼器、車載慣性測(cè)量單元、車載控制器、上位機(jī)； 所述的車載編碼器和車載慣性測(cè)量單元連接車載控制器，所述的車載控制器連接上位機(jī)； 所述的車載傳感器與上位機(jī)連接。2. 根據(jù)權(quán)利要求1所述的一種融合車載多傳感器的SLAM裝置，其特征在于:所述的車載 傳感器為Kinect體感傳感器。3. 根據(jù)權(quán)利要求1所述的一種融合車載多傳感器的SLAM裝置，其特征在于:所述的機(jī)器 人車體為正三角三輪機(jī)構(gòu)。4. 根據(jù)權(quán)利要求1所述的一種融合車載多傳感器的SLAM裝置，其特征在于:所述的機(jī)器 人車體尺寸為長(zhǎng)85cm，寬78cm，高50cm。5. 根據(jù)權(quán)利要求2所述的一種融合車載多傳感器的SLAM裝置，其特征在于:所述的三個(gè) 輪的旋轉(zhuǎn)軸互成120度。
【文檔編號(hào)】G01C21/16GK205426175SQ201620175291
【公開日】2016年8月3日
【申請(qǐng)日】2016年3月8日
【發(fā)明人】成慧, 吳依瑩, 朱啟源, 林倞
【申請(qǐng)人】中山大學(xué)