一種基於多種群協同PF算法的SINS/CNS/SAR組合導航方法與流程

2023-10-09 15:39:41 2

一種基於多種群協同pf算法的sins/cns/sar組合導航方法
技術領域
1.本發明涉及一種基於多種群協同pf算法的sins/cns/sar組合導航技術，屬於導航與制導技術領域。


背景技術：

2.隨著實際工程中對導航系統的系統精度和可靠性要求越來越高，單一的導航系統與定位難以滿足實際工程中日益增長的需求。為提高導航系統的性能，滿足我國高精度軍事裝備與社會經濟發展要求，具備高精度、高可靠與長航時的組合導航已成為導航、制導與控制領域的前沿科學技術之一。sins/cns/sar組合導航技術以捷聯慣性導航(sins)為主體，利用天文導航(cns)與合成孔徑雷達(sar)為輔的組合導航系統，具有抗幹擾能力強、定位精度高、自主性能優良的特點。適用於對導航系統定位具有高精度要求的飛行器，是具備發展前景的自主式組合導航系統。
3.sins/cns/sar組合導航技術根據量測信息與濾波算法實時估算飛行器狀態量信息，即通過葉貝斯公式結合觀測信息與狀態變化梯度，將狀態量轉換為狀態後驗概率密度問題。在動態導航濾波計算中，傳統導航系統濾波器設計一般採用卡爾曼濾波方法，卡爾曼濾波對於線性系統具有較高的精度，要求系統具有精確的狀態方程和觀測方程，且系統噪聲和觀測噪聲均為白噪聲，只適用於線性系統模型。然而工程應用時，sins/cns/sar組合系統實際導航過程中，噪聲大部分為非高斯噪聲，卡爾曼濾波方法對於非高斯分布的系統狀態模型濾波則性能較差，導航精度降低。為解決這一問題，ukf(unscented kalman filter,無損卡爾曼濾波)算法、pf算法被相繼提出。ukf算法保證了非線性系統的普遍適應性，然而對於無法準確估計模型參數的複雜系統，該算法的性能受到一定限制；pf算法存在粒子退化與樣本貧化的問題，導致粒子集難以描述系統狀態，飛行器狀態估計精確度下降；同時算法迭代速度降低，無法滿足導航系統實時性要求。


技術實現要素：

4.本發明解決的技術問題是：克服現有技術的不足，提供了一種基於多種群協同pf算法的sins/cns/sar組合導航方法，提高sins/cns/sar組合導航技術的高精度與高實時性。
5.本發明解決技術的方案是：一種基於多種群協同pf算法的sins/cns/sar組合導航方法，該方法包括如下步驟：
6.s1、根據sins/cns/sar組合導航系統狀態量的初始分布p(x0)抽取產生m個粒子並初始化粒子的權值為在下一個測量周期進入步驟s2，其中，i為粒子序號，m為粒子數，初始化測量周期n為1；
7.s2、基於初始化粒子，第n個測量周期sins/cns/sar組合導航系統狀態量測量值，計算得出重要性密度函數，按照重要性密度函數抽樣產生m個粒子構成粒子種群
8.s3、採用多種群協同粒子群算法，算出粒子種群的全局最優值，使之逼近於測量周期sins/cns/sar組合導航系統狀態量的測量值；
9.s4、獲取sins/cns/sar組合導航系統量測狀態值，更新粒子權值並進行歸一化，將n加1更新n，在組合導航系統下一個測量周期，返回步驟s2重新執行。
10.優選地，所述步驟s3中多種群協同粒子群算法的具體實現為：
11.s3.1、利用logistic映射將m個粒子種群映射至混沌空間中，生成隨機的粒子種群；
12.s3.2、將映射後的粒子種群按照粒子權值大小劃分為3個粒子子群，分別記為高權值子群、低權值子群、中權值子群；
13.s3.3、對各子群獨立搜索更新，分別計算各粒子子群中每個粒子的適應度值，並更新粒子個體極值和每個子群全局最優值；
14.s3.4、將互相排斥的子群合併，並將粒子位置協同，選取互斥子群中的其中一個子群，將該粒子子群每個粒子個體位置最優值對應更改為另一個子群每個粒子的個體位置最優值，該粒子子群的全局位置最優值更改為另一個種群的全局位置最優值；
15.s3.5、更新粒子個體位置最優值與群體位置最優值、每個粒子的速度值和位置值，迭代執行步驟s3.3～s3.5直到合併成為一個粒子群，並且該粒子群的全局最優值與測量周期sins/cns/sar組合導航系統狀態量測量值之間的偏差小於預設門限。
16.優選地，所述sins/cns/sar組合導航系統的狀態量為：
[0017][0018]
其中，ψ,γ分別表示偏航角、俯仰角和滾轉角，p為sins/cns/sar組合導航系統的位置值，v為sins/cns/sar組合導航系統的速度值，ω為角速度實際測量值，a為加速度實際測量值。
[0019]
優選地，所述步驟s3.2的具體分類方法：
[0020]
設置第一閾值a
max
和第二閾值a
min
，將映射後的粒子群按照權值大小劃分為3個種群，將權值大於第一閾值a
max
的粒子歸為高權值種群，將權值小於第二閾值a
min
的粒子歸為低權值種群，權值介於二者中間的粒子歸為中權值種群。
[0021]
優選地，所述粒子更新速度和速度更新的計算方法為：
[0022][0023][0024][0025]
其中，i＝1,2,...,n，n為粒子數，d＝1,2,...,d，d為粒子坐標維數；χ為收縮因子，ω為慣性權重，c1為第一加速常數和c2為第二加速常數，r1為第一隨機常數和r2為第二隨機常數，為粒子i當前的速度；為粒子i更新後的速度；為粒子i當前的位置；為粒子i更新後的位置，t為迭代次數。
[0026]
優選地，所述粒子個體位置最優值與群體位置最優值為：
[0027][0028][0029]
其中，f(
·
)表示適應值計算函數，t為迭代次數。
[0030]
優選地，所述第一隨機常數r1和第二隨機常數r2為在(0,1)之間均勻分布的隨機數。
[0031]
優選地，步驟s3.3採用多線程的方式並行計算。
[0032]
優選地，所述適應值計算函數為：
[0033][0034]
其中，r為觀測噪音方差，x為最新實際狀態值，x為粒子中的元素預測值。
[0035]
優選地，所述粒子權值通過如下方式計算：
[0036][0037]
其中，為更新後的粒子權值，為更新前的粒子權值，p(zn|xn)為組合導航系統量測量的後驗概率密度函數。
[0038]
本發明與現有技術相比的有益效果是：
[0039]
(1)、本發明採用多種群協同pf(particle filter，粒子濾波)算法，該算法適用於非高斯分布的系統狀態模型，不需要考慮隨機量必須滿足高斯分布的約束條件，濾波性能更優良；
[0040]
(2)、本發明通過多種群協同智能優化算法，緩解了pf算法粒子退化與樣本貧化問題，提高導航系統準確性。基於多種群系統的pf算法與常規pf算法及ukf算法相比，更加有效地降低變量的誤差均方值，提高了濾波性能；
[0041]
(3)、本發明實現時採用多線程同時在不同子群搜索，提高了算法效率，進一步確保sins/cns/sar組合導航系統的實時性；
[0042]
(4)、本發明方法適用於非線性系統模型且算法迭代速度較快，進而滿足組合導航系統的要求。
附圖說明
[0043]
圖1為本發明實施例基於多種群協同pf算法的sins/cns/sar組合導航方法流程圖。
具體實施方式
[0044]
下面結合實施例對本發明作進一步闡述。
[0045]
以下將結合附圖及實施例來詳細說明本發明的實施方式，藉此對本發明如何應用技術手段來解決技術問題，並達成技術效果的實現過程能充分理解並據以實施。需要說明的是，只要不構成衝突，本發明實施例中的各個特徵可以相互結合，所形成的技術方案均在本發明的保護範圍之內。
[0046]
本發明將群體智能優化算法與pf算法相結合，將粒子看作生物群體的個體，通過
模擬生物集群的運動規律調整粒子分布，避免直接捨棄權重較低的粒子而造成粒子貧化現象，保證sins/cns/sar組合導航系統精確度滿足要求。多種群協同則是將整個群體分為多個子群，每個子群並行搜索解空間中不同的潛在可行解。另外，為了避降低子群重複搜索，在群體中引入合理的自適應交叉操作，引導子群對整個空間進行協同搜索從而保障粒子的多樣性，提高算法迭代效率，確保sins/cns/sar組合導航系統的實時性。
[0047]
以下對本發明進行詳細說明：
[0048]
1、算法原理
[0049]
多種群協同粒子群算法
[0050]
粒子群算法根據對環境的適應度將群體中的個體移動到更優的區域，通過粒子本身找到的最優解與整個種群找到的最優解動態調整自身動態。設第i個粒子的位置和速度分別表示為xi＝(x
i1
，x
i2
，
…
，x
id
)和vi＝(v
i1
，v
i2
，
…
，v
id
)，粒子個體最優p
best
的位置為pi＝(p
i1
，p
i2
，
…
，p
id
)，種群全局最優g
best
位置為pg＝(p
g1
，p
g2
，
…
，p
gd
)。粒子更新速度和位置更新：
[0051][0052][0053][0054]
其中，i＝1,2,...,n，n為粒子數，d＝1,2,...,d，d為坐標維數；χ為收縮因子，通常χ＜1，用於降低收斂速度、延緩粒子多樣性衰減速度，ω為慣性權重，c1為第一加速常數和c2為第二加速常數，r1為第一隨機常數和r2為第二隨機常數，均在(0,1)之間均勻分布的隨機數；為粒子i當前的速度；為粒子i更新後的速度；為粒子i當前的位置；為粒子i更新後的位置，t為迭代次數。
[0055]
粒子個體位置最優值與群體位置最優值為：
[0056][0057][0058]
其中，f(
·
)表示適應值計算函數。
[0059]
多種群協同的基本思想就是將整個群體分為不同子群，每個子群各自搜索解空間中不同的潛在可行解，該設計方法可以保證群體多樣性，防止粒子退化和樣本貧化問題的出現。任意兩個子群在各自搜索過程中，都存在同時收斂於一個局部最優解的可能性，為了避免重複搜索，提高算法運行效率，多種群協同粒子群優化算法的流程圖如圖1所示。
[0060]
為保持群體的原有信息，同時增強群體的多樣性，當兩個子群之間的最小距離ds
i'j'
小於排斥半徑r
ex
時，保留該兩個群體中最優的個體，之後對兩個群體中的部分個體同時進行隨機初始化。進而群體不僅能夠保留自身信息，同時又能繼續探索未知解空間,i'、j'分別表示兩個子群。
[0061]
2、基於多種群協同的粒子濾波算法
[0062]
以捷聯慣導(sins)為核心，sins/cns/sar組合導航系統的狀態方程為：
[0063][0064]
式中定義狀態量為其中ψ,γ分別表示偏航角、俯仰角和滾轉角，p為導航系統位置值，v為導航系統速度值，ω為角速度實際測量值，f為加速度實際測量值。w表示導航系統的白噪聲。
[0065]
sins/cns/sar組合導航系統將相對於本坐標系的實際測量值與理論值進行比較，二者的差值為組合導航系統模型的觀測量。該組合導航系統的量測方程為：
[0066][0067]
式中，rb、pb分別表示星光矢量的測量值和理論值，λ為經度，l表示緯度。q表示系統量測噪聲。
[0068]
組合導航系統的離散化模型如下：
[0069][0070]
其中w
n-1
、qn為服從某種分布的隨機噪聲，上標n表示sins/cns/sar組合導航系統第n個測量周期，上標n-1表示第n-1個測量周期；
[0071]
由蒙特卡洛方法求解離散系統狀態更新和量測更新方程，即系統狀態值分布函數p(x0)由一系列樣本粒子和其對應的權值來表示。其中樣本為是從p(xn)抽樣產生，對應的權值為m為粒子個數，n為測量周期。系統狀態值分布函數和狀態估計值由式(9)計算得到：
[0072][0073]
基於上述公式，可採用粒子濾波算法進行求解。選取後驗概率密度函數作為重要性函數，即：
[0074][0075]
粒子的權值計算公式為：
[0076][0077]
常規的粒子濾波算法由於在重要性採樣過程中，採用的重要性函數是次優的，且在重新採樣過程中，粒子樣本易出現貧化現象，導致算法性能降低。因此本發明引入多協同種群粒子群優化算法對pf算法的重要性採樣過程進行優化。
[0078]
多種群協同粒子群優化pf的重要性採樣過程如下：
[0079]
設多種群協同粒子群算法的適應值計算函數如公式(12)所示，即：
[0080][0081]
其中r為觀測噪音方差，xn為sins/cns/sar組合導航系統狀態量，為sins/cns/sar組合導航系統狀態量的預測值。通過各子群比較適應值的大小找出最優個體粒子p
best
，並記錄最優粒子的值，使所有粒子按照式(1)和式(2)進化。
[0082]
如圖1所示，本發明提供的一種基於多種群協同pf算法的sins/cns/sar組合導航方法包括如下步驟：
[0083]
s1、根據sins/cns/sar組合導航系統狀態量的初始分布p(x0)抽取產生m個粒子並初始化粒子的權值為在下一個測量周期進入步驟s2，其中，i為粒子序號，m為粒子數，初始化測量周期n為1；
[0084]
所述sins/cns/sar組合導航系統的狀態量為：
[0085][0086]
其中，ψ,γ分別表示偏航角、俯仰角和滾轉角，p為sins/cns/sar組合導航系統的位置值，v為sins/cns/sar組合導航系統的速度值，ω為角速度實際測量值，a為加速度實際測量值。
[0087]
s2、基於初始化粒子，第n個測量周期sins/cns/sar組合導航系統狀態量測量值，計算得出重要性密度函數，按照重要性密度函數抽樣產生m個粒子構成粒子種群對系統狀態值進行更新；
[0088]
s3、採用多種群協同粒子群算法，算出粒子種群的全局最優值，使之逼近於測量周期sins/cns/sar組合導航系統狀態量的測量值；
[0089]
粒子種群的全局最優值，使之逼近於測量周期sins/cns/sar組合導航系統狀態量的測量值為：
[0090]
該粒子群的全局最優值與測量周期sins/cns/sar組合導航系統狀態量測量值之間的偏差小於預設門限。
[0091]
所述步驟s3中多種群協同粒子群算法的具體為：
[0092]
s3.1、利用logistic映射將m個粒子種群映射至混沌空間中，生成隨機的粒子種群；
[0093]
s3.2、將映射後的粒子種群按照粒子權值大小劃分為3個粒子子群，分別記為高權值子群、低權值子群、中權值子群；
[0094]
s3.3、對各子群獨立搜索更新，分別計算各粒子子群中每個粒子的適應度值，並更新粒子個體極值p
best
和每個子群全局最優值g
best
；
[0095]
s3.4、將互相排斥的子群合併，並將粒子位置協同，選取互斥子群中的其中一個子群，將該粒子子群每個粒子個體位置最優值對應更改為另一個子群每個粒子的個體位置最優值，該粒子子群的全局位置最優值更改為另一個種群的全局位置最優值；
[0096]
判斷子群是否排斥方法為：
[0097]
當兩個種群全局最優值之間偏差小於ε時，認為這兩個種群互為排斥，ε為誤差常數。
[0098]
s3.5、更新粒子個體位置最優值與群體位置最優值、每個粒子的速度值和位置值，重複執行步驟s3.3～s3.5直到合併成為一個粒子群，並且該粒子群的全局最優值與測量周期sins/cns/sar組合導航系統狀態量測量值之間的偏差小於預設門限。
[0099]
s4、獲取sins/cns/sar組合導航系統量測狀態值，更新粒子權值並進行歸一化，將n加1更新n，在組合導航系統下一個測量周期，返回步驟s2重新執行。
[0100]
所述步驟s3.2的具體分類方法：
[0101]
設置第一閾值a
max
和第二閾值a
min
，將映射後的粒子群按照權值大小劃分為3個種群，將權值大於第一閾值a
max
的粒子歸為高權值種群，將權值小於第二閾值a
min
的粒子歸為低權值種群，權值介於二者中間的粒子歸為中權值種群。
[0102]
所述粒子更新速度和速度更新的計算方法為：
[0103][0104][0105][0106]
其中，i＝1,2,...,n，n為粒子數，d＝1,2,...,d，d為粒子坐標維數；χ為收縮因子，ω為慣性權重，c1為第一加速常數和c2為第二加速常數，r1為第一隨機常數和r2為第二隨機常數，為粒子i當前的速度；為粒子i更新後的速度；為粒子i當前的位置；為粒子i更新後的位置，t為迭代次數。
[0107]
所述粒子個體位置最優值與群體位置最優值為：
[0108][0109][0110]
其中，f(
·
)表示適應值計算函數，t為迭代次數。
[0111]
所述第一隨機常數r1和第二隨機常數r2為在(0,1)之間均勻分布的隨機數。
[0112]
所述收縮因子χ的取值範圍為χ＜1。
[0113]
所述適應值計算函數為：
[0114][0115]
其中，r為觀測噪音方差，x為最新實際狀態值，x為粒子中的元素預測值。
[0116]
所述粒子權值通過如下方式計算：
[0117][0118]
其中，為更新後的粒子權值，為更新前的粒子權值，p(zn|xn)為組合導航系統量測量的後驗概率密度函數。
[0119]
歸一化計算方法為每個粒子的權值除以所有權值的最大值。
[0120]
本發明雖然已以較佳實施例公開如上，但其並不是用來限定本發明，任何本領域技術人員在不脫離本發明的精神和範圍內，都可以利用上述揭示的方法和技術內容對本發明技術方案做出可能的變動和修改，因此，凡是未脫離本發明技術方案的內容，依據本發明的技術實質對以上實施例所作的任何簡單修改、等同變化及修飾，均屬於本發明技術方案的保護範圍。