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

張亞文，莫明崗，馬小艷，鄧繼權

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

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

張亞文，莫明崗，馬小艷，鄧繼權

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

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

組合導航；相對測量信息；直接輸出

0 引言

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

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

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

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

(1)

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

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

(2)

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

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

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

本文提出并設計的SINS/USBL組合導航算法在慣導誤差建模的基礎上，將對測距誤差、測角誤差和兩坐標系間的安裝角誤差等三項誤差源進行建模補償。

2 組合導航模型及濾波方程

2.1 組合導航系統狀態模型

組合導航系統誤差狀態模型包括SINS的誤差狀態模型和USBL的誤差狀態模型。

2.1.1SINS的誤差狀態模型

(3)

捷聯慣性導航誤差狀態方程為

(4)

2.1.2USBL的誤差狀態模型

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

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

綜上所述，USBL的誤差狀態量為

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

(5)

由于安裝誤差角、測角誤差和USBL測距精度誤差等均為小量，因此視為隨機常數。對應的卡爾曼濾波狀態方程為

(6)

其中：FUSBL=[06×6]。

2.1.3 組合導航系統誤差狀態模型

結合2.1.1和2.1.2可得，21維的組合導航系統誤差狀態量為

X=[XSINSXUSBL]T

(7)

則組合導航系統誤差狀態方程為

(8)

簡化誤差狀態方程為

(9)

2.2 組合導航系統觀測模型

基陣坐標系中應答器相對于慣導的高度角、方位角和斜距為

(10)

對其求偏微分可得

(11)

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

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

(12)

通過坐標變化并補償桿臂誤差，得到基陣坐標系u中應答器相對慣導的位置

(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)進行離散化處理可得SINS/USBL組合導航的卡爾曼濾波模型為

(21)

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

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

3 理論數據仿真分析

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

表1 傳感器誤差源和導航初始誤差設置Tab.1 Error of the sensor and initial navigation

按照本文設計的基于相對測量信息組合導航算法進行仿真，并與純慣性導航和USBL定位進行對比，如表2所示，本方法1σ定位精度2倍優于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定位誤差曲線可以看出，本算法能夠有效地抑制位置誤差發散，且位置誤差不隨時間增長；圖6速度誤差曲線顯示出本方法能夠有效地抑制速度誤差發散；從圖7航向姿態誤差曲線可以看出，本方法能夠比較有效地抑制航向角誤差和水平姿態角誤差。綜上所述，在理論仿真情況下，本文設計的基于相對測量信息的SINS/USBL水下組合導航算法，不僅能有效地抑制速度誤差和位置誤差，還能夠估計慣導的航向姿態誤差，驗證了理論仿真情況下的本算法的可行性。

4 湖面實驗數據處理分析

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

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

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

表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數據有效性判定結果Fig.11 USBL data valid judge result

5 結論

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

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

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

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

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

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

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

[7] 秦永元.慣性導航[M].北京:科學出版社,2006.

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

[9] 史海鋒,張衛斌.空間直角坐標與大地坐標轉換算法研究[J].大地測量與地球動力學,2012,32(5):78-81.

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

[11] 唐康華,江明明,翁炬.基于自適應濾波的水下SINS/相控陣DVL組合導航算法設計[J].中國慣性技術學報,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-)，男，碩士，助理工程師，主要從事慣性/水聲組合導航技術研究。E-mail:zhangyawen0927@126.com

V249

A

2095-8110(2016)02-0007-07