一種基于相對測量信息的SINS/USBL組合導(dǎo)航算法

張亞文，莫明崗，馬小艷，鄧?yán)^權(quán)

(北京自動化控制設(shè)備研究所，北京 100074)

一種基于相對測量信息的SINS/USBL組合導(dǎo)航算法

張亞文，莫明崗，馬小艷，鄧?yán)^權(quán)

(北京自動化控制設(shè)備研究所，北京 100074)

針對使用傳統(tǒng)位置匹配算法解決SINS/USBL組合導(dǎo)航的問題，需要先利用USBL輸出信息實時求解出載體的絕對位置，設(shè)計了一種基于相對測量信息的SINS/USBL組合導(dǎo)航算法。該算法無需求解絕對位置，直接利用超短基線系統(tǒng)原始輸出的高度角、方位角和斜距等相對測量信息進(jìn)行卡爾曼濾波組合導(dǎo)航，且在濾波器設(shè)計時能夠根據(jù)傳感器測量精度對濾波參數(shù)進(jìn)行設(shè)計，更有針對性。通過理論數(shù)據(jù)仿真和湖面試驗數(shù)據(jù)處理，證明了該組合導(dǎo)航算法能夠在一定程度上提高導(dǎo)航精度，具有可行性。

組合導(dǎo)航；相對測量信息；直接輸出

0 引言

捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)能夠在較短的時間內(nèi)為水中載體提供精確的位置信息，但隨著時間推移，誤差累積會嚴(yán)重影響導(dǎo)航精度，需要額外的信息對該誤差進(jìn)行修正[1]。超短基線(USBL)定位系統(tǒng)能夠測量應(yīng)答器相對于基陣的高精度位置信息[2-3]，可以用來抑制慣導(dǎo)系統(tǒng)誤差的發(fā)散。此外，超短基線定位系統(tǒng)以其成本低、便攜性強(qiáng)、靈活性高和操作簡單等特點，在海洋工程、海洋礦產(chǎn)資源、水下考古、海洋國防建設(shè)等領(lǐng)域中得到了廣泛應(yīng)用[4]。因此，SINS/USBL組合導(dǎo)航成為了水下導(dǎo)航技術(shù)研究的熱點。

傳統(tǒng)的SINS/USBL組合導(dǎo)航方法，主要是參考通用位置匹配算法，即以慣性導(dǎo)航系統(tǒng)給出的經(jīng)度、緯度和高度與輔助導(dǎo)航定位設(shè)備給出的相應(yīng)信息的差值作為位置觀測量。然而，由于USBL直接輸出的是應(yīng)答器相對于基陣的距離和方位信息，即高度角α、方位角β和斜距R[5]。若利用傳統(tǒng)位置匹配算法，則需要先利用USBL輸出的相對距離和方位、應(yīng)答器的絕對位置、姿態(tài)矩陣和USBL與姿態(tài)傳感器之間的標(biāo)定矩陣等信息，求解出載體的絕對位置信息(經(jīng)緯高)，然后才能進(jìn)行組合導(dǎo)航。由于受到標(biāo)定誤差、姿態(tài)誤差等影響，直接解算的位置存在一定的誤差，會影響精度。此外，在濾波器設(shè)計時，無法根據(jù)USBL傳感器精度，合理選取部分濾波參數(shù)。針對以上問題，結(jié)合USBL自身輸出特點，提出并設(shè)計一種直接利用USBL輸出的高度角、方位角和斜距等相對測量信息的SINS/USBL組合導(dǎo)航算法。在模型中考慮姿態(tài)誤差和安裝誤差等因素，能夠在一定程度上提高定位精度。

1 超短基線定位方式及定位誤差分析

超短基線定位系統(tǒng)主要由水聲收發(fā)基陣和應(yīng)答器構(gòu)成。通過測量聲波傳回基陣各基元之間的相位差，實現(xiàn)對應(yīng)答器的測向，獲得應(yīng)答器相對于基陣的高度角α和方位角β；通過測量接收時刻和發(fā)射時刻的時間延遲，實現(xiàn)對應(yīng)答器的測距，獲得應(yīng)答器相對于基陣的斜距R[6]。最終利用高度角α和方位角β將斜距R投影到基陣坐標(biāo)系中，可得基陣坐標(biāo)系中應(yīng)答器相對于基陣中心的位置信息(x，y，z)

(1)

這便是USBL相對定位的基本原理。USBL絕對定位工作原理如圖1所示。

圖1 USBL定位原理圖Fig.1 The scheme of USBL positioning theory

(2)

超短基線系統(tǒng)的定位誤差源可分為以下兩類：

1)傳感器誤差：USBL基陣測距誤差、USBL測角誤差、測距交會求解造成的誤差、超短基線基陣陣元相位差引起的誤差、聲傳播引起的誤差、聲學(xué)噪聲引起的誤差等，此類誤差大都受制于超短基線設(shè)備本身的性能指標(biāo)。

2)系統(tǒng)誤差：此類誤差常存在于使用過程中，包括：外部航向姿態(tài)誤差、基陣坐標(biāo)系和姿態(tài)傳感器坐標(biāo)系之間的安裝角誤差和應(yīng)答器的絕對位置誤差等。后兩種誤差源常通過事先高精度的標(biāo)定校準(zhǔn)來減小。

本文提出并設(shè)計的SINS/USBL組合導(dǎo)航算法在慣導(dǎo)誤差建模的基礎(chǔ)上，將對測距誤差、測角誤差和兩坐標(biāo)系間的安裝角誤差等三項誤差源進(jìn)行建模補(bǔ)償。

2 組合導(dǎo)航模型及濾波方程

2.1 組合導(dǎo)航系統(tǒng)狀態(tài)模型

組合導(dǎo)航系統(tǒng)誤差狀態(tài)模型包括SINS的誤差狀態(tài)模型和USBL的誤差狀態(tài)模型。

2.1.1SINS的誤差狀態(tài)模型

(3)

捷聯(lián)慣性導(dǎo)航誤差狀態(tài)方程為

(4)

2.1.2USBL的誤差狀態(tài)模型

通過實驗數(shù)據(jù)處理發(fā)現(xiàn)，USBL自身測角精度以及USBL基陣坐標(biāo)系到SINS載體坐標(biāo)系的安裝角標(biāo)定精度對定位精度影響很大。從數(shù)值角度粗略分析：如果USBL測量的方位角β或者航向安裝誤差角θy有1°誤差，則會引入約1.7%R的水平位置誤差，而且隨著斜距R變大誤差會進(jìn)一步增大。USBL的測角精度和噪聲受USBL設(shè)備的限制，但測角誤差在工作范圍內(nèi)都為小角度。此外，安裝角誤差可以通過事先標(biāo)定使其減小為一個小角度誤差，因此，本文將USBL到SINS的安裝誤差角θ=[θxθyθz]T和高度角α和方位角β的誤差[δαδβ]設(shè)計為卡爾曼濾波組合導(dǎo)航的誤差狀態(tài)量。

由于海洋環(huán)境并不安靜，波浪、船只和波浪所形成的空氣氣泡及溫度、鹽度等水文氣象要素都對超短基線水聲定位系統(tǒng)起著不可忽視的作用[8]，主要影響聲波的傳播，即對USBL的測距精度產(chǎn)生影響。故也將USBL測距精度誤差δKd設(shè)計為卡爾曼濾波組合導(dǎo)航的誤差狀態(tài)量。

綜上所述，USBL的誤差狀態(tài)量為

XUSBL=[θxθyθzδαδβδKd]T

(5)

由于安裝誤差角、測角誤差和USBL測距精度誤差等均為小量，因此視為隨機(jī)常數(shù)。對應(yīng)的卡爾曼濾波狀態(tài)方程為

(6)

其中：FUSBL=[06×6]。

2.1.3 組合導(dǎo)航系統(tǒng)誤差狀態(tài)模型

結(jié)合2.1.1和2.1.2可得，21維的組合導(dǎo)航系統(tǒng)誤差狀態(tài)量為

X=[XSINSXUSBL]T

(7)

則組合導(dǎo)航系統(tǒng)誤差狀態(tài)方程為

(8)

簡化誤差狀態(tài)方程為

(9)

2.2 組合導(dǎo)航系統(tǒng)觀測模型

基陣坐標(biāo)系中應(yīng)答器相對于慣導(dǎo)的高度角、方位角和斜距為

(10)

對其求偏微分可得

(11)

為了求得更加精準(zhǔn)的位置信息，避免球面坐標(biāo)系中小角度近似誤差，因此本文選取在模型更加精準(zhǔn)的地球直角坐標(biāo)系中進(jìn)行位置求解。地球直角坐標(biāo)系的原點o位于旋轉(zhuǎn)橢球體中心，z軸與地球自轉(zhuǎn)軸重合，正向沿地球自轉(zhuǎn)方向，x軸和y軸位于赤道平面內(nèi)，x軸穿過本初子午線，y軸穿過東經(jīng)90°子午線。

應(yīng)答器的位置為Pr=[φrhrλr]，慣導(dǎo)解算的位置為Ps=[φshsλs]，通過地球定位中球面坐標(biāo)系到直角坐標(biāo)系的轉(zhuǎn)換公式[9]，則二者在地球直角坐標(biāo)系的相對位置為re

(12)

通過坐標(biāo)變化并補(bǔ)償桿臂誤差，得到基陣坐標(biāo)系u中應(yīng)答器相對慣導(dǎo)的位置

(13)

引入誤差量可得

(14)

則有

(15)

綜合式(10)～式(15)可得

(16)

其中：

令高度角、方位角和斜距的真值依次為α、β、R，則有

(17)

故觀測方程為

(18)

則觀測矩陣為

H=[ 03×3(Hφ)3×3(HδP)3×306×3

(Hθ)3×3(HU)3×3]

(19)

簡化觀測方程為

Z=HX+v

(20)

2.3 卡爾曼濾波模型

對式(9)和式(20)進(jìn)行離散化處理可得SINS/USBL組合導(dǎo)航的卡爾曼濾波模型為

(21)

其中，Wk和Vk+1均為白噪聲，且方差陣依次為Q、R。

本方法采用標(biāo)準(zhǔn)卡爾曼濾波算法基本方程[10]，選擇合適的濾波初值(狀態(tài)初始值、初始估計均方誤差陣、系統(tǒng)噪聲初始方差陣以及量測噪聲方差陣)，進(jìn)行濾波估計，并實時修正速度、姿態(tài)和位置信息。

3 理論數(shù)據(jù)仿真分析

慣導(dǎo)初始地理位置北緯39.8°東經(jīng)116.2°高度為0，以3m/s(約6節(jié))的前向速度環(huán)繞應(yīng)答器(北緯39.8000235°東經(jīng)116.199167°，高度-50m)行駛1000s，理想初始航向姿態(tài)角都為0°。慣導(dǎo)采樣周期為5ms，USBL數(shù)據(jù)更新周期為2s，組合導(dǎo)航周期為2s。各傳感器誤差源和初始誤差設(shè)置如表1所示。

表1 傳感器誤差源和導(dǎo)航初始誤差設(shè)置Tab.1 Error of the sensor and initial navigation

按照本文設(shè)計的基于相對測量信息組合導(dǎo)航算法進(jìn)行仿真，并與純慣性導(dǎo)航和USBL定位進(jìn)行對比，如表2所示，本方法1σ定位精度2倍優(yōu)于USBL定位。誤差曲線如圖2～圖7。

表2 1σ定位精度對比Tab.2 The comparison of positioning precision

圖2 北向位置誤差圖Fig.2 North position error

圖3 天向位置誤差Fig.3 Up position error

圖4 東向位置誤差Fig.4 East position error

圖5 水平定位誤差Fig.5 Horizontal position error

圖6 北天東速度誤差Fig.6 The comparison of velocity error

圖7 橫滾角、航向角、俯仰角誤差Fig.7 The comparison of attitude error

從圖2～圖5定位誤差曲線可以看出，本算法能夠有效地抑制位置誤差發(fā)散，且位置誤差不隨時間增長；圖6速度誤差曲線顯示出本方法能夠有效地抑制速度誤差發(fā)散；從圖7航向姿態(tài)誤差曲線可以看出，本方法能夠比較有效地抑制航向角誤差和水平姿態(tài)角誤差。綜上所述，在理論仿真情況下，本文設(shè)計的基于相對測量信息的SINS/USBL水下組合導(dǎo)航算法，不僅能有效地抑制速度誤差和位置誤差，還能夠估計慣導(dǎo)的航向姿態(tài)誤差，驗證了理論仿真情況下的本算法的可行性。

4 湖面實驗數(shù)據(jù)處理分析

湖面試驗系統(tǒng)主要包括：光纖捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)、USBL系統(tǒng)和DGPS系統(tǒng)。光纖陀螺漂移優(yōu)于0.02(°)/h，加表零偏優(yōu)于30μg(1σ)。USBL系統(tǒng)采用NavQuest公司的Track Link 1500HA水下基陣和應(yīng)答器，其原始輸出為高度角、方位角和斜距，其測角精度優(yōu)于1°，定位精度為0.5%，數(shù)據(jù)更新周期為2s。

利用本文設(shè)計的基于相對測量信息的SINS/USBL組合導(dǎo)航算法對湖面試驗采集的一組約為5 000s導(dǎo)航原始數(shù)據(jù)進(jìn)行處理分析，并以DGPS的位置為基準(zhǔn)信息，考核組合導(dǎo)航算法的定位精度，并與傳統(tǒng)位置匹配算法進(jìn)行對比。組合導(dǎo)航初始時刻，先用DGPS對慣導(dǎo)的解算的位置速度進(jìn)行重置，以避免原始積累的誤差對組合導(dǎo)航精度考核產(chǎn)生影響[11]。

從表3湖面試驗定位精度可以看出，本方法定位精度(1σ)相比傳統(tǒng)位置匹配算法有一定的提高。從圖8～圖10湖面試驗定位誤差曲線可以看出，相比傳統(tǒng)算法，本算法不僅能夠有效抑制慣導(dǎo)位置的發(fā)散，且定位誤差更小、更加平滑。結(jié)合圖11 USBL數(shù)據(jù)有效性判定結(jié)果可以看出，位置誤差噪聲主要由于USBL數(shù)據(jù)判定無效，導(dǎo)致組合導(dǎo)航不再進(jìn)行量測更新，只進(jìn)行時間更新引起。綜上所述，本文設(shè)計的基于相對測量信息的SINS/USBL水下組合導(dǎo)航算法能有效地抑制速度誤差，且能一定程度上提高組合導(dǎo)航定位精度，即驗證了湖面試驗數(shù)據(jù)情況下本算法的可行性。

表3 湖面試驗定位精度Tab.3 The comparison of lake test position precision

圖8 北向位置誤差曲線Fig.8 North position error

圖9 東向位置誤差曲線Fig.9 East position error

圖10 水平定位誤差曲線Fig.10 Horizontal position error

圖11 USBL數(shù)據(jù)有效性判定結(jié)果Fig.11 USBL data valid judge result

5 結(jié)論

本文設(shè)計的基于相對測量信息的SINS/USBL組合導(dǎo)航算法，利用了工程使用中USBL設(shè)備實際輸出為高度角、方位角和斜距的特點，摒棄了傳統(tǒng)算法中絕對位置的解算，且在濾波器設(shè)計時能夠根據(jù)傳感器測量精度對量測噪聲進(jìn)行設(shè)計，更有針對性地解決了SINS/USBL組合導(dǎo)航的問題。理論仿真和湖面試驗數(shù)據(jù)處理表明，本算法能夠?qū)崿F(xiàn)高精度的水下導(dǎo)航定位，且能夠一定程度上提高組合導(dǎo)航定位精度，具有可行性。

[1] 蘭華林，孫大軍，張殿倫，李想.單應(yīng)答器導(dǎo)航深海實驗[J].海洋工程，2007,25(2)：110-113.

[2] 隋海琛，田春和，韓德忠，王崇明.水下定位系統(tǒng)誤差分析[J].水道港口,2010,31(1):69-72.

[3] LinkQuestInc.TrackLink 1500 USBL Tracking System,User’s Guide Version 1500.7.2.

[4] 王德剛，韓富江，來向華，茍諍慷，傅曉明.國家海洋局第二海洋研究所研究報告[J].海洋科學(xué)，2011,35(2):77-78.

[5] LinkQuestInc.TrackLink 1500 USBL Tracking System,User’s Guide Version 1500.7.2

[6] 王德剛,韓富江,來向華,茍諍慷,傅曉明.超短基線定位原理及校正方法研究[J].海洋科學(xué),2011,35(2):77-80.

[7] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2006.

[8] 馮守珍,吳永亭,唐秋華.超短基線聲學(xué)定位原理及其應(yīng)用[J].海岸工程,2002,21(4):13-18.

[9] 史海鋒,張衛(wèi)斌.空間直角坐標(biāo)與大地坐標(biāo)轉(zhuǎn)換算法研究[J].大地測量與地球動力學(xué),2012,32(5):78-81.

[10] 秦永元,張洪鉞,汪淑華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社,1998.

[11] 唐康華,江明明,翁炬.基于自適應(yīng)濾波的水下SINS/相控陣DVL組合導(dǎo)航算法設(shè)計[J].中國慣性技術(shù)學(xué)報,2013,21(1):66-70.

An Algorithm Used in Underwater SINS/USBL Integrated Navigation

ZHANG Ya-wen，MO Ming-gang，MA Xiao-yan，DENG Ji-quan

(Beijing Institute of Automatic Control Equipment，Beijing 100074，China)

For the problem that the traditional position integrated navigation must be working after the absolute position of the USBL (Ultra Short Base Line) was evaluated，design a new integrated navigation algorithm which observes the relative measurement information.The algorithm no longer needs to evaluate the absolute position，and it can do the KF integrated navigation by means of observing the immediate data of USBL outputs，the relative measurement information(depression，bearing and range )，and design the KF parameter based on the USBL sensor measurement precision.Besides，a theory simulation and a lake test data analysis prove the improvement and the practicability of the new integrated navigation algorithm.

Integrated navigation；Relative measurement information；Immediate output

2014-11-18；

2015-03-02。

張亞文(1990-)，男，碩士，助理工程師，主要從事慣性/水聲組合導(dǎo)航技術(shù)研究。E-mail:zhangyawen0927@126.com

V249

A

2095-8110(2016)02-0007-07