GNSS/INS高精度反向平滑濾波算法在移動(dòng)測(cè)圖中的應(yīng)用

王立兵，馬國(guó)駒，賈瑞才,劉鵬飛，智奇楠

(1.中國(guó)電子科技集團(tuán)公司第五十四研究所， 石家莊 050081；2.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室， 石家莊 050081；3.中國(guó)人民解放軍63961部隊(duì)， 北京 100012)

測(cè)繪工作在國(guó)民經(jīng)濟(jì)發(fā)展中起著重要的基礎(chǔ)性作用。隨著科技發(fā)展和社會(huì)進(jìn)步，目前測(cè)繪工作正朝著以地理信息系統(tǒng)(GIS)、遙感技術(shù)(RS)、全球定位系統(tǒng)(GNSS)和衛(wèi)星通信技術(shù)為支撐的戰(zhàn)略新型產(chǎn)業(yè)化方向快速發(fā)展。隨著“數(shù)字城市”理念以及“數(shù)字地球”理念的不斷發(fā)展。傳統(tǒng)測(cè)繪方式已經(jīng)不能滿足人們空間信息快速獲取與更新需要。在這種需求下，MMS(移動(dòng)測(cè)量系統(tǒng))應(yīng)運(yùn)而生，成為制圖新技術(shù)的典型代表。現(xiàn)今隨著激光測(cè)量技術(shù)、GPS技術(shù)、慣性導(dǎo)航技術(shù)以及CCD技術(shù)的發(fā)展，基于多傳感器集成的車載三維移動(dòng)測(cè)量系統(tǒng)已成為對(duì)地觀測(cè)技術(shù)的必要補(bǔ)充。

高精度定位測(cè)姿系統(tǒng)作為高精度位置、姿態(tài)和速度測(cè)量設(shè)備，是移動(dòng)測(cè)量系統(tǒng)中的核心部件之一，可與航測(cè)相機(jī)、紅外相機(jī)、高光譜設(shè)備、激光雷達(dá)、SAR等載荷結(jié)合起來，裝載在汽車、火車、飛行器等不同的載體上，在高速移動(dòng)狀態(tài)下完成測(cè)量與地理信息采集工作，可對(duì)目標(biāo)區(qū)域進(jìn)行高效率的攝影測(cè)量，顯著提升了地理信息生產(chǎn)能力。通過迅速、及時(shí)地獲取多頻段、多時(shí)相、高精度、高分辨率的位置與圖像信息，可以快速建立數(shù)字高程模型，生產(chǎn)數(shù)字正射影像圖、數(shù)字地形圖及可量測(cè)實(shí)景影像，搭建智慧城市的地理數(shù)據(jù)基礎(chǔ)——城市地理空間框架。

全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system,GNSS)具有全天候、全球、連續(xù)實(shí)時(shí)的精密三維導(dǎo)航和定位能力。GNSS能夠提供高精度的定位、測(cè)速以及授時(shí)服務(wù)，目前已經(jīng)廣泛應(yīng)用到導(dǎo)航、定位、測(cè)速與授時(shí)等各個(gè)領(lǐng)域。RTK(Real-time kinematic)技術(shù)利用載波相位差分技術(shù)利用雙差模型能夠削弱或者消除衛(wèi)星鐘差、對(duì)流層誤差、電離層誤差從而提高流動(dòng)站的觀測(cè)精度，所得到的測(cè)量精度可達(dá)厘米級(jí)甚至亞厘米級(jí)。

慣導(dǎo)具有較高的信息更新頻率，在初始化完成后，依靠自身測(cè)得的角速度以及比力信息能夠計(jì)算出載體的位置、速度、姿態(tài)等信息，然而由于受誤差積累的影響，導(dǎo)致導(dǎo)航定位解算精度隨時(shí)間發(fā)散[1-2]。在GNSS/INS組合導(dǎo)航中，GNSS提供慣導(dǎo)需要的更新信息，從而抑制慣導(dǎo)信息的發(fā)散，而慣導(dǎo)又能輸出高采樣率的導(dǎo)航定位信息，當(dāng)GNSS因?yàn)樾盘?hào)受到遮擋或者干擾而中斷時(shí)，慣導(dǎo)仍能繼續(xù)工作從而增加系統(tǒng)的可靠性和健壯性。利用高精度GNSS定位技術(shù)和慣導(dǎo)組合導(dǎo)航定位能夠提供厘米級(jí)精度的定位信息以及高精度的測(cè)速和定姿信息[3-4]。

研究高精度GNSS/INS組合定位技術(shù)具有重要的意義和應(yīng)用價(jià)值，本文針對(duì)移動(dòng)測(cè)圖領(lǐng)域?qū)Ω呔榷ㄎ弧y(cè)速以及定姿信息的需求，推導(dǎo)并建立了一套高精度組合導(dǎo)航反向?yàn)V波模型，針對(duì)典型城市場(chǎng)景下GNSS信號(hào)衰落中斷的情況，推導(dǎo)并建立了基于載體運(yùn)動(dòng)的約束模型，從而有效提高導(dǎo)航定位精度，能夠在一定復(fù)雜場(chǎng)景下替代人工測(cè)量，從而大大地提高了高精度測(cè)圖的工作效率。

1 高精度組合濾波模型

1.1 擴(kuò)展卡爾曼濾波模型

卡爾曼濾波算法是大部分導(dǎo)航系統(tǒng)狀態(tài)估計(jì)的理論基礎(chǔ)，它廣泛的應(yīng)用于衛(wèi)星導(dǎo)航、衛(wèi)星定軌、組合導(dǎo)航以及多種數(shù)據(jù)融合算法中[5]。卡爾曼濾波從被提出之后，為了得到更好的工程應(yīng)用，被不斷地完善、改進(jìn)，在原來的數(shù)學(xué)模型基礎(chǔ)上相繼出現(xiàn)了多種適合實(shí)際應(yīng)用的模型。GNSS/INS組合導(dǎo)航系統(tǒng)并非線性連續(xù)系統(tǒng)，因此標(biāo)準(zhǔn)的卡爾曼濾波模型并不適用于該系統(tǒng)的狀態(tài)或者參數(shù)估計(jì)[6]。在本文中采用擴(kuò)展卡爾曼濾波對(duì)數(shù)學(xué)模型進(jìn)行線性化從而實(shí)現(xiàn)導(dǎo)航信息和慣導(dǎo)誤差的最優(yōu)估計(jì)，在本文中選取位置誤差δrn、速度誤差δvn、姿態(tài)誤差ψ、陀螺零偏誤差δbg以及加速度計(jì)零偏誤差δba共計(jì)15維。該狀態(tài)向量可以表示為

(1)

建立的狀態(tài)方程如下

(2)

經(jīng)過離散化的狀態(tài)方程如下

Xk=Φk,k-1Xk-1+Γk-1Wk-1

(3)

其中：Xk、Xk-1分別為k和k-1時(shí)刻的狀態(tài)向量；Γk-1為系統(tǒng)噪聲驅(qū)動(dòng)陣；Wk-1為狀態(tài)的噪聲向量；Φk,k-1為離散后的狀態(tài)轉(zhuǎn)移矩陣，只考慮到一次項(xiàng)，Φk,k-1可以表示為

(4)

式中各項(xiàng)參數(shù)的計(jì)算公式參考文獻(xiàn)[7-11]。

1.2 更新算法

1) 衛(wèi)導(dǎo)更新算法

(5)

(6)

其中：Hk代表卡爾曼濾波模型的系數(shù)矩陣；I3×3代表三維單位矩陣。

(7)

2) 零速修正算法

車體坐標(biāo)系速度Vm可表示為

(8)

Vn為慣導(dǎo)系統(tǒng)輸出的導(dǎo)航坐標(biāo)系下的速度分量。由運(yùn)動(dòng)約束條件提供的觀測(cè)值構(gòu)成如下

(9)

(10)

對(duì)式(10)進(jìn)行微分，可得

Μ1φ+M2δVn+M3α

(11)

(12)

式中：M1(1,×)表示矩陣M1的第一行；M3(1,3)表示矩陣M3的第1行3列的元素。

至此，系統(tǒng)量測(cè)方程可表示為

Z=HX+V

(13)

其中

(14)

(15)

1.3 反向?yàn)V波模型

反向平滑最優(yōu)估計(jì)綜合利用了前向信息和反向平滑信息因此能有有效提高信息融合的精度。反向平滑分為固定點(diǎn)平滑、固定延遲平滑、固定區(qū)間平滑。固定區(qū)間平滑是對(duì)整個(gè)數(shù)據(jù)段的平滑，因此廣泛應(yīng)用于測(cè)繪等適合后處理的領(lǐng)域，固定延遲平滑最常用的方法是R-T-S方法，R-T-S算法結(jié)構(gòu)如圖1所示。在進(jìn)行前向?yàn)V波的過程中保存下濾波得到的方差信息在反向平滑過程中對(duì)這些信息進(jìn)行利用從而有效提高組合精度。

圖1 反向?yàn)V波結(jié)構(gòu)

反向平滑濾波的數(shù)學(xué)模型為

(16)

式中：k=N-1,N-2,…,0,N為測(cè)量值的總數(shù)。

2 試驗(yàn)及測(cè)試結(jié)果

2.1 試驗(yàn)描述

為了驗(yàn)證組合導(dǎo)航反向平滑算法的精度，采用成熟商用組合導(dǎo)航系統(tǒng)天寶POS LV610作為參考基準(zhǔn)，該系統(tǒng)采用其后處理軟件PosPac，利用RTK定位結(jié)果進(jìn)行組合導(dǎo)航解算時(shí)得到的位置精度優(yōu)于5 cm，測(cè)速精度優(yōu)于0.01 m/s，水平姿態(tài)精度優(yōu)于0.005°，航向精度優(yōu)于0.05°，該系統(tǒng)可以作為高精度的位姿參考。測(cè)試地點(diǎn)選擇在石家莊市西三環(huán)，該場(chǎng)景大部分為開闊場(chǎng)景，也包含典型城市復(fù)雜場(chǎng)景以及立交橋等遮擋場(chǎng)景，測(cè)試開始前在已知點(diǎn)架設(shè)基準(zhǔn)站用于高精度的后處理，基準(zhǔn)站如圖2所示，跑車測(cè)試過程中將參考基準(zhǔn)和被測(cè)基準(zhǔn)牢固固定保證相對(duì)位置的固定，測(cè)試圖如圖3所示。

圖2 基準(zhǔn)站 圖3 跑車測(cè)試圖

被測(cè)系統(tǒng)的慣性器件主要技術(shù)指標(biāo)如表1所示。

表1 慣組器件主要技術(shù)指標(biāo)

2.2 試驗(yàn)結(jié)果及分析

跑車測(cè)試的軌跡圖如圖4所示。

圖4 跑車測(cè)試軌跡

利用天寶組合導(dǎo)航系統(tǒng)高精度的位姿信息作為參考，計(jì)算得到的位置、速度、以及姿態(tài)誤差如圖5所示。可以看出位置誤差最大值小于0.2 m，速度誤差最大值小于0.1 m/s，水平姿態(tài)誤差最大值小于0.05°，航向誤差最大值小于0.1°。

為了進(jìn)一步評(píng)估導(dǎo)航定位精度對(duì)誤差進(jìn)行了統(tǒng)計(jì)，結(jié)果如表2所示，結(jié)果表明，位置誤差優(yōu)于5 cm，速度誤差優(yōu)于0.01 m/s，水平姿態(tài)優(yōu)于0.005°，航向精度優(yōu)于0.05°，該結(jié)果驗(yàn)證了算法的正確性和可靠性。

表2 測(cè)試結(jié)果

圖5 跑車測(cè)試誤差

3 結(jié)論

基于擴(kuò)展卡爾曼濾波的高精度GNSS/INS反向平滑濾波模型，可以廣泛應(yīng)用于高精度移動(dòng)測(cè)圖領(lǐng)域，提供高精度的位置、速度、姿態(tài)。