基于地球重力場(chǎng)模型的重力匹配數(shù)據(jù)隨機(jī)線性化方法

黃 炎，李?yuàn)檴?，譚勖立，王傲明，范 雕，付林威

（中國(guó)人民解放軍戰(zhàn)略支援部隊(duì)信息工程大學(xué) 地理空間信息學(xué)院，鄭州 450001）

慣性/重力/重力梯度組合導(dǎo)航系統(tǒng)由于其無源、自主、隱蔽性好和不受時(shí)域限制等優(yōu)勢(shì)，成為了水下潛航器定位導(dǎo)航研究的重點(diǎn)[1]。重力匹配算法均是以地形匹配算法為基礎(chǔ)發(fā)展而來，主要匹配算法有：基于相關(guān)分析技術(shù)的地形輪廓匹配（Terrain Contour Matching, TRECOM）算法、迭代最近等值線點(diǎn)（Iterated Closest Contour Point, ICCP）算法和基于遞推濾波技術(shù)的桑迪亞慣性地形輔助導(dǎo)航（Sandia Inertial Terrain Aided Navigation, SITAN）算法[2,3]。經(jīng)過國(guó)內(nèi)外學(xué)者的研究與改進(jìn)，重力匹配導(dǎo)航算法得到了快速發(fā)展，不僅對(duì)上述三種匹配算法進(jìn)行了改進(jìn)，提高了匹配精度，還相繼提出了矢量匹配算法、具有約束條件的匹配算法以及基于人工智能的蜂群匹配算法等[4]。

由于水下潛航器定位導(dǎo)航對(duì)實(shí)時(shí)性的要求較高，目前國(guó)內(nèi)主要利用濾波技術(shù)進(jìn)行重力匹配導(dǎo)航，其中以SITAN算法為基礎(chǔ)的擴(kuò)展Kalman濾波（Extended Kalman Filter, EKF）算法以其實(shí)時(shí)性好、敏感度高的優(yōu)勢(shì)被廣泛研究和應(yīng)用[5]。EKF算法核心技術(shù)由兩部分構(gòu)成，一是重力隨機(jī)線性化技術(shù)、二是Kalman濾波技術(shù)。因此其顯著缺點(diǎn)也與SITAN算法相似，即EKF算法僅是將非線性的觀測(cè)方程在慣導(dǎo)指示位置利用隨機(jī)線性化技術(shù)進(jìn)行線性化處理，而不考慮重力場(chǎng)固有屬性以及隨機(jī)變量的驗(yàn)前誤差，因此會(huì)產(chǎn)生較大的線性化誤差，從而導(dǎo)致濾波性能損失，甚至快速發(fā)散無法得到較好的結(jié)果。

地球重力場(chǎng)模型是用一個(gè)截?cái)嗟接邢揠A次的引力位球諧函數(shù)級(jí)數(shù)來逼近地球重力場(chǎng)[3]，其中，EIGEN-6C4地球重力場(chǎng)模型是一個(gè)不僅包含了LAGEOS、GRACE、GOCE等衛(wèi)星測(cè)高、重力、重力梯度衛(wèi)星數(shù)據(jù)，還包含了實(shí)測(cè)海洋重力數(shù)據(jù)，并在DTU全球重力異常數(shù)據(jù)以及EGM2008地球重力場(chǎng)模型的基礎(chǔ)上，構(gòu)建到了2190階的超高階地球重力場(chǎng)模型。該地球重力場(chǎng)模型包含大量實(shí)測(cè)數(shù)據(jù)，能夠在一定程度上反映真實(shí)地球物理場(chǎng)信息，因此，本文選用EIGEN-6C4地球重力場(chǎng)模型用以反映重力場(chǎng)包含的物理信息，并利用其對(duì)重力和重力梯度進(jìn)行線性化處理。

1 重力EKF匹配導(dǎo)航算法

由于水下潛器航行的隱蔽性需求，重力和重力梯度測(cè)量不需要對(duì)外界發(fā)出信號(hào)，具有較好的無源性，且地球重力場(chǎng)相對(duì)穩(wěn)定，不會(huì)產(chǎn)生較大的波動(dòng)，因此重力和重力梯度輔助慣性導(dǎo)航成為水下潛航器無源自主導(dǎo)航的重要研究?jī)?nèi)容，其流程圖如圖1所示。

圖1 重力匹配EKF算法Fig.1 Gravity matching EKF algorithm

1.1 EKF狀態(tài)方程構(gòu)建

重力/重力梯度信息對(duì)速度誤差的反應(yīng)不夠敏感，因此，本文僅選用 X=(δφ,δλ)T作為濾波狀態(tài)變量，式中δφ、δλ分別表示水下潛航器的緯度誤差和經(jīng)度誤差。

由慣性導(dǎo)航誤差方程和濾波狀態(tài)變量可知，慣性導(dǎo)航系統(tǒng)定位導(dǎo)航位置平面誤差微分方程表達(dá)式如式(1)所示。

式中，ve表示東向速度，RN表示指示位置點(diǎn)卯酉圈曲率半徑，φ表示地心緯度，Wφ與Wλ表示由系統(tǒng)噪聲產(chǎn)生的緯向和經(jīng)向誤差。

將式(1)化簡(jiǎn)為：

若濾波周期為T，則將式(2)進(jìn)行離散化處理后可得：

式中，Φk/k-1表示狀態(tài)轉(zhuǎn)移矩陣，可由系統(tǒng)狀態(tài)矩陣F通過Laplace變換得到 Φk/k-1= I +F＊T。

1.2 EKF量測(cè)方程構(gòu)建

由于地球重力場(chǎng)是一個(gè)與位置有關(guān)的連續(xù)物理場(chǎng)，因此，假設(shè)水下潛航器真實(shí)位置的重力異常和重力梯度（本文均以重力梯度垂直分量Tzz為例）在慣導(dǎo)指示位置點(diǎn)(φi,λi)的鄰域內(nèi)存在一階導(dǎo)數(shù)，利用泰勒級(jí)數(shù)將其展開可得：

式中，φt、λt分別表示潛航器真實(shí)位置點(diǎn)處的緯度和經(jīng)度；vgi與vTi表示泰勒級(jí)數(shù)展開過程中的截?cái)嗾`差，ΔgM(φi,λi)與 TzzM(φi,λi)表示利用慣導(dǎo)指示位置坐標(biāo)在重力異常和重力梯度基準(zhǔn)圖上提取出的重力異常值和重力梯度垂直分量值，Δ g (φt,λt)與 Tzz(φt,λt)則表示水下潛航器真實(shí)位置處的重力異常值和重力梯度垂直分量值。

又因?yàn)樗聺摵狡髯陨泶钶d有重力儀以及重力梯度儀，因此，Δg(φ,λ)與 Tzz(φ,λ)又可以由如式(5)所示。

式中，vgs與vTs表示由于重力儀和重力梯度儀測(cè)量產(chǎn)生的觀測(cè)誤差， Δgs(φt,λt)與 Tzzs(φt,λt)表示由重力儀和重力梯度儀測(cè)量并計(jì)算得到的重力異常和重力梯度垂直分量觀測(cè)值。

由式(4)(5)即可得到重力異常和重力梯度垂直張量組合匹配導(dǎo)航的濾波觀測(cè)方程分別為：

同理亦可得到重力異常與任意重力梯度張量間的組合匹配濾波觀測(cè)方程。

2 基于地球重力場(chǎng)模型的隨機(jī)線性化技術(shù)

2.1 隨機(jī)線性化原理

隨機(jī)線性化技術(shù)原理如圖2所示。

圖2 隨機(jī)線性化原理示意圖Fig.2 Schematic diagram of stochastic linearization principle

重力匹配導(dǎo)航的本質(zhì)是通過重力基準(zhǔn)圖與重力儀實(shí)時(shí)測(cè)量數(shù)據(jù)的匹配實(shí)現(xiàn)對(duì)慣性導(dǎo)航系統(tǒng)的校正，由于地球重力場(chǎng)是關(guān)于位置的非線性函數(shù)。因此在利用Kalman濾波進(jìn)行匹配導(dǎo)航建立量測(cè)方程時(shí)，首先需要獲得量測(cè)值（重力場(chǎng)值）與狀態(tài)參數(shù) X=(δφ,δλ)T之間的線性關(guān)系，即對(duì)重力場(chǎng)與位置之間的非線性關(guān)系進(jìn)行線性化，而線性化過程的準(zhǔn)確與否又直接關(guān)系到量測(cè)方程是否構(gòu)建準(zhǔn)確。常用的隨機(jī)線性化技術(shù)有一階泰勒展開法、九點(diǎn)擬合法、全平面擬合法、平均切線法等[6]，同時(shí)也有學(xué)者針對(duì)地球重力場(chǎng)非線性特性，提出了雙二次曲面擬合等方法[5]。雙二次曲面法是在泰勒展開法的基礎(chǔ)上，考慮了重力場(chǎng)的非線性特性，利用一個(gè)二次曲面對(duì)區(qū)域進(jìn)行擬合，從而削弱線性化截?cái)嗾`差的影響。

由于重力場(chǎng)中大量相關(guān)信息的缺失，上述隨機(jī)線性化方法對(duì)匹配點(diǎn)緯向和經(jīng)向“斜率”的估計(jì)會(huì)存在較大的誤差，從而使得濾波不準(zhǔn)確乃至發(fā)散。而利用EIGEN-6C4地球重力場(chǎng)模型進(jìn)行計(jì)算，不僅包含了LAGEOS、GRACE、GOCE等衛(wèi)星測(cè)高、重力、重力梯度衛(wèi)星數(shù)據(jù)，還包含了實(shí)測(cè)海洋重力數(shù)據(jù)，能夠最大程度地利用實(shí)測(cè)數(shù)據(jù)反映地球重力場(chǎng)特征，適用于水下重力/重力梯度匹配導(dǎo)航。

2.2 基于重力場(chǎng)模型的線性化方法

地球重力場(chǎng)模型是利用現(xiàn)有實(shí)測(cè)重力場(chǎng)相關(guān)數(shù)據(jù)利用反演的方法，用一個(gè)截?cái)嗟接邢揠A次的球諧級(jí)數(shù)式來逼近地球重力場(chǎng)[3]。以一個(gè)半徑等于地球平均半徑R的球來近似表示地球，可以得到如下擾動(dòng)引力位球諧級(jí)數(shù)表達(dá)式：

式中，ρ、φ、λ分別表示空間任意一點(diǎn)的地心向徑、地心緯度和經(jīng)度，表示完全正?；木喓侠兆尩潞瘮?shù)，表示地球重力場(chǎng)模型。因此可以得到任意一點(diǎn)的重力異常Δg與擾動(dòng)重力梯度垂直分量Tzz與擾動(dòng)位T之間的范函關(guān)系如下：

此次計(jì)算對(duì)象是海洋重力數(shù)據(jù)，可近似認(rèn)為（ R / r）≈ 1。將式(7)帶入式(8)可得：

式中，

利用地球重力場(chǎng)模型對(duì)重力異常以及擾動(dòng)重力梯度進(jìn)行線性化，就是求得重力異常與擾動(dòng)重力梯度在緯向和經(jīng)向的導(dǎo)數(shù)和，式(9)兩式左右同時(shí)對(duì)φ和λ求偏導(dǎo)可得：

式中，

利用式(10)即可完成基于重力場(chǎng)模型的隨機(jī)線性化計(jì)算。

2.3 線性化方法并行加速方案

利用地球重力場(chǎng)模型進(jìn)行隨機(jī)線性化的計(jì)算速度對(duì)于重力/重力梯度匹配導(dǎo)航的實(shí)時(shí)性要求還有一定差距[7-9]。因此，為了有效提高地球重力場(chǎng)模型線性化的計(jì)算效率，本文提出基于矩陣運(yùn)算的GPU并行計(jì)算方案，首先將式(10)標(biāo)量運(yùn)算推導(dǎo)至矩陣運(yùn)算形式，并利用GPU對(duì)矩陣運(yùn)算過程進(jìn)行并行化處理提高計(jì)算效率。

2.3.1 隨機(jī)線性化矩陣計(jì)算公式

而為了使得式(10)左右兩端矩陣行列相等，則需要對(duì)等式左端和進(jìn)行拆分，拆分方法如下式：

將式(11)(13)帶入式(10)得：

式中，“°”表示矩陣對(duì)應(yīng)元素相乘。

式(14)即為利用地球重力場(chǎng)模型進(jìn)行隨機(jī)線性化計(jì)算的矩陣計(jì)算公式。

2.3.2 基于GPU的并行加速方案

本文選用CUDA對(duì)上述矩陣運(yùn)算過程進(jìn)行并行化。CUDA是英偉達(dá)公司推出的CPU+GPU異構(gòu)并行編程平臺(tái)，其可以充分發(fā)揮GPU的計(jì)算能力，在各個(gè)領(lǐng)域得到了廣泛的應(yīng)用[10-12]。在使用CUDA按照式(17)矩陣計(jì)算公式進(jìn)行計(jì)算時(shí)需要將矩陣空余部分用“0”補(bǔ)齊，因此在計(jì)算中有一半的線程在做“無用”計(jì)算，極大的浪費(fèi)計(jì)算能力。因此本文提出“矩陣拆補(bǔ)法”并行計(jì)算方案，將n+1行m+1列的下三角矩陣按照一定的規(guī)則，差分并重新補(bǔ)齊為一個(gè)（ n+ 2）/2行m+1列的矩陣，如圖3所示。若n為偶數(shù)，則將下三角矩陣的前n/2行進(jìn)行拆分，其中第i行補(bǔ)至第n+1-i行，組成一個(gè)新的n/2+1行m+1列的矩陣；若n為奇數(shù)，則將下三角矩陣的前（n-1）/2行進(jìn)行拆分，其中第i行補(bǔ)至第n+1-i行，第（n+1）/2行用0元素補(bǔ)齊，組成一個(gè)新的（ n+ 1）/2 +1行m+1列的矩陣。以截?cái)嚯A數(shù)N=8為例，將下三角矩陣前4行進(jìn)行拆分，將第1行補(bǔ)至第8行，將第2行補(bǔ)至第7行，將第3行補(bǔ)至第6行，將第4行補(bǔ)至第5行，組成一個(gè)5行9列的新矩陣。

圖3 矩陣差補(bǔ)法示意圖Fig.3 Schematic diagram of matrix splitting and complement method

通過并行計(jì)算方案，即可完成地球重力場(chǎng)模型線性化并行計(jì)算。

2.3.3 并行加速效果

為驗(yàn)證上述GPU并行計(jì)算方案有效性，本節(jié)利用筆記本電腦，處理器為Intel(R) Core(TM) i7-8750H CPU @ 2.20GHz；GPU為NVIDIA RTX 2070；內(nèi)存16.00GB。軟件環(huán)境為64位Windows 10操作系統(tǒng)，VS2013 + CUDA9.2平臺(tái)。利用串行方案與本節(jié)所提GPU并行方案計(jì)算不同階數(shù)和計(jì)算耗時(shí)與加速比如表1所示。

表1 地球重力場(chǎng)模型線性化計(jì)算耗表Tab.1 Linearization calculation of earth gravity field model

由表1可知，利用本文所提GPU并行計(jì)算方案進(jìn)行隨機(jī)線性化計(jì)算加速比均在10倍以上，最高可達(dá)13.52倍，計(jì)算360階線性化參數(shù)僅需2.56 ms，計(jì)算2160階線性化參數(shù)僅需66.40 ms。充分證明了本文所提GPU并行計(jì)算方案的有效性。

3 重力場(chǎng)模型線性化實(shí)驗(yàn)驗(yàn)證與分析

利用慣性/重力/重力梯度組合導(dǎo)航仿真程序進(jìn)行水下潛航器航跡軌跡與測(cè)量數(shù)據(jù)仿真，慣性導(dǎo)航采樣率為100 Hz。仿真數(shù)據(jù)包含慣性元器件輸出（角增量與速度增量）、重力異常觀測(cè)數(shù)據(jù)、重力梯度觀測(cè)數(shù)據(jù)以及水下潛航器真實(shí)航跡[13]。重力異常基準(zhǔn)圖選用丹麥科技大學(xué)（Technical University of Denmark, DTU）利用衛(wèi)星測(cè)高數(shù)據(jù)、實(shí)測(cè)重力異常數(shù)據(jù)等反演計(jì)算發(fā)布的DTU18全球重力異常模型，數(shù)據(jù)分辨率1'，中誤差2~3 mGal[14]。重力梯度基準(zhǔn)圖則在EIGEN-6C4地球重力場(chǎng)模型與DTU18全球重力異常模型基礎(chǔ)上，基于Stokes理論利用移去-恢復(fù)技術(shù)計(jì)算得到，數(shù)據(jù)分辨率計(jì)算至1'。慣性元器件誤差設(shè)置如表2所示。

表2 慣性元器件誤差設(shè)置Tab.2 Error setting of inertial components

當(dāng)前，海洋重力儀動(dòng)態(tài)測(cè)量精度已達(dá)到1 mGal（如德國(guó)的KSS系列和美國(guó)的L&R系列重力儀），因此重力儀測(cè)量誤差一般取標(biāo)準(zhǔn)差為1 mGal的白噪聲，此外還需要增加一個(gè)誤差項(xiàng)用以表示由于厄特弗斯改正等因素造成的影響，根據(jù)經(jīng)驗(yàn)取為3 mGal。重力梯度儀的動(dòng)態(tài)測(cè)量精度可達(dá)到7E（如美國(guó)的HD-AGG和英國(guó)的EGG系列重力梯度儀），因此重力梯度儀測(cè)量誤差取標(biāo)準(zhǔn)差為7E的白噪聲。同時(shí)結(jié)合本文所用的重力與重力梯度基準(zhǔn)圖分辨率以及水下潛航器的航行速度，EKF量測(cè)方程更新時(shí)間間隔選取180 s（即每隔180 s進(jìn)行一次組合匹配導(dǎo)航校正）。慣導(dǎo)解算采用雙子樣算法，每次慣導(dǎo)更新均進(jìn)行一次狀態(tài)方程更新，因此EKF狀態(tài)方程更新時(shí)間間隔為0.02 s。

選擇重力異常Δg與重力梯度垂直張量Tzz作為濾波觀測(cè)量，仿真試驗(yàn)區(qū)選在某海域，區(qū)域大小為2.5°×4°，試驗(yàn)區(qū)重力異常與重力梯度垂直張量基準(zhǔn)圖如圖4所示。

圖4 試驗(yàn)區(qū)基準(zhǔn)圖Fig.4 Reference map of test area

分別生成兩條航跡，航跡1的航行時(shí)長(zhǎng)4小時(shí)，初始速度與姿態(tài)分別為（0 m/s，0 m/s，0 m/s）和（0 °，0 °，0 °），起始坐標(biāo)為（0.5 °，0.5 °，-100 m），潛航器沿著正北方向先以0.0086 m/s2的加速度，勻加速600 s，速度達(dá)到10節(jié)/h，然后勻速直線航行3h 3000s；航跡2的航行時(shí)長(zhǎng)24小時(shí)，初始速度與姿態(tài)分別為（0 m/s，0 m/s，0 m/s）和（0 °，0 °，-10 °），起始坐標(biāo)為（0.5 °，0.4 °，-100 m），潛航器沿著北偏東45 °方向先以0.0086 m/s2的加速度，勻加速600 s，速度達(dá)到10節(jié)/h，然后勻速直線航行11 h 3000 s后，右轉(zhuǎn)45 °，轉(zhuǎn)彎時(shí)長(zhǎng)300 s，再勻速航行11 h 3300 s。

3.1 EKF-mx不同截?cái)嚯A數(shù)匹配效果分析

在試驗(yàn)區(qū)域內(nèi)以航跡1為實(shí)驗(yàn)對(duì)象，選取重力異常與重力梯度垂直分量作為觀測(cè)量，分別使用360階、720階、1080階、1440階、1800階和2160階重力場(chǎng)模型進(jìn)行隨機(jī)線性化，計(jì)算并統(tǒng)計(jì)匹配航跡位置相較于參考位置的緯向誤差Δy、經(jīng)向誤差Δx以及導(dǎo)航定位誤差航跡1航行4 h，初始誤差設(shè)置為0.4 n mile，重力匹配校正79次，對(duì)重力匹配校正位置的誤差進(jìn)行統(tǒng)計(jì)，結(jié)果如表3所示，并繪制誤差曲線如圖5所示。

圖5 不同截?cái)嚯A數(shù)匹配誤差Fig.5 Matching error of different truncation orders

表3 誤差統(tǒng)計(jì)Tab.3 Error statistics

由表3與圖5結(jié)合可知，使用360至2160階地球重力場(chǎng)模型進(jìn)行隨機(jī)線性化的EKF-mx匹配算法精度均優(yōu)于EKF-9d與EKF-s2，匹配精度相較于INS均提高了61.3%以上，相較于EKF-9d和EKF-s2分別提高了18.7%和12.6%以上。匹配精度會(huì)隨截?cái)嚯A數(shù)的升高而提高，360~1080階之間，截?cái)嚯A數(shù)的提高可以有效提升匹配精度；而1080階以上，截?cái)嚯A數(shù)的增加對(duì)計(jì)算精度的提升效果較弱，因此選用1080階地球重力場(chǎng)模型能夠更好地兼顧匹配精度和實(shí)時(shí)性。

3.2 EKF不同線性化方法匹配效果分析

在試驗(yàn)區(qū)域內(nèi)以航跡2為實(shí)驗(yàn)對(duì)象，分別使用九點(diǎn)擬合法（EKF-9d）、雙二次曲面擬合法（EKF-s2）以及本文所提地球重力場(chǎng)模型法（EKF-mx）對(duì)重力異常與重力梯度進(jìn)行隨機(jī)線性化，模型計(jì)算至1080階。計(jì)算并統(tǒng)計(jì)純慣性導(dǎo)航（INS）、EKF-9d、EKF-s2與EKF-mx相較于參考位置的緯向誤差Δy、經(jīng)向誤差Δx 以及導(dǎo)航定位誤差航跡2航行24 h，初始誤差設(shè)置為0.9 n mile，重力匹配校正479次，對(duì)重力匹配校正位置的誤差進(jìn)行統(tǒng)計(jì)，結(jié)果如表4所示，誤差曲線如圖6所示。

表4 誤差統(tǒng)計(jì)Tab.4 Error statistics

圖6 航跡與誤差曲線Fig.6 Track and error curve

由表4與圖6可知，24 h航行過程中，INS導(dǎo)航定位誤差絕對(duì)值最大達(dá)到7.656 n mile，使用重力、重力梯度EKF匹配算法后，導(dǎo)航定位誤差最大值減少了65.1%以上，導(dǎo)航定位精度提高了72.2%以上。使用EKF-mx匹配算法的導(dǎo)航定位誤差控制在1.215 n mile以內(nèi)，平均值0.527 n mile，相較于EKF-9d和EKF-s2算法，導(dǎo)航定位誤差平均值分別減少了56.2%和48.2%，定位精度分別提高了55.3%和37.5%，充分證明了利用EKF-mx匹配算法可以有效減小INS累積誤差，提高匹配精度。

4 結(jié)論與討論

本文針對(duì)慣性/重力/重力梯度組合導(dǎo)航EKF算法中由于隨機(jī)線性化誤差而造成的線性化模型不準(zhǔn)確引起濾波發(fā)散的問題，利用重力、重力梯度固有屬性，提出了基于地球重力場(chǎng)球諧模型的隨機(jī)線性化方法，并利用并行計(jì)算提高其計(jì)算效率，滿足濾波實(shí)時(shí)性需求。經(jīng)實(shí)驗(yàn)計(jì)算分析表明：（1）所提并行計(jì)算方案可以有效提高重力、重力梯度線性化過程計(jì)算效率，各個(gè)階次模型線性化計(jì)算加速比均達(dá)到10倍以上，有效提高了計(jì)算的實(shí)時(shí)性。計(jì)算至1080階的地球重力場(chǎng)模型以兼顧導(dǎo)航定位精度和計(jì)算實(shí)時(shí)性，線性化耗時(shí)優(yōu)于0.02 s。（2）利用所提重力場(chǎng)模型線性化方法進(jìn)行EKF-mx濾波匹配，在24 h航行過程中，相較于INS系統(tǒng)導(dǎo)航定位精度提高了87.6%，相較于使用九點(diǎn)擬合法的EKF-9d和使用雙二次曲面擬合法的EKF-s2匹配算法導(dǎo)航定位精度分別提高了55.3%和37.5%，可以有效控制隨機(jī)線性化誤差，提高濾波精度。