劉宗明，牟金震，張碩，杜宣，曹姝清，張宇,*

1. 上海航天控制技術(shù)研究所, 上海 201109 2. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室, 上海 201109 3. 上海航天技術(shù)研究院, 上海 201109

針對諸如空間碎片、失效衛(wèi)星和小行星等非合作目標(biāo)的相對測量問題，其中一種解決思路是利用視覺傳感器直接對目標(biāo)進(jìn)行在線三維重建。隨著機(jī)器人技術(shù)的快速發(fā)展，同步定位與制圖(SLAM)[1]技術(shù)可以有益地借鑒到針對非合作目標(biāo)的測量任務(wù)中來。SLAM技術(shù)已在機(jī)器人領(lǐng)域得到了廣泛的應(yīng)用，它可以幫助機(jī)器人在一個(gè)未知的環(huán)境中計(jì)算出自己的運(yùn)動(dòng)軌跡，同時(shí)可以重建周圍環(huán)境的地圖。在大多數(shù)應(yīng)用中，機(jī)器人運(yùn)行的環(huán)境被認(rèn)為是靜止不動(dòng)的，但是如果動(dòng)態(tài)場景中存在的是剛體目標(biāo)的話，SLAM也是可以處理的。因而，完全可以將SLAM技術(shù)應(yīng)用到空間非合作目標(biāo)的測量中來。

Sonnenburg等[2]提出了一種基于特征和EKF(Extended Kalman Filter)濾波器的SLAM算法，該算法聚焦于相對導(dǎo)航濾波器結(jié)構(gòu)設(shè)計(jì)，特征點(diǎn)集假設(shè)利用雙目相機(jī)已經(jīng)完成了提取，并且作為輸入提供給濾波器；在濾波器中建立的平移運(yùn)動(dòng)方程是基于Hill相對運(yùn)動(dòng)模型的，該模型假設(shè)目標(biāo)飛行器和追蹤飛行器之間的相對軌道是圓形的，但是濾波器的狀態(tài)量中不包含目標(biāo)的慣量信息；考慮了兩種不同的絕對轉(zhuǎn)動(dòng)動(dòng)力學(xué)情況，在這兩種情況中都忽略了目標(biāo)飛行器和追蹤飛行器在LVLH(Local Vertical Local Horizontal)坐標(biāo)系下的指向差異；同時(shí)，提出了利用EKF進(jìn)行單目相機(jī)特征提取和跟蹤的濾波器框架。

Augenstein和Rock[3]提出了一種基于單目視覺SLAM的非合作目標(biāo)位姿跟蹤和外形重建算法，利用了Rao-Blackwellized粒子濾波器架構(gòu)計(jì)算相對旋轉(zhuǎn)和平移動(dòng)力學(xué)；假設(shè)目標(biāo)以恒定角速度進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng)，并且為了給濾波器提供一個(gè)先驗(yàn)估計(jì)，同時(shí)需要假設(shè)初始位姿已經(jīng)獲得；由于算法只能得到目標(biāo)的離散三維點(diǎn)云圖，所以無法對慣性矩進(jìn)行估計(jì)；對算法進(jìn)行了數(shù)學(xué)仿真，并且利用水下繩系目標(biāo)進(jìn)行了試驗(yàn)驗(yàn)證。

Schnitzer等[4]融合了基于EKF濾波的SLAM算法和基于RANSAC(RANdom SAmple Consensus)的表面重建算法對非合作目標(biāo)進(jìn)行建模，利用SURF(Speeded-Up Robust Features)算子[5]描述目標(biāo)表面的特征點(diǎn)，基于立體視覺原理計(jì)算目標(biāo)特征點(diǎn)的位置信息；利用小型接近操作模擬器在實(shí)驗(yàn)室內(nèi)對算法性能進(jìn)行了驗(yàn)證，試驗(yàn)結(jié)果表明盡管存在噪聲干擾，同時(shí)目標(biāo)三維點(diǎn)云的分布是稀疏的、不均勻的，該算法仍然能夠得到較好的表面重建結(jié)果，但完成一次計(jì)算需要耗時(shí)幾秒鐘，實(shí)時(shí)性難以保證。

Tweddle[6]在其博士畢業(yè)論文中在Kaess等[7]工作的基礎(chǔ)上提出了一種繞其慣性主軸旋轉(zhuǎn)的非合作目標(biāo)SLAM測量方法，該算法采用的平滑濾波器相較于常規(guī)的卡爾曼濾波器而言具有更好的收斂性，但是帶來的問題同樣也是計(jì)算負(fù)擔(dān)較重、耗時(shí)較長；該算法假設(shè)追蹤飛行器靜止不動(dòng)，模型方程中不考慮外部干擾力矩對相對運(yùn)動(dòng)動(dòng)力學(xué)的影響；Tweddle[6]利用球形衛(wèi)星模型對算法進(jìn)行了試驗(yàn)驗(yàn)證，基于雙目視覺完成三維重建，結(jié)合濾波器可以實(shí)現(xiàn)對完整的相對狀態(tài)量的估計(jì)，包括目標(biāo)線速度和角速度、質(zhì)心位置和慣性主軸等。

Segal等[8]提出了一種基于多iEKF(iterated Extended Kalman Filter)濾波器和最大后驗(yàn)概率的慣性張量辨識(shí)算法，采用雙目視覺的方式獲取目標(biāo)特征點(diǎn)的三維位置信息，濾波器方程中基于希爾方程建立相對運(yùn)動(dòng)模型，同時(shí)考慮了旋轉(zhuǎn)和平移運(yùn)動(dòng)的動(dòng)力學(xué)耦合[9]，通過數(shù)學(xué)仿真對算法性能進(jìn)行了評(píng)估；在仿真中假設(shè)通過雙目視覺相機(jī)可以穩(wěn)定地獲得均勻分布于目標(biāo)表面的10個(gè)特征點(diǎn)，雙目相機(jī)的基線為1 m，最遠(yuǎn)測量距離大于100 m，高斯噪聲為10-5rad，平均測量精度為厘米級(jí)。

Conway等[10]提出了一種微小型目標(biāo)接近過程的導(dǎo)航策略，采用了RGB-D(RGB-Deepth)相機(jī)，采樣幀頻為30 fps，可以同時(shí)獲得圖像信息和深度信息；特征點(diǎn)提取和匹配采用的是ORB(Oriented fast and Rotated Brief)描述子，通過二維圖像特征點(diǎn)及其對應(yīng)深度值的匹配可以得到特征點(diǎn)的三維信息；進(jìn)行了數(shù)學(xué)仿真，并利用微軟公司的Kinect進(jìn)行了測試試驗(yàn)。

Pesce等[11]提出了一種相對運(yùn)動(dòng)估計(jì)和慣性參數(shù)重建的iEKF方法，文獻(xiàn)[11]中的濾波器結(jié)構(gòu)與文獻(xiàn)[6]相似，但是文獻(xiàn)[11]采用的平滑方法提高了計(jì)算效率；由于文獻(xiàn)[11]并未考慮外部干擾力矩對相對旋轉(zhuǎn)動(dòng)力學(xué)模型的影響，因?yàn)橹荒芮蟮脩T量比，但是可以通過一個(gè)離線的圖像處理過程獲得目標(biāo)完整的三維重建模型；與文獻(xiàn)[8]類似，Pesce等[11]利用簡化的立體視模型，通過數(shù)學(xué)仿真對算法性能進(jìn)行了驗(yàn)證，并且對離線三維重建算法進(jìn)行了初步的測試評(píng)估。

從現(xiàn)有文獻(xiàn)的分析來看，目前針對非合作旋轉(zhuǎn)目標(biāo)的測量方案主要還是集中在基于特征的濾波器設(shè)計(jì)方面，利用濾波算法對目標(biāo)狀態(tài)進(jìn)行預(yù)測，常用的濾波方法在一個(gè)較小的運(yùn)動(dòng)尺度和較低的運(yùn)動(dòng)速度范圍內(nèi)，通過持續(xù)地維護(hù)和升級(jí)攝像機(jī)狀態(tài)向量和特征點(diǎn)向量進(jìn)行測量，可以表現(xiàn)出較好的跟蹤效果。但是其計(jì)算精度受限于所構(gòu)建的狀態(tài)模型和測量模型的精度，模型的非線性和相機(jī)的測量噪聲等會(huì)導(dǎo)致測量誤差放大，甚至發(fā)散。計(jì)算速度也受到狀態(tài)向量維數(shù)的嚴(yán)重制約，即使是通過減少特征數(shù)來保持計(jì)算效率，也很難避免高維的狀態(tài)向量導(dǎo)致的“維數(shù)災(zāi)難”問題[12]。

為了克服以上問題，本文構(gòu)建了目標(biāo)特征數(shù)據(jù)庫，將當(dāng)前幀與特征數(shù)據(jù)庫進(jìn)行比較，完成特征匹配，采用關(guān)鍵幀共視圖對特征點(diǎn)進(jìn)行搜索跟蹤?；谖蛔藞D優(yōu)化完成對目標(biāo)位姿的局部和全局最優(yōu)化求解。在李代數(shù)空間下建立位姿向量微分?jǐn)_動(dòng)方程，獲得測量值與估計(jì)值的殘差目標(biāo)函數(shù)，基于貝葉斯法則最大后驗(yàn)概率和李群與李代數(shù)的對指變換法則，求取位姿向量最優(yōu)解，以期解決李群空間下由于位姿變換矩陣不可加性而無法優(yōu)化的問題，提高連續(xù)測量過程中系統(tǒng)的測量精度。

1 失效慢旋衛(wèi)星視覺特征跟蹤

1.1 構(gòu)建目標(biāo)特征點(diǎn)數(shù)據(jù)庫

空間的光照條件較為復(fù)雜，視覺系統(tǒng)采集到的目標(biāo)航天器圖像會(huì)發(fā)生部分灰度退化或者幾何退化，如圖像模糊、圖像對比度變化和圖像灰度退化等。主要的干擾因素如下：

1) 目標(biāo)表面反射不均勻。由于空間目標(biāo)表面大都包覆有高反射系數(shù)的熱控多層保溫材料，太陽光、目標(biāo)表面和相機(jī)在一定的角度范圍內(nèi)會(huì)出現(xiàn)目標(biāo)成像效果局部過亮或過暗的現(xiàn)象，導(dǎo)致特征點(diǎn)無法有效提取。

2) 雜散光干擾。相機(jī)鏡頭安裝在航天器艙外，所以會(huì)受到光的照射，陽光、地氣光、月光、恒星光和行星光等天體照射艙體外各種部件導(dǎo)致光反射，形成大量的雜散光干擾。

特征點(diǎn)提取的快速性、描述子的尺度不變性和旋轉(zhuǎn)不變性是實(shí)現(xiàn)非合作旋轉(zhuǎn)目標(biāo)實(shí)時(shí)測量的基礎(chǔ)保證。為了保證特征的不變特性，需要在多個(gè)尺度空間下對圖像特征點(diǎn)進(jìn)行提取，并且賦予旋轉(zhuǎn)不變特性的描述子，將高維的原始圖像轉(zhuǎn)化為低維的描述向量。目標(biāo)特征子的提取速度和描述子的復(fù)雜程度將直接影響測量系統(tǒng)的計(jì)算效率和匹配效果，選取的圖像特征表達(dá)在提高計(jì)算速度的同時(shí)，也要保證在后續(xù)圖像匹配時(shí)的正確率，這兩點(diǎn)同時(shí)滿足才能保證特征檢測的精度與速度。為保證特征匹配的尺度不變性，使用尺度因子將當(dāng)前幀圖像轉(zhuǎn)換到8個(gè)尺度空間上進(jìn)行FAST(Features from Accelerated Segment Test)角點(diǎn)提取。為了確保在圖像核心目標(biāo)區(qū)域角點(diǎn)的均勻分布，對每個(gè)尺度的圖像進(jìn)行網(wǎng)格劃分，將其等分為若干份，在每個(gè)網(wǎng)格中至少提取5個(gè)角點(diǎn)。最后對提取到的FAST角點(diǎn)進(jìn)行Brief(Binary robust independent elementary features)描述子和方向的計(jì)算，共同組成一個(gè)ORB特征點(diǎn)。

在針對非合作目標(biāo)的測量過程中，目標(biāo)模型的具體尺寸是無法預(yù)先得知的，目標(biāo)表面的紋理特征就更無法提前獲取了。當(dāng)前時(shí)刻位姿取決于前一時(shí)刻的位姿和前一時(shí)刻與當(dāng)前時(shí)刻的位姿變換的乘積，該流程雖然計(jì)算簡單，運(yùn)算量少，但是如果當(dāng)前幀中特征點(diǎn)缺失，容易造成跟蹤失敗。并且當(dāng)前一幀的位姿估計(jì)誤差較大時(shí)，還會(huì)出現(xiàn)顯著的漂移，造成誤差累積，直至發(fā)散。因而考慮構(gòu)建目標(biāo)的二維和三維特征數(shù)據(jù)庫，關(guān)注的重點(diǎn)變?yōu)楫?dāng)前幀與特征數(shù)據(jù)庫的準(zhǔn)確跟蹤匹配和特征數(shù)據(jù)庫中特征點(diǎn)的優(yōu)化問題。假設(shè)只考慮前后兩圖像幀之間運(yùn)動(dòng)關(guān)系，設(shè)前一幀為參考幀，用ref表示，當(dāng)前幀用cur表示，以參考幀為基準(zhǔn)，計(jì)算當(dāng)前幀與其的運(yùn)動(dòng)關(guān)系。假設(shè)參考幀和當(dāng)前幀相對世界坐標(biāo)系的變換矩陣分別為Trw和Tcw，兩幀之間的變換矩陣為Tcr，則它們之間的關(guān)系可表示為

(1)

在圖1中，在t0～t1時(shí)刻，以t0為參考，求t1時(shí)刻的運(yùn)動(dòng)；然后在t1～t2時(shí)刻，又以t1時(shí)刻為參考幀，考慮t1～t2時(shí)刻間的運(yùn)動(dòng)關(guān)系，如此往復(fù)。如果當(dāng)前幀或參考幀中紋理稀少、光照變化和遮擋等原因?qū)е绿卣鼽c(diǎn)缺失，容易造成跟蹤失敗。并且，當(dāng)參考幀的位姿估計(jì)誤差較大時(shí)，還會(huì)出現(xiàn)顯著的漂移，造成誤差累積，直至發(fā)散。因此，考慮構(gòu)建目標(biāo)的二維和三維特征數(shù)據(jù)庫，將當(dāng)前幀與特征數(shù)據(jù)庫進(jìn)行比較，如圖2所示。問題轉(zhuǎn)變?yōu)橄鄼C(jī)在每一個(gè)t時(shí)刻采集到的圖像與之前時(shí)刻重建的特征數(shù)據(jù)庫進(jìn)行準(zhǔn)確匹配和特征數(shù)據(jù)庫中特征點(diǎn)的優(yōu)化問題。在一個(gè)不斷更新的特征點(diǎn)數(shù)據(jù)庫中，只要特征點(diǎn)正確，即使過程幀出現(xiàn)了差錯(cuò)，仍然有可能求出后續(xù)圖像幀的正確位置。特征數(shù)據(jù)庫又分為局部和全局兩種，局部特征數(shù)據(jù)庫描述了當(dāng)前幀附近的特征信息，可以提高當(dāng)前位姿的計(jì)算效率。全局特征數(shù)據(jù)庫則記錄了系統(tǒng)運(yùn)行以來，根據(jù)一定規(guī)則選擇保留的特征點(diǎn)，主要用于閉環(huán)檢測。

目標(biāo)數(shù)據(jù)庫的特征點(diǎn)是指可靠的目標(biāo)三維點(diǎn)，每個(gè)數(shù)據(jù)庫特征點(diǎn)存包含的信息包括：

1) 其在世界坐標(biāo)系下的三維坐標(biāo)。

2) 平均視線方向(由該點(diǎn)指向所有共有這個(gè)點(diǎn)的關(guān)鍵幀光心的平均法向量)。

3) ORB特征算子[13]和描述子[14]。

4) 根據(jù)ORB尺度因子(1.2)確定的最大和最小可視范圍。

其中，關(guān)鍵幀是指從連續(xù)采集的圖像中選出來的幀，每個(gè)關(guān)鍵幀包括：

1) 相機(jī)姿態(tài)矩陣。

2) 相機(jī)的內(nèi)參矩陣。

3) 本幀所有的ORB特征點(diǎn)。

為保證特征庫點(diǎn)云數(shù)量控制在一定范圍內(nèi)，不至于過于膨脹，當(dāng)有更合適點(diǎn)出現(xiàn)時(shí)，需要將其加入到特征庫中；當(dāng)出現(xiàn)冗余或相似點(diǎn)云時(shí)，需要將其從特征庫中剔除。

1.2 利用特征數(shù)據(jù)庫進(jìn)行特征點(diǎn)跟蹤

根據(jù)上一幀和勻速運(yùn)動(dòng)模型估計(jì)的當(dāng)前幀位姿變換為

(2)

假設(shè)相機(jī)的內(nèi)參為K，上一幀的三維特征點(diǎn)集為Piw，其在當(dāng)前幀中的投影為

(3)

將估計(jì)的特征點(diǎn)與實(shí)際提取的ORB特征點(diǎn)進(jìn)行匹配，如果當(dāng)前幀與上一幀沒有足夠的匹配點(diǎn)，通過在上一幀預(yù)估的位置附近擴(kuò)大一定的搜索范圍繼續(xù)搜索。當(dāng)找到對應(yīng)的匹配點(diǎn)后，利用它們對相機(jī)的預(yù)估位姿進(jìn)行基于捆集調(diào)整優(yōu)化處理，便可以得到當(dāng)前幀較為準(zhǔn)確的位姿變換。為了提高測量的可靠性和精度，在得到了一個(gè)相機(jī)的位姿估計(jì)和一個(gè)初始的特征匹配集的基礎(chǔ)上，便可以將特征數(shù)據(jù)庫中的三維點(diǎn)投影到當(dāng)前幀中搜索更多的匹配點(diǎn)對。為了限制計(jì)算的復(fù)雜度，只投影局部三維特征數(shù)據(jù)庫，此局部特征數(shù)據(jù)庫包括一個(gè)關(guān)鍵幀集合K1，此集合與當(dāng)前幀有公共的三維特征點(diǎn)；還包括一個(gè)關(guān)鍵幀集合K2，此集合與K1相鄰，具有共視圖；最后還包括一個(gè)參考關(guān)鍵幀Kref，其中，Kref∈K1，Kref與當(dāng)前幀有最多的共視點(diǎn)[15]。它們之間的關(guān)系如圖3所示。其中，F(xiàn)cur表示當(dāng)前幀；K11、K12和K13構(gòu)成集合K1；K13為參考關(guān)鍵幀Kref；K21、K22和K23構(gòu)成集合K2。在測量過程中，關(guān)鍵幀和特征點(diǎn)數(shù)據(jù)集不應(yīng)該一直無限增大，這樣不僅是對搜索的運(yùn)算量還是存儲(chǔ)空間而言都是巨大的壓力。在局部建圖過程中有一個(gè)冗余關(guān)鍵幀和特征點(diǎn)剔除機(jī)制，因此，在跟蹤過程中要實(shí)時(shí)確定該點(diǎn)是否為可用特征點(diǎn)；在跟蹤的最后一步是確定當(dāng)前幀是否為關(guān)鍵幀，這樣可以使得對相機(jī)運(yùn)動(dòng)的追蹤變得更加穩(wěn)定。每一個(gè)在K1和K2集合中可以看到的三維點(diǎn)在當(dāng)前幀中按照以下規(guī)則進(jìn)行搜索：

1) 計(jì)算三維點(diǎn)在當(dāng)前幀中的投影點(diǎn)，如果其超出了預(yù)先設(shè)置的圖像邊界，將該點(diǎn)剔除，不將其用于特征接力。

2) 計(jì)算當(dāng)前視軸(當(dāng)前幀法向量)和該三維點(diǎn)的平均視線(該三維點(diǎn)指向所屬的所有關(guān)鍵幀光心的平均法向量)的夾角，如果大于60°則將該點(diǎn)剔除，不將其用于特征接力。

3) 計(jì)算該三維點(diǎn)到當(dāng)前幀所對應(yīng)的光心距，如果超出其對應(yīng)的尺度不變域范圍則將該點(diǎn)剔除，不將其用于特征接力。

4) 計(jì)算當(dāng)前幀尺度。

在尺度范圍內(nèi)，將估計(jì)的特征點(diǎn)與實(shí)際提取的ORB特征點(diǎn)進(jìn)行匹配，如果當(dāng)前幀與上一幀沒有足夠的匹配點(diǎn)，則通過在上一幀預(yù)估的位置附近擴(kuò)大一定的搜索范圍繼續(xù)搜索。當(dāng)找到對應(yīng)的匹配點(diǎn)后，則將其與當(dāng)前三維點(diǎn)關(guān)聯(lián)起來，據(jù)此，可以將當(dāng)前幀與特征數(shù)據(jù)庫中的公共點(diǎn)對應(yīng)起來，完成特征接力。利用當(dāng)前幀中的特征點(diǎn)對相機(jī)的位姿進(jìn)行基于捆集調(diào)整優(yōu)化處理，便可以得到當(dāng)前幀較為準(zhǔn)確的位姿變換。

2 李代數(shù)空間下的位姿圖優(yōu)化

2.1 圖優(yōu)化概念

假設(shè)狀態(tài)變量x=[x1,x2,…,xn]T，其中xk表示在k時(shí)刻相機(jī)的位姿節(jié)點(diǎn)。兩節(jié)點(diǎn)之間的觀測方程為

zij=h(xi,xj)+vij

(4)

式中：h為測量函數(shù)；vij為觀測噪聲，由于各種誤差因素的存在，產(chǎn)生了測量殘差：

eij(xi,xj)=zij-h(xi,xj)

(5)

假設(shè)以xk為優(yōu)化變量，對于一個(gè)具有n條邊的圖，其每條邊的優(yōu)化目標(biāo)函數(shù)為

(6)

式中：Ωij為信息矩陣，是協(xié)方差陣的逆，是一個(gè)對稱矩陣。所有邊的優(yōu)化目標(biāo)函數(shù)為

(7)

最終的優(yōu)化目標(biāo)誤差函數(shù)可以寫成：

(8)

(9)

(10)

(11)

如果考慮所有邊，則式(11)可改寫為

HΔx=-b

(12)

式中:H為信息矩陣;b為標(biāo)量系數(shù)。

系統(tǒng)的解就是在初始值上疊加此增量：

(13)

GN迭代就是不斷重復(fù)式(13)直至收斂，LM迭代則引入了一個(gè)松弛因子λ控制迭代速度：

(H+λI)Δx=-b

(14)

由于GN迭代有可能出現(xiàn)雅可比矩陣乘積奇異或者病態(tài)的情況，導(dǎo)致增量的穩(wěn)定性差，算法因此將會(huì)發(fā)散，LM迭代在一定程度上修正了這個(gè)問題，因而采用LM迭代。

2.2 位姿圖優(yōu)化

在2.1節(jié)中定義的目標(biāo)誤差函數(shù)可能會(huì)沒有意義，因?yàn)榧臃ú僮鲗ξ蛔俗儞Q矩陣而言是不封閉的，即兩個(gè)變換矩陣的和并不是變換矩陣。而且，在構(gòu)建優(yōu)化問題時(shí)，旋轉(zhuǎn)矩陣作為待計(jì)算的優(yōu)化變量，必須是一個(gè)行列式為1的正交矩陣，這樣會(huì)引入額外的約束條件，使優(yōu)化問題變得更復(fù)雜，因而不便于在李群空間下直接對其進(jìn)行優(yōu)化操作。筆者先在李代數(shù)空間進(jìn)行加法與求導(dǎo)操作，再將計(jì)算結(jié)果轉(zhuǎn)換為李群[19]，便可完成對測量誤差函數(shù)的優(yōu)化求解。

在剛體變換過程中定義三維歐氏群為

SE(3)=

(15)

式中：g為位姿變換矩陣；SO(3)為三維旋轉(zhuǎn)群。

定義三維歐氏群對應(yīng)的李代數(shù)為

se(3)=

(16)

式中：ξ為一個(gè)六維向量；上標(biāo)∧代表求反對稱陣運(yùn)算；ω為旋轉(zhuǎn)角速度；v為速度；so(3)為三維旋轉(zhuǎn)李代數(shù)。

李代數(shù)對應(yīng)的六維向量為ξ=[v，ω]T∈6，可以得到三維歐氏李代數(shù)到三維歐氏李群的指數(shù)映射：

(17)

式中：eξ為李代數(shù)到李群的指數(shù)映射；eω∧為旋轉(zhuǎn)角速度的指數(shù)映射；V為變換系數(shù),可表示為

(18)

式中：α為三維向量ω的方向；θ為三維向量ω的模。

由于ω是一個(gè)三維向量，由以上分析可知，相應(yīng)的對數(shù)運(yùn)算可以將李群映射到李代數(shù)：

(19)

式中：上標(biāo)∨表示從矩陣到向量的運(yùn)算。

在位姿圖優(yōu)化中，節(jié)點(diǎn)表示相機(jī)的位姿，用ξ1,ξ2,…,ξn表述；邊表示兩個(gè)位姿節(jié)點(diǎn)之間相對運(yùn)動(dòng)狀態(tài)的估計(jì)。假設(shè)兩個(gè)位姿節(jié)點(diǎn)在三維歐氏李代數(shù)se(3)上的表示為ξi和ξj，它們之間的運(yùn)動(dòng)估計(jì)為ξij，則其對應(yīng)關(guān)系為

(20)

如果在三維歐氏李群SE(3)上的表示為Ti、Tj和位姿i、j間的變換矩陣Tij，則其對應(yīng)關(guān)系為

(21)

構(gòu)建誤差函數(shù)：

(22)

利用李代數(shù)擾動(dòng)模型將誤差函數(shù)eij分別對ξi和ξj各加一個(gè)左擾動(dòng)δξi和δξj，式(20)變?yōu)?/p>

(23)

根據(jù)SE(3)上的伴隨性質(zhì)[20]：

eξ∧T=Te(Ad(T-1)ξ)∧

(24)

式中：

(25)

(26)

根據(jù)李代數(shù)上的求導(dǎo)法則可以得到關(guān)于位姿Ti和Tj的雅可比矩陣，其中：

(27)

式中：Jr為雅可比矩陣。

在此對雅可比矩陣行近似，可以得到

(28)

3 測量實(shí)驗(yàn)與結(jié)果

實(shí)驗(yàn)環(huán)境為超近距離相對運(yùn)動(dòng)實(shí)驗(yàn)室，如圖6所示；所用運(yùn)動(dòng)裝置為高精度三軸轉(zhuǎn)臺(tái)，如圖7所示。對目標(biāo)星的模擬采用了1∶1的真實(shí)模型，表面貼有熱控多層反光材料，同時(shí)，采用高亮度LED光源模擬入射的太陽光。模型分別固定于轉(zhuǎn)臺(tái)上，以一定的角速度繞豎直軸勻速運(yùn)動(dòng)。由于目標(biāo)的轉(zhuǎn)動(dòng)，相機(jī)成像的亮度值發(fā)生了明顯變化，甚至出現(xiàn)曝光飽和現(xiàn)象。實(shí)驗(yàn)中先后進(jìn)行了基于前后圖像幀信息的EKF濾波算法、無位姿優(yōu)化和有位姿優(yōu)化的測量算法，從實(shí)驗(yàn)結(jié)果來看，所提算法能較好地適應(yīng)光照條件變化，具有較強(qiáng)的魯棒性。

3.1 基于前后圖像幀信息的EKF濾波算法驗(yàn)證

目標(biāo)以5 (°)/s的角速度繞垂直地面軸做自旋運(yùn)動(dòng)，每當(dāng)視覺相機(jī)檢測到新的特征時(shí)，都將其添加到狀態(tài)方程中，組成全狀態(tài)模型[22]：

(29)

圖8為連續(xù)測量過程中保存的圖像幀，圖9為相機(jī)等效的運(yùn)行軌跡。圖9可以反映出目標(biāo)是在繞其固定軸做旋轉(zhuǎn)運(yùn)動(dòng)，但是軌跡圖并不完整，只繪制出了3/4的圓周運(yùn)動(dòng)。究其原因在于模型的非線性和相機(jī)的測量噪聲等因素影響，導(dǎo)致測量誤差放大，無法收斂，通過前后幀估計(jì)得到的特征點(diǎn)位置和位姿變換矩陣無法繼續(xù)完成運(yùn)動(dòng)狀態(tài)的跟蹤測量。由于測量結(jié)果無尺度因子，因而圖中未標(biāo)出具體量綱。隨著特征點(diǎn)數(shù)量的增多，狀態(tài)向量的維度越來越大，數(shù)據(jù)更新的速率明顯降低，如圖10所示。

3.2 基于特征數(shù)據(jù)庫的無位姿優(yōu)化測量算法驗(yàn)證

目標(biāo)以12 (°)/s的角速度繞垂直地面軸做自旋運(yùn)動(dòng)，從圖11可以看出，在目標(biāo)衛(wèi)星旋轉(zhuǎn)過程中，盡管光照強(qiáng)度發(fā)生了比較明顯的變化，但是仍然能夠?qū)μ崛〕龅奶卣鼽c(diǎn)進(jìn)行比較好的跟蹤。從圖12可以看出，隨著時(shí)間的延長，累積誤差愈來愈大，三維點(diǎn)云逐步發(fā)散，無法較好地繪制出目標(biāo)的三維形態(tài)。圖13和圖14分別為無位姿優(yōu)化時(shí)針對以12 (°)/s旋轉(zhuǎn)的常規(guī)立方衛(wèi)星大模型的位姿測試曲線。從圖13中可以看出自旋軸旋轉(zhuǎn)角在-180°～180°范圍內(nèi)變化，與自旋軸垂直的兩軸角度隨著測量時(shí)間的延長并未趨于收斂。由于目標(biāo)模型固定安裝在三軸轉(zhuǎn)臺(tái)，其三軸位置不隨測量時(shí)間的延長發(fā)生明顯變化，為了更直觀地反映相機(jī)測量效果，在相機(jī)對目標(biāo)完成旋轉(zhuǎn)一周的測量后，其等效運(yùn)動(dòng)軌跡應(yīng)該是個(gè)理想的圓形，因而采用繪制相機(jī)等效運(yùn)動(dòng)的方式說明非線性優(yōu)化對位置測量的重要性。從圖15中也可以看出隨著時(shí)間的延長，位置曲線所構(gòu)成的圓的半徑在逐漸擴(kuò)大，呈發(fā)散狀態(tài)。

3.3 基于特征數(shù)據(jù)庫的有位姿優(yōu)化測量算法驗(yàn)證

圖16為包含位姿優(yōu)化的測量算法三維重建效果圖，稀疏點(diǎn)云較好地重建了目標(biāo)的三維形態(tài)，與實(shí)際試驗(yàn)情況相符。圖17和圖18分別為有閉環(huán)檢測時(shí)針對以12 (°)/s旋轉(zhuǎn)的常規(guī)立方衛(wèi)星大模型的位姿測試曲線。從圖中可以看出，測量曲線明顯連續(xù)光滑了很多，而且位置姿態(tài)是收斂狀態(tài)。圖17中自旋轉(zhuǎn)角在-180°～180°范圍內(nèi)變化，之所以出現(xiàn)明顯的階躍現(xiàn)象，一方面是由于畫圖采用的是關(guān)鍵幀位姿，而關(guān)鍵幀相對來說比較稀疏；另一方面是由于過程中包含有位姿閉環(huán)優(yōu)化算法，當(dāng)目標(biāo)旋轉(zhuǎn)回初始位置附近時(shí)，進(jìn)行了位姿的閉環(huán)檢測和位姿優(yōu)化，使原本趨于發(fā)散的位姿收斂了回來。

從圖19中可以明顯看出，由于全局優(yōu)化的存在，原本趨于發(fā)散的軌跡曲線收斂了回來，最終呈穩(wěn)定的圓形，與試驗(yàn)設(shè)置的真實(shí)情況相符。圖20給出的是擬合直線及其殘差，測量穩(wěn)定誤差在2°以內(nèi)，在個(gè)別點(diǎn)誤差會(huì)稍有增加。

擬合后的直線表達(dá)式為

a(n)=1.189n-725.2

(30)

式中：n為幀序號(hào)；a(n)為對應(yīng)的旋轉(zhuǎn)角度。分析可知，估計(jì)的平均角速度誤差約為0.12 (°)/s。

4 結(jié) 論

1) 構(gòu)建了一個(gè)可靠的目標(biāo)優(yōu)化特征數(shù)據(jù)庫，只要庫中的特征點(diǎn)正確，即使過程幀出現(xiàn)了差錯(cuò)，仍然有較高的概率求出后續(xù)圖像幀的正確位置。

2) 在李代數(shù)空間下建立位姿向量微分?jǐn)_動(dòng)方程，獲得測量值與估計(jì)值的殘差目標(biāo)函數(shù)，基于貝葉斯法則最大后驗(yàn)概率和李群與李代數(shù)的對指變換法則求取位姿向量最優(yōu)解。解決了李群空間下由于位姿變換矩陣不可加性而難以優(yōu)化的問題，提高了連續(xù)測量過程中系統(tǒng)的測量精度。

3) 利用實(shí)際衛(wèi)星等比例模型開展了測試實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明無優(yōu)化測量方法無法保證整個(gè)旋轉(zhuǎn)周期的有效測量；通過增加位姿優(yōu)化過程，連續(xù)穩(wěn)定測量時(shí)間明顯延長，所提算法都具有較好的跟蹤測量能力。