[發明專利]一種基于單目視覺的空間非合作目標相對狀態估計方法

有效

• 申請號: 201910593684.3
• 申請日: 2019-07-03
• 公開（公告）號: CN110186465B
• 公開（公告）日: 2022-08-05
• 發明（設計）人: 孟中杰;郭新程;黃攀峰;張夷齋;張帆;劉正雄
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/24
• 分類號: G01C21/24
• 代理公司: 西安凱多思知識產權代理事務所(普通合伙) 61290
• 代理人: 劉新瓊

1.一種基于單目視覺的空間非合作目標相對狀態估計方法，其特征在于包括下述步驟：

步驟一：建立目標與服務航天器之間相對運動的運動學和動力學模型；

定義如下坐標系：

(1)地心慣性坐標系I：原點位于地球質心，z軸指向地球北極，x軸指向春分點，y軸由右手法則確定；

(2)LVLH坐標系H：坐標系原點位于服務航天器的質心，x軸由地心指向服務航天器質心，z軸垂直于軌道面且與軌道角動量方向一致，y軸由右手法則確定；

(2)服務航天器本體坐標系A：坐標原點固定在服務航天器質心上，坐標軸與服務航天器慣量主軸重合；

(3)目標本體坐標系B：坐標原點固定在目標質心上，坐標軸與目標的慣量主軸重合，規定最小慣量主軸為x軸，最大慣量主軸y軸；

(5)相機坐標系C：坐標原點固定在相機光心上，z軸沿相機光軸方向，x軸y軸平行于成像平面；

目標本體坐標系B相對于服務航天器本體坐標系A的相對姿態用四元數q表示，四元數q_v＝[q₁ q₂ q₃]^T為四元數向量部分，q₄為四元數的標量部分，與四元數q對應的旋轉矩陣為：

其中[q×]為向量q＝[q₁ q₂ q₃]^T的反對稱矩陣：

定義四元數b和q之間乘法運算如下：

定義服務航天器本體坐標A系和目標本體坐標系B的角速度在各自本體坐標系下的表示分別為目標相對于服務航天器的角速度ω表示在目標本體坐標系B下為：

相對姿態運動學方程為：

其中服務航天器角速度通過服務航天器的慣性設備測量得到，作為已知量用于模型方程中；

目標的姿態動力學方程為：

其中ω_t表示目標角速度，即為τ_t為噪聲項，J_t為目標的慣量矩陣，且J_t＝diag(J_xx,J_yy,J_zz)，J_xx,J_yy,J_zz分別為目標主軸x軸、y軸、z軸的轉動慣量；令則式轉化為：

目標轉動慣量比為常量，應滿足：

其中，表示k₁關于時間的變化率，表示k2關于時間的變化率；

定義ρ₀為目標質心相對于服務航天器質心的位置向量，在LVLH坐標系下表示為[x y z]^T，考慮到服務航天器近距離接近目標時ρ₀很小，則相對位置動力學方程為：

其中：為服務航天器軌道真近點角速度，r_c為服務航天器軌道半徑，p_c為半正焦弦，分別表示x,y,z的二階導數，表示r_c的一階導數，服務航天器軌道參數r_c、p_c、通過服務航天器軌道確定系統得到，將作為已知量用于模型方程中；

定義p₁,…,p_N分別為目標上N個特征點相對于目標質心的位置矢量，表示在目標本體系上，由目標的剛體假設有：

其中，表示[p₁^T,…,p_N^T]^T關于時間的變化率；

最終選取待估計狀態則相對運動動力學模型寫為：

其中：f(X)由式(5)、(7)、(8)、(9)和(10)分別得到；w(t)為系統噪聲項；

步驟二：建立觀測方程，其中加入目標特征點之間的約束關系方程；

定義目標上特征點i在相機坐標系下的位置矢量ρ_i為：

式中：表示由服務航天器本體坐標系到相機坐標系的旋轉矩陣，是已知量；d為相機坐標系原點在服務航天器本體坐標系下的位置向量，是已知量；表示由LVLH坐標系到服務航天器本體坐標系的旋轉矩陣，通過自身測量設備得到；與待估計狀態q相關，由式(1)給出；

特征點p_i的圖像投影坐標與ρ_i＝[x_i y_i z_i]^T關系為：

其中f_x,f_y,c_x,c_y為所用相機的內參參數；

此外考慮特征點i,j,k，定義相對特征ρ_ij為：

同理定義ρ_jk和ρ_ki，ρ_ij在圖像中的投影坐標為y_ij，兩者滿足的關系為：

其中u_ij＝u_i-u_j,v_ij＝v_i-v_j，

考慮到矢量和ρ_ij+ρ_jk+ρ_ki＝0，結合式，特征點i,j,k之間應滿足約束：

記為M(i,j,k)＝y_i,j,k；

對于目標上N個特征點進行編號1,2,…,N，以三點為一組構成約束方程加入觀測方程；假設s₁,…,s_m為一種選點組合方式，覆蓋所有待估計特征點，選點組合方式分別為s₁＝1,2,3，s₂＝2,3,4，…，s_N-1＝N-1,N,1；

最終觀測方程為：

觀測方程中：z為所有觀測量，v為觀測噪聲，并且所有ρ_i、ρ_ij的相關分量都應用式和式進行替換，即可得到狀態到觀測的映射關系；

步驟三：設計濾波器，實現目標相對狀態的估計；

式描述的是連續系統狀態方程，先轉換為離散模型，之后再進行估計，離散模型為：

式中：X(k)為k時刻狀態，Δt為濾波周期，

對式和式分別表示系統狀態方程和觀測方程，采用迭代擴展卡爾曼濾波進行狀態估計；具體估計的步驟如下：

步驟3.1：初始化：設置初始的濾波估計值和誤差方差矩陣P(0)；

步驟3.2：狀態預測值為：

預測誤差方差陣為：

式中：P(k+1/k)表示預測誤差方差矩陣，P(k/k)表示k時刻的濾波誤差方差矩陣；

其中：Q_k為系統噪聲方差；

步驟3.3：狀態更新：

步驟3.3.1：將預測值作為迭代初值：

P(k+1/k+1)₀＝P(k+1/k) (22)

步驟3.3.2：迭代過程中，第i次迭代為：

其中R_k+1為測量噪聲方差，K(k+1)_i表示第i次迭代時的增益；

其中，(r(k)_i表示第i次迭代時的觀測殘差，)是將第i-1的迭代結果代入觀測方程(17)得到；

其中z(k+1)為k+1時刻所有觀測量；

P(k+1/k+1)_i＝[I-K(k+1)_iH(k+1)_i-1]P(k+1/k+1)_i-1 (26)

步驟3.3.3：當迭代次數達到最大迭代次數或者狀態迭代誤差小于設定的閾值時，迭代終止，狀態迭代誤差定義如下：

其中||·||₂表示向量二范數；假設第n次迭代時迭代終止，將此時的迭代結果作為最終估計結果輸出：

P(k+1/k+1)＝P(k+1/k+1)_n (29)

其中，即為k+1時刻的最終狀態估計結果。