[發明專利]一種粒子群算法的粗對準方法

有效

• 申請號: 202010196578.4
• 申請日: 2020-03-19
• 公開（公告）號: CN111323049B
• 公開（公告）日: 2021-04-20
• 發明（設計）人: 徐博;金坤明;吳磊;李盛新;張嬌;王連釗;郭瑜;張大龍
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種粒子群算法的粗對準方法，其特征在于，包括以下步驟：

步驟一：通過多矢量定姿方法，求解水平姿態，具體為：

載體坐標系相對導航坐標系姿態矩陣可用四個矩陣相乘表示：

地球坐標系相對導航坐標系的旋轉矩陣可由載體所在緯度求得：

其中，L為載體所在位置的緯度；

地心慣性系相對地球坐標系的旋轉矩陣可由地球自轉角速率和對準時間求得：

其中，ω_ie為地球自轉角速度，Δt＝t-t₀為距對準起始時刻的時間間隔；

利用陀螺儀輸出的角速度信息，求得載體坐標系相對載體慣性系的旋轉矩陣即：

其中，表示陀螺儀輸出構成的反對稱矩陣，并且矩陣的初值為單位陣；

捷聯慣導比力方程如下所示：

對比力方程進行變形得：

由化簡得：

在搖擺情況下，轉化為：

將兩邊左乘即在i_b0上的投影得：

將上式兩邊在[t₀,t_k]內積分得：

由于gⁿ＝[0 0 -g]^T，則重力加速度在地心慣性系下的投影為：

對上式進行積分得：

選擇m個時刻的速度vⁱ、作為參考矢量，構造目標方程：

整理變形得：

Q(C)＝Q′-tr(CB^T)

其中

對B進行奇異值分解，得

B＝U₁SU₂^T

式中：U₁和U₂為正交矩陣；S＝diag(s₁,s₂,s₃)，s₁≥s₂≥s₃≥0；

使Q取最小值的最優解：

其中：

進行載體坐標系對導航坐標系姿態矩陣的更新，得到水平姿態：

步驟二：建立粒子群算法模型，具體為：通過事先設置一群粒子，并讓群體中的每個粒子通過不斷的跟蹤兩個極值和來更新自己的位置和速度其中，稱為個體最優，是每個粒子自身找到的最優解；稱為全局最優，是整個種群目前找到的最優解，每個粒子的速度和位置迭代計算公式為：

式中，k＝1,2,...,G，G表示最大的迭代次數；i＝1,2,...,Size，Size表示種群規模，r₁和r₂表示0到1之間的隨機數；c₁表示局部學習因子，代表個體學習能力，c₂表示全局學習因子，代表社會學習能力，二者的取值范圍為[0，2]；ω(t)表示慣性權重，其值越大越有利于展開全局尋優，越小越有利于局部尋優，取值范圍為(0，1)；V_i表示第i個粒子的速度；X_i表示第i個粒子的位置；p_i表示個體最優位置；BestS_i表示群體最優位置；

步驟三：構建粒子群適應度函數，具體為：采用多矢量對準算法對水平姿態進行估計，將載體的航向作為粒子群算法的待估參數，由姿態角計算得到姿態矩陣并計算得到

通過vⁱ、和建立誤差方程如下：

選擇誤差量的平方和作為粒子群算法適應度函數，適應度函數如下：

f＝z(1)²+z(2)²+z(3)²

利用粒子群算法不斷更新載體航向角，使得適應度函數達到最小值，得到當前時刻最優的航向角；

步驟四：解決粒子早熟收斂問題，具體為：利用設置的載體艏搖最大角速率b_w與計算的當前時刻艏搖角速度w_k相比較，當計算的當前時刻艏搖角速度大于設置的最大艏搖角速度時，則去除掉當前時刻全局最優解所求的利用前一時刻全局最優解所求的代替；

步驟五：進行載體坐標系對導航坐標系姿態矩陣的更新，并利用姿態矩陣求解姿態。