基于雙模式切換的機(jī)載慣性/雷達(dá)組合導(dǎo)航方法

摘 要： 針對(duì)機(jī)載慣性導(dǎo)航系統(tǒng)（inertial navigation system， INS）/毫米波雷達(dá)組合導(dǎo)航系統(tǒng)中，毫米波雷達(dá)測(cè)角隨機(jī)常值誤差會(huì)帶來(lái)定位精度損失問(wèn)題，提出一種基于容積卡爾曼濾波算法的雙模式自適應(yīng)雙重切換INS/毫米波雷達(dá)組合導(dǎo)航方法。將組合導(dǎo)航過(guò)程分為斜距角位置匹配與相對(duì)位置矢量匹配兩個(gè)階段，并對(duì)導(dǎo)航系統(tǒng)建模方法進(jìn)行改進(jìn)，將測(cè)角常值誤差導(dǎo)致的位置誤差從組合誤差中分離出來(lái)。導(dǎo)航初階段采用斜距角位置匹配模式，針對(duì)近距時(shí)濾波發(fā)散的問(wèn)題，采用基于滑動(dòng)窗方差判別的模式切換方法對(duì)切換條件進(jìn)行實(shí)時(shí)判定，判定滿足條件后自動(dòng)切換組合模式為相對(duì)位置矢量匹配模式。以載機(jī)著陸為背景進(jìn)行仿真驗(yàn)證，結(jié)果表明，所提方法有效提升了組合導(dǎo)航精度，具有一定的工程意義。

關(guān)鍵詞： 機(jī)載慣性導(dǎo)航系統(tǒng); 毫米波雷達(dá); 組合導(dǎo)航; 雙模式切換

中圖分類號(hào)： TP 273+.2

文獻(xiàn)標(biāo)志碼： A

DOI：10.12305/j.issn.1001-506X.2024.08.24

Airborne inertial navigation system/radar integrated navigation method

based on dual-mode switching

ZHANG Dong^1，2， XING Fuyi^3，*， XU Yunhe²， QIAN Peng²

（1. Department of Aeronautics and Astronautics， Fudan University， Shanghai 201203， China; 2. Shenyang

Aircraft Design amp; Research Institute， Aviation Industry Corporation of China， Co.， Ltd.， Shenyang 110035， China;

3. Beijing Aerospace Times Laser Inertial Technology Co.， Ltd， Beijing 100094， China）

Abstract： In the airborne inertial navigation system （INS）/millimeter wave radar integrated navigation system， the positioning accuracy would be decreased due to the random constant error contained in the angle measurement of millimeter wave radar. Aiming at this problem， a kind of adaptive dual-mode switching INS/millimeter wave radar integrated navigation method based on cubature Kalman filter （CKF） algorithm is proposed. The integrated navigation process is divided into the slant range and angular position matching stage and the relative position vector matching stage， and meanwhile the modeling method is improved in order to separate the position error caused by the angle measurement constant error from the combined error. In the navigation primary stage， the slant range and angular position matching mode is adopted. Aiming at the filtering divergence problem caused by close range， mode switching method based on sliding window variance detection is used to judge the switching conditions in real time. Once the switch condition is satisfied， the integrated mode automatically switched to the relative position vector matching mode. The proposed algorithm is simulated on the background of airborne landing. Simulation results show that the proposed method effectively improves the precision of integrated navigation， and has certain engineering significance.

Keywords： airborne inertial navigation system （INS）; millimeter wave radar; integrated navigation; dual-mode switching

0 引 言

慣性導(dǎo)航系統(tǒng)是一種完全自主的導(dǎo)航系統(tǒng)，且其具有導(dǎo)航信息全面、短時(shí)精度高、輸出頻率高等優(yōu)點(diǎn)，在各領(lǐng)域均獲得了廣泛的應(yīng)用。然而，由于慣性導(dǎo)航系統(tǒng)是積分系統(tǒng)，其誤差隨時(shí)間振蕩發(fā)散^［1-2］，因此常采用與其他導(dǎo)航系統(tǒng)進(jìn)行信息融合的方式來(lái)提升組合導(dǎo)航系統(tǒng)性能。對(duì)機(jī)載導(dǎo)航設(shè)備來(lái)說(shuō)，目前最常用的組合方式為慣性/衛(wèi)星組合，但衛(wèi)星導(dǎo)航系統(tǒng)易受到屏蔽和干擾，最終導(dǎo)致組合導(dǎo)航系統(tǒng)失效。在此情況下，需要尋找其他導(dǎo)航設(shè)備來(lái)輔助慣導(dǎo)以保持長(zhǎng)航時(shí)高精度導(dǎo)航輸出^［3-5］。

毫米波雷達(dá)是近年來(lái)比較成熟的空中目標(biāo)跟蹤定位設(shè)備，其作用范圍在基準(zhǔn)站方圓數(shù)十千米，測(cè)距、測(cè)角精度較高，同時(shí)可結(jié)合事先裝訂的基準(zhǔn)站位置及方位信息得到目標(biāo)機(jī)的定位信息。該信息可通過(guò)無(wú)線電上行鏈路發(fā)送給目標(biāo)機(jī)，具有輸出頻率高的特點(diǎn)^［6-9］。

目前，在慣性導(dǎo)航系統(tǒng)/雷達(dá)組合導(dǎo)航系統(tǒng)中，一般僅對(duì)慣性導(dǎo)航系統(tǒng)誤差進(jìn)行建模估計(jì)，雷達(dá)信息僅作為量測(cè)量使用。對(duì)雷達(dá)量測(cè)量的構(gòu)建方式主要有兩種：一是利用雷達(dá)輸出的地球坐標(biāo)系下的位置信息構(gòu)建量測(cè)量^［10-14］;二是直接采用雷達(dá)輸出的斜距及角度信息作為量測(cè)量^［15-16］。由于雷達(dá)輸出的角度信息含有常值誤差，這將導(dǎo)致雷達(dá)定位誤差隨相對(duì)距離增大而增大。而對(duì)于現(xiàn)有組合導(dǎo)航模式而言，組合精度主要取決于雷達(dá)直接測(cè)量精度，不論采用何種量測(cè)構(gòu)建方式，組合誤差中均存在與相對(duì)距離成正比的誤差項(xiàng)。此外，對(duì)于第二種量測(cè)模型，由于計(jì)算角度所用的距離量均含有誤差，當(dāng)斜距較小時(shí)，可能會(huì)存在由距離信息信噪比過(guò)低導(dǎo)致的角度計(jì)算誤差大的問(wèn)題，最終導(dǎo)致濾波發(fā)散^［17-20］。

針對(duì)上述問(wèn)題，為削弱雷達(dá)測(cè)角常值誤差的影響、提升組合導(dǎo)航精度，本文提出一種基于雙模式自適應(yīng)切換的慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合導(dǎo)航方法。在導(dǎo)航過(guò)程的第一階段采用雷達(dá)輸出的斜距及角位置作為量測(cè)量，同時(shí)對(duì)雷達(dá)角位置常值誤差建模為狀態(tài)量進(jìn)行實(shí)時(shí)估計(jì)，從而將該項(xiàng)誤差導(dǎo)致的位置誤差分離出來(lái)。待角位置誤差收斂后，對(duì)雷達(dá)輸出角度進(jìn)行修正，并將量測(cè)量切換為相對(duì)位置矢量，從而避免了因角度計(jì)算奇異導(dǎo)致的濾波發(fā)散問(wèn)題。針對(duì)所建立模型的非線性特性，采用簡(jiǎn)化容積卡爾曼濾波（simplified cubature Kalman filter， SCKF）進(jìn)行組合導(dǎo)航算法仿真。仿真結(jié)果表明，該方法能顯著提升組合導(dǎo)航算法穩(wěn)定性與導(dǎo)航系統(tǒng)精度，具有一定的工程應(yīng)用價(jià)值。

1 基于自適應(yīng)雙模式切換的慣性導(dǎo)航系統(tǒng)/

毫米波雷達(dá)組合方案設(shè)計(jì)

針對(duì)毫米波雷達(dá)測(cè)角隨機(jī)常值誤差帶來(lái)的組合導(dǎo)航系統(tǒng)精度損失，為實(shí)現(xiàn)精度提升并防止濾波奇異，本文設(shè)計(jì)了斜距角位置匹配與相對(duì)位置矢量匹配雙模式自適應(yīng)切換的慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合方案，如圖1所示。

所設(shè)計(jì)的組合方案分為兩個(gè)階段。第一階段采用斜距、角位置匹配組合模式，并對(duì)組合導(dǎo)航系統(tǒng)建模方法進(jìn)行了改進(jìn)設(shè)計(jì)。在對(duì)慣性導(dǎo)航系統(tǒng)誤差建模的基礎(chǔ)上，增加雷達(dá)測(cè)量高低角、方位角常值誤差作為狀態(tài)量進(jìn)行估計(jì)，將由測(cè)角常值誤差導(dǎo)致的定位誤差從總誤差中分離出來(lái)，組合精度不完全依賴于雷達(dá)直接測(cè)量精度，達(dá)到提升組合導(dǎo)航精度的目的。

在第一階段組合過(guò)程中，實(shí)時(shí)對(duì)雷達(dá)測(cè)角誤差估值收斂程度進(jìn)行判定。估值收斂后，轉(zhuǎn)入組合導(dǎo)航第二階段。在此階段，對(duì)雷達(dá)輸出高低角、方位角進(jìn)行修正，并將修正后的雷達(dá)輸出信息轉(zhuǎn)換為相對(duì)位置矢量，采用相對(duì)位置匹配模式進(jìn)行組合濾波。組合模式的切換能夠有效避免近距情況下因角度計(jì)算誤差大導(dǎo)致的濾波奇異問(wèn)題。

2 兩種模式下慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合導(dǎo)航模型設(shè)計(jì)

本文設(shè)計(jì)的慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合方案中包含兩種組合模式，每種模式對(duì)應(yīng)一種信息融合濾波模型，本節(jié)將分別對(duì)兩種組合模式下的模型設(shè)計(jì)方法進(jìn)行闡述。由于導(dǎo)航參數(shù)的表達(dá)涉及多種坐標(biāo)系，在介紹模型設(shè)計(jì)方法之前，首先對(duì)所需坐標(biāo)系定義進(jìn)行說(shuō)明。

2.1 坐標(biāo)系定義

慣性坐標(biāo)系（i系）：以地心O_e為原點(diǎn)，O_eZ_i軸沿地軸指向北極方向，O_eX_i軸和O_eY_i軸在赤道平面內(nèi)。該兩軸具體指向沒(méi)有明確要求，三軸關(guān)系需符合右手定則。

地球坐標(biāo)系（e系）：以地心O_e為原點(diǎn)，O_eZ_e軸與O_eZ_i軸重合，O_eX_e軸和O_eY_e軸在赤道平面內(nèi)，O_eX_e軸指向格林威治經(jīng)線，O_eY_e軸指向東經(jīng)90°方向。

導(dǎo)航坐標(biāo)系（n系）：該坐標(biāo)系為慣性導(dǎo)航系統(tǒng)的導(dǎo)航基準(zhǔn)，本文選取東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系，即以慣性導(dǎo)航質(zhì)心O為原點(diǎn)，OX_n、OY_n、OZ_n軸分別指向東、北、天方向。

慣性測(cè)量坐標(biāo)系（b系）：以慣性導(dǎo)航質(zhì)心O為原點(diǎn)，OX_b軸沿慣性導(dǎo)航橫軸指向右，OY_b軸與OX_b軸垂直，沿慣性導(dǎo)航縱軸指向前，OZ_b軸垂直于OX_bY_b平面指向上。

2.2 削弱雷達(dá)測(cè)角常值誤差影響的組合導(dǎo)航建模方法

2.2.1 狀態(tài)方程改進(jìn)設(shè)計(jì)

為削弱毫米波雷達(dá)測(cè)角隨機(jī)常值誤差對(duì)定位精度的影響，對(duì)組合導(dǎo)航建模方法進(jìn)行了改進(jìn)設(shè)計(jì)。在對(duì)慣導(dǎo)誤差建模的基礎(chǔ)上，狀態(tài)方程中增加對(duì)雷達(dá)測(cè)角常值誤差的建模，以將該誤差導(dǎo)致的定位誤差分離出來(lái)，達(dá)到提升組合精度的目的。同時(shí)，由于慣性導(dǎo)航高度和天向速度通?？赏ㄟ^(guò)阻尼計(jì)算得到，組合導(dǎo)航時(shí)僅對(duì)水平通道導(dǎo)航誤差進(jìn)行建模。綜上，所選取的狀態(tài)變量為：

X₁=［?_e，?_n，?_u，δv_e，δv_n，δλ，δL，ε_x，ε_y，ε_z，

Δ_x，Δ_y，Δ_z，e_θ，e_φ］^T（1）

式中：?_e，?_n，?_u為姿態(tài)誤差角;δv_e，δv_n為東向速度誤差、北向速度誤差;δλ，δL為經(jīng)度誤差、緯度誤差;ε_x，ε_y，ε_z和Δ_x，Δ_y，Δ_z分別為b系三軸陀螺儀和加速度計(jì)隨機(jī)常值零偏;e_θ，e_φ為雷達(dá)測(cè)量的高低角常值誤差、方位角常值誤差。

基于慣性導(dǎo)航誤差方程以及所建模誤差的常值特性，可建立組合導(dǎo)航系統(tǒng)的狀態(tài)方程如下：

0_m×n表示m行n列零矩陣;Cⁿ_b為姿態(tài)矩陣;G₁為系統(tǒng)噪聲陣;W為系統(tǒng)噪聲。其中，

ω_ie為地球自轉(zhuǎn)角速度，ω_ie=15.04 107°/h;v_E，v_N，v_U為n系三軸速度;L表示緯度;h表示高度;f_E，f_N，f_U為n系三軸比力;R_M，R_N為子午圈半徑和卯酉圈半徑，R_M，R_N可由下式計(jì)算得到：

R_M=R_e（1－2f+3fsin²L）

R_N=R_e（1+fsin²L）

其中，R_e為地球長(zhǎng)軸半徑;f為地球扁率。若采用2000國(guó)家大地坐標(biāo)系，R_e=6 378 137 m，f=1/298.257 222 101。

2.2.2 量測(cè)方程建立

由慣性導(dǎo)航系統(tǒng)解算的載機(jī)位置信息和雷達(dá)基準(zhǔn)站位置可計(jì)算得到載機(jī)在雷達(dá)基準(zhǔn)站位置地理坐標(biāo)系下的斜距、高低角和方位角。由雷達(dá)基準(zhǔn)站位置和慣性導(dǎo)航系統(tǒng)輸出的載機(jī)位置信息可分別計(jì)算得到雷達(dá)基準(zhǔn)站和載機(jī)在地球坐標(biāo)系下的坐標(biāo)：

式中：λ_r，L_r，h_r為雷達(dá)基準(zhǔn)站經(jīng)度、緯度、高度;N_r為雷達(dá)基準(zhǔn)站處卯酉圈曲率半徑，N_r=R_e（1+fsin²L_r）;λ_I，L_I，h_I為慣性導(dǎo)航系統(tǒng)輸出的載機(jī)經(jīng)度、緯度、高度;N_p為載機(jī)處卯酉圈曲率半徑，N_p=R_e（1+fsin²（L_I－δL））;e為地球偏心率，e²=f（2－f）。若采用2000國(guó)家大地坐標(biāo)系，e=0.081 819 191 042 8。

由式（3）和式（4）可得雷達(dá)處地理坐標(biāo)系下載機(jī)與雷達(dá)相對(duì)位置矢量：

由式（5）可計(jì)算得到載機(jī)相對(duì)雷達(dá)的斜距，以及在雷達(dá)地理坐標(biāo)系下的高低角和方位角：

R=Rⁿ²x+Rⁿ²y+Rⁿ²z（6）

θ=arctanRⁿ_zRⁿ²x+Rⁿ²y（7）

φ=arctanRⁿ_xRⁿ_y（8）

選取雷達(dá)輸出的斜距及角位置為量測(cè)量，則量測(cè)方程為

式中：e_θ為雷達(dá)高低角隨機(jī)常值誤差;e_φ為雷達(dá)方位角隨機(jī)常值誤差;V₁為量測(cè)噪聲。

2.3 防止奇異的組合導(dǎo)航建模方法

若全程采用第2.2節(jié)斜距角位置匹配方式進(jìn)行組合導(dǎo)航，當(dāng)載機(jī)與雷達(dá)基準(zhǔn)站相對(duì)高度減小到十幾米時(shí)，由于慣性導(dǎo)航系統(tǒng)阻尼后高度誤差與相對(duì)高度量級(jí)相當(dāng)，采用相對(duì)高度計(jì)算角位置會(huì)產(chǎn)生較大的誤差，最終導(dǎo)致濾波發(fā)散。

針對(duì)近距情況下濾波發(fā)散的問(wèn)題，可以在雷達(dá)測(cè)角誤差估值收斂后，切換為水平相對(duì)位置匹配模式。在該模式下，無(wú)需利用相對(duì)高度計(jì)算角位置，因此不會(huì)產(chǎn)生濾波奇異的情況。在切換組合模式后，可利用角位置誤差估值對(duì)雷達(dá)輸出的角位置信息進(jìn)行修正，以減小利用雷達(dá)輸出信息計(jì)算得到的相對(duì)位置量測(cè)量的誤差。

在相對(duì)位置匹配組合模式下，僅選擇慣性導(dǎo)航系統(tǒng)誤差作為狀態(tài)量，狀態(tài)向量如下：

X₂=［?_e，?_n，?_u，δv_e，δv_n，δλ，δL，ε_x，ε_y，ε_z，Δx，Δy，Δz］^T（10）

系統(tǒng)的狀態(tài)方程可表示為

選取水平相對(duì)距離作為量測(cè)信息，根據(jù)式（3）～式（5），可得量測(cè)方程為

式中：V₂為量測(cè)噪聲。

3 基于滑動(dòng)窗方差判別的組合模式自適應(yīng)切換方法

組合模式切換的條件為雷達(dá)測(cè)角誤差估值收斂，因此，需要在濾波過(guò)程中對(duì)估值收斂情況進(jìn)行實(shí)時(shí)判定。隨著濾波不斷推進(jìn)，誤差協(xié)方差矩陣會(huì)逐漸變小。當(dāng)其趨于穩(wěn)定時(shí)，可判定濾波收斂。然而，濾波收斂不代表估值收斂可用。因此，本文在判定濾波收斂的基礎(chǔ)上，增加對(duì)估值收斂性的判斷。

在穩(wěn)定濾波過(guò)程中，估值產(chǎn)生的數(shù)據(jù)序列存在由非平穩(wěn)向平穩(wěn)的過(guò)渡過(guò)程?；谠撎攸c(diǎn)，本文直接對(duì)濾波估值的平穩(wěn)性進(jìn)行判定，采用滑動(dòng)窗方差判別法來(lái)判定估值收斂情況。

定義協(xié)方差比例因子β_i（k）=P_i，i（k）/P_i，i（k－1）來(lái)表征協(xié)方差矩陣的收斂程度，當(dāng)協(xié)方差矩陣趨于穩(wěn)定時(shí)，β_i（k）接近1。設(shè)定較小閾值ε_i，當(dāng)連續(xù)N次濾波均滿足|β_i（k）－1|≤ε_i時(shí)，可判定濾波收斂。其中k表示濾波次數(shù);P_i，i表示協(xié)方差矩陣中測(cè)角誤差狀態(tài)量所對(duì)應(yīng)的元素;i=14，15，分別代表雷達(dá)高低角、方位角誤差估值。在組合濾波過(guò)程中，實(shí)時(shí)存儲(chǔ)測(cè)角誤差濾波估值。設(shè)定窗口N，當(dāng)數(shù)據(jù)序列長(zhǎng)度等于N時(shí)，計(jì)算數(shù)據(jù)序列方差值D_i。設(shè)定閾值d_i，當(dāng)D_ilt;d_i時(shí)，可認(rèn)為數(shù)據(jù)序列已達(dá)到平穩(wěn)，誤差估值收斂。

在濾波收斂并且雷達(dá)高低角、方位角誤差估值均收斂后，對(duì)雷達(dá)輸出進(jìn)行修正，切換組合模式。此外，若載機(jī)高度小于50 m時(shí)仍未滿足收斂條件，則直接切換組合模式?；诨瑒?dòng)窗方差判別的組合模式自適應(yīng)切換流程圖如圖2所示。

4 基于SCKF的組合導(dǎo)航信息融合方法

適用于非線性系統(tǒng)信息融合的典型濾波方法有擴(kuò)展卡爾曼濾波^［21］（extended Kalman filter， EKF）、無(wú)跡卡爾曼濾波^［22］（unscented Kalman filter， UKF）、中心差分卡爾曼濾波^［23］（central difference Kalman filter， CDKF）、容積卡爾曼濾波（cubature Kalman filter， CKF）等^［24-25］。其中，CKF是一種新型高斯濾波算法，相比其他非線性濾波器具有更高的非線性逼近性能和估計(jì)精度^［26-27］。

CKF基本容積點(diǎn)及其對(duì)應(yīng)的權(quán)值按如下規(guī)則^［28］選?。?/p>

式中，ξ_i為容積點(diǎn)集;ε_i為對(duì)應(yīng)的權(quán)值;n為系統(tǒng)狀態(tài)維數(shù);［1］_i表示第i個(gè)容積點(diǎn)。舉例說(shuō)明，2維系統(tǒng)的容積點(diǎn)集為：

對(duì)于狀態(tài)方程和量測(cè)方程均為非線性的系統(tǒng)，使用CKF算法進(jìn)行信息融合需要分別在狀態(tài)更新和量測(cè)更新過(guò)程中進(jìn)行兩次容積點(diǎn)采樣及非線性容積變換，這將在很大程度上影響算法的實(shí)時(shí)性。而對(duì)于具有線性狀態(tài)方程或線性量測(cè)方程的非線性系統(tǒng)，CKF算法可以進(jìn)行相應(yīng)的簡(jiǎn)化。

第2節(jié)中所建立兩種組合導(dǎo)航模型可統(tǒng)一表示為如下形式：

可見(jiàn)，本文所建立模型的狀態(tài)方程具有線性特性，可對(duì)CKF狀態(tài)更新過(guò)程進(jìn)行簡(jiǎn)化，即采用常規(guī)卡爾曼濾波的狀態(tài)更新方式?；诖?，在不降低組合精度的前提下，為提升算法實(shí)時(shí)性，本文采用狀態(tài)更新過(guò)程SCKF算法進(jìn)行組合導(dǎo)航信息融合，具體步驟^［29-30］如下。

步驟 1 濾波初始化

x-₀=E（x₀）

P₀=E［（x₀－x-₀）（x₀－x-₀）^T］

步驟 2 狀態(tài)更新

計(jì)算狀態(tài)一步預(yù)測(cè)值x^_k/k－1和一步預(yù)測(cè)誤差協(xié)方差矩陣P_k/k－1：

x^_k/k－1=Φx^_k-1/k－1

P_k/k－1=ΦP_k－1/k－1Φ^T+ΓQ_k－1Γ^T

步驟 3 量測(cè)更新

對(duì)P_k/k－1進(jìn)行Cholesky分解，并對(duì)x^_k/k－1進(jìn)行容積點(diǎn)采樣：

S_k/k－1=Chol（P_k/k－1）

X_{i，k/k－1}=S_k/k－1ξ_i+x^_k/k－1， i=1，2，…，2n

利用量測(cè)方程容積點(diǎn)進(jìn)行變換：

Z_{i，k/k－1}=h（X_i，k/k－1）

計(jì)算量測(cè)一步預(yù)測(cè)值z(mì)^_k/k－1、量測(cè)預(yù)測(cè)協(xié)方差矩陣P_{zz，k/k－1}以及互協(xié)方差矩陣P_{xz，k/k－1}：

z^_k/k－1=12n∑2ni=1Z_i，k/k－1

P_{zz，k/k－1}=12n∑2ni=1Z_{i，k/k－1}Z^T_i，k/k－1－z^_k/k－1z^^T_k/k－1+R_k

P_{xz，k/k－1}=12n∑2ni=1X_i，k/k－1Z^T_i，k/k－1－x^_k/k－1z^^T_k/k－1

計(jì)算增益矩陣K_k、狀態(tài)更新值x^_k/k和協(xié)方差陣更新值P_k/k：

K_k=P_{xz，k/k－1}P^－1_zz，k/k－1

x^_k/k=x^_k/k－1+K_k（z_k－z^_k/k－1）

P_k/k=P_k/k－1－K_kP_{zz，k/k－1}K^T_k

5 仿真驗(yàn)證

5.1 仿真條件設(shè)定

以載機(jī)著陸為背景進(jìn)行算法仿真驗(yàn)證。毫米波雷達(dá)在載機(jī)距離基準(zhǔn)站15 km時(shí)開(kāi)始輸出有效測(cè)量信息，載機(jī)降落過(guò)程歷時(shí)3～5 min?；诖耍奢d機(jī)飛行軌跡，如圖3所示。

在仿真時(shí)，設(shè)定慣性儀表誤差參數(shù)如下：

設(shè)置陀螺儀常值零偏為0.008°/h，隨機(jī)游走誤差為0001°/h，加速度計(jì)常值零偏為5×10^-5 g，隨機(jī)噪聲為5×10^-6 g。

設(shè)置毫米波雷達(dá)系統(tǒng)相關(guān)參數(shù)如下：設(shè)置斜距測(cè)量精度為5 m（1σ）;高低角、方位角測(cè)量白噪聲為0.03°（1σ）;數(shù)據(jù)輸出頻率為40 Hz。

在上述條件下，分別設(shè)定角位置測(cè)量常值誤差為003°、0.06°、0.1°進(jìn)行仿真。為更加充分地驗(yàn)證所設(shè)計(jì)算法性能，在與毫米波雷達(dá)數(shù)據(jù)組合之前，慣導(dǎo)系統(tǒng)運(yùn)行在純慣性狀態(tài)，未進(jìn)行任何修正。

5.2 雙模式切換組合導(dǎo)航方法性能仿真分析

在第5.1節(jié)所設(shè)定仿真條件下對(duì)本文所提算法進(jìn)行仿真，組合導(dǎo)航誤差曲線及角誤差估計(jì)曲線如圖4和圖5所示，精度統(tǒng)計(jì)結(jié)果如表1所示。由于濾波均存在收斂過(guò)程，表1中組合導(dǎo)航精度是基于組合導(dǎo)航10 s之后的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)的。

通過(guò)圖4和表1可以看出，本文所提雙模式切換慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合導(dǎo)航方法濾波收斂快、精度高，不存在與斜距成正比的斜率誤差項(xiàng)。這是因?yàn)樵摲椒軌蚩焖?、?zhǔn)確地估計(jì)出雷達(dá)測(cè)角常值誤差，將該誤差造成的位置誤差與雷達(dá)直接測(cè)量誤差分離開(kāi)來(lái)，從而提升組合導(dǎo)航精度。

5.3 與現(xiàn)有算法對(duì)比仿真分析

為進(jìn)一步說(shuō)明本文所提方法的優(yōu)越性，在相同仿真條件下，采用地球系下位置信息匹配模型以及斜距角位置匹配模型分別進(jìn)行組合導(dǎo)航解算，精度統(tǒng)計(jì)結(jié)果列于表2中，組合導(dǎo)航誤差曲線如圖6和圖7所示。為便于對(duì)比分析，將本文所提方法精度提升統(tǒng)計(jì)結(jié)果一同列于表2中。同樣地，現(xiàn)有算法組合導(dǎo)航精度也是基于組合導(dǎo)航10 s之后的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)的。

表2中的本文方法精度提升百分比（%）的含義為，本文方法相較于其他方法的精度提升量與其他方法精度的比值。以雷達(dá)測(cè)角常值誤差為0.03°時(shí)的東速誤差為例，本文方法精度為0.06，地球系下位置信息匹配方法精度為0.07，則本文方法精度提升百分比為（0.07-0.06）/0.07=14.3%。

從圖6可以看出，在地球系及位置信息匹配組合模式下，組合位置誤差隨斜距增大而增大，位置誤差的斜率會(huì)進(jìn)一步導(dǎo)致速度誤差的偏移，組合速度、位置誤差隨雷達(dá)測(cè)角誤差線性增長(zhǎng)。導(dǎo)致這些誤差的根本原因就是雷達(dá)測(cè)角常值誤差。對(duì)于斜距角位置匹配組合方法，同樣存在組合導(dǎo)航誤差隨斜距增大而增大的情況，這說(shuō)明采用雷達(dá)原始輸出做觀測(cè)量無(wú)法消除雷達(dá)測(cè)角常值誤差帶來(lái)的與相對(duì)距離成正比的誤差項(xiàng)。同時(shí)可以看出，在組合導(dǎo)航180 s左右，組合導(dǎo)航誤差開(kāi)始增大，出現(xiàn)了濾波發(fā)散的情況。這是由于此時(shí)載機(jī)與雷達(dá)基準(zhǔn)站相對(duì)高度較小，僅為十幾米，而慣性導(dǎo)航系統(tǒng)阻尼后高度誤差也為十米量級(jí)。此時(shí)利用相對(duì)高度計(jì)算角位置會(huì)產(chǎn)生較大誤差，使得濾波新息產(chǎn)生奇異值，最終導(dǎo)致濾波發(fā)散。近距時(shí)，濾波發(fā)散與是否對(duì)雷達(dá)測(cè)角常值誤差進(jìn)行建模估計(jì)無(wú)關(guān)，僅與角位置匹配方式以及慣性導(dǎo)航系統(tǒng)高度通道特性及其外部阻尼源精度有關(guān)。

而本文所提方法對(duì)雷達(dá)測(cè)角誤差進(jìn)行了建模估計(jì)，故而將該項(xiàng)導(dǎo)致的位置、速度誤差分離出來(lái)，組合位置誤差中不存在與斜距成正比的斜率誤差。同時(shí)，本文所提方法在角位置誤差估值收斂之后或相對(duì)高度小于50 m時(shí)，自動(dòng)切換為水平相對(duì)距離匹配模式，可避免由于相對(duì)高度過(guò)小導(dǎo)致的濾波發(fā)散。從表2可以看出，相較于兩種現(xiàn)有算法，本文所提方法大幅提升了組合導(dǎo)航精度。

5.4 雙模式切換決策風(fēng)險(xiǎn)仿真分析

本文所提方法模式切換的條件存在兩種情況：一是濾波收斂且測(cè)角誤差估值收斂;二是相對(duì)高度小于50 m。在第一種情況下，會(huì)利用雷達(dá)測(cè)角誤差估值將測(cè)角信息修正后再計(jì)算相對(duì)位置矢量，從而提升相對(duì)位置矢量匹配模式組合導(dǎo)航精度。若未滿足收斂條件，則會(huì)在相對(duì)高度小于50 m時(shí)進(jìn)行切換，此時(shí)不會(huì)對(duì)雷達(dá)測(cè)角信息進(jìn)行修正。但在相對(duì)高度小于50 m時(shí)，斜距也已經(jīng)很小，此時(shí)切換為相對(duì)位置矢量匹配組合模式，組合精度所受影響較小。

在第5.1節(jié)所設(shè)定仿真條件下，對(duì)滿足相對(duì)高度小于50 m時(shí)切換組合模式的情況進(jìn)行仿真（人為設(shè)置該條件，實(shí)際濾波及估值均已收斂），此時(shí)不對(duì)雷達(dá)測(cè)角誤差進(jìn)行反饋修正。圖8為組合導(dǎo)航誤差曲線，表3為組合導(dǎo)航精度統(tǒng)計(jì)。通過(guò)圖8和表3可以看出，切換模式后組合精度可以保持原有水平。

需要說(shuō)明的是，若雷達(dá)測(cè)量信息正常，雷達(dá)測(cè)角常值誤差估值均可收斂，一般不存在無(wú)法反饋修正的情況。在模式切換條件中增加對(duì)相對(duì)高度的判斷，目的是為提升算法魯棒性。本文所提方法通過(guò)對(duì)組合導(dǎo)航系統(tǒng)建模進(jìn)行改進(jìn)設(shè)計(jì)以及采用雙模式自適應(yīng)切換方式，可顯著提升慣性導(dǎo)航系統(tǒng)/雷達(dá)組合導(dǎo)航精度，在載機(jī)著陸全程中提供高精度組合導(dǎo)航信息。

6 結(jié) 論

在機(jī)載慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合導(dǎo)航系統(tǒng)中，毫米波雷達(dá)測(cè)角隨機(jī)常值誤差會(huì)帶來(lái)精度損失。針對(duì)該問(wèn)題，本文提出一種基于CKF算法的雙模式自適應(yīng)切換慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合導(dǎo)航方法。所設(shè)計(jì)組合方案分為斜距角位置匹配與相對(duì)位置矢量匹配兩個(gè)階段，并對(duì)組合導(dǎo)航系統(tǒng)建模方法進(jìn)行了改進(jìn)設(shè)計(jì)，將由雷達(dá)測(cè)角常值誤差導(dǎo)致的定位誤差從組合誤差中分離出來(lái)，提升了組合導(dǎo)航精度。組合過(guò)程中采用基于滑動(dòng)窗方差判別的模式切換方法進(jìn)行實(shí)時(shí)判定，判別滿足條件后自動(dòng)切換為相對(duì)位置矢量匹配模式，并利用所估計(jì)的測(cè)角誤差校正雷達(dá)輸出，解決了近距情況下由角度計(jì)算誤差大導(dǎo)致的濾波發(fā)散問(wèn)題。為兼顧算法精度與實(shí)時(shí)性，采用SCKF算法進(jìn)行慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)非線性信息融合，通過(guò)仿真驗(yàn)證了算法的有效性。仿真結(jié)果表明，本文所提慣性導(dǎo)航系統(tǒng)/毫米波組合導(dǎo)航方法在很大程度上提升了慣性導(dǎo)航系統(tǒng)/毫米波雷達(dá)組合導(dǎo)航精度，具有較高的工程應(yīng)用價(jià)值。

參考文獻(xiàn)

［1］JIANG S A， WANG B N， XIANG M S， et al. Method for InSAR/INS navigation system based on interferogram matching［J］. IET Radar， 2018， 12（9）： 938-944.

［2］JIANG S A， WANG B N， XIANG M S， et al. On designing PMI Kalman filter for INS/GPS integrated systems with unknown sensor errors［J］. IEEE Sensors Journal， 2015， 15（1）： 535-544.

［3］HU G G， GAO B B， ZHONG Y M， et al. Unscented Kalman filter with process noise covariance estimation for vehicular INS/GPS integration system［J］. Information Fusion， 2020， 64： 194-204.

［4］WANG G C， XU X S， ZHANG T. M-M estimation-based robust cubature Kalman filter for INS/GPS integrated navigation system［J］. IEEE Trans.on Instrumentation and Measurement， 2021， 70： 1511-1522.

［5］LIU Y H， LUO Q S， ZHOU Y M. Deep learning-enabled fusion to bridge GPS outages for INS/GPS integrated navigation［J］. IEEE Sensors Journal， 2022， 24（9）： 8974-8985.

［6］MA Z X， CHOI J， YANG L， et al. Structural displacement estimation using accelerometer and FMCW millimeter wave radar［J］. Mechanical Systems and Signal Processing， 2022， 182： 109582.

［7］WANG C M， ZHAO X H， LI Z. DCS-CTN： subtle gesture recognition based on TD-CNN-transformer via millimeter wave radar［J］. IEEE Internet of Things Journal， 2023， 10（20）： 17680-17693.

［8］CAO Z P， DING W， CHEN R H， et al. A joint global-local network for human pose estimation with millimeter wave radar［J］. IEEE Internet of Things Journal， 2023， 10（1）： 434-446.

［9］ZHANG Z Y， QIU Z R， HU W C， et al. Research on wingtip distance measurement of high-speed coaxial helicopter based on 77 GHz FMCW millimeter-wave radar［J］. Measurement Science and Technology， 2023， 34（8）： 085111.

［10］ELKHOLY M， ELSHEIKH M， El-SHEIMY N. Radar/INS integration and map matching for land vehicle navigation in urban environments［J］. Sensors， 2023， 23： 5119.

［11］ELKHOLY M， ELSHEIKH M， El-SHEIMY N. Analysis and application of geometric dilution of precision based on altitude-assisted INS/SAR integrated navigation［J］. IEEE Trans.on Instrumentation and Measurement， 2021， 70： 9501010.

［12］MENG Y， WANG W， HAN H， et al. A vision/radar/INS integrated guidance method for shipboard landing［J］. IEEE Trans.on Industrial Electronics， 2019， 66（11）： 8803-8810.

［13］吳楓， 秦永元， 張金亮. 極區(qū)內(nèi)基于雷達(dá)輔助的SINS空中對(duì)準(zhǔn)算法［J］. 西北工業(yè)大學(xué)學(xué)報(bào)， 2014， 32（1）： 131-136.

WU F， QIN Y Y， ZHANG J L. In-flight alignment algorithm for radar aided SINS in polar regions［J］. Journal of Northwestern Polytechnical University， 2014， 32（1）： 131-136.

［14］SINGH B， MENGHAL P M， SURI N. Integration of INS and GPS for radar aided inertial navigation system［C］∥Proc.of the IEEE 7th International Conference for Convergence in Technology， 2022.

［15］徐博， 王連釗， 吳雯昊， 等. 基于雷達(dá)測(cè)距與角位置輔助的SINS 空中對(duì)準(zhǔn)方法［J］. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2019， 27（5）： 573-579.

XU B， WANG L Z， WU W H， et al. SINS alignment method in air based on slant and angle position assisted by radar［J］. Journal of Chinese Inertial Technology， 2019， 27（5）： 573-579.

［16］趙薇， 王楠， 高顯忠， 等. 基于增廣粒子濾波算法的毫米波雷達(dá)/GPS/INS組合導(dǎo)航方法研究［J］. 光電與控制， 2016， 23（6）： 54-59.

ZHAO W， WANG N， GAO X Z， et al. Millimeter-wave radar/GPS/INS integrated navigation system and its application in UAV autonomous landing［J］. Electronics Optics amp; Control， 2016， 23（6）： 54-59.

［17］ZENG Y H， ZHAO J， HAN H， et al. INS error correction method based on passive radar angle tracking information［C］∥Proc.of the 2nd International Conference on Frontiers of Sensors Technologies， 2017： 417-420.

［18］ELKHOLY M， ELSHEIKH M， El-SHEIMY N. Radar/INS integration for pose estimation in GNSS-denied environments［C］∥Proc.of the International Archives of the Photogrammetry， Remote Sensing and Spatial Information Sciences， 2022： 137-142.

［19］周峰， 梁祿揚(yáng)， 林平. 協(xié)方差交叉融合的慣性/衛(wèi)星/雷達(dá)組合導(dǎo)航研究［J］. 航天控制， 2021， 39（4）： 22-27.

ZHOU F， LIANG L Y， LIN P. Research on INS/GNSS/radar integrated navigation with covariance intersection fusion［J］. Aerospace Control， 2021， 39（4）： 22-27.

［20］KLEIN K T J， UYSAL F， CUENCA M C， et al. Motion estimation and improved SAR imaging for agile platforms using omnidirectional radar and INS sensor fusion［J］. IEEE Trans.on Aerospace and Electronic Systems， 2023， 59（1）： 153- 171.

［21］XU J L， JU Y S. Brushless DC motor control research based on extended Kalman filter algorithm［J］. Proceedings of SPIE， 2022： 574-578.

［22］WU Q Y， JIA Q Z， SHAN J Y， et al. Angular velocity estimation based on adaptive simplified spherical simplex unscented Kalman filter in GFSINS［J］. Proc IMechE Part G： J Aerospace Engineering， 2014， 228（8）： 1375-1388.

［23］GONG X L， DING X S. Adaptive CDKF based on gradient descent with momentum and its application to POS［J］. IEEE Sensors Journal， 2021， 21（14）： 16201-16212.

［24］WANG J W， CHEN X Y， LIU J G， et al. A robust backtracking CKF based on Krein space theory for in-motion alignment process［J］. IEEE Trans.on Intelligent Transportation Systems， 2023， 24（2）： 1909-1925.

［25］MA Y， LI M Y. A neural network aided CKF algorithm for SINS/GN integrated navigation system［C］∥Proc.of the International Conference on Guidance， Navigation and Control， 2020： 1637-1648.

［26］ARASARATNAM I， HAYKIN S. Cubature Kalman filters［J］. IEEE Trans.on Automatic Control， 2009， 54（6）： 1254-1269.

［27］LI D G， WU Y Q， ZHAO J M. Novel hybrid algorithm of improved CKF and GRU for GPS/INS［J］. IEEE Access， 2020， 8： 202836-202847.

［28］QIAO M Y， GAO Y F， LI W N， et al. Attitude estimation of MEMS-IMU based on fuzzy robust adaptive CKF algorithm［J］. Journal of Chinese Inertial Technology， 2022， 30（3）： 296-303.

［29］LU H， HAO S Y， PENG Z Y. Simplified 5th-degree multiple fading CKF for GPS/INS integrated navigation system［C］∥Proc. of the 37th Chinese Control Conference， 2018： 4938-4945.

［30］石桂欣， 鄢社鋒， 劉宇. 水下目標(biāo)跟蹤的改進(jìn)非線性濾波快速算法［J］. 應(yīng)用聲學(xué)， 2020， 39（1）： 90-96.

SHI G X， YAN S F， LIU Y. Improved simplified unscented/cubature Kalman filter algorithm for underwater target tracking system［J］. Journal of Applied Acoustics， 2020， 39（1）： 90-96.

作者簡(jiǎn)介

張 冬（1981—），男，研究員，博士研究生，主要研究方向?yàn)殡娮有畔ⅰw行感知、飛行控制。

邢福逸（1985—），男，研究員，碩士，主要研究方向?yàn)閼T性導(dǎo)航、組合導(dǎo)航。

徐允鶴（1983—），男，高級(jí)工程師，碩士，主要研究方向?yàn)轱w行感知、飛行控制。

錢 鵬（1990—），男，高級(jí)工程師，碩士，主要研究方向?yàn)轱w行器設(shè)計(jì)、飛行感知。