[發明專利]一種非合作目標運動與慣性參數的時頻域混合辨識方法

有效

• 申請號: 201710772043.5
• 申請日: 2017-08-31
• 公開（公告）號: CN107702709B
• 公開（公告）日: 2020-09-25
• 發明（設計）人: 黃攀峰;韓冬;劉正雄;孟中杰;張夷齋;張帆
• 申請（專利權）人: 西北工業大學
• 主分類號: G01C21/10
• 分類號: G01C21/10;G01M1/12
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 王鮮凱

1.一種非合作目標運動與慣性參數的時頻域混合辨識方法，其特征在于步驟如下：

步驟1：由視覺相機獲得目標位姿信息；

步驟2、根據視覺相機獲得的目標位姿信息以及航天器運動方程，采用頻域最優濾波器對非合作目標的質心位置及平動速度進行估計：

2-1)設計頻域濾波器，對非合作目標位置序列進行DFT變換，相當于把這個序列通過一個FIR數字濾波器，位置序列r_s(n)的離散付里葉變換式

式中，N為變換點數目，r_s(n)為n時刻目標的位置，由視覺相機獲得，這相當于使序列r_s(n)中的每個樣本有相位滯后最后在n＝N-1時刻把經過相移的各個樣本疊加起來，就得出了序列r_s(n)中頻率為kΩ的頻譜分量的付里葉系數X(k)；

2-2)由于目標存在平動與轉動，將方程進行平動與轉動分解，

r_s＝r+R(q)ρ＝r₀+vt+R(q)ρ

其中：ρ為衛星質心到目標點位置矢量，ω為目標旋轉角速度，ρ,ω均為在相機所在坐標系下的表達，r為目標質心位置，r₀為初始時刻目標質心位置；相機所在坐標系相對于慣性坐標系的旋轉通過四元數q表示；

角速度與四元數的運動學關系表示為：

式中，同樣，對于姿態變化所采用的四元數乘法，做如下定義：

式中，E為單位矢量，q_v為四元數矢量部分，q₀為四元數標量部分，四元數表示為q＝[q_v^Tq₀]^T；

四元數運算通過旋轉矩陣R(q₂)R(q₁)描述，R(q)定義為：

R(q)＝(2q₀²-1)E₃+2q₀[q_v×]+2q_vq_v^T

質心位置r₀與平動線速度v均為目標位置序列及其差分的直流分量；

步驟3、辨識質心位置r₀與平動線速度v：根據視覺相機獲得的目標位姿信息以及航天器運動方程，在時域上采用擴展有限沖擊相應濾波器對非合作目標的運動角速度以及慣性參量進行估計：

3-1)設計時域濾波器，

EFIR濾波器首先假定窗口長度N，通過計算量測偏差的均方值在線估計得出；當獲得第n時刻量測值時，從時刻m＝n-N+1到時刻n之間的N個量測值為有效量測值，目標狀態的估計值表示為

其中，為l時刻狀態估計值，為狀態預估計，l取值范圍從m+k到n，k為狀態數，當l＝n時，輸出n時刻狀態估計值，Z_l為l時刻觀測值，K_l為l時刻濾波器增益，為觀測矩陣；

式中G_l為廣義噪聲功率增益，通過下式迭代計算得出

對與轉動有關的狀態矢量x＝[q_v^T ω^T p^T ρ^T μ_v^T]^T的辨識流程如下所示：

輸出即為目標參數的狀態估計值；

其中：

2.根據權利要求1所述非合作目標運動與慣性參數的時頻域混合辨識方法，其特征在于：求解H和Φ的過程如下：

根據航天器動力學方程，設計需要估計的狀態變量x、推導算法所需要的觀測矩陣H以及狀態轉移矩陣Φ表達式：

建立通過歐拉方程描述的目標姿態動力學方程：

其中，I為轉動慣量，τ為擾動力矩；

引入慣量參數表示：

式中，I_xx,I_yy,I_zz為目標主慣量，滿足以下條件：

I_xx+I_yy＞I_zz

I_yy+I_zz＞I_xx

I_zz+I_xx＞I_yy

進而求得慣量參數的約束條件

p_x＞-1,p_y＞-1,p_z＞-1

用慣性參數改寫動力學方程為：

式中

I_c＝diag(I_xx,I_yy,I_zz)，tr(I_c)表示I_c的跡，ε_τ用來描述太陽能帆板支架、重力梯度產生的阻尼效果，選擇采用過程噪聲來描述；

視覺測量系統獲得帶有噪聲的目標點相對位姿并解算到慣性坐標系下，目標位置由矢量r_s表示，目標所在目標坐標系{C}相對于慣性坐標系{A}的姿態由四元數η表示，測量向量Z表示為：

式中：為測量噪聲，對于空間自旋目標，測量得到的目標姿態η由預先未知的兩個姿態變換得到，設目標坐標系{C}相對于相機坐標系{B}的姿態由四元數μ表示；相機坐標系{B}相對于慣性坐標系{A}的姿態由四元數q表示，則有下式成立：

式中為四元數乘，定義與轉動有關的狀態矢量：

x＝[q_v^T ω^T p^T ρ^T μ_v^T]^T

式中：q_v,μ_v分別為四元數的矢量部分；則觀測方程中轉動部分表示為：

式中

該方程為四元數q,μ的非線性方程，為滿足線性濾波器的要求，需要對觀測方程線性化；有||δq_v||＜＜1，||δμ_v||＜＜1,δq₀≈1，觀測矩陣中位置部分的線性化由公式R(q)＝(2q₀²-1)E₃+2q₀[q_v×]+2q_vq_v^T得一階近似公式：

描述微小旋轉的四元數方程：

將公式代入公式并忽略高階小項得：

式中：

至此，

公式給出了觀測方程的一階近似值，并由此得出量測矩陣：

根據公式所描述的目標動力學方程，將連續時間狀態空間模型描述為：

式中，為過程噪聲，考慮到自適應EFIR濾波器為線性濾波器，為滿足濾波器要求，

對狀態方程線性化為：

對狀態方程線性化直接由全微分公式給出：

根據剛體轉動的性質，設狀態參數中微分不變量為θ，則θ＝[p^T,ρ^T,μ_v^T]^T，并且由此可得出連續時間狀態轉移矩陣：

將方程寫成離散形式，有

X(n+1)＝Φ(n)X(n)+V(n)

其中，V(n)表示離散的過程噪聲，Φ(n)表示離散狀態轉移矩陣；

對于時常系統有