基于ORB-SLAM的低照度空間非合作目標(biāo)的姿態(tài)估計*

周朋博 劉曉峰 蔡國平

（上海交通大學(xué)工程力學(xué)系，海洋工程國家重點實驗室，上海 200240）

引言

隨著航天事業(yè)的發(fā)展，太空碎片清除、延長航天器壽命等在軌服務(wù)技術(shù)日益重要，空間目標(biāo)與服務(wù)衛(wèi)星之間的相對姿態(tài)估計則是在軌服務(wù)得以順利實施的關(guān)鍵.空間目標(biāo)可以分為合作目標(biāo)和非合作目標(biāo)［1］，合作目標(biāo)可以向服務(wù)衛(wèi)星提供自身運動等信息，目前已有完備的姿態(tài)估計方案；而非合作目標(biāo)無法與服務(wù)衛(wèi)星進(jìn)行通訊，其姿態(tài)需要通過視覺觀測處理圖像等手段獲得.

視覺觀測是指利用傳感器采集目標(biāo)圖像，根據(jù)圖像信息和物方空間內(nèi)幾何信息的映射關(guān)系實現(xiàn)測量的方法.由于視覺觀測不需要目標(biāo)的先驗信息，越來越多的研究人員用其解決非合作目標(biāo)的姿態(tài)估計問題［2-10］.張世杰等［5］將單目相機采集的目標(biāo)航天器圖像簡化為特征點，然后利用非線性優(yōu)化算法計算出目標(biāo)的相對位姿.Kanani等［6］提出了用于單目相機的模板匹配算法，分離出圖像中的航天器并與離線構(gòu)建的三維模型投影數(shù)據(jù)庫匹配估計目標(biāo)位姿.Liu和Hu［7］提出了一種為非合作圓柱形航天器定制的姿態(tài)采集算法，將圖像中提取的匹配橢圓與模型中的橢圓匹配之后，利用航天器的非對稱分量計算位姿.Greenspan［8］提出首先找到目標(biāo)主軸的方向來解出兩個旋轉(zhuǎn)自由度.然后將模板匹配算法用于在離線建立的數(shù)據(jù)庫內(nèi)尋找余下旋轉(zhuǎn)自由度的最佳估計.楊立峰等［9］將卡爾曼濾波與立體視覺結(jié)合估計目標(biāo)相對追蹤者的位姿.徐文福等［10］提出以目標(biāo)航天器的三角形衛(wèi)星帆板為觀測目標(biāo)，利用雙目測量計算三角形頂點的三維信息，得到目標(biāo)航天器姿態(tài).由以上可以看出，目前人們已經(jīng)在空間非合作目標(biāo)的視覺運動觀測做了一些研究工作，并取得了一些研究進(jìn)展.然而值得在此指出的是，現(xiàn)有研究仍存在一些問題有待深入探索，例如，目前對空間非合作目標(biāo)姿態(tài)估計的研究幾乎都是在空間目標(biāo)三維幾何模型已知的情況下完成的，是通過與離線建立的模板庫進(jìn)行匹配而計算得出姿態(tài)的.因此研究方法無法應(yīng)用于三維模型未知的目標(biāo)，因此模板庫是由固定姿態(tài)參數(shù)的三維模型生成的模板圖像組成的.另外，太空中弱光照條件會降低相機等無源傳感器所獲信息的完整度，進(jìn)而影響視覺運動觀測的精度.因此，研究在弱光照情況下空間非合作目標(biāo)的視覺運動觀測技術(shù)具有重要的意義.

本文對弱光照情況下空間非合作目標(biāo)的視覺運動觀測技術(shù)進(jìn)行研究，提出一種有效的視覺運動觀測方法 .本文將 LIME［11］增強算法與 ORB-SLAM［12，13］算法進(jìn)行結(jié)合，采用LIME增強弱光照情況下雙目相機采集的圖像，然后將ORB-SLAM用于衛(wèi)星運動姿態(tài)估計.仿真結(jié)果驗證了本文方法的有效性.

1 LIME算法基本原理

常用的低照度增強算法是先將采集的低照度圖像反轉(zhuǎn)，利用去霧算法處理圖像得到無霧圖，再反轉(zhuǎn)得到最終的增強圖像.該算法雖然效果良好，但缺乏物理模型解釋.而LIME算法基于Retinex物理模型［14］，同樣具有很好的效果.本節(jié)首先介紹了LIME算法的理論模型Retinex模型，然后使用MAX-RGB方法［14］估計亮度圖的初始值，并通過優(yōu)化函數(shù)優(yōu)化亮度圖，最后對亮度圖進(jìn)行γ變換并利用BM3D方法［15］去噪，和反射圖重組成最終增強圖像.

1.1 理論模型

LIME算法基于如下Retinex物理模型［14］：

其中，S代表原始圖像，R代表反射圖（Desired recovery），L代表亮度圖（Illumination map），x代表像素坐標(biāo).公式（1）的物理意義是指圖像由亮度圖和反射圖兩部分所組成，通過分離并增強亮度圖達(dá)到對弱光照情況下采集圖像增強的目的.

1.2 亮度估計

從公式（1）可以看出，計算反射圖的關(guān)鍵是亮度圖的估計.本文使用經(jīng)典的MAX-RGB方法［14］估計亮度圖的初始值

并計算出反射圖：

對亮度圖進(jìn)行增強和優(yōu)化.為使增強后的亮度圖既不失真、又能與紋理相符，本文采用如下的優(yōu)化函數(shù)［11］：

為簡化計算，將優(yōu)化函數(shù)（4）近似為

式中，權(quán)值矩陣值為：

其中，Wh和Wv是水平和豎直方向的權(quán)值矩陣，Ω（x）是以像素x為中心的區(qū)域，y是區(qū)域內(nèi)的位置索引，Gσ（x，y）是標(biāo)準(zhǔn)差為2的高斯核函數(shù).

1.3 圖像矯正

為使圖像特征更為明顯，需要對經(jīng)過公式（5）優(yōu)化后的亮度圖采用如下γ變換進(jìn)行非線性拉伸，以提升圖像對比度：

然后將反射圖從RGB顏色空間轉(zhuǎn)換到Y(jié)CbCr顏色空間，并在Y通道使用BM3D濾波［15］.為了解決去噪造成的失真，將濾波后的反射圖與亮度圖重組為如下最終增強圖像：

其中，Sf代表最終結(jié)果，Rd代表去噪后的反射圖.

2 ORB-SLAM基本原理

ORB-SLAM是基于特征和非線性優(yōu)化的SLAM方案.非合作目標(biāo)運動時，相機連續(xù)采集圖像，SLAM系統(tǒng)根據(jù)圖像中重復(fù)觀測到的路標(biāo)點實時計算目標(biāo)姿態(tài)，并建立起環(huán)境地圖（在本文中即為目標(biāo)模型）.整個系統(tǒng)包括三個并行進(jìn)程：跟蹤進(jìn)程通過將每一幀提取的特征與局部地圖匹配來定位相機位姿，并且通過應(yīng)用姿態(tài)光流法平差來減小投影誤差；局部地圖進(jìn)程管理并優(yōu)化局部地圖，實施局部光流法平差；閉環(huán)進(jìn)程檢測相機路徑是否達(dá)成閉環(huán)條件，并在檢測到閉環(huán)時通過實施位姿圖優(yōu)化來糾正累計漂移，然后在位姿圖優(yōu)化后實施全局優(yōu)化來優(yōu)化結(jié)構(gòu)和位姿解.SLAM系統(tǒng)僅采用ORB特征進(jìn)行特征檢測和描述，同時利用基于ORB特征的詞袋DBoW2［16］進(jìn)行位置識別和回環(huán)檢測.

2.1 特征點提取

圖像特征點能夠識別圖像中的物體，通過匹配特征點能夠匹配圖像進(jìn)而計算位姿，因此使用ORB-SLAM時首先需要進(jìn)行ORB特征點的提取.ORB（Oriented FAST and Rotated BRIEF）特征是一種具有尺度不變性和旋轉(zhuǎn)不變性的特征點描述方法，該方法利用增強的FAST（Features from Accelerated Segment Test）關(guān)鍵點提取方法提取關(guān)鍵點，并使用方向歸一化的BRIEF（Binary Robust Independent Elementary Features）描述子對關(guān)鍵點進(jìn)行描述.當(dāng)進(jìn)行特征提取時，ORB首先對圖像構(gòu)建圖像金字塔，然后運用FAST方法提取金字塔每一層的角點，并通過Harris角點檢測方法保留前N個角點作為關(guān)鍵點.當(dāng)計算特征點描述子時,首先定義以關(guān)鍵點為中心的二維區(qū)域，并通過圖像矩找到區(qū)域質(zhì)心.然后將關(guān)鍵點到區(qū)域質(zhì)心的向量作為關(guān)鍵點方向，并將圖像按區(qū)域方向旋轉(zhuǎn)。最后，使用BRIEF算法對圖像區(qū)域內(nèi)256個預(yù)先定義的像素進(jìn)行二進(jìn)制測試生成256位的字符串作為關(guān)鍵點的特征描述子.

特征點在相機上的投影關(guān)系如圖1，非合作目標(biāo)特征點P在左相機像素坐標(biāo)系O1-u1v1對應(yīng)點的坐標(biāo)為（uL，vL），在右相機像素坐標(biāo)系O2-u2v2對應(yīng)點的坐標(biāo)為（uR，vR）.則可以得到特征點P的深度為［13］：

圖1 特征點在左右相機上的投影點Fig.1 Projection points of feature point on the stereo camera

其中，fx為水平焦距，b為相機基線.

ORB-SLAM從左相機圖片提取特征點并與右相機匹配，如果匹配不到對應(yīng)點，則將其設(shè)為單目特征點，否則設(shè)為雙目特征點.雙目特征點又根據(jù)它的深度d與基線倍數(shù)的關(guān)系分為近點和遠(yuǎn)點.近點可以提供有效的旋轉(zhuǎn)和平移信息，而遠(yuǎn)點只能提供有效的旋轉(zhuǎn)信息.單目特征點與雙目特征點一同參與后續(xù)光束法平差.系統(tǒng)啟動時，以初始幀為關(guān)鍵幀，以該幀對應(yīng)的相機位姿作為起點，提取其全部雙目特征點作為初始地圖.

2.2 跟蹤

式中，ρ是Huber代價函數(shù)；Σ是協(xié)方差矩陣；π（?）是是針孔相機投影函數(shù)，定義如下：

其中，fy是豎直焦距，（cx，cy）是相機主點，都可以從相機標(biāo)定中獲取.

此外，由于局部建圖進(jìn)程中存在剔除冗余關(guān)鍵幀的機制，快速補給后續(xù)關(guān)鍵幀可以使系統(tǒng)在復(fù)雜衛(wèi)星運動情況下的跟蹤更具魯棒性.插入新關(guān)鍵幀前，需要滿足以下條件：

（1）距離上一次全局重定位超過20幀.

（2）局部建圖進(jìn)程處于空閑狀態(tài)，且距離上一次關(guān)鍵幀插入超過20幀.

（3）當(dāng)前幀至少跟蹤到了50個點.

（4）當(dāng)前幀能夠跟蹤到的地圖點比參考關(guān)鍵幀少90%.

在我們的仿真中，設(shè)置關(guān)鍵幀最大數(shù)量為20幀，以保證系統(tǒng)的速度和魯棒性.

2.3 局部建圖

對每個新插入的關(guān)鍵幀.首先計算該關(guān)鍵幀的詞袋，便于三角化新特征點；接著維護(hù)局部地圖，剔除異常地圖點；并且根據(jù)關(guān)鍵幀的連接關(guān)系拓展新的局部地圖點；然后運用局部BA優(yōu)化局部地圖中所有關(guān)鍵幀的位姿及局部地圖中的所有地圖點.光束法平差優(yōu)化一系列共視幀ΚL和這些幀看到的所有點PL.PL中能在其他關(guān)鍵幀ΚF看到的點在優(yōu)化中作為常值.定義χk作為PL中的點與關(guān)鍵幀k中的關(guān)鍵點匹配的集合，代價函數(shù)如下［13］：

其中，Rk和tk表示關(guān)鍵幀k代表的相機旋轉(zhuǎn)和平移.

同時剔除冗余關(guān)鍵幀，維持光束法平差的復(fù)雜度，使系統(tǒng)可以在固定環(huán)境中長久的運行.

2.4 閉環(huán)

閉環(huán)檢測能夠在系統(tǒng)中引入一條強力約束，對提高SLAM的精度至關(guān)重要.閉環(huán)檢測進(jìn)程處理局部建圖中的最近一幀關(guān)鍵幀，檢測軌跡是否閉環(huán)，并在檢測到閉環(huán)后矯正回路，首先提取該關(guān)鍵幀的詞袋向量并在數(shù)據(jù)庫中尋找閉環(huán)候選關(guān)鍵幀；接著計算候選幀與關(guān)鍵幀之間的剛體變換，如果檢測到足夠多的有效點，則將該關(guān)鍵幀設(shè)為閉環(huán)關(guān)鍵幀；然后將新的邊插入共視幀中，校正閉環(huán)關(guān)鍵幀和與它相連的關(guān)鍵幀，并將重復(fù)的地圖點融合；之后對基本圖進(jìn)行姿態(tài)圖優(yōu)化，分配閉環(huán)誤差；最后進(jìn)行全局BA，優(yōu)化所有關(guān)鍵幀位姿和所有地圖點，使地圖具有全局一致性.

3 仿真實驗

本節(jié)進(jìn)行數(shù)值仿真，以驗證本文方法的有效性.仿真中分別考慮良好光照和弱光照兩種情況.在使用SLAM方法進(jìn)行姿態(tài)計算前，需要先對雙目相機進(jìn)行標(biāo)定，本文標(biāo)定過程如下：首先利用雙目相機拍攝15張棋盤格在不同位姿下的圖片，然后輸入到Matlab中的Stereo Camera Calibrator工具箱，以計算出相機的內(nèi)外參數(shù)，這些參數(shù)將用于后續(xù)圖像處理.

數(shù)值仿真中，采用POV-Ray軟件（注：POVRay軟件長度無量綱）生成由太陽能帆板、衛(wèi)星主體等組成的衛(wèi)星模型，如圖2所示.仿真時，采用兩臺單目針孔相機在不同位置采集相同的模型運動來模擬雙目相機采集的圖像.設(shè)定衛(wèi)星模型繞Z軸作單軸勻速旋轉(zhuǎn)運動，旋轉(zhuǎn)速度為每幀0.5度，旋轉(zhuǎn)角度分別為30°、60°、90°、180°、360°、720°.將產(chǎn)生的雙目圖像以及先前標(biāo)定的相機參數(shù)導(dǎo)入ORBSLAM系統(tǒng)中，利用stereo模式計算出旋轉(zhuǎn)角度.

圖2 衛(wèi)星模型示意圖Fig.2 Satellite model

3.1 良好光照條件下的仿真

首先考慮良好光照條件下的仿真.在相機后方加入白色光源后，可以得到良好光照的衛(wèi)星模型運動圖像.表1給出了良好光照和弱光照情況下衛(wèi)星分別旋轉(zhuǎn)30°、60°、90°、180°、360°、720°時的姿態(tài)估計值、真實值和誤差對比.圖3給出了良好光照條件下衛(wèi)星分別旋轉(zhuǎn)30°、180°、360°、720°時的姿態(tài)結(jié)果，其中實線為真實值，點線為采用本文方法的估計值.從表1和圖3中可以看出，良好光照條件下，數(shù)據(jù)均方根誤差均在0.3°以下，720°情況的誤差在1.5°以下，這表明SLAM算法在計算衛(wèi)星運動姿態(tài)時擁有很高的精度.隨著旋轉(zhuǎn)角度增大，有少量的誤差增長，這是未能觸發(fā)閉環(huán)造成的誤差累積，如果能夠檢測到閉環(huán)觸發(fā)全局優(yōu)化，誤差將會被盡可能地消除.

圖3 良好光照下衛(wèi)星姿態(tài)計算結(jié)果：（a）30°，（b）180°，（c）360°，（d）720°Fig.3 Attitude estimation results under good illumination：（a）30°，（b）180°，（c）360°，（d）720°

表1 衛(wèi)星旋轉(zhuǎn)計算結(jié)果Table 1 Attitude estimation results

3.2 弱光照條件下的仿真

然后考慮弱光照條件下的仿真.在POV-Ray軟件中不添加光源，首先設(shè)定環(huán)境光參數(shù)為<0.01，0.01，0.01>.圖4顯示該條件下采集的以及經(jīng)過BIMEF、Dong、LIME、MSRCR、SRIE方法增強后的衛(wèi)星模型圖像.從圖4中可以看出BIMEF、Dong、SRIE方法對弱光照下采集的圖像增強效果有限，MSRCR方法增強后的圖像在衛(wèi)星周圍產(chǎn)生了光暈，LIME方案增強后的圖像效果最好.

圖4 不同情況下的衛(wèi)星模型圖像：（a）弱光照，（b）BIMEF增強后，（c）Dong增強后，（d）LIME增強后，（e）MSRCR增強后，（f）SRIE增強后Fig.4 Satellie model in different situation：（a）Low-light condition，（b）BIMEF，（c）Dong，（d）LIME，（e）MSRCR，（f）SRIE

然后設(shè)定環(huán)境光參數(shù)為<0.1，0.1，0.1>，生成弱光照條件下的衛(wèi)星模型運動，利用雙目相機采集圖像 .圖 5 給出了衛(wèi)星分別旋轉(zhuǎn)30°、180°、360°、720°時的姿態(tài)結(jié)果.從表1和圖5可以看出弱光照工況下，所有數(shù)據(jù)均方根誤差均在0.7°以下，720°情況的誤差在4.5°以下；且從表1的對比可知，衛(wèi)星轉(zhuǎn)動在180°以內(nèi)時，弱光照的估計比良好光照的估計誤差增加在0.5°以內(nèi)，衛(wèi)星轉(zhuǎn)動在720°以內(nèi)時誤差增加在3°以內(nèi)；這表明LIME與SLAM相結(jié)合的方案可以有效得到弱光照情況下的衛(wèi)星運動姿態(tài)結(jié)果.

圖5 低光照圖像增強后衛(wèi)星旋轉(zhuǎn)計算結(jié)果：（a）30°，（b）180°，（c）360°，（d）720°Fig.5 Attitude estimation results in low-light condition：（a）30°，（b）180°，（c）360°，（d）720°

4 結(jié)論

本文采用LIME算法和ORB-SLAM算法，對弱光照條件下的空間非合作目標(biāo)的姿態(tài)觀測問題進(jìn)行了研究，LIME算法用于圖像增強，ORB-SLAM用于姿態(tài)估計.數(shù)值仿真結(jié)果驗證了本文方法的有效性.目前關(guān)于非合作目標(biāo)視覺運動估計的研究大多是基于目標(biāo)的3D模型已知的情況進(jìn)行的，而且較少考慮弱光照的情況，因此本文研究具有良好的參考價值.

在此值得說明的是，本文仿真中未能觸發(fā)ORB-SLAM的閉環(huán)進(jìn)程，若能觸發(fā)閉環(huán)則可以提高估計精度，這有待于今后做進(jìn)一步的探索.