呂思男,黃家才,高芳征,李毅搏,陳 田

(1. 南京工程學(xué)院工業(yè)中心、創(chuàng)新創(chuàng)業(yè)學(xué)院, 江蘇 南京 211167;2. 南京工程學(xué)院自動(dòng)化學(xué)院, 江蘇 南京 211167;3. 南京工程學(xué)院機(jī)器人與系統(tǒng)集成研究所, 江蘇 南京 211167)

隨著國內(nèi)外四足機(jī)械狗研究的發(fā)展,仿生機(jī)械狗在軍事儲(chǔ)備、警戒巡檢、抗震救災(zāi)、科學(xué)研究等領(lǐng)域的作用備受關(guān)注[1].目前國內(nèi)對機(jī)械狗的運(yùn)動(dòng)控制研究趨成熟,但對其定位研究相對較少,足式里程計(jì)定位誤差較大.因此,本文通過融合定位提高機(jī)械狗定位精度,在此基礎(chǔ)上,再實(shí)現(xiàn)目標(biāo)定位.不僅可以獲取機(jī)械狗的位置坐標(biāo),還能獲取機(jī)械狗視覺中目標(biāo)物體的坐標(biāo),更好地實(shí)現(xiàn)仿生機(jī)械狗在抗震救災(zāi)、警戒巡檢等場景中的應(yīng)用[2-3].

同步定位與建圖(simultaneous localization and mapping,SLAM)技術(shù)自1986年開始,先后經(jīng)歷了貝葉斯估計(jì)理論、統(tǒng)計(jì)學(xué)框架描述觀測路標(biāo)與特征點(diǎn)的相關(guān)性以及基于圖優(yōu)化等方法以實(shí)現(xiàn)在未知環(huán)境中定位到自身位置[4-7],但是在大范圍環(huán)境中存在累計(jì)誤差.衛(wèi)星定位技術(shù)沒有累積誤差,其中 全球定位系統(tǒng)(GPS)定位精度為10～15 m、北斗衛(wèi)星導(dǎo)航系統(tǒng)(BDS)定位的實(shí)測精度(按中誤差算)為4～5 m,且BDS的覆蓋范圍更廣闊[8].但在有遮擋或者商場等環(huán)境,衛(wèi)星定位系統(tǒng)無法進(jìn)行精準(zhǔn)定位.衛(wèi)星定位與SLAM 的定位方法具有鮮明的互補(bǔ)性.文獻(xiàn)[9]提出GPS 與 VSLAM 緊耦合的低成本定位方案,在GPS信號差的區(qū)域依然能保持系統(tǒng)的魯棒性,但其定位精度不高;文獻(xiàn)[10]引入 GPS信號輔助SLAM進(jìn)行重定位,減少SLAM定位的誤差,但是該方法未將兩者的定位信息進(jìn)行融合,以至于GPS定位信息對SLAM的定位精度提高效果不明顯.

基于以上問題,本文提出一種VSLAM與機(jī)載北斗導(dǎo)航數(shù)據(jù)相融合的機(jī)械狗視覺目標(biāo)定位方法,通過分析機(jī)械狗移動(dòng)過程中視覺關(guān)鍵幀圖像的特征點(diǎn)數(shù)據(jù),將得到的機(jī)械狗位姿移動(dòng)信息與BDS得到的實(shí)時(shí)地理信息進(jìn)行融合處理;分析圖像幀與幀之間提取的特征點(diǎn),運(yùn)用三角化等手段獲取三維空間的RT矩陣,與BDS每段時(shí)間返回的地理路標(biāo)數(shù)據(jù)作信息融合,得到實(shí)際的移動(dòng)距離以及轉(zhuǎn)角方向,最終實(shí)現(xiàn)機(jī)械狗視覺中感興趣目標(biāo)點(diǎn)的查詢.在衛(wèi)星定位信號微弱的環(huán)境中可以實(shí)現(xiàn)機(jī)械狗較高精度的定位,同時(shí)解決沒有融合其他信息源的情況下基于視覺的SLAM方法無尺度信息的問題,使其定位信息可以應(yīng)用于真實(shí)尺度的環(huán)境中.

1 算法設(shè)計(jì)

算法設(shè)計(jì)主要步驟流程為(見圖1):

1) 單目相機(jī)作為主要傳感器,采用VSLAM算法對每幀圖像進(jìn)行特征點(diǎn)提取匹配,進(jìn)行三角化、PNP等運(yùn)算[11-12],將二維圖像信息轉(zhuǎn)換為所需要的三維空間信息;

2) 將從BDS獲取到的實(shí)時(shí)經(jīng)緯度信息進(jìn)行數(shù)據(jù)處理得到每段時(shí)間的位移信息;

3) 將單目相機(jī)獲取的估計(jì)距離和BDS獲取到的位移信息進(jìn)行融合處理,得到CGCS2000坐標(biāo)系下的相機(jī)位姿;

4) 對相機(jī)中采集到的感興趣目標(biāo)點(diǎn),可以在相鄰的關(guān)鍵幀中通過極線搜索匹配算法尋找到相似的特征點(diǎn),再通過反投影手段獲取感興趣目標(biāo)點(diǎn)在空間坐標(biāo)系下的實(shí)際坐標(biāo),最后利用BDS查詢該點(diǎn)的經(jīng)緯度坐標(biāo).

圖1 算法設(shè)計(jì)流程圖

1.1 機(jī)械狗視覺目標(biāo)定位的基本原理

相機(jī)跟蹤過程中,相機(jī)位姿定義為歐氏空間中的變換矩陣,旋轉(zhuǎn)矩陣集合Rwc和變換矩陣twc可以構(gòu)成群:

(1)

式中:SE(3)為在空間上一個(gè)連續(xù)的群,是一種李群;twc為相機(jī)在歐氏空間中的位置,twc=[X,Y,Z]T;Rwc為實(shí)數(shù)域內(nèi)的矩陣,Rwc∈R3×3,是相機(jī)在歐氏空間中的姿態(tài).

設(shè)t0時(shí)刻的相機(jī)位姿為:

(2)

(3)

考慮到變換前后要保持兩點(diǎn)間距離不變,為提高計(jì)算速度,可直接進(jìn)行幀間位姿估計(jì),將李群SE(3)轉(zhuǎn)換為對應(yīng)的李代數(shù)形式:

ξ=[ω1,ω2,ω3,v1,v2,v3]T=[ω,v]T

(4)

式中:ξ為一個(gè)六維向量,ξ∈R6;v為相機(jī)的平移向量,v=[v1,v2,v3]T;ω為相機(jī)的旋轉(zhuǎn)向量,ω=[ω1,ω2,ω3]T.

通過冪指數(shù)映射規(guī)則把李代數(shù)中的元素映射到相應(yīng)的李群中[13],有:

(5)

將空間中某點(diǎn)由相機(jī)坐標(biāo)系pc=[Xc,Yc,Zc]T映射為像素坐標(biāo)的投影函數(shù)為:

(6)

式中:fx為x軸的相機(jī)焦距;fy為y軸的相機(jī)焦距;u0、v0為相機(jī)主點(diǎn)中心.

給定像素坐標(biāo)[u,v]T,機(jī)械狗視覺內(nèi)的相機(jī)坐標(biāo)系中,在Zc=1平面上對應(yīng)的相機(jī)坐標(biāo)系函數(shù)為:

(7)

設(shè)λ為pc三維坐標(biāo)點(diǎn)沿行進(jìn)方向與相機(jī)的距離(深度),則相機(jī)坐標(biāo)系pc所對應(yīng)的有真實(shí)尺度的空間坐標(biāo)系為λpc.

1.2 實(shí)時(shí)相機(jī)位姿估計(jì)

由單目相機(jī)實(shí)現(xiàn)的VSLAM技術(shù)中較為重要的有初始位姿估計(jì)以及稀疏點(diǎn)云圖生成,是視覺里程計(jì)的關(guān)鍵工作,其重要組成部分為檢測圖像序列、相機(jī)追蹤、運(yùn)動(dòng)估計(jì)以及定位優(yōu)化(捆集約束).在初始位姿估計(jì)過程中,選取重投影誤差小的關(guān)鍵幀矩陣作為兩幀圖像間的變換.位姿的初始化數(shù)值選取t0時(shí)刻的關(guān)鍵幀,用于估計(jì)相機(jī)的初始姿態(tài),再將相鄰關(guān)鍵幀的變換連成一條曲線構(gòu)成機(jī)械狗的行走路徑,達(dá)到定位目的.在相機(jī)推算路徑階段,e為當(dāng)前幀的像素坐標(biāo)與特征點(diǎn)坐標(biāo)的差值,則相機(jī)的位姿為:

(8)

1.3 關(guān)鍵幀處理與生成

特征點(diǎn)匱乏以及誤匹配點(diǎn)的增加會(huì)使視覺里程計(jì)的誤差增加,所以初始選擇關(guān)鍵幀時(shí),要選擇像素點(diǎn)明顯的關(guān)鍵幀并且去除冗余關(guān)鍵幀,相機(jī)位姿旋轉(zhuǎn)與平移的權(quán)重組合為兩幀之間特征點(diǎn)的距離[14],即:

(9)

式中,W為一個(gè)對角權(quán)重參數(shù)矩陣,對定位精度有重要影響.

在關(guān)鍵幀處理過程中,兩特征點(diǎn)之間的匹配距離超過權(quán)重距離會(huì)產(chǎn)生誤匹配點(diǎn),影響相機(jī)跟蹤軌跡的精度.為減小誤差,需要將當(dāng)前圖像幀標(biāo)記為新關(guān)鍵幀,聯(lián)合其他關(guān)鍵幀通過BoW加速匹配[15],并由單獨(dú)的構(gòu)圖線程進(jìn)行處理,將錯(cuò)位關(guān)鍵幀重新插入到閉環(huán)關(guān)鍵幀中.對新關(guān)鍵幀的處理步驟為:

1) 提取新關(guān)鍵幀的特征描述符,將新關(guān)鍵幀插入到回環(huán)幀中,計(jì)算特征點(diǎn)的BoW關(guān)系,用于后續(xù)的關(guān)鍵幀快速檢索;

2) 將插入的關(guān)鍵幀與相鄰關(guān)鍵幀進(jìn)行特征匹配,驗(yàn)證插入位置;

3) 利用三角法生成新的地圖空間點(diǎn);

4) 聯(lián)合相機(jī)位姿狀態(tài)對極線匹配進(jìn)行過濾和聯(lián)合局部優(yōu)化,設(shè)與當(dāng)前關(guān)鍵幀特征點(diǎn)相似幀的集合為KL,當(dāng)前關(guān)鍵幀對應(yīng)的特征點(diǎn)集合為pL,則聯(lián)合局部優(yōu)化公式為:

(10)

1.4 特征點(diǎn)均勻分布策略

圖2 特征點(diǎn)均勻分布策略流程

特征點(diǎn)檢測大多在形狀突變的位置,紋理弱的區(qū)域特征點(diǎn)少,這就導(dǎo)致了環(huán)境描述子過于單調(diào),感知能力弱,不利于后續(xù)對于感興趣目標(biāo)點(diǎn)的查詢.因此,本文提出一種特征點(diǎn)均勻分布策略,利用四叉樹結(jié)構(gòu)改進(jìn)特征點(diǎn)的檢測方法,具體流程如圖2所示(圖2中L為圖像金字塔中圖像的層數(shù),W為圖像的寬度,H為圖像的高度).該策略在提取特征點(diǎn)時(shí),利用四叉樹來管理從金字塔圖像中提取的特征點(diǎn),設(shè)定可提取特征點(diǎn)的數(shù)量閾值Na,按照Harris響應(yīng)選取數(shù)量為Na的特征點(diǎn)[16].

本算法設(shè)計(jì)了2個(gè)特征點(diǎn)檢測閾值:每個(gè)格子中的初始特征點(diǎn)數(shù)量(Vini)和每個(gè)格子中的最少特征點(diǎn)數(shù)量(Vmin).如果每個(gè)格子中的特征點(diǎn)數(shù)量小于Vini,則認(rèn)為該區(qū)域紋理較弱,此時(shí)按照Vmin檢測特征點(diǎn).Vmin的設(shè)置可以保證在紋理弱的區(qū)域也能提取到特征點(diǎn).

1.5 VSLAM與BDS融合

在沒有融合其他信息源的情況下,基于視覺的SLAM方法沒有尺度信息,這就使得其定位信息無法應(yīng)用于真實(shí)尺度的環(huán)境中;而BDS只能獲得相機(jī)位置.針對兩種方法單獨(dú)使用時(shí)的局限性,本文將BDS信息與VSLAM位姿估計(jì)融合,幫助SLAM進(jìn)行閉環(huán)檢測,獲得更精確的位姿來提高其目標(biāo)定位精度,融合步驟如圖3所示.

圖3 BDS與VSLAM融合流程圖

受BDS更新頻率影響,BDS設(shè)備與圖像幀之間存在時(shí)間差tD,將BDS信號觸發(fā)時(shí)間與tD相加實(shí)現(xiàn)BDS與VSLAM系統(tǒng)的時(shí)間同步,以獲取任意圖像幀對應(yīng)的CGCS2000坐標(biāo)系下粗略的BDS導(dǎo)航坐標(biāo).初始化時(shí),選取第1幀圖像坐標(biāo)對應(yīng)的BDS導(dǎo)航坐標(biāo)為原點(diǎn),則北斗導(dǎo)航第j個(gè)時(shí)刻的位置估計(jì)融合誤差為:

(11)

對構(gòu)建地圖中相機(jī)估計(jì)位姿與BDS融合的聯(lián)合優(yōu)化為:

(12)

式中:KL為VSLAM地圖中的關(guān)鍵幀集合;Λ為兩個(gè)關(guān)鍵幀間的協(xié)方差矩陣.

利用優(yōu)化結(jié)果提高特征匹配精度,得到融合后的相機(jī)位姿為:

(13)

繼而得到 CGCS2000坐標(biāo)系下的相機(jī)位姿.

1.6 極線匹配與北斗導(dǎo)航查詢

對圖像中感興趣目標(biāo)點(diǎn)進(jìn)行查詢,需要選取融合位姿估計(jì)中有同一物體的兩幀圖像,通過聯(lián)合三角測量的信息和相機(jī)的運(yùn)動(dòng)估計(jì)來確定該像素點(diǎn)對應(yīng)的空間坐標(biāo)[17].在相機(jī)運(yùn)動(dòng)估計(jì)誤差一定的情況下,為減少特征點(diǎn)匹配的誤差,首先嚴(yán)格剔除冗余的關(guān)鍵幀和特征點(diǎn),再通過GMS算法將匹配點(diǎn)轉(zhuǎn)換為高質(zhì)量的匹配點(diǎn)[18],對應(yīng)像素點(diǎn)與目標(biāo)點(diǎn)匹配的一致,通過三角運(yùn)算確定其在三維空間中的北斗導(dǎo)航坐標(biāo).

2 試驗(yàn)與結(jié)果

選用宇樹科技機(jī)械狗加強(qiáng)版、搭載英特爾D435i高清實(shí)感相機(jī)進(jìn)行試驗(yàn).機(jī)械狗視覺獲取當(dāng)前環(huán)境地圖信息,對獲取的地圖進(jìn)行里程計(jì)算、回環(huán)檢測等處理,獲得相機(jī)的相對位姿估計(jì).

通過估計(jì)得到的相機(jī)位姿可以清晰地再現(xiàn)機(jī)械狗的行走軌跡.將北斗導(dǎo)航與VSLAM的對應(yīng)幀時(shí)間對齊后,得到對應(yīng)關(guān)鍵幀的CGCS2000坐標(biāo),從而使相機(jī)估計(jì)軌跡得到真實(shí)尺度信息.圖4為單目VSLAM估計(jì)的相機(jī)軌跡與參考軌跡(虛線),相機(jī)軌跡與參考軌跡對比的絕對位姿誤差如圖4所示;機(jī)械狗的北斗導(dǎo)航融合軌跡與參考軌跡的絕對位姿誤差如圖5所示.

圖4 單目VSLAM估計(jì)的相機(jī)軌跡與參考軌跡(m)

圖5 相機(jī)軌跡和北斗導(dǎo)航軌跡融合與參考軌跡(m)

由圖4、圖5可知,由相機(jī)估計(jì)得到的軌跡與對應(yīng)的北斗導(dǎo)航軌跡具有相似軌跡路線.由于融合了北斗導(dǎo)航坐標(biāo)的尺度信息,絕對位姿誤差絕對值由0.134下降到0.049,軌跡精度得到提高.將北斗導(dǎo)航信息與VSLAM融合(Camera+BDS),擬合后軌跡與文獻(xiàn)[10]中算法的擬合軌跡(Camera Trajectory)相比，更靠近參考軌跡,如圖6所示.

圖6 擬合軌跡對比圖(m)

由圖6可知,擬合后北斗導(dǎo)航軌跡與參考軌跡幾乎重合,定位精度得到提高,可使后續(xù)的路標(biāo)定位準(zhǔn)確度更高.融合北斗導(dǎo)航信息后,給單目VSLAM在地圖中的估計(jì)軌跡增加了坐標(biāo)信息.在單目SLAM所攝地圖中標(biāo)定60個(gè)北斗導(dǎo)航路標(biāo),通過映射變換到CGCS2000坐標(biāo)系中,將融合后的像素坐標(biāo)疊加顯示在機(jī)械狗視覺中的圖像幀上,將三角化新的坐標(biāo)點(diǎn)放到跟蹤模塊,達(dá)到手動(dòng)標(biāo)記路標(biāo)與機(jī)械狗視覺地圖疊加顯示的目的,以實(shí)時(shí)查詢路標(biāo).

對在SLAM地圖中手工標(biāo)定的路標(biāo)進(jìn)行反投影操作,得到CGCS2000坐標(biāo)系下坐標(biāo),對比北斗導(dǎo)航坐標(biāo)系與融合坐標(biāo)點(diǎn)之間的誤差來驗(yàn)證本算法的精度.北斗導(dǎo)航路標(biāo)平面誤差如圖7所示.

圖7 北斗導(dǎo)航路標(biāo)平面誤差

在北斗導(dǎo)航采用的CGCS2000坐標(biāo)系下,本文定位方法的尺度漂移和全局優(yōu)化糾正度更高,對感興趣目標(biāo)點(diǎn)的查詢精度可以達(dá)到0.8 m以內(nèi).地圖自動(dòng)初始化估計(jì)路標(biāo)點(diǎn)的3D位置和高精度回環(huán)檢測是本文算法的關(guān)鍵.對地圖構(gòu)建(更新坐標(biāo)點(diǎn)和插入關(guān)鍵幀)和主線程相機(jī)跟蹤(相機(jī)位姿和關(guān)鍵幀)進(jìn)行記錄,運(yùn)行效率結(jié)果如表1所示.

表1 性能測試結(jié)果 ms

由表1見,在視覺跟蹤階段,每秒可以處理69幀的關(guān)鍵幀,處理幀數(shù)完全滿足誤差優(yōu)化函數(shù)的運(yùn)行速度,實(shí)現(xiàn)相機(jī)位姿的實(shí)時(shí)軌跡跟蹤,從而保障后續(xù)回環(huán)檢測的糾正速度.

3 結(jié)語

將相機(jī)跟蹤的位姿信息與北斗導(dǎo)航坐標(biāo)系下的標(biāo)定路標(biāo)進(jìn)行融合,將融合得到的坐標(biāo)系投影到同一個(gè)球平面,通過三角運(yùn)算得到新的地圖點(diǎn);利用融合后的相機(jī)位姿估計(jì)標(biāo)定路標(biāo)的坐標(biāo)位置,實(shí)現(xiàn)機(jī)械狗視覺中像素點(diǎn)與北斗導(dǎo)航的疊加顯示,從而得到地圖中標(biāo)定路標(biāo)的坐標(biāo).本文提供了一種基于視覺SLAM的北斗導(dǎo)航路標(biāo)定位并融合了北斗導(dǎo)航來進(jìn)行位置查詢的一種方法,為仿生機(jī)械狗在警戒巡檢、抗震救災(zāi)等的目標(biāo)查詢提供了一種可用方法.通過北斗坐標(biāo)和SLAM坐標(biāo)的融合,絕對位姿誤差由0.134 m下降到0.049 m,使得巡檢或救災(zāi)場景中目標(biāo)點(diǎn)的查詢精度可以達(dá)到0.8 m以內(nèi).