[發明專利]基于UKF的光電吊艙穩定平臺被動測距方法

在審

• 申請號: 202011378193.6
• 申請日: 2020-11-30
• 公開（公告）號: CN112558051A
• 公開（公告）日: 2021-03-26
• 發明（設計）人: 陳小天;賀遵亮
• 申請（專利權）人: 湖南傲英創視信息科技有限公司
• 主分類號: G01S11/12
• 分類號: G01S11/12
• 代理公司: 長沙朕揚知識產權代理事務所(普通合伙) 43213
• 代理人: 馬家駿

1.基于UKF的光電吊艙穩定平臺被動測距方法，其特征在于，包括以下步驟：

1)、定義吊艙穩定平臺的各項參數：做以下定義：I為慣性坐標系，B為吊艙坐標系，O為穩定平臺的光軸坐標系；所述B的X軸為吊艙的軸線，其Y軸為垂直于吊艙的垂直軸面，其Z軸為垂直于吊艙的水平軸面；

所述I到所述B通過繞慣性坐標軸Y，X，Z轉動得到的歐拉角分別為：ψ,θ,φ，在所述B下吊航的慣性角速度的分量分別為：p,q,r，通過平臺的角速率陀螺測量所述O下光軸的慣性角速率分別為：q_O,r_O；

X'_O為光軸在平面X_BZ_B的投影，對于所述O的原點為穩定平臺的光心，X軸為光軸，Z軸在平面X_BZ_B內并垂直于X'_O，Y軸按右手法則給出；光軸相對于吊艙的框架角包括俯仰和偏航，其中俯仰φ_z為X_O與X'_O的夾角；所述I的原點位于吊艙的質心，X軸指向東，Y軸指向北，Z軸垂直向上，則XY平面為水平面；

吊艙與目標相對位移為r，分量r_h為r在XY平面內的投影，r_h與X的夾角η為光軸在所述I下的偏航角，r與r_h的夾角ε為光軸在所述I下的俯仰角，目標加速度在所述O下每個坐標軸的投影分量分別為：a_tz,a_ty,a_tx，其對應的吊艙的加速度分量分別為：a_mx,a_my,a_mz；

2)、獲得測量值，設置狀態量和狀態以及測量方程：在穩定平臺中，得到的測量值包括：光軸相對于吊艙的框架角分別為：φ_y,φ_z，光軸在所述O下的慣性角速率分別為：p_O,q_O,r_O；所述B下的慣性角速率p,q,r，吊艙對于所述I的姿態分別為：ψ,θ,φ,吊艙的加速度a^I_mx,a^I_my,a^I_mz；穩定平臺的狀態矢量的計算公式為：

狀態矢量在GSC下的狀態量的計算公式為：

使測量值為Z，其計算公式為：

其中W為測量誤差矩陣；

3)、基于坐標系變換一步遞推的UKF被動測距算法：假設狀態向量x為n維隨機變量，并已知其均值和方差P_xx可以通過2n+1個加權點來近似這個隨機變量的分布：

W_i＝1/2(n+κ),i＝1，2，...，n

W_i+n＝1/2(n+κ)，i＝1，2，…，n

式中：k為自然數，可以起到調節高階矩的作用并減小預測誤差，其值班x分布的不同而不同，如果假設x服從正態分布，則取n+k＝3，為矩陣(n+κ)P_xx的均方根的第i行或者第i列，對于該矩陣均方根的求解可以利用AR分解或Cholesky分解得到，W_i為與第i個點相對應的權重；

4)、相對運動量與狀態量的相互轉換，得到預測值和預測方差矩陣，實現被動測距：吊艙與目標相對運動的狀態量為慣性系統下X,Y,Z方向的位移和速度：X_c＝[x y z v_x v_yv_z]′，狀態方程為：X_c(k+1)＝ΦX_c(k)+BU，其中：其中T為采樣時間，I3為三階單位矩陣，a^I_t,a^I_m,分別為吊艙與目標在所述I下的加速度，其中吊艙的加速度由吊艙的慣性設備測量得到；使光軸相對于慣性坐標系的俯仰和偏航分別為ε和η，吊艙相對于所述I的轉動歐拉角為θ和φ，計算公式為：和其中

所述I下的運動狀態量和GMSC下的狀態量有以下轉換關系：

(1)從GMSC到慣性直角坐標系統下的狀態量變換：h_mc：x→x_c，其中：

x＝rcosεcosη

y＝rcosεsinη

z＝rsinε

(2)、從所述I下的狀態量到GMSC的變換：h_cm：x_c→x，其中：通過非線性轉移函數表示為：h(χ_i(k))＝h_cm(Φ(h_mc(χ_i(k))))，之后再通過點集經加權平均和協方差得到預測值和預測方差矩陣，再利用UKF進行濾波，從而實現被動測距。