北斗衛(wèi)星導(dǎo)航系統(tǒng)在近海無(wú)人水下航行器路徑跟蹤方面的應(yīng)用

(上海海洋大學(xué) 工程學(xué)院 上海 201306)

0 引言

隨著近年我國(guó)提出建設(shè)海洋強(qiáng)國(guó)的戰(zhàn)略，可用于海洋水質(zhì)檢測(cè)及海平面測(cè)繪的無(wú)人水下航行器(UUV)得到了快速發(fā)展.UUV 是一種可在水下運(yùn)動(dòng)，具有視覺(jué)和感知系統(tǒng)，通過(guò)遙控操作方式，配備機(jī)械手代替或輔助人工完成水下作業(yè)任務(wù)的機(jī)電裝置[1].先進(jìn)的UUV 離不開先進(jìn)的導(dǎo)航控制系統(tǒng).

北斗衛(wèi)星導(dǎo)航系統(tǒng)(BDS)，是我國(guó)基于國(guó)家安全和經(jīng)濟(jì)社會(huì)發(fā)展自主研發(fā)的全球衛(wèi)星導(dǎo)航系統(tǒng)，是為全球用戶提供全天候、全天時(shí)、高精度、高可靠的定位、導(dǎo)航和授時(shí)服務(wù)的國(guó)家重要時(shí)空基礎(chǔ)設(shè)施，繼美國(guó)的GPS，俄羅斯的GLONASS之后，第三個(gè)成熟的衛(wèi)星導(dǎo)航系統(tǒng)[2].BDS 作為后起之秀，不僅擁有其他導(dǎo)航系統(tǒng)的所具有的功能，而且還擁有其他導(dǎo)航系統(tǒng)不具有的雙向短報(bào)文通信功能[3].2020 年BDS 完成了全球組網(wǎng)使其在交通運(yùn)輸業(yè)、農(nóng)業(yè)、漁業(yè)及防災(zāi)減災(zāi)方面得到了廣泛應(yīng)用.例如姜少杰等[4]提出了一種BDS/GPS 雙模組合定位的水下機(jī)器人高精度導(dǎo)航定位系統(tǒng)，并通過(guò)設(shè)置單模定位系統(tǒng)作為雙模定位系統(tǒng)的對(duì)比方案研究雙模系統(tǒng)的性能，通過(guò)對(duì)實(shí)驗(yàn)數(shù)據(jù)的濾波處理得出雙模定位系統(tǒng)比單模定位系統(tǒng)性能優(yōu)越的結(jié)論.崔秀芳等[5]為解決漁船遇險(xiǎn)搜救難題，提出了運(yùn)用BDS 監(jiān)控遠(yuǎn)洋漁船的方法，利用BDS 系統(tǒng)監(jiān)控范圍廣，穩(wěn)定性好及獨(dú)有的雙向短報(bào)文通信優(yōu)勢(shì)，實(shí)現(xiàn)了對(duì)遠(yuǎn)洋漁船的實(shí)時(shí)監(jiān)管及海上突發(fā)事件的及時(shí)救援.

無(wú)線信號(hào)很難抵達(dá)深海區(qū)域，所以BDS 在UUV 上的應(yīng)用較少.研究結(jié)果表明，當(dāng)UUV 工作在近海水深小于300m 海域時(shí)，無(wú)線電導(dǎo)航和衛(wèi)星導(dǎo)航技術(shù)均可正常使用[6].本文將BDS 應(yīng)用在近海UUV路徑跟蹤方面，通過(guò)UUV 定位終端接收BDS 定位數(shù)據(jù)，STM32 單片機(jī)模塊接收并處理UUV 位置數(shù)據(jù)并通過(guò)無(wú)線通訊模塊將數(shù)據(jù)發(fā)送給上位機(jī)，上位機(jī)接收、儲(chǔ)存并比較UUV 實(shí)際路徑數(shù)據(jù)與給定路徑數(shù)據(jù)偏差，并向STM32 模塊下達(dá)控制指令控制UUV 驅(qū)動(dòng)模塊改變UUV 的航向，實(shí)現(xiàn)精確的路徑跟蹤.

1 UUV 系統(tǒng)及結(jié)構(gòu)設(shè)計(jì)

UUV 是一個(gè)復(fù)雜的無(wú)人潛航系統(tǒng)，通常由驅(qū)動(dòng)系統(tǒng)、控制系統(tǒng)(STM32 作為主控芯片)、導(dǎo)航定位系統(tǒng)、無(wú)線通訊系統(tǒng)、信息采集系統(tǒng)及載體等組成[7].本文建立的UUV 主要包括：1 對(duì)42BYGH直流無(wú)刷電機(jī)(通過(guò)電機(jī)轉(zhuǎn)速差實(shí)現(xiàn)轉(zhuǎn)向)、水平方向舵(STM32 控制舵機(jī)轉(zhuǎn)角實(shí)現(xiàn)浮潛)、雙葉螺旋槳、KXD-48-V-80AH 動(dòng)力鋰電池、40A 雙向無(wú)刷電調(diào)組成的驅(qū)動(dòng)系統(tǒng)；岸基監(jiān)控平臺(tái)(主要為上位機(jī)，PC 機(jī)組成)、工控機(jī)(STM32 為主控制器)、直流穩(wěn)壓模塊組成的控制系統(tǒng)；導(dǎo)航定位系統(tǒng)包括：MPU6000 電子陀螺儀和UM220-ⅣN北斗定位模塊.無(wú)線通訊系統(tǒng)主要為RS485 串口、5G 通訊及北斗短報(bào)文通訊模塊.信息采集系統(tǒng)包括：PCI8601 數(shù)據(jù)采集卡、圖像采集模塊、溫鹽深儀(CTD)及定點(diǎn)測(cè)繪模塊等.載體選擇一體化鋁合金耐壓機(jī)身(便于密封防水).圖1 為UUV 硬件系統(tǒng)結(jié)構(gòu).

圖1 UUV 硬件系統(tǒng)結(jié)構(gòu)

2 BDS 在UUV 上的應(yīng)用

2.1 BDS 定位系統(tǒng)原理

UUV 在不同深度水域航行時(shí)，采用不同的導(dǎo)航定位方式，當(dāng)工作水域深度小于300m 時(shí)，可以使用BDS 或其他無(wú)線電導(dǎo)航技術(shù).

對(duì)近海UUV 進(jìn)行準(zhǔn)確定位，需要測(cè)量4 顆已知坐標(biāo)的BDS 衛(wèi)星到UUV北斗定位模塊之間的距離.衛(wèi)星到定位模塊的距離可由真空中的光速乘以衛(wèi)星信號(hào)傳播到定位模塊接收機(jī)的時(shí)間得到.假設(shè)UUV 坐標(biāo)(x0,y0,z0)為未知參數(shù)，已知空間衛(wèi)星端衛(wèi)星的瞬時(shí)坐標(biāo)為(xi,yi,zi)，其中i=1,2,3,4(可由衛(wèi)星星歷，衛(wèi)星鐘校正參數(shù)等數(shù)據(jù)計(jì)算出)，以已知衛(wèi)星坐標(biāo)為圓心，衛(wèi)星坐標(biāo)到UUV 坐標(biāo)的距離為半徑，采用三維空間交會(huì)的方法計(jì)算UUV 三維空間坐標(biāo)，解方程組就得到定位模塊接收機(jī)坐標(biāo)，計(jì)算公式如下：

式中:Di為衛(wèi)星到定位模塊觀測(cè)的距離;c為真空中的光速；?ti為衛(wèi)星信號(hào)到接收機(jī)的時(shí)間；Vt0為接收機(jī)時(shí)鐘差；Vti即衛(wèi)星時(shí)鐘差，由衛(wèi)星星歷得出.

聯(lián)立上述方程，便可求出UUV 的空間坐標(biāo)(x0,y0,z0)及接收裝置的時(shí)鐘差.圖2 為BDS 定位系統(tǒng)框圖.

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

2.2 定位系統(tǒng)模型

當(dāng)UUV 在近海海域航行時(shí)，為便于研究定位系統(tǒng)模型，可以將UUV 的運(yùn)動(dòng)看作是在二維平面xoy上的運(yùn)動(dòng)，因此其從E(x1,y1)點(diǎn)運(yùn)動(dòng)到F(x2,y2)點(diǎn)的運(yùn)動(dòng)模型為

式中：V0為UUV 的初速度；θ為初始方位角；S為E(x1,y1)至F(x2,y2)點(diǎn)的距離；E(x1,y1)點(diǎn)為UUV 的初始位置，可以由北斗定位模塊測(cè)出；UUV 從E(x1,y1)點(diǎn)運(yùn)動(dòng)到F(x2,y2)點(diǎn)時(shí)北斗定位模塊可測(cè)出此時(shí)F(x2,y2)點(diǎn)的相對(duì)位置.同時(shí)F(x2,y2)點(diǎn)的相對(duì)位置也可以由運(yùn)動(dòng)模型推斷得出.兩個(gè)數(shù)據(jù)通過(guò)相互修正得到UUV 的具體位置，提高了UUV 的整體定位精度.

3 導(dǎo)航定位分系統(tǒng)的設(shè)計(jì)

導(dǎo)航定位分系統(tǒng)采用模塊化設(shè)計(jì)，包括單片機(jī)模塊，北斗定位模塊，無(wú)線通訊模塊和PC 上位機(jī)[8].當(dāng)UUV 運(yùn)行時(shí)，上位機(jī)向北斗定位模塊下達(dá)接受定位數(shù)據(jù)的命令，此時(shí)北斗定位模塊開啟，接收來(lái)自BDS 的導(dǎo)航信號(hào)，實(shí)現(xiàn)定位后將數(shù)據(jù)發(fā)送給STM32 單片機(jī)，STM32 單片機(jī)通過(guò)算法解析、中值濾波及卡爾曼濾波處理得出UUV精確的位置坐標(biāo)、航行軌跡、航速航向等數(shù)據(jù)并通過(guò)無(wú)線通訊模塊將數(shù)據(jù)發(fā)送給上位機(jī)，上位機(jī)存儲(chǔ)UUV 位置數(shù)據(jù)并在界面上顯示.上位機(jī)模塊將UUV 實(shí)際運(yùn)動(dòng)軌跡與給定路徑進(jìn)行對(duì)比，如果出現(xiàn)路徑偏差，上位機(jī)經(jīng)過(guò)算法計(jì)算將控制命令下達(dá)給STM32 單片機(jī)，STM32 接受命令并解析后下達(dá)指令給UUV 驅(qū)動(dòng)模塊，通過(guò)改變UUV 雙電機(jī)轉(zhuǎn)速差及舵機(jī)舵角控制UUV 沿給定路徑運(yùn)動(dòng)至特定區(qū)域，待UUV 到達(dá)目的地后，打開信息采集模塊及定點(diǎn)測(cè)繪模塊，采集水樣并得出水溫、鹽度、PH 值等數(shù)據(jù)并記錄采樣點(diǎn)位置坐標(biāo)，并將采集到的數(shù)據(jù)發(fā)送到STM32單片機(jī).單片機(jī)經(jīng)過(guò)數(shù)據(jù)處理后，再發(fā)送給上位機(jī)記錄和保存.完成一次海水取樣及測(cè)繪任務(wù)后巡航至下一測(cè)繪點(diǎn).圖3 為基于導(dǎo)航定位分系統(tǒng)的UUV 工作流程圖.

圖3 導(dǎo)航定位分系統(tǒng)的UUV 工作流程圖

3.1 單片機(jī)模塊

作為UUV 控制系統(tǒng)的核心.單片機(jī)負(fù)責(zé)接收北斗定位模塊傳輸?shù)?BDGGA 格式數(shù)據(jù)，并對(duì)數(shù)據(jù)進(jìn)行算法解析，采用中值濾波消除數(shù)據(jù)噪聲，卡爾曼濾波使輸出的定位數(shù)據(jù)變得更加平滑[9].并將解析后的數(shù)據(jù)通過(guò)無(wú)線通訊模塊傳送給上位機(jī).本文選用高性能STM32 單片機(jī)，該芯片采用Cortex-M4 內(nèi)核，180M主頻CPU，具有256K 的SRAM，支持SDRAM,包含140 個(gè)GPI 口，引腳為176pin，擁有更快的A/D轉(zhuǎn)換速度和更低的ADC/DAC 工作電壓[10].高性能STM32 單片機(jī)作為主控制器，一方面接收信息采集模塊采集的測(cè)繪圖像及水質(zhì)參數(shù)等信息，并對(duì)數(shù)據(jù)進(jìn)行解讀、分析、濾波處理后發(fā)送到岸上監(jiān)控系統(tǒng)上位機(jī)顯示界面顯示并存儲(chǔ)；另一方面接收UM220-ⅣN 定位模塊采集到的UUV 實(shí)時(shí)位置，航向航速等信息.通過(guò)數(shù)據(jù)處理后經(jīng)無(wú)線通訊模塊發(fā)送給上位機(jī)，上位機(jī)將其與跟蹤路徑比較進(jìn)而控制UUV 沿跟蹤路徑航行.

3.2 北斗定位模塊

北斗定位模塊是UUV 導(dǎo)航定位分系統(tǒng)的核心，它的優(yōu)劣決定了UUV 能否實(shí)現(xiàn)精確的路徑跟蹤，本文選用北京和芯星通科技有限公司研發(fā)的UM220 第四代產(chǎn)品UM220-ⅣN 定位模塊，該模塊主要針對(duì)車輛監(jiān)控、手持終端以及船舶定位開發(fā)的BDS/GPS雙模定位模塊，采用了低功耗抗干擾GNSS SoC 芯片UC6226，主要特征如下：

1)集成度高，功耗低(53mw),尺寸小(16mm×12.2mm×2.4mm)，抗干擾能力強(qiáng)，可以在復(fù)雜電磁場(chǎng)環(huán)境下工作；采用24 引腳，SMD 表面貼裝封裝技術(shù)，極大地方便了生產(chǎn)；

2)UM220-ⅣN 模塊可以滿足BDS 和GPS 單系統(tǒng)導(dǎo)航定位，同時(shí)可以進(jìn)行BDS/GPS 雙模塊聯(lián)合導(dǎo)航定位，可直接輸出NMEA 數(shù)據(jù)不需要外接CPU；

3)BDS 系統(tǒng)亞太地區(qū)定位精度為10m,速度精度為0.2m/s，授時(shí)精度達(dá)10ns.BDS/GPS 雙模塊水平定位精度為2.5m CEP，速度精度為0.1m/s.模塊冷啟動(dòng)需30s，熱啟動(dòng)1s.工作溫度?40℃～+85℃，存儲(chǔ)溫度?45℃～+90℃，可在極端氣候環(huán)境下使用.

4)UM220-ⅣN 定位模塊中BDS 和GPS 模塊靈敏度對(duì)照如表1.

表1 BDS 和GPS 靈敏度對(duì)照dBm

選擇UM220 第四代產(chǎn)品UM220-ⅣN 定位模塊，可實(shí)現(xiàn)復(fù)雜環(huán)境中對(duì)UUV 的高精度定位，該定位模塊可方便的與STM32 單片機(jī)進(jìn)行信息傳輸，也可以連接PC 機(jī)，運(yùn)用軟件實(shí)現(xiàn)定位功能.圖4 為UM220-ⅣN 定位模塊結(jié)構(gòu)框圖.

圖4 UM220-IV N 定位模塊結(jié)構(gòu)框圖

3.3 無(wú)線通訊模塊

無(wú)線通訊模塊是UUV 信息傳遞的中介，對(duì)整個(gè)UUV 的導(dǎo)航定位及控制至關(guān)重要.本文中無(wú)線通訊模塊采用RS485 串口電路，5G 通訊模塊及北斗短報(bào)文模塊協(xié)同作用，實(shí)現(xiàn)BDS 衛(wèi)星端、船載定位終端及地面控制端之間數(shù)據(jù)信息的雙向交換.RS485 串口相較于工業(yè)廣泛運(yùn)用的RS232 串口數(shù)據(jù)傳輸速率更高(達(dá)到10Mbps),傳輸距離更遠(yuǎn)(達(dá)到3000m)，采用平衡驅(qū)動(dòng)器和差分接收器的組合，抗噪聲干擾性好.滿足多數(shù)工業(yè)中信息的傳輸.5G 通信技術(shù)是目前移動(dòng)通信技術(shù)發(fā)展的最高峰，其具有高速(最高可達(dá)10Gbit/s)、低功耗、低延時(shí)性能，是目前以及未來(lái)時(shí)間段內(nèi)工業(yè)無(wú)線數(shù)據(jù)傳輸?shù)闹髁?UUV 在近海活動(dòng)中，當(dāng)海上環(huán)境劇烈變化，無(wú)線通訊難以保證時(shí)，北斗定位系統(tǒng)所獨(dú)有的雙向短報(bào)文通信就可發(fā)揮作用.

北斗短報(bào)文通信模塊選用北京國(guó)翼恒達(dá)導(dǎo)航科技公司的GYM2003B，模塊采用振芯科技GM4660芯片組研制，通過(guò)北斗短報(bào)文收發(fā)信號(hào)的低噪聲放大、功率放大、上下變頻和基帶處理，實(shí)現(xiàn)北斗短報(bào)文收發(fā)及有源定位功能[11].模塊集成了BDS 衛(wèi)星無(wú)線電定位系統(tǒng)(RDSS)射頻收發(fā)芯片，基帶電路，功放芯片等，具有BDS RDSS 導(dǎo)航定位、短報(bào)文通信及衛(wèi)星授時(shí)等功能.

北斗短報(bào)文工作過(guò)程是發(fā)送方將包含接收方ID 和通訊內(nèi)容的通訊申請(qǐng)信號(hào)加密后通過(guò)衛(wèi)星轉(zhuǎn)發(fā)至地面中心站，地面中心站接收到通訊申請(qǐng)信號(hào)后，經(jīng)脫密和再次加密后加入持續(xù)廣播的出站廣播電文中，經(jīng)衛(wèi)星廣播給用戶，用戶接收機(jī)接收出站信號(hào)，解調(diào)解密出站電文，完成一次通訊[12].

3.4 上位機(jī)模塊

上位機(jī)模塊主要對(duì)UUV 采集到的環(huán)境數(shù)據(jù)進(jìn)行接收、顯示和存儲(chǔ)，同時(shí)監(jiān)控UUV 路徑軌跡的變化.并根據(jù)UUV 實(shí)際運(yùn)動(dòng)情況發(fā)送相應(yīng)的控制指令，上位機(jī)軟件界面使用Visual C++開發(fā).可方便實(shí)現(xiàn)二次開發(fā).船載北斗定位模塊將接收到的BDS 定位數(shù)據(jù)通過(guò)無(wú)線通訊模塊發(fā)送給STM32單片機(jī)，單片機(jī)將數(shù)據(jù)濾波處理后發(fā)送給上位機(jī)顯示界面，上位機(jī)對(duì)數(shù)據(jù)進(jìn)行讀取，顯示及存儲(chǔ).上位機(jī)的功能主要有信息顯示、遠(yuǎn)程監(jiān)控以及動(dòng)態(tài)預(yù)警等.圖5 為上位機(jī)工作流程.

圖5 上位機(jī)模塊工作流程

4 水下測(cè)試及分析

在建立了UUV 模型及北斗定位分系統(tǒng)后，對(duì)UUV 以恒定速度跟蹤目標(biāo)路徑進(jìn)行水下實(shí)驗(yàn)，將UUV 外形簡(jiǎn)化為一個(gè)總長(zhǎng)0.85m、寬0.50m、高0.24m 的均質(zhì)方體，假定其重心與浮心重合.為了驗(yàn)證UUV 路徑跟蹤情況，將UUV 實(shí)際路徑起點(diǎn)與目標(biāo)直線路徑起點(diǎn)取不同坐標(biāo)位置，給定UUV初始狀態(tài)：初始位置坐標(biāo)(2，8)，初始航向角45°.初始速度0.5m/s，水域流速為0.1m/s，目標(biāo)路徑為坐標(biāo)(0，0)～(60，60)的直線，目標(biāo)航向角45°.UUV 開啟后，上位機(jī)軟件向其發(fā)送目標(biāo)路徑軌跡，UUV 在北斗定位模塊的作用下向目標(biāo)路徑靠近，在坐標(biāo)點(diǎn)(18，18)處與目標(biāo)路徑重合.圖6 為UUV 的路徑跟蹤實(shí)驗(yàn)曲線圖.

圖6 UUV 路徑跟蹤線圖

上位機(jī)顯示界面在UUV 運(yùn)行過(guò)程中記錄多處UUV 經(jīng)緯度坐標(biāo)與目標(biāo)路徑經(jīng)緯度坐標(biāo)，如表2 所示，通過(guò)實(shí)驗(yàn)可知，UUV 在初始階段距離目標(biāo)路徑較遠(yuǎn)，但在BDS 及上位機(jī)、單片機(jī)的共同作用下，UUV 不斷向目標(biāo)路徑靠近，并最終沿著目標(biāo)路徑運(yùn)動(dòng)，在路徑終點(diǎn)采集水質(zhì)信息并運(yùn)動(dòng)到下一采集點(diǎn).導(dǎo)航定位分系統(tǒng)運(yùn)行狀況良好，并能及時(shí)修復(fù)路徑偏差.提高了近海UUV 的路徑跟蹤的精度.

表2 UUV 目標(biāo)位置與實(shí)際位置經(jīng)緯度對(duì)照

UM220-ⅣN 定位模塊不僅適用于BDS 導(dǎo)航系統(tǒng)也可用于GPS 導(dǎo)航系統(tǒng)，為了驗(yàn)證BDS 在UUV路徑跟蹤方面的精度，選取在GPS 作用下UUV 的路徑跟蹤實(shí)驗(yàn)，比較兩者導(dǎo)航定位性能的優(yōu)劣.圖7 為兩種導(dǎo)航方式下UUV 路徑跟蹤經(jīng)緯度測(cè)試數(shù)據(jù)折線圖，通過(guò)比較UUV 真實(shí)曲線數(shù)據(jù)，BDS 測(cè)試數(shù)據(jù)以及GPS 測(cè)試數(shù)據(jù)可以清晰地看到，由于BDS 定位系統(tǒng)是具有同步軌道并能支持三個(gè)頻率工作的系統(tǒng)，在亞太地區(qū)BDS 的定位精度和GPS 不相上下，甚至比GPS 定位精度更高，完全滿足近海UUV 的定位導(dǎo)航及路徑跟蹤.

圖7 經(jīng)緯度測(cè)試數(shù)據(jù)折線圖

5 結(jié)論及展望

通過(guò)將我國(guó)自主研發(fā)的北斗定位模塊運(yùn)用到UUV 上，確保了UUV 能夠?qū)崟r(shí)改變自身航向跟蹤目標(biāo)直線路徑，北斗定位模塊實(shí)時(shí)獲取UUV 的位置坐標(biāo)及航向信息，通過(guò)無(wú)線傳輸模塊將UUV 坐標(biāo)信息發(fā)送到高性能STM32 單片機(jī)，經(jīng)過(guò)單片機(jī)模塊的中值濾波及卡爾曼濾波，消除數(shù)據(jù)噪聲及缺陷數(shù)據(jù).上位機(jī)模塊接收到來(lái)至單片機(jī)的數(shù)據(jù)后，一方面分析，顯示及保存；另一方面比較UUV 實(shí)際路徑與目標(biāo)路徑偏差，通過(guò)高性能STM32單片機(jī)將新的控制指令發(fā)送給運(yùn)動(dòng)部件，控制UUV 的航向，同時(shí)采用北斗短報(bào)文通信實(shí)現(xiàn)了極端環(huán)境無(wú)線信號(hào)難以抵達(dá)的條件下北斗定位模塊，高性能STM32 單片機(jī)及上位機(jī)信息的雙向傳輸.通過(guò)各模塊協(xié)同配合實(shí)現(xiàn)了近海UUV 精確的路徑跟蹤.

由于深海中無(wú)線信號(hào)很難抵達(dá)，所以選擇BDS作為導(dǎo)航定位系統(tǒng)的UUV 只能在近海海域工作，未來(lái)要想走向深海，還需要研究基于水聲定位系統(tǒng)或慣性導(dǎo)航系統(tǒng)(INS)的導(dǎo)航定位方式.復(fù)合導(dǎo)航及多種控制方式結(jié)合的UUV 將是發(fā)展的趨勢(shì).