[發(fā)明專利]一種基于雙目視覺的非合作目標(biāo)相對狀態(tài)解算方法

有效

• 申請?zhí)枺?/td>: 201810041155.8
• 申請日: 2018-01-16
• 公開（公告）號: CN108376411B
• 公開（公告）日: 2021-09-21
• 發(fā)明（設(shè)計）人: 朱程廣;趙健康;劉傳奇
• 申請（專利權(quán)）人: 上海交通大學(xué)
• 主分類號: G06T7/73
• 分類號: G06T7/73
• 代理公司: 上海漢聲知識產(chǎn)權(quán)代理有限公司 31236
• 代理人: 莊文莉

1.一種基于雙目視覺的非合作目標(biāo)相對狀態(tài)解算方法，其特征在于，包括如下步驟：

步驟S1：解算載體表面特征點(diǎn)在相機(jī)坐標(biāo)系下的3D坐標(biāo)；

步驟S2：解算目標(biāo)坐標(biāo)系相對相機(jī)坐標(biāo)系的相對姿態(tài)；

步驟S3：解算目標(biāo)坐標(biāo)系相對相機(jī)坐標(biāo)系的相對轉(zhuǎn)速；

步驟S1，包括如下步驟：

步驟S1.1：在航天器的質(zhì)心處安裝雙目相機(jī)，航天器記為L，以左相機(jī)的相機(jī)坐標(biāo)系作為參考坐標(biāo)系，記為非合作目標(biāo)記為F，由初始時刻獲取的特征點(diǎn)建立的坐標(biāo)系建立目標(biāo)坐標(biāo)系，記為利用雙目相機(jī)采集圖像，記錄圖像采集的時刻t_i或記錄相鄰兩個圖像采集的間隔Δt_i，其中i表示采集圖像的索引；

Δt_i＝t_i+1-t_i (1)；

步驟S1.2：利用SIFT或SURF算法匹配雙目相機(jī)的左、右相機(jī)圖像的載體表面特征點(diǎn)，記ⁱN表示在t_i時刻匹配成功的特征點(diǎn)個數(shù)；

步驟S1.3：利用雙目視覺原理計算載體表面特征點(diǎn)在雙目相機(jī)的左相機(jī)坐標(biāo)系下的3D坐標(biāo)；利用雙目相機(jī)計算特征點(diǎn)的3D坐標(biāo)時涉及到計算機(jī)圖像坐標(biāo)系，成像平面坐標(biāo)系，相機(jī)坐標(biāo)系和目標(biāo)坐標(biāo)系；設(shè)，O_F-xyz表示目標(biāo)坐標(biāo)系，即^lO₀-uv和^rO₀-uv分別表示左右相機(jī)的計算機(jī)圖像坐標(biāo)系；和分別表示左右相機(jī)的成像平面坐標(biāo)系，O_l-xyz和O_r-xyz分別表示左右相機(jī)坐標(biāo)系，(u₀，v₀)為^lO₀-uv和^rO₀-uv的中心，也是和的原點(diǎn)，O_l-xyz即為空間中一點(diǎn)P在中的坐標(biāo)為(x，y，z)，該點(diǎn)在^lO₀-uv和^rO₀-uv中的坐標(biāo)記為(u_l，v_l)和(u_r，v_r)；

特征點(diǎn)P在^lO₀-uv和^rO₀-uv中的坐標(biāo)與(x，y，z)的關(guān)系用(1a)～(1b)表示，聯(lián)立式(1a)～(1b)可解出(x，y，z)，如式(1c)所示：

其中，f表示相機(jī)的焦距；dx，dy分別表示每個像素點(diǎn)在計算機(jī)成像平面坐標(biāo)系的x軸和y軸方向上的物理尺寸；b表示左右相機(jī)之間的基線長度；T_b＝[-b，0，0]′；

記表示在t_i時刻匹配成功的第k個特征點(diǎn)在下的3D坐標(biāo)，k＝1，2，…，ⁱN；

步驟S2，包括以下步驟：

步驟S2.1：建立目標(biāo)坐標(biāo)系

利用載體表面特征點(diǎn)在相機(jī)坐標(biāo)系的坐標(biāo)建立目標(biāo)坐標(biāo)系在目標(biāo)坐標(biāo)系下的坐標(biāo)為二者存在以下關(guān)系：

其中，T_3×1＝[T₁ T₂ T₃]′；R表示旋轉(zhuǎn)矩陣，T表示平移向量；r₁、r₂、r₃分別表示旋轉(zhuǎn)矩陣的列向量r_ij表示旋轉(zhuǎn)矩陣的元素；T₁、T₂、T₃分別表示平移向量的元素；符號[]′表示矩陣或向量的轉(zhuǎn)置，x′_k，y′_k，z′_k表示在目標(biāo)坐標(biāo)系下的坐標(biāo)值；0表示在t₀時刻；

目標(biāo)坐標(biāo)系建立方法如下：1)假設(shè)在t₀時刻，三個特征點(diǎn)在中的坐標(biāo)值分別為⁰P₁^L、相應(yīng)地在中的坐標(biāo)值分別記為⁰P₁^F、2)因相機(jī)坐標(biāo)系遵守右手法則，為使目標(biāo)坐標(biāo)系與其一致，⁰P₁^L為坐標(biāo)系原點(diǎn)，選擇向量的方向?yàn)樽鴺?biāo)系的x軸正向，此時，應(yīng)保證x₂＞x₁，改變特征點(diǎn)的順序，易滿足條件；3)坐標(biāo)系的y軸在由點(diǎn)確定的平面內(nèi)，且與x軸垂直；4)z軸的方向與⁰P₁^L、確定的平面垂直，遵守右手法則；

根據(jù)坐標(biāo)系建立的原則可知，特征點(diǎn)在的坐標(biāo)為：⁰P₁^F＝(0，0，0)、x′₂為特征點(diǎn)⁰P₁^L、之間的距離，式(3c)～(3d)確定及y軸正向：

其中，α表示向量與的夾角；

將式(3a)～(3d)代入式(2)求解R和T：

r₃＝r₁×r₂ (3g)

T＝⁰P₁^L (3h)；

步驟S2.2：建立七參數(shù)模型

坐標(biāo)系與之間的變換關(guān)系利用七參數(shù)模型表示，即：

μ_i，R_i，T_i分別表示在t_i時刻變換到的尺度因子、旋轉(zhuǎn)矩陣和平移向量；為解算出旋轉(zhuǎn)矩陣，應(yīng)保證在t_i時刻ⁱN≥4；

歐拉角與旋轉(zhuǎn)矩陣存在變換關(guān)系，即用歐拉角表示旋轉(zhuǎn)矩陣，在ti時刻，經(jīng)過一定順序的三次歐拉角θ_i，ψ_i旋轉(zhuǎn)可以與平行，相應(yīng)的旋轉(zhuǎn)矩陣采用下式表示，為便于簡潔表達(dá)，在此將歐拉角的下標(biāo)省略掉：

步驟S2.3：求解載體表面特征點(diǎn)在下的坐標(biāo)

在t₀時刻，已建立目標(biāo)坐標(biāo)系利用t_i-1時刻的變換關(guān)系計算t_i時刻載體表面特征點(diǎn)在目標(biāo)坐標(biāo)系中的坐標(biāo)，進(jìn)而得到t_i時刻同一組載體表面特征點(diǎn)分別在和中的坐標(biāo)；相鄰時刻，匹配成功的特征點(diǎn)ⁱN≥4即可解算旋轉(zhuǎn)矩陣R_i、μ_i和T_i；

步驟S2.4：求解目標(biāo)坐標(biāo)系與相機(jī)坐標(biāo)系的相對姿態(tài)

從步驟S2.3，得到在t_i時刻ⁱN個匹配成功的特征點(diǎn)在和下的坐標(biāo)，分別記為和利用兩個坐標(biāo)系下的特征點(diǎn)計算出七參數(shù)其中，Δx，Δy，Δz分別表示和之間的平移量，也即T_i＝[Δx，Δy，Δz]′；

在處做泰勒展開如下：

其中，Y表示在t_i時刻特征點(diǎn)在中坐標(biāo)組成的列向量，大小為3ⁱN×1；A表示系數(shù)矩陣，矩陣維數(shù)為3ⁱN×7；l表示在t_i時刻與dx無關(guān)的常數(shù)項(xiàng)，大小為3ⁱN×1；

A＝[E_3×3，μM，N] (8b)

其中，E_3×3表示單位矩陣，M表示對旋轉(zhuǎn)矩陣求導(dǎo)結(jié)果相關(guān)的系數(shù)矩陣，N表示與dμ相關(guān)的系數(shù)向量；

當(dāng)ⁱN＞3時，利用最小二乘方法求解七參數(shù)向量，迭代過程如下：

步驟a，設(shè)置初始參數(shù)

步驟b，將代入上式(8a)～(8d)，計算A,l；

步驟c，根據(jù)誤差方程求解其中，V表示誤差；

步驟d，根據(jù)最小二乘原理求解改正數(shù)其中，A'表示A的轉(zhuǎn)置；

步驟e，更新七參數(shù)值

步驟f，若則停止迭代；否則執(zhí)行步驟g；

步驟g，重復(fù)步驟b～步驟f；

步驟S2.5：利用整體最小二乘算法進(jìn)一步提高解算精度；

在步驟S2.4迭代過程中，僅考慮特征點(diǎn)在中的誤差，而實(shí)際應(yīng)用中，系數(shù)矩陣也存在誤差；針對這一問題，采用整體最小二乘原理進(jìn)一步提高七參數(shù)的解算精度；新建立的EIV模型如式(10a)所示：

e_A＝vec(E_Ai)，vec(·)表示將矩陣按列堆疊轉(zhuǎn)化為列向量；E_A表示系數(shù)矩陣的殘差，表示每次迭代中系數(shù)矩陣殘差的預(yù)測值；

目標(biāo)函數(shù)：

其中，e_y表示觀測量Y的殘差，e_A表示系數(shù)矩陣的殘差重構(gòu)的列向量，λ表示m×1 拉格朗日乘數(shù)向量，Q_y和Q_A分別表示觀測量Y權(quán)逆陣和系數(shù)矩陣殘差的權(quán)逆陣，表示直乘，I_m表示m×m單位矩陣，Y表示由特征點(diǎn)在坐標(biāo)系下的坐標(biāo)構(gòu)成的列向量；

為求解Φ(e_y，e_A，λ，dζ)＝min，分別對變量求導(dǎo)，求目標(biāo)函數(shù)最值過程如下，其中“～”表示相應(yīng)變量的預(yù)測值；

對e_y求導(dǎo)得：

對e_A求導(dǎo)得：

對λ求導(dǎo)得：

對dζ求導(dǎo)得：

由式(12a～12b)可得：

將式(13a)～(13b)代入(12c)得：

其中，

其中，下標(biāo)i表示第i次迭代，表示單位權(quán)方差，Q_l表示Q₀表示系數(shù)矩陣方差，Q_E表示權(quán)逆陣，表示第i+1次迭代的觀測量誤差，表示第i+1次迭代的系數(shù)矩陣誤差；

步驟S2.5中，迭代計算的初始值由步驟S2.4的結(jié)果給出，根據(jù)以上迭代計算，當(dāng)時迭代停止，求解出的x即為進(jìn)一步迭代后的七參數(shù)；

步驟S2.6：計算相對姿態(tài)

根據(jù)步驟S2.5中得到的歐拉角求解出旋轉(zhuǎn)矩陣，依據(jù)下式求解出四元數(shù)q＝[ξ₁ ξ₂ ξ₃ξ₄]^T：

步驟S3，包括如下步驟：

步驟S3.1，建立動力學(xué)模型

步驟S3.2，建立運(yùn)動學(xué)模型

ρ＝[ξ₁ ξ₂ ξ₃]^T (17)

步驟S3.3，相對轉(zhuǎn)速計算公式

ω＝D(q)ω_F-ω_L (18)

其中：ω_L，ω_F，ω分別表示相機(jī)坐標(biāo)系的轉(zhuǎn)速、目標(biāo)坐標(biāo)系的轉(zhuǎn)速及目標(biāo)坐標(biāo)系相對相機(jī)坐標(biāo)系的轉(zhuǎn)速；表示相對轉(zhuǎn)速的導(dǎo)數(shù)；I_L和I_F分別表示航天器和非合作目標(biāo)的轉(zhuǎn)動慣量，N_L和N_F分別表示航天器和非合作目標(biāo)受到的外力，D(q)表示由四元數(shù)表示的旋轉(zhuǎn)矩陣，q表示四元數(shù)，表示四元數(shù)的導(dǎo)數(shù)，ρ表示四元數(shù)的矢量部分；

D(q)＝[(ξ₄)²-ρ^Tρ]I_3×3+2ρρ^T-2ξ₄[ρ×] (19)

[ρ×]表示叉乘；

步驟3.4，無損卡爾曼濾波估計轉(zhuǎn)速

定義狀態(tài)方程如下：

其中，表示狀態(tài)量的導(dǎo)數(shù)，η＝[ξ₁，ξ₂，ξ₃，ξ₄，ω_x，ω_y，ω_z，I_T1，I_T2，I_T3，I_T4，I_T5，I_T6]^T

f(η)表示狀態(tài)量與其導(dǎo)數(shù)映射函數(shù)，w表示均值為零的高斯噪聲；

觀測方程如下：

Z＝h(η)+v (21)

其中，z表示觀測量，η表示狀態(tài)量，v表示均值為零的高斯白噪聲，h(η)表示狀態(tài)量與觀測量的映射關(guān)系；

其中，ξ₁，ξ₂，ξ₃，ξ₄，分別為相對姿態(tài)的四元數(shù)；ω＝[ω_x，ω_y，ω_z]^T分別表示相對轉(zhuǎn)速；I_T1，I_T2，I_T3，I_T4，I_T5，I_T6分別表示轉(zhuǎn)動慣量的元素；

其中，轉(zhuǎn)動慣量為觀測量為：Z＝[ξ₁，ξ₂，ξ₃，ξ₄]^T

根據(jù)觀測量和測量量的關(guān)系建立模型，采用UKF濾波解算狀態(tài)，包括如下步驟：

步驟A：利用UT變換計算Sigma點(diǎn)

其中，ζ＝α²(n+κ)-n α表示Sigma點(diǎn)圍繞η_k的分布特性，κ表示比例因子；

步驟B：一步預(yù)測

步驟C：二次采樣

步驟D：測量更新

(Z_k+1|k)_i＝h((χ_k+1|k)_i) (27)

步驟E：濾波更新

β結(jié)合η的先驗(yàn)分布信息得出；

χ_k表示k時刻的Sigma點(diǎn)集；

η_k表示第k次迭代的狀態(tài)量；

表示變換后Sigma點(diǎn)集的第i列；

W^m表示均值的加權(quán)值；

P_k+1|k表示后驗(yàn)方差陣；

W^c表示方差的權(quán)值；

Q_k表示噪聲的方差矩陣；

Z_k+1|k表示k時刻預(yù)測k+1的測量值的估計值；

表示預(yù)測觀測值；

χ_k+1|k表示根據(jù)一步預(yù)測值再次使用UT變換，產(chǎn)生的新Sigma點(diǎn)集；

K_k+1表示在k+1時刻的濾波增益矩陣；

表示k+1時刻的狀態(tài)量估計值；

表示量測輸出變量的方差矩陣；

是協(xié)方差陣；

ζ表示一個標(biāo)量，由經(jīng)驗(yàn)得到；

n表示系統(tǒng)狀態(tài)維數(shù)；

UKF濾波器的輸入量為由四元數(shù)組成的測量值，根據(jù)不同時刻的輸入通過不斷迭代，最終可求出不同時刻，非合作目標(biāo)相對相機(jī)坐標(biāo)系的相對狀態(tài)；

非合作目標(biāo)相對相機(jī)坐標(biāo)系的相對狀態(tài)表示為：

η′＝[ξ₁，ξ₂，ξ₃，ξ₄，ω_x，ω_y，ω_z，T₁，T₂，T₃]。

2.根據(jù)權(quán)利要求1所述的基于雙目視覺的非合作目標(biāo)相對狀態(tài)解算方法，其特征在于，β最優(yōu)值設(shè)置為2。