本發(fā)明屬于慣性導(dǎo)航初始對(duì)準(zhǔn)領(lǐng)域，尤其涉及一種未知撓曲變形情況下的自適應(yīng)快速傳遞對(duì)準(zhǔn)方法。

背景技術(shù)：

傳遞對(duì)準(zhǔn)過(guò)程是利用精度較高的主慣導(dǎo)系統(tǒng)來(lái)校準(zhǔn)尚未對(duì)準(zhǔn)的子慣導(dǎo)系統(tǒng)，傳遞對(duì)準(zhǔn)技術(shù)不僅可以降低對(duì)慣性器件的要求，而且可以大大提高對(duì)準(zhǔn)的速度。目前，傳遞對(duì)準(zhǔn)技術(shù)被廣泛的應(yīng)用于艦載、機(jī)載以及車載設(shè)備等多種場(chǎng)合。不同的應(yīng)用環(huán)境具有不同的特點(diǎn)，對(duì)于艦載設(shè)備，由于艦船在水中航行時(shí)會(huì)受到來(lái)自于外界的海浪、陣風(fēng)、海水溫度變化等影響，同時(shí)也受到艦船本身發(fā)動(dòng)機(jī)、負(fù)荷、自重等影響，從而船體發(fā)生變形，所以安裝在艦載設(shè)備上的子慣導(dǎo)系統(tǒng)與主慣導(dǎo)系統(tǒng)不可避免的存在姿態(tài)誤差。姿態(tài)誤差包含安裝誤差和撓曲變形。

初始對(duì)準(zhǔn)不僅要估計(jì)出安裝誤差，也要實(shí)時(shí)的估計(jì)出船體的撓曲變形。在慣導(dǎo)系統(tǒng)中，撓曲變形、桿臂效應(yīng)和信息傳輸延時(shí)是影響系統(tǒng)精度的三個(gè)主要因素。其中，剛性桿臂引起的桿臂效應(yīng)可以通過(guò)物理方法得到精確補(bǔ)償，信息傳輸?shù)臅r(shí)間延時(shí)可以通過(guò)外推等算法得到補(bǔ)償。但是船體的撓曲變形是隨著船體受外界的影響變化而變化的，沒(méi)有辦法事先獲得準(zhǔn)確的變形量。

研究表明船體的變形對(duì)船體橫搖、縱搖、航向的影響可以達(dá)到幾十角分、幾分之一角分、幾角分等，在海況惡劣的情況下更會(huì)遠(yuǎn)遠(yuǎn)大于這一量級(jí)。因此，實(shí)時(shí)測(cè)出撓曲變形角對(duì)提高傳遞對(duì)準(zhǔn)精度有著十分重要的意義。

目前，對(duì)傳遞對(duì)準(zhǔn)中撓曲變形的研究分為三個(gè)方面：1、傳遞對(duì)準(zhǔn)中有多種信息的匹配方式，不同的匹配方式對(duì)船體的機(jī)動(dòng)方式要求不同，對(duì)準(zhǔn)的時(shí)間和效果也不同；2根據(jù)撓曲變形的特點(diǎn)建立慣導(dǎo)的誤差模型；3設(shè)計(jì)具有一定魯棒性的濾波器。

艦船在航行過(guò)程中環(huán)境復(fù)雜，對(duì)艦船的影響也不能通過(guò)單一的擾動(dòng)模型進(jìn)行完全的估計(jì)。傳統(tǒng)的kalman濾波技術(shù)是基于線性模型的，當(dāng)船體受海浪等影響，姿態(tài)誤差角不再滿足小角的情況時(shí)，傳統(tǒng)誤差模型存在較大的模型誤差，使用線性kalman濾波可能會(huì)使濾波結(jié)果發(fā)散。單純使用H_∞濾波雖然可以增加系統(tǒng)的魯棒性，但是在風(fēng)浪較小的情況下，H_∞的濾波精度比kalman濾波低。

技術(shù)實(shí)現(xiàn)要素：

發(fā)明目的：針對(duì)以上問(wèn)題，本發(fā)明提出一種自適應(yīng)快速傳遞對(duì)準(zhǔn)方法。

技術(shù)方案：為實(shí)現(xiàn)本發(fā)明的目的，本發(fā)明所采用的技術(shù)方案是：一種自適應(yīng)快速傳遞對(duì)準(zhǔn)方法，包括以下步驟：

(1)建立主慣導(dǎo)和子慣導(dǎo)系統(tǒng)的“速度+姿態(tài)”匹配模型，將主慣導(dǎo)和子慣導(dǎo)之間的撓曲變形作為能量有限的量測(cè)噪聲；

(2)設(shè)計(jì)基于自適應(yīng)加權(quán)算法的H₂/H_∞濾波算法，完成主慣導(dǎo)和子慣導(dǎo)的傳遞對(duì)準(zhǔn)。

步驟(1)具體包括：

步驟1.1：建立系統(tǒng)誤差模型：

其中，X_k∈R^n×1為系統(tǒng)的n維狀態(tài)向量；F_k,k-1∈R^n×n為n×n維一步狀態(tài)預(yù)測(cè)陣；W_k∈R^n×1是系統(tǒng)的過(guò)程噪聲；Γ_k,k-1為系統(tǒng)過(guò)程噪聲輸入矩陣；Y_k∈R^m×1是m維的觀測(cè)量；H_k∈R^m×n為觀測(cè)矩陣；V_k∈R^m×1為系統(tǒng)量測(cè)噪聲。

步驟1.2：設(shè)置15維的狀態(tài)向量X：

其中，δV_E、δV_N、δV_U分別為子慣導(dǎo)的東、北、天向的速度誤差；φ_E、φ_N、φ_U分別為子慣導(dǎo)的縱搖、橫搖、航向姿態(tài)誤差；δλ、δL、δh分別為子慣導(dǎo)的經(jīng)度、緯度、高度誤差；ε_E、ε_N、ε_U分別為子慣導(dǎo)的三軸陀螺漂移；分別為子慣導(dǎo)的三軸加表零偏。

步驟1.3：建立速度誤差方程：

其中，φ是子慣導(dǎo)的姿態(tài)誤差，f表示比力，δV是子慣導(dǎo)的速度誤差，V是子慣導(dǎo)的速度，是子慣導(dǎo)的加表零偏；n，i，e分別為導(dǎo)航系、慣性系和地球坐標(biāo)系，分別為地球自轉(zhuǎn)在n系中的投影及其誤差，表示n系相對(duì)e系的轉(zhuǎn)動(dòng)角速度及其誤差；上標(biāo)n表示在n系中的投影；“×”表示反斜陣。

步驟1.4：建立系統(tǒng)誤差方程：

姿態(tài)誤差方程：

位置誤差方程：

其中，ε是子慣導(dǎo)的陀螺漂移，表示n系相對(duì)i系的轉(zhuǎn)動(dòng)角速度及其誤差，R是地球半徑，L是子慣導(dǎo)的緯度，V_E子慣導(dǎo)東向的速度。

陀螺漂移與加表零偏的一階Markov方程：

步驟1.5：建立系統(tǒng)量測(cè)方程；

計(jì)算量測(cè)值Y：

Y＝[V_E-V_ME V_N-V_MN V_U-V_MU φ_E-φ_ME φ_N-φ_MN φ_U-φ_MU] 7

其中，V_ME、V_MN、V_MU表示主慣導(dǎo)的東向、北向、天向速度，φ_ME、φ_MN、φ_MU表示主慣導(dǎo)的姿態(tài)角；

計(jì)算量測(cè)矩陣H：

H＝[I_6×6 0_6×9] 8

其中，I_6×6是6×6維的單位陣，0_6×9是6×9維的0矩陣。

步驟(2)具體包括：

步驟2.1：計(jì)算加權(quán)系數(shù)d：

其中，tr()表示矩陣的跡；P⁽²⁾(k)、P^(∞)(k)分別表示前一次的kalman濾波和H_∞濾波的估計(jì)誤差方差；d為實(shí)數(shù)常數(shù)，0≤d≤1。

步驟2.2：計(jì)算前一次的H₂/H_∞濾波器增益K(k)：

K(k)＝dK⁽²⁾(k)+(1-d)K^(∞)(k) 10

其中，K⁽²⁾(k)為前一次的kalman濾波增益，K^(∞)(k)為前一次的H_∞濾波增益。

步驟2.3：計(jì)算本次狀態(tài)估計(jì)值

其中，是前一次的狀態(tài)估計(jì)值，y(k+1)是當(dāng)前觀測(cè)值，F(xiàn)_k+1k是一次狀態(tài)轉(zhuǎn)移陣，H_k+1是當(dāng)前狀態(tài)觀測(cè)矩陣。

步驟2.4：計(jì)算本次的H_∞濾波增益K^(∞)(k+1)和估計(jì)誤差方差P^(∞)(k+1)：

其中，L(k)∈R為適當(dāng)維數(shù)給定的矩陣，I是單位陣，δ是實(shí)系數(shù)，根據(jù)工程實(shí)際情況來(lái)確定，當(dāng)穩(wěn)定性要求高，δ取小些，當(dāng)精度要求高，則δ取大些。

令表示在給定測(cè)量值{Y₀,Y₁,…Y_k}條件下對(duì)Y_k的估計(jì)，定義如下濾波誤差：

設(shè)T_k(F_f)表示將不確定的干擾映射至濾波誤差{e_k}的傳遞函數(shù)，H_∞次優(yōu)濾波問(wèn)題可以描述為：給定正數(shù)γ>0，尋找次優(yōu)H_∞估計(jì)使得||T_k(F_f)||<γ，及滿足

其中，P₀為零時(shí)刻狀態(tài)向量的估計(jì)誤差，W_k和V_k分別為k時(shí)刻的系統(tǒng)噪聲序列和量測(cè)噪聲序列。

步驟2.5：計(jì)算本次kalman濾波增益K⁽²⁾(k+1)和估計(jì)誤差方差P⁽²⁾(k+1)：

P⁽²⁾(k+1)＝A(k)((P⁽²⁾(k))^-1+C^T(k)C(k))^-1A^T(k)+B(k)B^T(k) 17

K⁽²⁾(k+1)＝P⁽²⁾(k+1)C^T(k+1)(I+C(k+1)P⁽²⁾(k+1)C^T(k+1))^-1 18

步驟2.6：回到步驟2.1，繼續(xù)迭代計(jì)算。

有益效果：本發(fā)明的自適應(yīng)快速傳遞對(duì)準(zhǔn)方法使用了基于自適應(yīng)加權(quán)算法的H₂/H_∞濾波算法，算法能夠根據(jù)主/子慣導(dǎo)間的撓曲形變的大小自動(dòng)調(diào)節(jié)H₂濾波增益和H_∞濾波增益的加權(quán)算子；撓曲變形大時(shí)增加H_∞濾波增益陣的加權(quán)算子，降低H₂濾波增益陣的加權(quán)算子；撓曲變形小時(shí)增加H₂濾波增益陣的加權(quán)算子，降低H_∞濾波增益陣的加權(quán)算子。該方法能夠在未知撓曲變形的情況下，根據(jù)形變量大小自動(dòng)調(diào)節(jié)H₂濾波和H_∞濾波增益陣的加權(quán)算子，有效提高濾波精度，實(shí)現(xiàn)未知撓曲變形的主子慣導(dǎo)快速傳遞對(duì)準(zhǔn)。

附圖說(shuō)明

圖1是是主子慣導(dǎo)的相對(duì)安裝位置，1a是無(wú)撓曲變形時(shí)，主子慣導(dǎo)的相對(duì)安裝位置；1b是有撓曲變形時(shí)，主子慣導(dǎo)的相對(duì)安裝位置；

圖2是不存在撓曲變形時(shí)H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；

圖3是不存在撓曲變形時(shí)自適應(yīng)H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；

圖4是不存在撓曲變形時(shí)本發(fā)明的H₂/H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；

圖5是存在撓曲變形時(shí)H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；

圖6是存在撓曲變形時(shí)自適應(yīng)H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；

圖7是存在撓曲變形時(shí)本發(fā)明的H₂/H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明的技術(shù)方案作進(jìn)一步的說(shuō)明。

本發(fā)明所述的自適應(yīng)快速傳遞對(duì)準(zhǔn)方法，包括以下步驟：

S1.建立主/子慣導(dǎo)系統(tǒng)的“速度+姿態(tài)”匹配模型，將撓曲變形作為能量有限的量測(cè)噪聲V_k進(jìn)行處理。具體包括以下步驟：

步驟1.1：建立系統(tǒng)的誤差模型：

其中，X_k∈R^n×1為系統(tǒng)的n維狀態(tài)向量；F_k,k-1∈R^n×n為n×n維一步狀態(tài)預(yù)測(cè)陣；W_k∈R^n×1是系統(tǒng)的過(guò)程噪聲；Γ_k,k-1為系統(tǒng)過(guò)程噪聲輸入矩陣；Y_k∈R^m×1是m維的觀測(cè)量；H_k∈R^m×n為觀測(cè)矩陣；V_k∈R^m×1為系統(tǒng)量測(cè)噪聲。

步驟1.2：選取系統(tǒng)15維的狀態(tài)向量如下：

其中，δV_E、δV_N、δV_U分別為子慣導(dǎo)的東、北、天向的速度誤差；φ_E、φ_N、φ_U分別為子慣導(dǎo)的縱搖、橫搖、航向姿態(tài)誤差；δλ、δL、δh分別為子慣導(dǎo)的經(jīng)度、緯度、高度誤差；ε_E、ε_N、ε_U分別為子慣導(dǎo)的三軸陀螺漂移；分別為子慣導(dǎo)的三軸加表零偏。

步驟1.3：建立速度誤差方程：

其中，φ是子慣導(dǎo)的姿態(tài)誤差，f表示比力，δV是子慣導(dǎo)的速度誤差，V是子慣導(dǎo)的速度，是子慣導(dǎo)的加表零偏；n，i，e分別為導(dǎo)航系、慣性系和地球坐標(biāo)系，分別為地球自轉(zhuǎn)在n系中的投影及其誤差，表示n系相對(duì)e系的轉(zhuǎn)動(dòng)角速度及其誤差；上標(biāo)n表示在n系中的投影；“×”表示反斜陣。

步驟1.4：建立系統(tǒng)的誤差方程；

姿態(tài)誤差方程：

位置誤差方程：

其中，ε是子慣導(dǎo)的陀螺漂移，表示n系相對(duì)i系的轉(zhuǎn)動(dòng)角速度及其誤差；R為地球半徑，L是子慣導(dǎo)的緯度，V_E子慣導(dǎo)東向的速度。

假設(shè)陀螺漂移與加表零偏都為一階Markov過(guò)程，則有：

步驟1.5：建立系統(tǒng)量測(cè)方程；

將主、子慣導(dǎo)的速度與姿態(tài)相減作為量測(cè)值Y：

Y＝[V_E-V_ME V_N-V_MN V_U-V_MU φ_E-φ_ME φ_N-φ_MN φ_U-φ_MU] 7

其中，V_ME、V_MN、V_MU表示主慣導(dǎo)的東向、北向、天向速度，φ_ME、φ_MN、φ_MU表示主慣導(dǎo)的姿態(tài)角。

量測(cè)矩陣H為：

H＝[I_6×6 0_6×9] 8

其中，I_6×6是6×6維的單位陣，0_6×9是6×9維的0矩陣。

因?yàn)榧装鍝锨冃未嬖冢瑢⒘繙y(cè)噪聲V_k作為能量有限的噪聲進(jìn)行處理。

S2.設(shè)計(jì)基于自適應(yīng)加權(quán)算法的H₂/H_∞濾波算法，完成“速度+姿態(tài)”匹配的主/子慣導(dǎo)傳遞對(duì)準(zhǔn)。具體包括以下步驟：

步驟2.1：求加權(quán)系數(shù)d；

其中，tr()表示矩陣的跡，P⁽²⁾(k)、P^(∞)(k)分別表示前一次的kalman濾波和H_∞濾波的估計(jì)誤差方差，d為實(shí)數(shù)常數(shù)，0≤d≤1。

步驟2.2：計(jì)算前一次的H₂/H_∞濾波器增益K(k)，H₂/H_∞濾波器增益是kalman濾波增益和H_∞濾波增益的加權(quán)和；

K(k)＝dK⁽²⁾(k)+(1-d)K^(∞)(k) 10

其中，K⁽²⁾(k)為前一次的kalman濾波增益，K^(∞)(k)為前一次的H_∞濾波增益。

步驟2.3：計(jì)算本次狀態(tài)估計(jì)值

其中，是前一次的狀態(tài)估計(jì)值，y(k+1)是當(dāng)前觀測(cè)值，F(xiàn)_k+1,k是一次狀態(tài)轉(zhuǎn)移陣，H_k+1是當(dāng)前狀態(tài)觀測(cè)矩陣。

步驟2.4：計(jì)算本次的H_∞濾波增益K^(∞)(k+1)和估計(jì)誤差方差P^(∞)(k+1)：

其中，L(k)∈R為適當(dāng)維數(shù)給定的矩陣，I是單位陣，δ是實(shí)系數(shù)，根據(jù)工程實(shí)際情況來(lái)確定，當(dāng)穩(wěn)定性要求高，δ取小些，當(dāng)精度要求高，則δ取大些。

令表示在給定測(cè)量值{Y₀,Y₁,…Y_k}條件下對(duì)Y_k的估計(jì)，定義如下濾波誤差：

設(shè)T_k(F_f)表示將不確定的干擾映射至濾波誤差{e_k}的傳遞函數(shù)，H_∞次優(yōu)濾波問(wèn)題可以描述為：給定正數(shù)γ>0，尋找次優(yōu)H_∞估計(jì)使得||T_k(F_f)||<γ，及滿足

其中，P₀為零時(shí)刻狀態(tài)向量的估計(jì)誤差，W_k和V_k分別為k時(shí)刻的系統(tǒng)噪聲序列和量測(cè)噪聲序列。

步驟2.5：計(jì)算本次kalman濾波增益K⁽²⁾(k+1)和估計(jì)誤差方差P⁽²⁾(k+1)：

P⁽²⁾(k+1)＝A(k)((P⁽²⁾(k))^-1+C^T(k)C(k))^-1A^T(k)+B(k)B^T(k) 17

K⁽²⁾(k+1)＝P⁽²⁾(k+1)C^T(k+1)(I+C(k+1)P⁽²⁾(k+1)C^T(k+1))^-1 18

步驟2.6：回到步驟2.1，繼續(xù)迭代計(jì)算。

本發(fā)明的自適應(yīng)快速傳遞對(duì)準(zhǔn)方法使用了基于自適應(yīng)加權(quán)算法的H₂/H_∞濾波算法，能夠根據(jù)主/子慣導(dǎo)間的撓曲形變的大小自動(dòng)調(diào)節(jié)H₂濾波增益和H_∞濾波增益的加權(quán)算子；撓曲變形大時(shí)增加H_∞濾波增益陣的加權(quán)算子，降低H₂濾波增益陣的加權(quán)算子；撓曲變形小時(shí)增加H₂濾波增益陣的加權(quán)算子，降低H_∞濾波增益陣的加權(quán)算子。該方法能夠在未知撓曲變形的情況下，根據(jù)形變量大小自動(dòng)調(diào)節(jié)H₂濾波和H_∞濾波增益陣的加權(quán)算子，有效提高濾波精度，實(shí)現(xiàn)未知撓曲變形的主子慣導(dǎo)快速傳遞對(duì)準(zhǔn)。

如圖1所示是主子慣導(dǎo)的相對(duì)安裝位置，圖1a是無(wú)撓曲變形時(shí)，主子慣導(dǎo)的相對(duì)安裝位置；圖1b是有撓曲變形時(shí)，主子慣導(dǎo)的相對(duì)安裝位置。如圖2～4所示分別是不存在撓曲變形時(shí)H_∞濾波、自適應(yīng)H_∞濾波和本發(fā)明的H₂/H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；具體數(shù)據(jù)如表1所示。如圖5～7所示分別是存在撓曲變形時(shí)H_∞濾波、自適應(yīng)H_∞濾波和本發(fā)明的H₂/H_∞濾波的縱搖、橫搖和航向姿態(tài)誤差；具體數(shù)據(jù)如表2所示。

表1

表2

仿真環(huán)境中傳感器的性能指標(biāo)設(shè)置為：陀螺儀的常值漂移為0.01/h，隨機(jī)漂移為加表零偏為100μg(g＝9.8m²/s)，隨機(jī)偏差為50μg；當(dāng)?shù)氐乩砭暥葹?2.37°，經(jīng)度為118.22°；仿真時(shí)間為1500s；艦船縱搖、橫搖、航向的搖擺幅值分別為6°、10°、5°；搖擺周期分別是8s、10s、6s；艦船以10m/s正北方向勻速航行。主/子慣導(dǎo)間的安裝誤差角為0.5°、0.6°、0.2°，甲板撓曲變形繞x、y、z軸方向的方差分別為30＇、5＇、8^＇，相關(guān)時(shí)間分別為2s、3s、5s。甲板的撓曲變形由二階Makov過(guò)程模擬。