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

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

基于立體視覺Tri-EKF算法的空間非合作目標(biāo)運(yùn)動(dòng)與結(jié)構(gòu)估計(jì)

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

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

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

非合作目標(biāo)；Tri-EKF算法；運(yùn)動(dòng)及結(jié)構(gòu)估計(jì)；立體視覺

0 引 言

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

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

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

1 非合作目標(biāo)運(yùn)動(dòng)及觀測(cè)模型

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

1.1坐標(biāo)系定義

建立如圖1所示的三個(gè)坐標(biāo)系，分別為：

視覺坐標(biāo)系OC，為立體視覺觀測(cè)系統(tǒng)測(cè)量目標(biāo)表面特征點(diǎn)的基準(zhǔn)坐標(biāo)系。

結(jié)構(gòu)坐標(biāo)系OS，位于剛體目標(biāo)表面并隨目標(biāo)運(yùn)動(dòng)，結(jié)構(gòu)特征點(diǎn)在該坐標(biāo)系下坐標(biāo)不隨時(shí)間變化。

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

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

圖1 坐標(biāo)系Fig.1 Coordinate system

1.2運(yùn)動(dòng)模型

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

1.2.1 平移運(yùn)動(dòng)

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

(1)

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

1.2.2 旋轉(zhuǎn)運(yùn)動(dòng)

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

(2)

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

1.2.3 運(yùn)動(dòng)學(xué)方程

(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立體視覺觀測(cè)模型

(5)

設(shè)tk時(shí)刻目標(biāo)表面有m特征點(diǎn)被同時(shí)觀測(cè)，定義三維坐標(biāo)向量Zk，則觀測(cè)量pk的三維結(jié)構(gòu)坐標(biāo)參數(shù)與目標(biāo)運(yùn)動(dòng)參量間的變換關(guān)系

(6)

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

(7)

2 Tri-EKF目標(biāo)運(yùn)動(dòng)與結(jié)構(gòu)估計(jì)

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

2.1特征線

(8)

兩式相減得

(9)

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

(10)

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

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

基于tk時(shí)刻m條特征線，建立目標(biāo)觀測(cè)方程

(11)

2.2Tri-EKF算法

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

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

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

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

(12)

式(11)為旋轉(zhuǎn)運(yùn)動(dòng)估計(jì)器觀測(cè)方程，為特征線組成的向量ZR,k。

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

(13)

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

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

(14)

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

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

(15)

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

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

(16)

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

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

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

(17)

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

2)旋轉(zhuǎn)運(yùn)動(dòng)估計(jì)器的觀測(cè)更新

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

(18)

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

3)平移運(yùn)動(dòng)估計(jì)器觀測(cè)更新

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

(19)

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

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

(20)

3 仿真

3.1仿真試驗(yàn)1

圖4所示運(yùn)動(dòng)目標(biāo)為底半徑5 m，高10 m圓柱體，圓柱體上、中、下圓面對(duì)稱分布4個(gè)點(diǎn)，共12個(gè)特征點(diǎn)。仿真假設(shè)特征點(diǎn)已經(jīng)提取并正確匹配。

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

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

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

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

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

1)位移誤差：

2)速度誤差：

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

4)角速度誤差：

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

圖5為平移運(yùn)動(dòng)估計(jì)曲線。Tri-EKF對(duì)目標(biāo)平移運(yùn)動(dòng)估計(jì)的收斂速度比Dual EKF快，Tri-EKF在50幀左右即可收斂，而Dual EKF至少需100幀。

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

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

圖5 平移運(yùn)動(dòng)估計(jì)曲線Fig.5 Estimation of translation motion

圖6 角速度估計(jì)曲線Fig.6 Estimation of angular velocity

圖7 估計(jì)誤差曲線Fig.7 Estimation error

3.2仿真試驗(yàn)2

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

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

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

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

圖9 平移運(yùn)動(dòng)估計(jì)結(jié)果Fig.9 Estimation of translation motion

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

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

圖10 角速度估計(jì)曲線Fig.10 Estimation of angular velocity

圖11 估計(jì)誤差Fig.11 Estimation error

3.3結(jié)果分析

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

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

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

4 試驗(yàn)

4.1試驗(yàn)平臺(tái)

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

圖12 試驗(yàn)平臺(tái)Fig.12 Experimental platform

4.2目標(biāo)運(yùn)動(dòng)估計(jì)

4.2.1 平移運(yùn)動(dòng)

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

圖13 平移運(yùn)動(dòng)估計(jì)結(jié)果Fig.13 Estimation results of translation motion

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

4.2.2 旋轉(zhuǎn)運(yùn)動(dòng)

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

圖14 旋轉(zhuǎn)運(yùn)動(dòng)估計(jì)結(jié)果Fig.14 Estimation results of rotational motion

4.2.3 復(fù)合運(yùn)動(dòng)

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

圖15 復(fù)合運(yùn)動(dòng)估計(jì)結(jié)果Fig.15 Estimation results of compound motion

4.3目標(biāo)結(jié)構(gòu)點(diǎn)估計(jì)

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

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

4.4結(jié)果分析

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

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

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

5 結(jié) 論

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

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

[16] 郭本生. 基于立體視覺的非合作目標(biāo)運(yùn)動(dòng)估計(jì)與結(jié)構(gòu)估計(jì)方法研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué), 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

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

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

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

電話：(0451)86418283

E-mail:wangke@hit.edu.cn

郭本生(1992-)，男，碩士，主要從事機(jī)器人視覺，運(yùn)動(dòng)目標(biāo)檢測(cè)等方面的研究。本文通信作者。

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

電話：(0451)86418283

E-mail:gbs@hit.edu.cn