基于立體視覺Tri-EKF算法的空間非合作目標運動與結(jié)構(gòu)估計

王 珂，于占海，劉 宏，李瑞峰，郭本生

(哈爾濱工業(yè)大學機器人研究所，哈爾濱150080)

基于立體視覺Tri-EKF算法的空間非合作目標運動與結(jié)構(gòu)估計

王 珂，于占海，劉 宏，李瑞峰，郭本生

(哈爾濱工業(yè)大學機器人研究所，哈爾濱150080)

針對空間非合作目標的運動狀態(tài)及外形結(jié)構(gòu)估計的雙重擴展卡爾曼濾波(Dual EKF)算法存在運動狀態(tài)變量耦合及收斂速度慢的問題?；诹Ⅲw視覺觀測模型，引入特征線作為新觀測量分析了目標平移與旋轉(zhuǎn)運動估計的解耦性，提出三重擴展卡爾曼濾波(Tri-EKF)算法，實現(xiàn)了對目標旋轉(zhuǎn)、平移運動以及結(jié)構(gòu)參數(shù)的串行估計。本文對兩種算法進行仿真比較，通過試驗校驗Tri-EKF算法。結(jié)果表明：有限次觀測下，Tri-EKF算法對目標運動估計收斂速度更快，平均估計誤差更小。

非合作目標；Tri-EKF算法；運動及結(jié)構(gòu)估計；立體視覺

0 引 言

空間航天器在故障、失效或完成任務后都可能在空間內(nèi)自由漂浮成為太空垃圾[1]。這些不提供有效合作信息的空間非合作目標占用空間軌道資源，給正常運行的航天器造成極大威脅。而具備太空垃圾清除能力的空間機器人是捕獲和回收非合作目標的有效手段。機器人對非合作目標自主操作，要求其有能力評估目標運動姿態(tài)和結(jié)構(gòu)外形。因此，實現(xiàn)目標位置姿態(tài)及目標三維結(jié)構(gòu)外形的估計，對機器人可靠作業(yè)具有重要意義。

針對非合作目標運動和結(jié)構(gòu)估計，Terui等[2]采用迭代最近點(Iterative closest point, ICP)算法將測量數(shù)據(jù)與3D模型進行匹配得到目標相對姿態(tài)的估計結(jié)果，但該方法需要先驗模型并且無法實時估計。斯坦福大學的Augenstein等[3]提出了基于粒子濾波的快速即時定位與地圖構(gòu)建(Fast simultaneous localization and mapping,Fast SLAM)遞歸估計6自由度姿態(tài)估計算法，該方法不需要目標先驗知識，但粒子濾波計算量大，實時性較差。Segal等[4]采用擴展卡爾曼(Extended Kalman filter,EKF)估計目標運動狀態(tài)和結(jié)構(gòu)參數(shù)，其集中估計方式的狀態(tài)向量會不斷增廣，可能產(chǎn)生 “維度災難”問題。國內(nèi)學者，蔡晗等[5]利用目標的幾何特征，通過雙目視覺測量算法解算出目標相對姿態(tài)信息，但該方法對目標幾何特征要求較高，目標形狀需是矩形。周軍等[6]提出基于陀螺儀、擴展卡爾曼和加速度計的非合作目標的位姿估計方法，該方法魯棒性較好，但收斂速度較慢。翟光等[7]在傳統(tǒng)卡爾曼算法基礎上提出了一種姿態(tài)指向跟瞄導航算法，避免了傳統(tǒng)擴展卡爾曼算法近似線性化過程中的大量矩陣求導運算，因而提高了跟蹤導航濾波的收斂速度。郝剛濤等[8]提出一種基于EKF的單目視覺圖像序列的非合作目標相對姿態(tài)估計方法，該方法會引起“維度災難”問題，同樣降低收斂速度。上述方法多以聯(lián)合擴展卡爾曼濾波(Joint EKF)[9]為基礎，因此會引入增廣狀態(tài)向量。Ljung[10]指出，增廣狀態(tài)向量會引入運動狀態(tài)和結(jié)構(gòu)參數(shù)間的耦合。Liu等[11]還證明，若增廣狀態(tài)向量維數(shù)為n+m(n為運動狀態(tài)參數(shù)維數(shù)，m為結(jié)構(gòu)特征點數(shù)量)，Joint EKF將出現(xiàn)n+m維矩陣的三次相乘，會因狀態(tài)向量未知量較多而使濾波估計不穩(wěn)定乃至發(fā)散，造成數(shù)值病態(tài)問題并限制結(jié)構(gòu)點數(shù)量。因此，Zhang等[12]提出雙重擴展卡爾曼濾波(Dual EKF)，將運動狀態(tài)參數(shù)和結(jié)構(gòu)點估計進行串行分離，以解決兩者耦合關系。Zhang[13]針對串/并行Dual EKF，證明了串行Dual EKF為無偏估計。雖然系統(tǒng)狀態(tài)維數(shù)降低，但目標平移及旋轉(zhuǎn)運動仍要同時估計。

本文針對空間機器人超近距離非合作目標外結(jié)構(gòu)與運動參數(shù)在線估計，通過雙目立體視覺觀測和跟蹤動態(tài)目標的圖像特征。主要貢獻在于建立了以特征線結(jié)構(gòu)點為觀測特征的觀測模型，提出三重擴展卡爾曼濾波(Tri-EKF)算法對目標平移狀態(tài)分量、旋轉(zhuǎn)狀態(tài)分量，以及表面三維結(jié)構(gòu)點進行串行估計。相對于Dual EKF，Tri-EKF進一步降低系統(tǒng)狀態(tài)向量維數(shù)，保持實時計算特性，并提高了運動參數(shù)的估計速度，同時實現(xiàn)了目標三維外形結(jié)構(gòu)點的在線估計。本文通過仿真試驗綜合比較Dual EKF和Tri-EKF算法的收斂速度，收斂精度等，并基于試驗平臺校驗Tri-EKF算法在目標特性估計和三維點云重構(gòu)估計的有效性。

1 非合作目標運動及觀測模型

自由懸浮狀態(tài)的空間非合作目標具有平動、繞自旋軸旋轉(zhuǎn)的運動特性，首先建立坐標系，運動模型，結(jié)構(gòu)模型以及觀測模型。

1.1坐標系定義

建立如圖1所示的三個坐標系，分別為：

視覺坐標系OC，為立體視覺觀測系統(tǒng)測量目標表面特征點的基準坐標系。

結(jié)構(gòu)坐標系OS，位于剛體目標表面并隨目標運動，結(jié)構(gòu)特征點在該坐標系下坐標不隨時間變化。

目標坐標系OT, 目標結(jié)構(gòu)未知及旋轉(zhuǎn)中心未知，旋轉(zhuǎn)中心在OS中位置作為估計參量。

OS與OT的坐標軸平行，定義d=[dx,dy,dz]T為目標坐標系OT相對于視覺坐標系OC的位移矢量。目標圍繞過OT的自旋軸旋轉(zhuǎn)。

圖1 坐標系Fig.1 Coordinate system

1.2運動模型

目標運動包括自旋轉(zhuǎn)動及OT相對于OC平動，表述為位移、速度、旋轉(zhuǎn)中心位置、姿態(tài)角和角速度為參數(shù)的目標運動狀態(tài)模型。

1.2.1 平移運動

設OT相對于OC的速度矢量為v=[vx,vy,vz]T,矢量r=[rx,ry,rz]T為自旋中心OT在OS的位置，將其歸結(jié)為平移運動參數(shù)，因此目標離散時間方程為

(1)

式中：tk-1、tk代表k-1和k兩個連續(xù)離散采樣時刻，τ表示采樣時間間隔,ξk表示加速度項。

1.2.2 旋轉(zhuǎn)運動

采用四元數(shù)q=[q0,q1,q2,q3]T描述目標與OC的相對旋轉(zhuǎn)姿態(tài)[14]。設其相對OC的角速度矢量為ω=[ωx,ωy,ωz]T，建立目標姿態(tài)四元數(shù)與旋轉(zhuǎn)角速度間的離散表達式為

(2)

式中：Φ(ωk-1)為關于ωk-1的4階矩陣[15]。

1.2.3 運動學方程

(3)

式中：FT和FR分別代表平移和旋轉(zhuǎn)狀態(tài)分量的狀態(tài)轉(zhuǎn)移矩陣，平移分量為XT=[dT,vT,rT]T，旋轉(zhuǎn)分量為XR=[qT,ωT]T，wk為高斯白噪聲。

1.3結(jié)構(gòu)模型

XS,k=XS,k-1

(4)

1.4立體視覺觀測模型

(5)

設tk時刻目標表面有m特征點被同時觀測，定義三維坐標向量Zk，則觀測量pk的三維結(jié)構(gòu)坐標參數(shù)與目標運動參量間的變換關系

(6)

由此，可以得到目標觀測量Zk與運動參數(shù)XM,k和結(jié)構(gòu)參數(shù)XS,k間的觀測方程

(7)

2 Tri-EKF目標運動與結(jié)構(gòu)估計

Tri-EKF在Dual EKF結(jié)構(gòu)及運動參數(shù)解耦基礎上，將特征線作為旋轉(zhuǎn)運動觀測量，實現(xiàn)目標旋轉(zhuǎn)與平移運動參數(shù)的解耦。

2.1特征線

(8)

兩式相減得

(9)

式(9)不含平移變量，整理得

(10)

圖2 OC坐標系與OS坐標系下的特征線Fig.2 The feature line under OC and OS coordinate

式(10)定義的新觀測量建立了目標特征線與旋轉(zhuǎn)向量q間的函數(shù)關系，不需要平移運動估計結(jié)果，僅通過特征線觀測量便可實現(xiàn)對目標旋轉(zhuǎn)運動的獨立估計。

基于tk時刻m條特征線，建立目標觀測方程

(11)

2.2Tri-EKF算法

圖3為Tri-EKF結(jié)構(gòu)框圖，以目標特征線為觀測量，采用旋轉(zhuǎn)運動估計器、平移運動估計器和結(jié)構(gòu)參數(shù)估計器對目標參數(shù)進行串行估計，從而實現(xiàn)狀態(tài)向量降維。

旋轉(zhuǎn)運動估計器先通過特征點構(gòu)造特征線，然后將特征線組成的向量ZR,k作為觀測量；平移運動估計器和結(jié)構(gòu)參數(shù)估計器分別使用特征點三維坐標所組成的向量ZT,k和ZS,k作為觀測量，隨著各估計器間估計結(jié)果的傳遞，觀測方程中未知量個數(shù)逐漸減少。

圖3 Tri-EKF結(jié)構(gòu)框圖Fig.3 Structure diagram of Tri-EKF

旋轉(zhuǎn)運動估計狀態(tài)向量為XR，向量元素為四元數(shù)q和角速度ω，狀態(tài)方程為

(12)

式(11)為旋轉(zhuǎn)運動估計器觀測方程，為特征線組成的向量ZR,k。

平移運動估計器狀態(tài)向量為XT，向量元素為平移量d，平移速度v和r，狀態(tài)方程為

(13)

旋轉(zhuǎn)估計結(jié)果XR,k|k在該階段為已知量，因此將其作為輸入量會使平移估計器觀測方程未知量個數(shù)減少，其觀測方程為

ZT,k=HTXT,k|k-1+nk=

(14)

式中：HT為關于平移狀態(tài)變量XT,k|k-1的觀測雅可比矩陣。

目標結(jié)構(gòu)參數(shù)估計器狀態(tài)向量為XS，狀態(tài)方程與Dual EKF結(jié)構(gòu)參數(shù)估計器相同，即：

(15)

旋轉(zhuǎn)和平移估計結(jié)果XR,k|k、XT,k|k作為已知量輸入到結(jié)構(gòu)參數(shù)估計器觀測方程中，因此觀測方程未知量個數(shù)在平移估計器的基礎上再次減少，其觀測方程為

ZS,k=HSXS,k|k-1+nk=

(16)

式中：HS為關于結(jié)構(gòu)狀態(tài)變量XS的觀測雅可比矩陣。

Tri-EKF按順序依次估計旋轉(zhuǎn)參數(shù)、平移參數(shù)以及結(jié)構(gòu)參數(shù)，前面濾波器的結(jié)果將作為后面濾波器觀測方程的輸入，由于估計結(jié)果為已知量，因此會減少后面濾波器觀測方程的未知量個數(shù)。Tri-EKF算法主要步驟如下：

1)旋轉(zhuǎn)、平移和結(jié)構(gòu)濾波器的時間更新

(17)

式中：Xk-1|k-1為tk時刻濾波估計值，Xk|k-1為tk時刻濾波預測值，F(xiàn)S為結(jié)構(gòu)狀態(tài)分量的狀態(tài)轉(zhuǎn)移矩陣。

2)旋轉(zhuǎn)運動估計器的觀測更新

XR,k|k=XR,k|k-1+KR,k(ZR,k-HRXR,k|k-1)

(18)

式中：Xk|k為tk時刻濾波估計值，Kk為卡爾曼增益，HR為關于旋轉(zhuǎn)狀態(tài)變量XR,k|k-1的觀測雅可比矩陣。

3)平移運動估計器觀測更新

XT,k|k=XT,k|k-1+

(19)

4)結(jié)構(gòu)參數(shù)估計器觀測更新

XS,k|k=XS,k|k-1+

(20)

3 仿真

3.1仿真試驗1

圖4所示運動目標為底半徑5 m，高10 m圓柱體，圓柱體上、中、下圓面對稱分布4個點，共12個特征點。仿真假設特征點已經(jīng)提取并正確匹配。

目標初始位置設在相機坐標系(50, 50, 50) m處，運動形式為平移和旋轉(zhuǎn)復合運動。目標沿相機坐標系X、Y、Z軸的平移速度均為0.1 m/s，同時以π/50 (rad·s-1)的角速度繞過體心的方向向量(1, 1, 1)進行旋轉(zhuǎn)。選擇圓柱體上表面圓周上一點為結(jié)構(gòu)坐標系原點，建立結(jié)構(gòu)坐標系OS。計算目標特征點在相機坐標系下理論坐標值并引入高斯白噪聲，將含有噪聲的特征點三維坐標作為算法實際觀測值。

圖4 仿真圓柱體模型Fig.4 Simulation cylinder model

各狀態(tài)向量初始值設定如表1所示。濾波開始階段將狀態(tài)協(xié)方差矩陣初始值P0設置較大。本仿真將方陣P0的對角元設為1，觀測噪聲協(xié)方差矩陣Sk對角元設為0.25，根據(jù)濾波器結(jié)構(gòu)分別設置協(xié)方差矩陣維數(shù)。

表1 仿真試驗1的狀態(tài)向量初值Table 1 The initial value of the state vector in the first simulation experiment

為了校驗Tri-EKF算法的有效性，定義狀態(tài)變量估計誤差并對Tri-EKF和Dual EKF兩種算法的估計結(jié)果進行定量比較：

1)位移誤差：

2)速度誤差：

3)旋轉(zhuǎn)中心位置誤差：

4)角速度誤差：

式中：上標“^”表示tk時刻狀態(tài)變量濾波估計值，上標“-”表示tk時刻狀態(tài)變量真實值。

圖5為平移運動估計曲線。Tri-EKF對目標平移運動估計的收斂速度比Dual EKF快，Tri-EKF在50幀左右即可收斂，而Dual EKF至少需100幀。

圖6為角速度估計曲線。角速度估計收斂速度相比于平移都有所提高。Tri-EKF收斂速度仍然比Dual EKF快，Tri-EKF在40幀左右即可收斂，Dual EKF在70幀左右收斂。

圖7為估計誤差，收斂后估計誤差基本一致，但Tri-EKF收斂速度快，估計誤差下降快，在有限次(無論是否穩(wěn)定)觀測下Tri-EKF的平均估計誤差更小。

圖5 平移運動估計曲線Fig.5 Estimation of translation motion

圖6 角速度估計曲線Fig.6 Estimation of angular velocity

圖7 估計誤差曲線Fig.7 Estimation error

3.2仿真試驗2

運動目標為棱長10 m的正方體，如圖8所示，同樣假設仿真過程中特征點已經(jīng)提取并正確匹配。目標初始位置設在相機坐標系(50, 50, 50) m處，運動形式為平移和旋轉(zhuǎn)的復合運動。目標沿相機坐標系X、Y、Z軸的平移速度均為0.1 m/s，同時以π/50(rad/s)的角速度分別繞相機坐標系的Z、X、Y旋轉(zhuǎn)。在正方體上表面某一頂點處建立結(jié)構(gòu)坐標系OS。同樣將含有高斯噪聲的特征點三維坐標作為算法觀測值。

圖8 仿真正方體模型Fig.8 Simulation cube model

ωk真實初值為(0.063, 0.063, 0.063)，本次仿真試驗仍設定算法初值為(0.0, 0.0, 0.0)，其他參數(shù)與仿真試驗1相同。狀態(tài)協(xié)方差矩陣P0是對角元為3的方陣，觀測噪聲協(xié)方差矩陣Sk是對角元為0.5的方陣。

平移運動估計結(jié)果如圖9所示，分析曲線可知，Tri-EKF在50幀左右收斂，Dual EKF在100幀左右收斂，說明Tri-EKF比 Dual EKF具有更快的平移估計收斂速度。

圖9 平移運動估計結(jié)果Fig.9 Estimation of translation motion

圖10為角速度估計曲線。Tri-EKF的收斂速度比Dual EKF快10幀左右，大約在50幀左右收斂，后者收斂至少需要60幀。算法旋轉(zhuǎn)估計收斂速度比各自的平移估計收斂速度快。

圖11為估計誤差，相比于Dual EKF前50幀，Tri-EKF誤差降低迅速，算法收斂后估計誤差基本一致，但Tri-EKF收斂速度快，估計誤差下降快，因此在有限次(無論是否穩(wěn)定)觀測下Tri-EKF平均估計誤差更小。

圖10 角速度估計曲線Fig.10 Estimation of angular velocity

圖11 估計誤差Fig.11 Estimation error

3.3結(jié)果分析

綜合分析以上兩個仿真試驗的估計結(jié)果可以得出以下結(jié)論：相比于Dual EKF算法，Tri-EKF算法優(yōu)勢主要體現(xiàn)在有限次平均估計誤差和收斂速度上。Tri-EKF對旋轉(zhuǎn)、平移、結(jié)構(gòu)參數(shù)分別估計，降低了系統(tǒng)狀態(tài)向量維數(shù)，當Tri-EKF旋轉(zhuǎn)估計收斂并開始進行平移估計時，Dual EKF仍在進行旋轉(zhuǎn)參數(shù)估計。旋轉(zhuǎn)估計收斂速度要快于平移估計收斂速度，因此Tri-EKF收斂速度快的優(yōu)勢在平移運動中體現(xiàn)得更加明顯。由于收斂速度快，使得Tri-EKF在有限次觀測情況下的平均估計誤差更小。

雖然Dual EKF實現(xiàn)了運動參數(shù)和結(jié)構(gòu)參數(shù)的解耦，但其運動參數(shù)間存在耦合，導致收斂速度慢，Tri-EKF實現(xiàn)了旋轉(zhuǎn)和平移參數(shù)的解耦，因此加速了各運動分量的估計速度。

Tri-EKF適應多種形式參數(shù)模型，由于基于EKF架構(gòu)采用串行估計策略，運動狀態(tài)向量維數(shù)不同，收斂速度有差異，但線性化過程相同，因此和Dual EKF收斂后的運動估計精度接近。

4 試驗

4.1試驗平臺

圖12所示試驗平臺主要由三部分組成，計算機處理系統(tǒng)所用CPU為酷睿i5- 4690 K，主頻3.50 GHz；立體視覺系統(tǒng)采用Bumblebee2相機，分辨率設為640×480；兩自由度運動平臺與計算機間通訊方式為RS 485，并通過PMC-20MT-3三軸控制器控制K101DX60J電動旋轉(zhuǎn)臺和DTX1205- 600直線絲杠導軌來模擬目標的兩種運動形式。所用目標模型為高20.5 cm，寬30 cm的“神舟九號”模型。采用ORB算法[17]提取左右圖像特征點，然后通過PyrLK[18]算法跟蹤前后兩幀左右圖像特征點，并篩選跟蹤結(jié)果得到新時刻左右圖像的特征匹配對。設置特征匹配對數(shù)量閾值，當匹配對數(shù)量小于閾值時則認為特征點對信息過少不足以實現(xiàn)對目標運動和結(jié)構(gòu)參數(shù)的估計，需要對當前圖像重新進行特征提取及匹配以補充新匹配對；若大于閾值，則用視差法計算特征點三維坐標，并作為Tri-EKF算法觀測量用于目標運動狀態(tài)和結(jié)構(gòu)參數(shù)的遞推估計。

圖12 試驗平臺Fig.12 Experimental platform

4.2目標運動估計

4.2.1 平移運動

設定目標在相機坐標系x軸方向和z軸方向平移速度分別為9.7 mm/s和6.8 mm/s。試驗得到的目標平移運動估計曲線與誤差曲線如圖13所示(僅繪制x軸和z軸方向估計數(shù)據(jù))。

圖13 平移運動估計結(jié)果Fig.13 Estimation results of translation motion

對于純平移運動，Tri-EKF算法在10幀左右即可收斂，說明算法能夠?qū)崿F(xiàn)對純平移運動的較快估計。10幀到150幀時估計結(jié)果相對穩(wěn)定，這是因為在該階段內(nèi)目標與相機間距離較近，特征點測量精度較高，使得位移估計與速度估計誤差較穩(wěn)定。幀數(shù)為150到200幀時，隨著目標與相機間距離的增大，測量噪聲增大，位移估計與速度估計誤差有小幅上揚趨勢。將穩(wěn)定階段的平均誤差作為算法估計精度的衡量標準，Tri-EKF算法在x軸和z軸方向上的速度估計平均誤差分別為0.41 mm/s和0.31 mm/s，位移估計平均誤差分別為6.24 mm和4.58 mm，估計誤差較小，精度較高。

4.2.2 旋轉(zhuǎn)運動

設定電動轉(zhuǎn)臺轉(zhuǎn)動(相機y軸)速度為20°/s，將直線滑臺移動速度設為零。試驗得到目標旋轉(zhuǎn)運動估計曲線與誤差曲線(見圖14)。圖14(a)為四元數(shù)估計曲線，對應的歐拉角如圖14(b)所示，其中偏航角，俯仰角和滾轉(zhuǎn)角平均誤差分別為3.07°,1.43°和1.40°。偏航角(取值范圍(-180°,180°))在±180°處存在切換跳變現(xiàn)象。圖14(c)為歐拉角誤差曲線，偏航角估計誤差平均在5°內(nèi)，當角度為±180°時會產(chǎn)生較大誤差，但最大誤差不超過10°。這是因為通過四元數(shù)對歐拉角進行反三角函數(shù)求解時在±180°位置處存在奇異。試驗中俯仰角和滾轉(zhuǎn)角估計曲線在零值附近以±3°幅度振蕩，這是因為相機y軸與旋轉(zhuǎn)臺軸線難以保證完全平行。圖14(d)為目標角速度估計曲線，估計結(jié)果平均0.356 rad/s，與理論值0.349 rad/s相差0.007 rad/s，誤差較小，并且曲線在20幀左右即可收斂，具有較快的估計速度。

圖14 旋轉(zhuǎn)運動估計結(jié)果Fig.14 Estimation results of rotational motion

4.2.3 復合運動

設定目標在相機坐標系x軸方向和z軸方向平移速度分別為8 mm/s和6 mm/s，電動轉(zhuǎn)臺轉(zhuǎn)動速度為20 °/s(0.349 rad/s)。試驗得到的目標運動估計曲線與誤差曲線如圖15所示。開始時算法估計誤差較大，曲線不平穩(wěn)，隨著不斷迭代，約在50幀左右收斂。由收斂后曲線可知，z軸和x軸方向位移估計平均誤差分別為5.9 mm和7.6 mm，平移速度平均估計誤差為0.77 mm/s和0.14 mm/s，偏航角、俯仰角和滾轉(zhuǎn)角平均估計誤差分別為7.43°、4.62°和5.17°。角速度估計平均值為0.367 rad/s，誤差為0.018 rad/s。相比于純平移和純旋轉(zhuǎn)運動，復合運動具有更加復雜的形式，因此在對運動參數(shù)進行估計時，算法的收斂速度有所降低，誤差也有所增大，但整體而言算法仍然能夠得到較好的估計結(jié)果。

圖15 復合運動估計結(jié)果Fig.15 Estimation results of compound motion

4.3目標結(jié)構(gòu)點估計

試驗中得到的目標點云含有少量背景特征點及觀測次數(shù)較少的目標特征點，試驗設定只在MATLAB中繪制觀測次數(shù)大于5次的特征點。圖16(a)～16(f)為復合運動下對目標進行三維稀疏點云重構(gòu)的過程，最終得到的數(shù)據(jù)點個數(shù)為2870。圖16(g)～16(i)分別為200幀時目標點云的x、y和z方向視圖，根據(jù)最終點云結(jié)果，計算目標寬度為29 cm，與實際寬度30 cm相差1 cm；計算目標高度為18.5 cm，與實際高度20.5 cm相差2 cm。計算目標模型太陽能帆板長度和寬度分別為11.5 cm和4.5 cm，與實際長度12 cm和實際寬度3.9 cm接近。目標模型頭部返回艙的計算高度和直徑分別為4.2 cm和4.6 cm，實際高度和直徑分別問6.5 cm和5 cm，主要是頂部特征不豐富，提取的特征點數(shù)量少。

圖16 目標結(jié)構(gòu)估計結(jié)果Fig.16 Target structure estimation results

4.4結(jié)果分析

和仿真相比，參與Tri-EKF算法迭代的特征點數(shù)量遠多于仿真試驗，試驗對目標運動估計收斂速度更快。此外，估計結(jié)果是所有特征點共同作用得出，少量特征點錯誤估計對最終估計影響較弱，系統(tǒng)估計穩(wěn)定。但隨著運動形式復雜度提高及特征點增多，收斂速度會降低。

試驗估計曲線平滑度下降。仿真試驗假設特征點匹配完全正確并能全部跟蹤，噪聲設定較理想。實際特征點會出現(xiàn)誤匹配及跟蹤丟失現(xiàn)象，還存在光照變化、設備測量及安裝精度等因素，使得估計曲線平滑度下降。

估計變化曲線收斂后存在一段誤差穩(wěn)定階段，該階段內(nèi)由于目標與相機間距離較近使得特征點三維坐標測量精度較高，因而算法能夠?qū)?shù)進行較穩(wěn)定地估計。隨著目標與相機間距離的增大，視覺系統(tǒng)測量噪聲增大，估計誤差出現(xiàn)小幅上揚趨勢，算法估計精度隨之降低。因此提高測量精度是提高估計精度的有效手段。

5 結(jié) 論

本文針對非合作目標運動和結(jié)構(gòu)估計問題進行研究，建立了立體視覺系統(tǒng)數(shù)學模型，提出了Tri-EKF算法，該算法通過將特征線作為旋轉(zhuǎn)觀測量，實現(xiàn)了旋轉(zhuǎn)與平移參數(shù)的解耦估計。通過仿真試驗校驗了Tri-EKF算法相比于傳統(tǒng)的Dual EKF算法具有更快的收斂速度以及更小的有限次觀測平均估計誤差。在此基礎上搭建了試驗平臺，并對本文算法進行校驗。試驗結(jié)果表明，Tri-EKF算法適用于不同目標運動形式，能夠?qū)崿F(xiàn)對目標的運動估計與結(jié)構(gòu)估計。下一步研究方向?qū)⑹窃诟嗟倪\動自由度條件下進行運動估計以及結(jié)構(gòu)稠密重建，深入開展運動參數(shù)及結(jié)構(gòu)耦合程度的定性定量分析，使用GPU并行計算進行算法加速，提高算法實時性。

[1] 余江華. 非合作航天器相對位姿測量方法研究 [D]. 哈爾濱：哈爾濱工業(yè)大學, 2012. [Yu Jiang-hua. Research on relative position measurement of non-cooperative spacecraft [D]. Harbin: Harbin Institute of Technology, 2012.]

[2] Terui F,Kamimura H, Nishida S. Motion estimation to a failed satellite on orbit using stereo vision and 3D model matching [C]. The 9th International Conference on Control, Automation, Robotics and Vision, Singapore, December 5-8, 2006.

[3] Augenstein S, Rock S M.Improved frame-to-frame pose tracking during vision-only SLAM/SFM with a tumbling target [C]. IEEE International Conference on Robotics and Automation, Shanghai, China, May 9-13, 2011.

[4] Segal S, Carmi A, Gurfil P. Vision-based relative state estimation of non-cooperative spacecraft under modeling uncertainty [C]. 2011 IEEE Aerospace Conference, Mantana, USA, March 5-12, 2011.

[5] 蔡晗, 張景瑞, 翟光, 等. GEO非合作目標超近距相對位姿視覺測量 [J]. 宇航學報, 2015, 36(6):715-722. [Cai Han, Zhang Jing-rui, Zhai guang, et al. Relative pose determination for GEO non-cooperative spacecraft under the ultra-close distance [J]. Journal of Astronautics, 2015, 36(6):715-722.]

[6] 周軍, 白博, 于曉洲. 一種非合作目標相對位置和姿態(tài)確定方法 [J]. 宇航學報, 2011, 32(3):516-521. [Zhou Jun, Bai Bo, Yu Xiao-zhou. A new method of relative position and attitude determination for non-cooperative target [J]. Journal of Astronautics, 2011, 32(3):516-521.]

[7] 翟光, 張景瑞. 空間非合作目標快速姿態(tài)跟蹤導航方法研究 [J]. 宇航學報, 2013, 34(3):362-368. [Zhai Guang, Zhang Jing-rui. Research on rapid attitude tracking for space non-cooperative target [J]. Journal of Astronautics, 2013, 34(3):362-368.]

[8] 郝剛濤, 杜小平. 基于單目視覺圖像序列的空間非合作目標相對姿態(tài)估計 [J]. 航天控制, 2014, 32(2):60-67. [Hao Gang-tao, Du Xiao-ping. Relative attitude estimation for space non-cooperative targets based on sequence of monocular images [J]. Aerospace control, 2014, 32(2):60-67.]

[9] Togneri R, Deng L. Joint state and parameter estimation for a target-directed nonlinear dynamic system model [J]. IEEE Transactions on Signal Processing, 2004, 51(12):3061-3070.

[10] Ljung L. Asymptotic behavior of the extended Kalman filter as a parameter estimator for linear systems [J]. Automatic Control IEEE Transactions on, 1979, 24(1):36-50.

[11] Liu X, Escamilla-Ambrosio P J, Lieven N A J. Extended Kalman filtering for the detection of damage in linear mechanical structures [J]. Journal of Sound and Vibration, 2009, 325(4-5):1023-1046.

[12] Zhang H S, Negahdaripour S. EKF-based recursive dual estimation of structure and motion from stereo data [J]. IEEE Journal of Oceanic Engineering, 2010, 35(2): 424-437.

[13] Zhang H. Automatic sensor platform positioning and three-dimensional target modeling from underwater stereo sequences [D]. Miami:University of Miami, 2005.

[14] Diebel J. Representing attitude: euler angles, unit quaternions, and rotation vectors [J]. Matrix, 2006, 58(15-16): 1-35.

[15] 白淵杰. 基于加計陣列的飛行器姿態(tài)解算方法研究[D]. 西安：西安電子科技大學, 2014. [Bai Yuan-jie. Research on aircraft attitude algorithm based on the array of accelerometer[D]. Xi’an: Xidian University, 2014.]

[16] 郭本生. 基于立體視覺的非合作目標運動估計與結(jié)構(gòu)估計方法研究[D]. 哈爾濱：哈爾濱工業(yè)大學, 2016. [Guo Ben-sheng. Research on motion and structure estimation methods of non-cooperative targets based on stereo vision[D]. Harbin: Harbin Institute of Technology, 2016.]

[17] Rublee E, Rabaud V, Konolige K, et al. ORB: an efficient alternative to SIFT or SURF [C]. IEEE International Conference on Computer Vision, Barcelona, Spain, November 6-11, 2011.

[18] Zhang D, Liang S, Zhang C, et al. 3D tree skeleton reconstruction based on enhanced PyrLK optical flow algorithm [J]. Journal of Computer-Aided Design and Computer Graphics, 2015, 27(7):1247-1254.

MotionandStructureEstimationofNon-CooperativeSpaceTargetBasedonTri-EKFAlgorithmandStereoVision

WANG Ke, YU Zhan-hai, LIU Hong, LI Rui-feng, GUO Ben-sheng

(State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150080，China)

For the motion state and structure estimation of a non-cooperative space target, the motion parameters are coupled, which leads to the slow convergence problem in the dual extended Kalman filter (Dual EKF) algorithm. In this paper, based on the stereo vision model, a feature line observation model is introduced for decoupling of the translational and rotational motion. Then the triple extended Kalman filter (Tri-EKF) algorithm is proposed to sequentially estimate the rotational, translational motion and structural parameters of the target. Compared with the Dual EKF algorithm, the simulation and experimental results prove that the Tri-EKF has a faster convergence speed on motion estimation and smaller average estimation error given limited observations.

Non-cooperative target; Tri-EKF algorithm; Motion and structure estimation; Stereo vision

V448

A

1000 -1328(2017)09- 0936- 10

10.3873/j.issn.1000-1328.2017.09.006

2016- 11- 02;

2017- 05- 24

國家自然科學基金創(chuàng)新群體基金(51521003)；黑龍江省自然科學基金(QC2014C072)；黑龍江省博士后基金(LBH-Z14108)

王珂(1979-)，男，講師，主要從事機器人視覺技術(shù)，移動機器人SLAM，定位及導航等研究。

通信地址：哈爾濱市南崗區(qū)一匡街2號哈工大科學園C1棟305(150080)

電話：(0451)86418283

E-mail:wangke@hit.edu.cn

郭本生(1992-)，男，碩士，主要從事機器人視覺，運動目標檢測等方面的研究。本文通信作者。

通信地址：哈爾濱市南崗區(qū)一匡街2號哈工大科學園C1棟501(150080)

電話：(0451)86418283

E-mail:gbs@hit.edu.cn