楊慶生，蔡文郁，官靖凱，楊俊毅

(杭州電子科技大學(xué)電子信息學(xué)院，浙江 杭州 310018)

0 引 言

工業(yè)4.0背景下，船舶自主化航行是水路運(yùn)輸工具變革的重要趨勢[1]。隨著無人水面艇(Unmanned Surface Vessel, USV)應(yīng)用場景的不斷拓展，USV需要面對復(fù)雜多變的工況，不同任務(wù)階段對導(dǎo)航定位精度的要求也不同，有時(shí)導(dǎo)航定位精度需要達(dá)到分米級[2]。海上定位技術(shù)中，使用較多的是全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System, GNSS)和慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)。高精度GNSS接收機(jī)通常結(jié)合多波段接收、載波相位信息和差分技術(shù)，但GNSS信號易受電離層異常等自然干擾及電磁信號干擾、欺騙等故意攻擊[3]。同時(shí)，定位精度隨衛(wèi)星數(shù)量變化而波動。INS運(yùn)用敏感元件測量載體在慣性空間的線運(yùn)動和角運(yùn)動參數(shù)，在已知初始條件下通過牛頓力學(xué)等原理融合成定位數(shù)據(jù)[4]。慣導(dǎo)定位的主要問題是誤差隨時(shí)間累積，需要輔助設(shè)備時(shí)刻進(jìn)行校正?；跀U(kuò)展卡爾曼濾波的GPS/INS組合定位算法結(jié)合GPS和INS的誤差特性，穩(wěn)定輸出定位坐標(biāo)，但其定位精度難以達(dá)到分米級[5]。Han等[6]在城市河道環(huán)境中，使用激光雷達(dá)傳感器和即時(shí)定位與地圖構(gòu)建(Simultaneous Localization And Mapping, SLAM)技術(shù)得到 USV相對坐標(biāo)。Ma等[7]提出一種基于雷達(dá)和衛(wèi)星圖像的實(shí)時(shí)定位算法，實(shí)現(xiàn)USV定位。目前，這些定位方法的實(shí)時(shí)性、精度仍不能完全滿足USV自動導(dǎo)航的需求。

超寬帶技術(shù)(Ultra-Wideband, UWB)擁有極寬電磁頻譜，在穿透能力、精細(xì)分辨、精確測距、抗多徑和抗干擾等方面具有獨(dú)特的優(yōu)勢，廣泛應(yīng)用于室內(nèi)高精度定位[8]。如果將UWB定位技術(shù)的通信距離進(jìn)行延伸，在USV自動回收、自動回塢和自動靠岸等近距離高精度定位場景中，可以彌補(bǔ)GNSS定位精度不足和易受干擾等缺點(diǎn)。因此，本文提出一種基于大功率超寬帶技術(shù)的USV定位系統(tǒng)，通過增加射頻信號發(fā)射的功率來擴(kuò)大定位范圍，并提出一種基于卡爾曼濾波測距優(yōu)化和幾何因子最優(yōu)的三邊定位算法，提高了USV定位系統(tǒng)的精度。

1 USV定位系統(tǒng)設(shè)計(jì)

圖1 USV定位系統(tǒng)框架

USV定位系統(tǒng)主要用于實(shí)現(xiàn)搭載UWB標(biāo)簽的USV定位和航向計(jì)算，由UWB基站系統(tǒng)和UWB移動信標(biāo)系統(tǒng)組成。UWB信標(biāo)系統(tǒng)測量基站之間的距離，將數(shù)據(jù)發(fā)送至USV數(shù)據(jù)處理單元，進(jìn)行濾波和定位定向解算，得到二維坐標(biāo)和航向?？傮w框架如圖1所示。

1.1 系統(tǒng)硬件設(shè)計(jì)

UWB基站系統(tǒng)和信標(biāo)系統(tǒng)由ST公司的微控制器STM32、Decawave公司的UWB測距芯片DW1000、射頻功率放大模塊、信號開關(guān)模塊和電源轉(zhuǎn)換模塊組成。射頻功放電路選用MACOM公司的射頻開關(guān)芯片MASW-007107，將射頻發(fā)送和接收分為2路：當(dāng)DW1000芯片發(fā)送信號時(shí)，射頻信號通過亞德諾半導(dǎo)體(ADI)公司的HMC326MS8G功放芯片將射頻信號放大后輸出；當(dāng)DW1000芯片接收數(shù)據(jù)時(shí)，射頻信號不經(jīng)過功放芯片輸入至DW1000芯片。硬件框圖如圖2所示。

圖2 定位系統(tǒng)硬件框圖

1.2 坐標(biāo)定位和航向計(jì)算

根據(jù)到達(dá)時(shí)間(Time of Arrival, TOA)測量方法定位[9]的原理可知，在二維平面下，需要至少3個(gè)已知位置的基站來進(jìn)行三邊定位。設(shè)3個(gè)參考節(jié)點(diǎn)RN1，RN2，RN3坐標(biāo)分別為(x1,y1)，(x2,y2)，(x3,y3)。由UWB測得參考節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)(x,y)通信的時(shí)間分別為t1,t2,t3。由此得到：

(1)

式中，c為電磁波在空中傳播速度，d1，d2，d3為信標(biāo)節(jié)點(diǎn)到基站的距離值。將式(1)的等式兩兩相減，得到：

(2)

將式(2)轉(zhuǎn)化為矩陣形式：

(3)

矩陣簡化表示為：

AlTag=B

(4)

由式(4)可以得到待定位信標(biāo)節(jié)點(diǎn)的位置坐標(biāo)：

lTag=A-1B

(5)

圖3 USV航向角計(jì)算原理

航向角計(jì)算原理如圖3所示，USV前后分別安裝信標(biāo)節(jié)點(diǎn)，以基站系統(tǒng)坐標(biāo)系Y軸正向?yàn)?°航向角，順時(shí)針為航向角正方向。

UWB定位系統(tǒng)通過TOA定位方法得到2個(gè)信標(biāo)節(jié)點(diǎn)坐標(biāo)后，計(jì)算2個(gè)信標(biāo)節(jié)點(diǎn)X軸和Y軸位置之差ΔX和ΔY，計(jì)算公式如下：

(6)

式中，(xT0,yT0)和(xT1,yT1)分別為船艏T0和船尾T1的信標(biāo)節(jié)點(diǎn)坐標(biāo)。

USV航向角θ計(jì)算公式如下：

(7)

式中，artan為反正切函數(shù)。

2 USV定位優(yōu)化方法

2.1 基于卡爾曼濾波的測距優(yōu)化

USV工作在開闊水域時(shí)，定位誤差主要來自隨機(jī)信號干擾和測量系統(tǒng)誤差，非視距(Not Line of Sight, NLOS)和多徑傳播對測距的影響較小。本文提出的USV定位系統(tǒng)采用線性卡爾曼濾波算法來處理原始測距數(shù)據(jù)。建立測距狀態(tài)方程和測量方程，測距狀態(tài)方程估算當(dāng)前時(shí)刻的狀態(tài)量，測量方程將原始測量值轉(zhuǎn)化為測量狀態(tài)量，根據(jù)最小方差準(zhǔn)則，融合估計(jì)狀態(tài)量和測量狀態(tài)量，迭代計(jì)算得到當(dāng)前時(shí)刻的最優(yōu)狀態(tài)量[10]。

在ti時(shí)刻針對一個(gè)基站的測量值建立狀態(tài)方程：

(8)

并建立測量方程：

y(ti)=Bd(ti)+v(ti)

(9)

式中，y(ti)為ti時(shí)刻的距離測量值，B為測量靈敏度矩陣，v(ti)為測量誤差。

卡爾曼濾波迭代方程如下：

(10)

2.2 基于幾何因子最優(yōu)的三邊定位算法

節(jié)點(diǎn)定位精度不僅依賴測距精度和定位算法，而且和節(jié)點(diǎn)相對于錨節(jié)點(diǎn)的幾何關(guān)系密切相關(guān)，錨節(jié)點(diǎn)構(gòu)成的多邊形對其內(nèi)點(diǎn)定位的精度要好于對外點(diǎn)定位的精度[11]。本文提出的定位系統(tǒng)針對海面環(huán)境定位，基站部署在船塢或者母船上，當(dāng)信標(biāo)遠(yuǎn)離基站的幾何中心時(shí)，相應(yīng)的定位精度出現(xiàn)部分下降。在多基站定位場景中，為了得到定位精度和基站幾何拓?fù)浣Y(jié)構(gòu)的關(guān)系，引入定位幾何精度因子(Geometric Dilution of Positioning Accuracy, GDOP)來描述定位誤差的幾何分布[12-13]。GDOP定義如下：

(11)

式中，μGDOP為定位幾何精度因子，dx，dy，dz分別為x，y，z方向的定位誤差均方差。

由幾何精度因子定義可知，需要已知節(jié)點(diǎn)的參考位置，才能計(jì)算μGDOP，工程上廣泛采用間接估算法，參考文獻(xiàn)[14]提出的最大四面體體積法方法計(jì)算μGDOP近似值：

(12)

USV定位系統(tǒng)布設(shè)4個(gè)基站，利用冗余測距信息，選取3個(gè)基站組合的測距信息代入三邊定位算法計(jì)算lTag，運(yùn)用式(12)計(jì)算μGDOP值，遍歷計(jì)算所有組合，選取μGDOP值最小的3個(gè)基站，所解算的定位坐標(biāo)即為最終結(jié)果。

定位算法流程如圖4所示。為了防止異常值干擾，原始測距數(shù)據(jù)先經(jīng)過窗口中值濾波算法去掉最大值和最小值，使用均值作為最新距離測量值。最新距離值作為觀測過程的新信息輸入到卡爾曼濾波器，運(yùn)用式(10)迭代計(jì)算預(yù)測誤差協(xié)方差和卡爾曼增益，得到最優(yōu)測距估計(jì)值。在定位算法流程中，比較多組μGDOP值，選取最小μGDOP值對應(yīng)的基站組合的定位結(jié)果作為最終輸出。

圖4 定位算法流程圖

3 系統(tǒng)測試與分析

3.1 實(shí)驗(yàn)環(huán)境與方法

為了驗(yàn)證定位系統(tǒng)的合理性和定位精度，在開闊湖面進(jìn)行定位數(shù)據(jù)采集、實(shí)驗(yàn)驗(yàn)證和誤差分析。測試方案如圖5(a)所示，測試區(qū)域長寬為100 m×100 m，基站布局為梯形，以基站布局中心為原點(diǎn)建立坐標(biāo)系，模擬USV進(jìn)塢的場景。湖試場景如圖5(b)所示。

圖5 湖試場景

信標(biāo)T0和T1分別安裝在USV的船艏與船船艉，距離固定為5.4 m，USV定位系統(tǒng)數(shù)據(jù)采集頻率為10 Hz。使用式(13)計(jì)算同一時(shí)刻T0坐標(biāo)與T1坐標(biāo)距離值，計(jì)算三邊定位算法和基于卡爾曼濾波測距優(yōu)化和幾何因子最優(yōu)的三邊定位算法的軌跡誤差和平均絕對誤差：

(13)

式中，d為船艏T0和船尾T1的信標(biāo)節(jié)點(diǎn)的距離值，(xT0,yT0)和(xT1,yT1)分別為船艏T0和船尾T1的信標(biāo)節(jié)點(diǎn)坐標(biāo)。

(14)

式中，δ為船艏T0和船尾T1的信標(biāo)節(jié)點(diǎn)實(shí)測距離值與真實(shí)值的平均絕對誤差，di為船艏T0和船尾T1的信標(biāo)節(jié)點(diǎn)的距離值。

3.2 實(shí)驗(yàn)結(jié)果及分析

圖6 不同三邊定位算法的定位誤差對比

為了驗(yàn)證本文提出的基于卡爾曼濾波測距優(yōu)化和幾何因子最優(yōu)的三邊定位算法的有效性，采集USV定位系統(tǒng)的10次定位測試軌跡，約10 000個(gè)坐標(biāo)點(diǎn)的原始測距數(shù)據(jù)，分別使用基本三邊定位算法和本文提出基于卡爾曼濾波測距優(yōu)化和幾何因子最優(yōu)的三邊定位算法解算定位坐標(biāo)，對比2種算法解算的定位坐標(biāo)的平均絕對誤差。隨著USV與基站之間的距離增加，未優(yōu)化的三邊定位算法的定位誤差上升很快，本文提出優(yōu)化的三邊定位算法比較好地抑制了誤差。在40 m范圍內(nèi)，優(yōu)化的三邊定位算法平均定位誤差約為0.5 m，相比基本三邊定位算法，誤差下降了約35%，在40～70 m范圍內(nèi)，優(yōu)化的三邊定位算法的平均定位誤差約為0.9 m，相比基本三邊定位算法，誤差下降了約40%。2種三邊定位算法的定位誤差對比如圖6所示。

圖7(a)是USV從起點(diǎn)環(huán)繞基站行駛一圈實(shí)測定位軌跡圖，T0和T1分別為船艏和船艉標(biāo)簽的定位軌跡，使用式(7)計(jì)算航向值的結(jié)果如圖7(b)所示，縱軸為對應(yīng)的航向值。USV從起點(diǎn)行駛至終點(diǎn)，軌跡和對應(yīng)的航向值變化連續(xù)且無異常波動，符合實(shí)際情況。

圖7 定位軌跡與航向

對局部定位軌跡進(jìn)行進(jìn)一步放大，USV航行軌跡局部放大對比如圖8所示，T0和T1分別為船艏和船艉標(biāo)簽的定位軌跡。從圖8可以看出，本文提出優(yōu)化的三邊定位算法的定位軌跡與實(shí)際軌跡基本重合，且軌跡平滑，而基本三邊定位算法的定位軌跡存在偏差，且軌跡波動較大。本文提出的基于卡爾曼濾波測距優(yōu)化和幾何因子最優(yōu)的三邊定位算法能有效提高定位軌跡平滑度。

圖8 USV局部定位軌跡

4 結(jié)束語

本文設(shè)計(jì)了一套在海面環(huán)境用于USV的定位系統(tǒng)，并提出一種基于卡爾曼濾波和幾何精度因子最優(yōu)的三邊定位算法。在距離基站50 m范圍內(nèi)，本文提出的USV定位系統(tǒng)的定位精度在分米級，滿足USV近距離高精度定位和定向的需求。后續(xù)將繼續(xù)進(jìn)一步研究UWB，GNSS和INS多源融合定位技術(shù)，改進(jìn)USV定位系統(tǒng)，以提高定位系統(tǒng)的精度和魯棒性。