楊少龍，孫延浩，黃 金，向先波

(華中科技大學(xué) 船舶與海洋工程學(xué)院，湖北 武漢 430074)

0 引 言

無(wú)人艇作為海上移動(dòng)平臺(tái)，受風(fēng)、浪、流等作用會(huì)產(chǎn)生六自由度運(yùn)動(dòng)，包括3 個(gè)方向的平動(dòng)和3 個(gè)方向的轉(zhuǎn)動(dòng)。對(duì)于旋轉(zhuǎn)運(yùn)動(dòng)，船體不同位置與旋轉(zhuǎn)中心的距離不等，旋轉(zhuǎn)運(yùn)動(dòng)產(chǎn)生的線速度不同，而船體任意點(diǎn)的合速度是平動(dòng)速度與轉(zhuǎn)動(dòng)速度的矢量和，故實(shí)際海上航行無(wú)人艇船體各點(diǎn)的合速度矢量均不相等。無(wú)人艇的許多作業(yè)場(chǎng)景依賴于觀測(cè)無(wú)人艇自身某一局部位置的位姿信息，例如，無(wú)人艇的布放和回收、多無(wú)人艇對(duì)接、無(wú)人艇靠離碼頭、艇載傳感器位姿補(bǔ)償（如攝像頭、測(cè)掃聲吶、避障聲吶、對(duì)底聲吶、激光雷達(dá)等）等。為此，如何獲取船體任意位置的實(shí)時(shí)位姿狀態(tài)成為無(wú)人艇智能化發(fā)展中信息感知關(guān)鍵技術(shù)的重要挑戰(zhàn)之一。

船體任意位置實(shí)時(shí)位姿狀態(tài)獲取，通常需要若干個(gè)慣導(dǎo)傳感器數(shù)據(jù)的融合處理。司帆等[1]為克服機(jī)翼?yè)锨冃螌?duì)機(jī)載導(dǎo)彈對(duì)準(zhǔn)產(chǎn)生的影響，根據(jù)主、子慣導(dǎo)的慣性器件輸出，得到子慣導(dǎo)所處位置的高精度實(shí)時(shí)姿態(tài)信息。王林等[2]針對(duì)單軸與雙軸旋轉(zhuǎn)慣導(dǎo)的信息融合問題，設(shè)計(jì)聯(lián)合旋轉(zhuǎn)調(diào)制策略，實(shí)現(xiàn)了單軸旋轉(zhuǎn)慣導(dǎo)定位誤差的預(yù)測(cè)與補(bǔ)償，提高了系統(tǒng)的導(dǎo)航定位可靠性。劉鎮(zhèn)波等[3]提出一種基于分布式IMU 的相對(duì)姿態(tài)估計(jì)方法，能夠在系統(tǒng)無(wú)高精度慣導(dǎo)及變形統(tǒng)計(jì)特性未知的情況下，獲取機(jī)載關(guān)鍵位置精確的相對(duì)姿態(tài)。這些研究著重關(guān)注多個(gè)物理傳感器的數(shù)據(jù)融合測(cè)算傳感器所在局部位置的實(shí)時(shí)位姿，雖然精度較高，但卻不能滿足任意多個(gè)測(cè)點(diǎn)的位姿實(shí)時(shí)觀測(cè)需求。隨著無(wú)人艇全船布置設(shè)備增多，有限個(gè)慣導(dǎo)傳感器擴(kuò)容卻顯著增加了傳感系統(tǒng)的復(fù)雜度及投資成本。

當(dāng)前，數(shù)字孿生技術(shù)的迅速發(fā)展為上述需求提供了新的解決思路。數(shù)字孿生是一種集成了多物理量、多尺度、多概率的仿真過程，以數(shù)字化方式創(chuàng)建物理實(shí)體的虛擬模型，通過虛實(shí)交互與數(shù)據(jù)融合實(shí)現(xiàn)物理實(shí)體的全生命周期服務(wù)[4-5]。這個(gè)概念最早被提出是用于描述產(chǎn)品的生產(chǎn)制造和實(shí)時(shí)虛擬化呈現(xiàn)，隨后該理念被推廣應(yīng)用到工業(yè)產(chǎn)品、大型裝備的實(shí)時(shí)運(yùn)行監(jiān)測(cè)[6]。數(shù)字孿生通常包括物理空間的實(shí)體、信息空間的虛擬孿生體、物理空間和信息空間之間的數(shù)據(jù)和信息交互接口三部分[7-9]。目前，已有多位學(xué)者對(duì)數(shù)字孿生在船舶領(lǐng)域的應(yīng)用進(jìn)行了初步研究。李凱等[10]基于數(shù)字孿生提出數(shù)字化艦船的總體框架，并探索基于數(shù)字孿生的產(chǎn)品設(shè)計(jì)、輔助決策、健康管理等潛在應(yīng)用場(chǎng)景。陶飛等[11]提出將數(shù)字孿生與船舶工業(yè)結(jié)合，參照數(shù)字孿生五維模型，探討基于數(shù)字孿生的船舶設(shè)計(jì)、制造、運(yùn)維、使用等全生命周期一體化管理理念。然而，由于缺乏系統(tǒng)的數(shù)字孿生理論支撐和應(yīng)用準(zhǔn)則指導(dǎo)，目前在船舶領(lǐng)域數(shù)字孿生的研究多集中于理論探討，鮮有實(shí)際應(yīng)用報(bào)道。

因此，面向無(wú)人艇任意位置實(shí)時(shí)姿態(tài)估計(jì)需求，本文設(shè)計(jì)基于數(shù)字孿生的無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)，通過剛體運(yùn)動(dòng)學(xué)建立無(wú)人艇物理空間時(shí)序數(shù)據(jù)與信息空間幾何信息的跨空間融合方法?；趩翁谞顟B(tài)感知設(shè)備獲取的信息，無(wú)人艇實(shí)現(xiàn)在信息空間實(shí)時(shí)映射來(lái)自物理空間的航行狀態(tài)，并可實(shí)時(shí)估算船體任意位置的位姿狀態(tài)。

1 無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)架構(gòu)

1.1 系統(tǒng)總體框架

如圖1 所示，無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)由實(shí)體無(wú)人艇、三維可視化交互軟件以及物理空間與信息空間的交互接口組成。無(wú)人艇是系統(tǒng)在物理空間的實(shí)體對(duì)象，搭載了姿態(tài)傳感器、位置傳感器等傳感設(shè)備，能夠感知航行信息并實(shí)時(shí)傳遞給上位機(jī)。三維可視化軟件是系統(tǒng)進(jìn)行無(wú)人艇姿態(tài)三維可視化顯示和姿態(tài)信息虛實(shí)交互的部分，由無(wú)人艇虛擬孿生體、UI 界面和Script 組成。信息交互接口主要由地面站軟件（QGroundControl，QGC）組成，QGC 在系統(tǒng)中發(fā)揮數(shù)據(jù)中介作用，一方面向無(wú)人艇發(fā)送任務(wù)信息，并接收無(wú)人艇傳回的航行信息；另一方面從無(wú)人艇傳回的航行信息中提取位姿數(shù)據(jù)，發(fā)送給三維可視化交互軟件。

圖1 無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)總體構(gòu)架Fig.1 Framework of the unmanned surface vehicles' virtual reality fusion three-dimensional interactive system

1.2 系統(tǒng)信息流傳輸機(jī)制

無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)能夠把實(shí)體無(wú)人艇傳感器感知到的位置、姿態(tài)等信息實(shí)時(shí)映射為信息空間中無(wú)人艇虛擬孿生體的運(yùn)動(dòng)信息。從無(wú)人艇傳感器實(shí)時(shí)數(shù)據(jù)的輸入到三維可視化軟件中無(wú)人艇虛擬孿生體運(yùn)動(dòng)的輸出，信息流需要跨越多個(gè)軟硬件平臺(tái)。如圖2 所示，在物理空間、信息空間以及數(shù)據(jù)交互接口中，均設(shè)有數(shù)據(jù)的傳遞、轉(zhuǎn)化模塊，各模塊共同組成了系統(tǒng)的信息流傳輸機(jī)制。

圖2 無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)數(shù)據(jù)傳輸機(jī)制Fig.2 The unmanned surface vehicles' virtual reality fusion threedimensional interactive system structure

無(wú)人艇與上位機(jī)通過無(wú)線數(shù)傳以MAVLINK 協(xié)議方式通信，上位機(jī)中安裝有二次開發(fā)的QGC 軟件，在QGC 軟件中解析MAVLINK 消息包提取傳感器數(shù)據(jù)，并用UDP 封裝姿態(tài)數(shù)據(jù)（橫滾角、首向角、俯仰角）和位置數(shù)據(jù)（經(jīng)度、緯度、相對(duì)海拔、速度）。通過本地UDP 套接字，將封裝好的數(shù)據(jù)發(fā)送至特定端口。同時(shí)，在Unity3D 數(shù)據(jù)處理Script 中創(chuàng)建Socket 對(duì)象，監(jiān)聽該端口，實(shí)時(shí)接收位姿數(shù)據(jù)。數(shù)據(jù)處理Script 把接收到的UDP 數(shù)據(jù)解包，并對(duì)位姿數(shù)據(jù)進(jìn)行預(yù)處理后分別賦予位置變量和姿態(tài)變量。運(yùn)動(dòng)控制Script 和任意點(diǎn)速度計(jì)算Script 分別調(diào)用這些變量實(shí)現(xiàn)無(wú)人艇虛擬孿生體的三維動(dòng)態(tài)顯示和實(shí)時(shí)位姿狀態(tài)估計(jì)功能。

2 虛擬無(wú)人艇及交互設(shè)計(jì)

2.1 虛擬無(wú)人艇孿生體及虛擬場(chǎng)景建立

精確的無(wú)人艇虛擬孿生體模型是三維可視化交互軟件實(shí)現(xiàn)無(wú)人艇數(shù)字孿生的基礎(chǔ)。本文用SolidWorks建立實(shí)體無(wú)人艇1∶1 三維模型，再用3Dmax 轉(zhuǎn)換為可供導(dǎo)入U(xiǎn)nity3D 的.fbx 文件。為了獲得逼真的三維仿真效果，在三維可視化交互軟件中構(gòu)建與真實(shí)環(huán)境相似的虛擬場(chǎng)景，例如基于Terrain 組件建立山脈，利用法線貼圖、菲涅爾效應(yīng)、漫反射著色器以及折射著色器等模擬水體，采用Skybox 及光源模擬天空和光線。

如圖3 所示，虛擬場(chǎng)景導(dǎo)入無(wú)人艇虛擬孿生體，把附體坐標(biāo)系的坐標(biāo)原點(diǎn)設(shè)置在無(wú)人艇質(zhì)心位置，xb軸正方向指向船首，zb軸正方向指向左舷，yb軸正方向豎直向上。無(wú)人艇虛擬孿生體初始位置位于虛擬場(chǎng)景中央，滿足航行試驗(yàn)需求。

圖3 虛擬場(chǎng)景及場(chǎng)景中的無(wú)人艇虛擬孿生體Fig.3 Virtual scene and twin USV inside

2.2 實(shí)測(cè)數(shù)據(jù)驅(qū)動(dòng)三維可視化交互設(shè)計(jì)

三維可視化交互軟件使用QGC 傳回的位姿數(shù)據(jù)來(lái)驅(qū)動(dòng)無(wú)人艇虛擬孿生體。對(duì)于位置數(shù)據(jù)，將GPS 測(cè)量得到的經(jīng)緯度信息經(jīng)坐標(biāo)變換轉(zhuǎn)化到虛擬場(chǎng)景的世界坐標(biāo)系；對(duì)于姿態(tài)數(shù)據(jù)，將歐拉角形式姿態(tài)信息轉(zhuǎn)換為四元數(shù)，避免萬(wàn)向節(jié)死鎖問題。

無(wú)人艇位姿數(shù)據(jù)采樣率取決于無(wú)人艇的控制周期和傳感器自身固有屬性，本文中位置和姿態(tài)數(shù)據(jù)采樣率分別為5 Hz 和50 Hz。而虛擬場(chǎng)景中流暢的畫面通常要求幀率在30FPS 以上，為此，對(duì)坐標(biāo)系轉(zhuǎn)換后的位置數(shù)據(jù)進(jìn)行線性插值和球面插值補(bǔ)幀，使得無(wú)人艇在虛擬場(chǎng)景中的動(dòng)態(tài)畫面流暢且過渡平滑。

真實(shí)位姿數(shù)據(jù)驅(qū)動(dòng)無(wú)人艇虛擬孿生體動(dòng)態(tài)畫面刷新原理如圖4 所示。在運(yùn)動(dòng)控制Script 中，在每一幀畫面刷新時(shí)調(diào)用一次Update 函數(shù)，經(jīng)差值優(yōu)化的位姿變量賦值給無(wú)人艇虛擬孿生體Transform 屬性，更新無(wú)人艇虛擬孿生體在下一幀動(dòng)畫中的位姿狀態(tài)，結(jié)合Unity3D 圖形引擎進(jìn)行動(dòng)態(tài)渲染，產(chǎn)生動(dòng)態(tài)畫面。

圖4 位姿數(shù)據(jù)驅(qū)動(dòng)無(wú)人艇虛擬孿生體原理Fig.4 The principle of position and attitude data driving twin USV

3 實(shí)體無(wú)人艇及監(jiān)控設(shè)計(jì)

3.1 實(shí)體無(wú)人艇基本組成

本系統(tǒng)實(shí)體無(wú)人艇為基于Pixhawk 的小型噴水推進(jìn)無(wú)人艇（總長(zhǎng)0.66 m，型寬0.20 m），由控制系統(tǒng)、動(dòng)力系統(tǒng)、感知系統(tǒng)、通信系統(tǒng)等構(gòu)成，各系統(tǒng)部件沿中縱剖面布置在無(wú)人艇兩舷。感知系統(tǒng)主要由GPS 和慣導(dǎo)組成。

3.2 實(shí)體無(wú)人艇監(jiān)控系統(tǒng)

實(shí)體無(wú)人艇監(jiān)控系統(tǒng)由無(wú)人艇和上位機(jī)組成，如圖5 所示。上位機(jī)運(yùn)行QGC 軟件，通過3DR 433 MHz數(shù)傳與無(wú)人艇通信，實(shí)現(xiàn)向無(wú)人艇發(fā)送指令，同時(shí)接收無(wú)人艇傳回各種狀態(tài)。本系統(tǒng)上位機(jī)可規(guī)劃無(wú)人艇的航行任務(wù)、航路、下達(dá)指令；接收無(wú)人艇的實(shí)時(shí)狀態(tài)反饋，在QGC 和三維可視化交互軟件界面實(shí)時(shí)顯示無(wú)人艇的航行狀態(tài)、地圖軌跡等信息。

圖5 無(wú)人艇監(jiān)控硬件系統(tǒng)Fig.5 USV monitoring hardware system

4 船體任意點(diǎn)速度估計(jì)原理與驗(yàn)證

4.1 虛實(shí)結(jié)合跨空間位姿數(shù)據(jù)融合原理

根據(jù)實(shí)體無(wú)人艇的位姿時(shí)序數(shù)據(jù)，結(jié)合虛擬孿生體幾何模型數(shù)據(jù)，本文實(shí)現(xiàn)了虛實(shí)結(jié)合跨空間數(shù)據(jù)融合，可估算船體任意位置的實(shí)時(shí)速度。

將無(wú)人艇視為剛體，虛擬孿生體質(zhì)心是旋轉(zhuǎn)中心，也是附體坐標(biāo)系原點(diǎn)Ob（與實(shí)體無(wú)人艇質(zhì)心位置相同）。如圖6 所示，船體的自身旋轉(zhuǎn)運(yùn)動(dòng)可表示為ω=[ωxωyωz]T，任意點(diǎn)i的速度為平移速度與轉(zhuǎn)動(dòng)線速度 ω ×r的向量和，即Vi=VOb+ω×r，其中 r=[rx ry rz]T是i點(diǎn)對(duì)Ob的向徑。

圖6 任意點(diǎn)i 的速度分析Fig.6 Velocity analysis of arbitrary point i

2）無(wú)人艇平移運(yùn)動(dòng)是前進(jìn)、橫漂、起伏3 個(gè)方向的疊加，即 VOb=[VObx VOby VObz]T。平移速度VOb由GPS 與慣導(dǎo)數(shù)據(jù)采集至Pixhawk 由擴(kuò)展卡爾曼濾波計(jì)算得出[12]。無(wú)人艇在水面起伏運(yùn)動(dòng)通?？珊雎?，即假設(shè)VOby=0 。VObx與VObz的 合速度用VG表示，其標(biāo)量值為 vG。由于橫漂的存在，導(dǎo)致無(wú)人艇平移運(yùn)動(dòng)方向（VG方向）與首向（VObx方向）存在偏差，其夾角用偏航角β 表示。

圖7 首向角與航向角分析Fig.7 Analysis of heading angle and course angle

根據(jù)公式Vi=VOb+ω×r，可以寫出任意點(diǎn)i在某一時(shí)刻t的離散形式速度表達(dá)式：

按照矢量求和方法可計(jì)算i點(diǎn)合速度值vi，最終可得vi與無(wú)人艇位姿信息的關(guān)系為：

其中，xie,yie,zie為無(wú)人艇虛擬孿生體在世界坐標(biāo)系的位置，由GPS 數(shù)據(jù)經(jīng)坐標(biāo)變換獲得；任意點(diǎn)的附體坐標(biāo)可由Unity3D 引擎提供的Ray 組件獲取。

4.2 無(wú)人艇任意位置實(shí)時(shí)速度估計(jì)湖試驗(yàn)證

進(jìn)行本文開發(fā)的無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)及任意位置速度估計(jì)功能的湖試。以某一段直角轉(zhuǎn)彎航路為例分析，航行軌跡如圖8 所示。

圖8 試驗(yàn)航行軌跡Fig.8 Experimental navigation trajectory

如圖9 所示，三維可視化交互軟件速度估計(jì)界面，鼠標(biāo)點(diǎn)擊無(wú)人艇虛擬孿生體任意位置可計(jì)算該點(diǎn)實(shí)時(shí)速度。在無(wú)人艇船體上任取四點(diǎn)作為速度估計(jì)觀測(cè)點(diǎn)，取Obxz平面內(nèi)一點(diǎn)A（0.1，0，0.1）、Obyz平面內(nèi)一點(diǎn)B（0，0.05，0.46）、附體坐標(biāo)系原點(diǎn)上方的投影點(diǎn)C（0，0.08，0）和甲板上任一點(diǎn)D（-0.2，0.05，-0.1）。

圖9 無(wú)人艇速度估計(jì)界面及試驗(yàn)點(diǎn)位置分布Fig.9 Speed estimate UI for USV and experimental point layout

受限于本試驗(yàn)無(wú)人艇尺寸，4 個(gè)試驗(yàn)點(diǎn)與無(wú)人艇質(zhì)心Ob距離較短。試驗(yàn)點(diǎn)的實(shí)際速度與質(zhì)心位置速度由于角速度變化引起的偏差不明顯。為說明常規(guī)無(wú)人艇[13]（船長(zhǎng)尺度5m 及以上）的任意點(diǎn)的速度差異，將所選試驗(yàn)點(diǎn)的坐標(biāo)值放大10 倍，即A′（1，0，1）、B′（0，0 .5，4.6）、C′（0，0.8，0）、D′（-2，0.5，-1）。對(duì)實(shí)際湖試航行過程各個(gè)試驗(yàn)點(diǎn)進(jìn)行速度對(duì)比分析，結(jié)果如圖10～圖13 所示（E和D分別表示試驗(yàn)點(diǎn)相對(duì)Ob點(diǎn)速度偏差的期望和方差）。

無(wú)人艇在415～435 s 和435～455 s 兩個(gè)時(shí)間段內(nèi)直線航行，為降低轉(zhuǎn)彎過程軌跡跟蹤誤差，無(wú)人艇自主程序控制會(huì)在435～440 s 時(shí)間段降速完成直角轉(zhuǎn)彎過程。由圖10～圖13 可見，試驗(yàn)點(diǎn)A-D的實(shí)際速度均與無(wú)人艇質(zhì)心Ob不等，但由于各點(diǎn)相對(duì)質(zhì)心距離較短，因此速度偏差并不顯著。然而，對(duì)于常規(guī)尺度無(wú)人艇，當(dāng)坐標(biāo)值放大10 倍后，試驗(yàn)點(diǎn)A′-D′與無(wú)人艇質(zhì)心Ob的速度偏差明顯增大。以D點(diǎn)和D′點(diǎn)為例，ED為2.62×10-4m/s，而ED′達(dá)到7.74×10-2m/s。盡管坐標(biāo)值僅變化10 倍，但2 個(gè)試驗(yàn)點(diǎn)的平均速度偏差卻相差2.95×102倍。此外，值得注意的是，在直角轉(zhuǎn)彎過程，無(wú)人艇會(huì)產(chǎn)生繞yb軸的明顯轉(zhuǎn)艏運(yùn)動(dòng)，旋轉(zhuǎn)角速度會(huì)顯著大于直航段，因此各個(gè)試驗(yàn)點(diǎn)在435～440 s時(shí)間段內(nèi)出現(xiàn)了較為顯著的速度偏差。尤其當(dāng)坐標(biāo)值放大10 倍后，無(wú)人艇的船體任意點(diǎn)速度會(huì)與質(zhì)心Ob位置的明顯不同。由此，對(duì)大尺度無(wú)人艇，由于自身姿態(tài)變化引起的船體局部位置的速度偏差將更加顯著，這對(duì)無(wú)人艇局部位置布置的設(shè)備運(yùn)轉(zhuǎn)將產(chǎn)生重要影響。

圖10 航行過程A 點(diǎn)、A′點(diǎn)與 Ob點(diǎn)的速度對(duì)比Fig.10 Velocity comparison between point A,A′ and point Ob

圖11 航行過程B 點(diǎn)、B′點(diǎn)與 Ob 點(diǎn)的速度對(duì)比Fig.11 Velocity comparison between point B,B′ and point Ob

圖12 航行過程C 點(diǎn)、C′點(diǎn)與 Ob 點(diǎn)的速度對(duì)比Fig.12 Velocity comparison between point C,C′ and point Ob

圖13 航行過程D 點(diǎn)、D′與 Ob點(diǎn)的速度對(duì)比Fig.13 Velocity comparison between point D,D′ and point Ob

5 結(jié) 語(yǔ)

本文建立基于數(shù)字孿生的無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)，實(shí)現(xiàn)了在信息空間真實(shí)映射物理空間無(wú)人艇航行狀態(tài)，并可在信息空間估計(jì)無(wú)人艇任意位置速度。該系統(tǒng)以自研無(wú)人艇為物理空間實(shí)體對(duì)象，使用Unity3D在信息空間搭建了無(wú)人艇虛擬孿生體，利用QGC 軟件建立物理空間與虛擬空間的交互接口。利用虛實(shí)結(jié)合跨空間姿態(tài)數(shù)據(jù)融合原理，將物理空間實(shí)時(shí)觀測(cè)位姿信息與信息空間無(wú)人艇幾何模型信息融合。基于有限數(shù)量的傳感器實(shí)現(xiàn)了對(duì)無(wú)人艇全船任意位置的實(shí)時(shí)姿態(tài)估計(jì)。通過湖試驗(yàn)證了本系統(tǒng)的三維交互與速度估計(jì)功能。當(dāng)無(wú)人艇角速度增大導(dǎo)致姿態(tài)變化較大時(shí)，無(wú)人艇局部位置的合速度將與質(zhì)心位置的偏差明顯增加，且隨著與質(zhì)心間距增加，這種偏差效應(yīng)更加顯著。

本文建立的無(wú)人艇虛實(shí)結(jié)合三維交互系統(tǒng)有助于船舶智能感知技術(shù)發(fā)展，但在信息空間估計(jì)數(shù)據(jù)服務(wù)于物理空間方面仍需進(jìn)一步探究.