基于立體視覺Tri-EKF算法的空間非合作目標運動與結構估計

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

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

基于立體視覺Tri-EKF算法的空間非合作目標運動與結構估計

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

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

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

非合作目標；Tri-EKF算法；運動及結構估計；立體視覺

0 引 言

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

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

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

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

自由懸浮狀態的空間非合作目標具有平動、繞自旋軸旋轉的運動特性，首先建立坐標系，運動模型，結構模型以及觀測模型。

1.1坐標系定義

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

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

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

目標坐標系OT, 目標結構未知及旋轉中心未知，旋轉中心在OS中位置作為估計參量。

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

圖1 坐標系Fig.1 Coordinate system

1.2運動模型

目標運動包括自旋轉動及OT相對于OC平動，表述為位移、速度、旋轉中心位置、姿態角和角速度為參數的目標運動狀態模型。

1.2.1 平移運動

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

(1)

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

1.2.2 旋轉運動

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

(2)

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

1.2.3 運動學方程

(3)

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

1.3結構模型

XS,k=XS,k-1

(4)

1.4立體視覺觀測模型

(5)

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

(6)

由此，可以得到目標觀測量Zk與運動參數XM,k和結構參數XS,k間的觀測方程

(7)

2 Tri-EKF目標運動與結構估計

Tri-EKF在Dual EKF結構及運動參數解耦基礎上，將特征線作為旋轉運動觀測量，實現目標旋轉與平移運動參數的解耦。

2.1特征線

(8)

兩式相減得

(9)

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

(10)

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

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

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

(11)

2.2Tri-EKF算法

圖3為Tri-EKF結構框圖，以目標特征線為觀測量，采用旋轉運動估計器、平移運動估計器和結構參數估計器對目標參數進行串行估計，從而實現狀態向量降維。

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

圖3 Tri-EKF結構框圖Fig.3 Structure diagram of Tri-EKF

旋轉運動估計狀態向量為XR，向量元素為四元數q和角速度ω，狀態方程為

(12)

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

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

(13)

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

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

(14)

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

目標結構參數估計器狀態向量為XS，狀態方程與Dual EKF結構參數估計器相同，即：

(15)

旋轉和平移估計結果XR,k|k、XT,k|k作為已知量輸入到結構參數估計器觀測方程中，因此觀測方程未知量個數在平移估計器的基礎上再次減少，其觀測方程為

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

(16)

式中：HS為關于結構狀態變量XS的觀測雅可比矩陣。

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

1)旋轉、平移和結構濾波器的時間更新

(17)

式中：Xk-1|k-1為tk時刻濾波估計值，Xk|k-1為tk時刻濾波預測值，FS為結構狀態分量的狀態轉移矩陣。

2)旋轉運動估計器的觀測更新

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

(18)

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

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

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

(19)

4)結構參數估計器觀測更新

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

(20)

3 仿真

3.1仿真試驗1

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

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

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

各狀態向量初始值設定如表1所示。濾波開始階段將狀態協方差矩陣初始值P0設置較大。本仿真將方陣P0的對角元設為1，觀測噪聲協方差矩陣Sk對角元設為0.25，根據濾波器結構分別設置協方差矩陣維數。

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

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

1)位移誤差：

2)速度誤差：

3)旋轉中心位置誤差：

4)角速度誤差：

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

圖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收斂速度快，估計誤差下降快，在有限次(無論是否穩定)觀測下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所示，同樣假設仿真過程中特征點已經提取并正確匹配。目標初始位置設在相機坐標系(50, 50, 50) m處，運動形式為平移和旋轉的復合運動。目標沿相機坐標系X、Y、Z軸的平移速度均為0.1 m/s，同時以π/50(rad/s)的角速度分別繞相機坐標系的Z、X、Y旋轉。在正方體上表面某一頂點處建立結構坐標系OS。同樣將含有高斯噪聲的特征點三維坐標作為算法觀測值。

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

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

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

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

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

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

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

圖11 估計誤差Fig.11 Estimation error

3.3結果分析

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

雖然Dual EKF實現了運動參數和結構參數的解耦，但其運動參數間存在耦合，導致收斂速度慢，Tri-EKF實現了旋轉和平移參數的解耦，因此加速了各運動分量的估計速度。

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

4 試驗

4.1試驗平臺

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

圖12 試驗平臺Fig.12 Experimental platform

4.2目標運動估計

4.2.1 平移運動

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

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

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

4.2.2 旋轉運動

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

圖14 旋轉運動估計結果Fig.14 Estimation results of rotational motion

4.2.3 復合運動

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

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

4.3目標結構點估計

試驗中得到的目標點云含有少量背景特征點及觀測次數較少的目標特征點，試驗設定只在MATLAB中繪制觀測次數大于5次的特征點。圖16(a)～16(f)為復合運動下對目標進行三維稀疏點云重構的過程，最終得到的數據點個數為2870。圖16(g)～16(i)分別為200幀時目標點云的x、y和z方向視圖，根據最終點云結果，計算目標寬度為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，主要是頂部特征不豐富，提取的特征點數量少。

圖16 目標結構估計結果Fig.16 Target structure estimation results

4.4結果分析

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

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

估計變化曲線收斂后存在一段誤差穩定階段，該階段內由于目標與相機間距離較近使得特征點三維坐標測量精度較高，因而算法能夠對參數進行較穩定地估計。隨著目標與相機間距離的增大，視覺系統測量噪聲增大，估計誤差出現小幅上揚趨勢，算法估計精度隨之降低。因此提高測量精度是提高估計精度的有效手段。

5 結 論

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

[1] 余江華. 非合作航天器相對位姿測量方法研究 [D]. 哈爾濱：哈爾濱工業大學, 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] 周軍, 白博, 于曉洲. 一種非合作目標相對位置和姿態確定方法 [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] 翟光, 張景瑞. 空間非合作目標快速姿態跟蹤導航方法研究 [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] 郝剛濤, 杜小平. 基于單目視覺圖像序列的空間非合作目標相對姿態估計 [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] 白淵杰. 基于加計陣列的飛行器姿態解算方法研究[D]. 西安：西安電子科技大學, 2014. [Bai Yuan-jie. Research on aircraft attitude algorithm based on the array of accelerometer[D]. Xi’an: Xidian University, 2014.]

[16] 郭本生. 基于立體視覺的非合作目標運動估計與結構估計方法研究[D]. 哈爾濱：哈爾濱工業大學, 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

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

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

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

電話：(0451)86418283

E-mail:wangke@hit.edu.cn

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

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

電話：(0451)86418283

E-mail:gbs@hit.edu.cn