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

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

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

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

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

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

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

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

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

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

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

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

2.1 坐標(biāo)系定義

慣性坐標(biāo)系（i系）：以地心O_e為原點(diǎn)，O_eZ_i軸沿地軸指向北極方向，O_eX_i軸和O_eY_i軸在赤道平面內(nè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軸分別指向東、北、天方向。

慣性測量坐標(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á)測角常值誤差影響的組合導(dǎo)航建模方法

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

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

基于慣性導(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為地球長軸半徑;f為地球扁率。若采用2000國家大地坐標(biāo)系，R_e=6 378 137 m，f=1/298.257 222 101。

2.2.2 量測方程建立

由慣性導(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國家大地坐標(biāo)系，e=0.081 819 191 042 8。

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

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

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

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

φ=arctanRⁿ_xRⁿ_y（8）

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

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

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

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

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

在相對位置匹配組合模式下，僅選擇慣性導(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)方程可表示為

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

式中：V₂為量測噪聲。

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

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

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

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

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

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

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

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

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

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

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

可見，本文所建立模型的狀態(tài)方程具有線性特性，可對CKF狀態(tài)更新過程進(jìn)行簡化，即采用常規(guī)卡爾曼濾波的狀態(tài)更新方式?；诖?，在不降低組合精度的前提下，為提升算法實(shí)時性，本文采用狀態(tài)更新過程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ù)測值x^_k/k－1和一步預(yù)測誤差協(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 量測更新

對P_k/k－1進(jìn)行Cholesky分解，并對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

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

Z_i，k/k－1=h（X_{i，k/k－1}）

計(jì)算量測一步預(yù)測值z^_k/k－1、量測預(yù)測協(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時開始輸出有效測量信息，載機(jī)降落過程歷時3～5 min?；诖?，生成載機(jī)飛行軌跡，如圖3所示。

在仿真時，設(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è)置斜距測量精度為5 m（1σ）;高低角、方位角測量白噪聲為0.03°（1σ）;數(shù)據(jù)輸出頻率為40 Hz。

在上述條件下，分別設(shè)定角位置測量常值誤差為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è)定仿真條件下對本文所提算法進(jìn)行仿真，組合導(dǎo)航誤差曲線及角誤差估計(jì)曲線如圖4和圖5所示，精度統(tǒng)計(jì)結(jié)果如表1所示。由于濾波均存在收斂過程，表1中組合導(dǎo)航精度是基于組合導(dǎo)航10 s之后的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)的。

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

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

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

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

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

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

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

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

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

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

參考文獻(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空中對準(zhǔn)算法［J］. 西北工業(yè)大學(xué)學(xué)報， 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á)測距與角位置輔助的SINS 空中對準(zhǔn)方法［J］. 中國慣性技術(shù)學(xué)報， 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.

作者簡介

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

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

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

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