褚金奎，林木音，王寅龍，李金山，郭曉慶

(大連理工大學(xué) 機(jī)械工程學(xué)院，遼寧 大連 116024)

1 引 言

地球約三分之二的面積被海洋覆蓋，但僅有為數(shù)不多的海域被完全探索[1]。氣候變化、環(huán)境異常、資源探索、國家安全等問題都引發(fā)了商業(yè)、科學(xué)和軍事領(lǐng)域?qū)ψ灾魉婧叫衅?Unmanned Surface Vehicle,USV)即無人船的強(qiáng)烈需求。因此，無人船得到了極大的發(fā)展，具有目標(biāo)探測、環(huán)境感知與自主航行等功能。但單個無人船的作業(yè)能力、搭載資源有限，難以執(zhí)行復(fù)雜的規(guī)模性任務(wù)，通過控制多無人船編隊(duì)協(xié)同執(zhí)行任務(wù)，擴(kuò)大了其作業(yè)范圍，提高了作業(yè)效率與任務(wù)復(fù)雜度[2]。

然而，導(dǎo)航與編隊(duì)協(xié)同以個體的導(dǎo)航信息準(zhǔn)確性為基石，無人船的傳感器受環(huán)境的干擾會導(dǎo)致個體的失控與編隊(duì)的失穩(wěn)。無人船采用的慣性導(dǎo)航、地磁導(dǎo)航等傳統(tǒng)導(dǎo)航方式存在誤差累積、易受干擾等缺點(diǎn)[3]，難以在復(fù)雜的水上環(huán)境(環(huán)境噪聲、傳感器漂移、時變模型的不確定性、傳感器誤差)中獲得精確穩(wěn)定的航向、位置信息[4]，因此，結(jié)合其他傳感器來輔助提高導(dǎo)航性能受到學(xué)者們的關(guān)注。Mohamed等人[5]提出一種匹配相機(jī)前后時刻圖像求位移的算法，在無GPS環(huán)境下將單目相機(jī)作為視覺里程計與慣導(dǎo)組合進(jìn)行無人船導(dǎo)航。Naus等人[6]利用激光雷達(dá)輔助進(jìn)行無人船航行、轉(zhuǎn)向速度的精確控制。Jeff[7]將多普勒測速儀與慣導(dǎo)組合應(yīng)用于無人船導(dǎo)航。雷達(dá)被用來探測航行中的障礙物[8-9]。上述文獻(xiàn)均僅在位置、速度信息上進(jìn)行修正，航向信息仍采用傳統(tǒng)地磁導(dǎo)航方式。而在海上環(huán)境復(fù)雜、地球磁場遷移變化[10]、磁羅盤易被干擾的今天，亟需采用新型傳感器來輔助校正航向信息。

偏振光導(dǎo)航是一種新興的導(dǎo)航方式，借鑒昆蟲利用天空偏振光進(jìn)行導(dǎo)航的思想發(fā)展而來，具有無誤差積累、不易受到干擾的特性。1949年，F(xiàn)risch[11]發(fā)現(xiàn)了蜜蜂利用天空偏振光進(jìn)行導(dǎo)航。1997年，Lambrinos等人[12]基于昆蟲偏振光導(dǎo)航思想搭建了偏振光導(dǎo)航傳感器用于地面機(jī)器人導(dǎo)航，證實(shí)了利用天空偏振光信息導(dǎo)航的可行性。2008年，褚金奎等人[13]搭建了國內(nèi)首個六通道偏振光傳感器，并實(shí)現(xiàn)了無人車的偏振光自主導(dǎo)航[14]。2012年，Chahl等人[15]將偏振光傳感器搭載于無人機(jī)測量其航向角。2017年，胡小平等人[16]提出一種偏振光組合導(dǎo)航方案并通過跑車實(shí)驗(yàn)證實(shí)了可行性。2018年，Julien等人[17]利用兩通道紫外偏振光傳感器實(shí)現(xiàn)了六足機(jī)器人的循跡。偏振光傳感器同時也被廣泛研究應(yīng)用于定位[18-21]、定姿[22]與組合導(dǎo)航中[23-25]。2018年，陳永臺等人[26]搭建了全天空偏振成像探測器。2019年，萬振華等人[27]對成像式偏振光傳感器進(jìn)行了誤差模型的建立與驗(yàn)證。上述文獻(xiàn)的偏振光導(dǎo)航應(yīng)用環(huán)境均為平穩(wěn)環(huán)境，而在波動水面的非穩(wěn)定環(huán)境下載體無法始終保持水平狀態(tài)，從而影響偏振光傳感器的測量精度。支煒等人[22]對偏振光傳感器傾斜的問題提出了補(bǔ)償算法。盧鴻謙等人[23]推導(dǎo)了偏振光在三維環(huán)境組合導(dǎo)航的應(yīng)用原理并進(jìn)行了仿真分析。

為了實(shí)現(xiàn)偏振光傳感器在水面波動環(huán)境下及編隊(duì)協(xié)同情境下的應(yīng)用，并提升無人船導(dǎo)航的抗電磁干擾能力，本文首先設(shè)計了偏振光傳感器/MIMU/GPS組合導(dǎo)航算法，并搭建了偏振光傳感器的無人船實(shí)驗(yàn)平臺，同時將偏振光傳感器集成于云臺以提升它在波動環(huán)境下的輸出穩(wěn)定性；然后與傳統(tǒng)地磁導(dǎo)航方式進(jìn)行了循跡對比實(shí)驗(yàn)，證實(shí)了偏振光傳感器應(yīng)用于水面環(huán)境的可行性及在磁羅盤受干擾情況下偏振光組合導(dǎo)航系統(tǒng)仍可正常工作；最后進(jìn)行了無人船的編隊(duì)實(shí)驗(yàn)，實(shí)現(xiàn)了基于偏振光組合導(dǎo)航系統(tǒng)的無人船編隊(duì)航行。

2 偏振光組合導(dǎo)航與編隊(duì)原理

2.1 偏振光導(dǎo)航原理

太陽輻射的無偏振態(tài)自然光在穿過大氣層時被大氣中的粒子吸收和散射，發(fā)生瑞利散射和米氏散射，從而在天空中產(chǎn)生穩(wěn)定且規(guī)律分布的偏振光。天空中任意一點(diǎn)偏振光的偏振方向(E矢量)總是垂直于由太陽、觀測點(diǎn)和被觀測點(diǎn)組成的平面。基于天空偏振光分布模式，本文采用的偏振光導(dǎo)航傳感器通過測量其體軸與天頂點(diǎn)入射光E矢量方向的夾角，從而計算得到其體軸與太陽子午線的夾角來獲得航向角。

圖1 偏振光導(dǎo)航原理Fig.1 Principle diagram of polarized light navigation

圖2 偏振光傳感器坐標(biāo)系Fig.2 Coordinate system of polarization sensor

(1)

(2)

太陽矢量OS在世界坐標(biāo)系的表示為：

OSn=

(3)

由瑞利散射規(guī)律得到：

En=OSn×OPn.

(4)

聯(lián)立式(1)～式(4)即可得到航向角：

(5)

2.2 偏振光組合導(dǎo)航算法

偏振光組合導(dǎo)航系統(tǒng)組成如圖3所示，包含了偏振光傳感器、加速度計、陀螺儀及GPS模塊。通過偏振光傳感器獲得航向角，GPS獲得位置及速度，利用加速度計與陀螺儀慣性解算位置和速度及姿態(tài)。以GPS、偏振光傳感器測得的位置/速度/姿態(tài)與慣性組件MIMU解算的位置/速度/姿態(tài)的差值作為觀測量，通過卡爾曼濾波器預(yù)測位置/速度/姿態(tài)誤差，最后將誤差通過PI控制器返回MIMU慣性解算作為補(bǔ)償，以補(bǔ)償后的姿態(tài)、位置和速度數(shù)據(jù)作為輸出。對比直接輸出卡爾曼濾波器預(yù)測姿態(tài)、位置的方式提高了姿態(tài)、位置信息的更新頻率。

圖3 偏振光傳感器/MIMU/GPS組合導(dǎo)航系統(tǒng)框圖Fig.3 Block diagram of polarization sensor /MIMU/GPS integrated navigation system

卡爾曼濾波器狀態(tài)方程：

X′=FX+W,

(6)

Z=HX+V.

(7)

以GPS與偏振光傳感器的輸出與MIMU解算的差值作為觀測量，則:

Z=

(8)

GPS與偏振光傳感器的測量誤差為:

(9)

H為狀態(tài)量與觀測量轉(zhuǎn)換矩陣:

(10)

(11)

其中KP,KI為比例、積分系數(shù)，下標(biāo)k代表時刻。

2.3 編隊(duì)控制策略

編隊(duì)控制策略基于制導(dǎo)-導(dǎo)航的思想，領(lǐng)航船獲得自身GPS坐標(biāo)后，將它與預(yù)設(shè)隊(duì)形的跟隨船位置矢量加和后得到跟隨船的期望坐標(biāo)并廣播，跟隨船收到期望坐標(biāo)信息后根據(jù)自身位置由各跟隨船控制器導(dǎo)航至目標(biāo)點(diǎn)，編隊(duì)策略如圖4所示。

圖4 無人船編隊(duì)策略框圖Fig.4 Block diagram of USV formation strategy

以路徑跟蹤、隊(duì)形保持任務(wù)為例，首先定義領(lǐng)航船和所有從船的相對位置向量Li，i=1,2,3…，如圖5所示，在東-北坐標(biāo)系下，L1,L2為預(yù)設(shè)隊(duì)形跟隨船與領(lǐng)航船相對位置矢量，領(lǐng)航船將自身GPS坐標(biāo)與相對位置向量Li加和得到各從船的期望位置并發(fā)送。獨(dú)立的設(shè)置各從船的控制導(dǎo)航系統(tǒng)以實(shí)現(xiàn)從船自治，使接收到領(lǐng)航船廣播的期望坐標(biāo)后根據(jù)自身的實(shí)時位置自動導(dǎo)航。

圖5 無人船預(yù)設(shè)隊(duì)形定義Fig.5 Definition of USV preset formation

定義跟隨船的期望路徑為：

ηdi(θ)=ηd(θ)+R(ψ(θ))Li,

(12)

(13)

若實(shí)現(xiàn)隊(duì)形保持即有：

(14)

各跟隨船實(shí)際坐標(biāo)與期望坐標(biāo)誤差趨近于0，Pi為跟隨船實(shí)際坐標(biāo)。

3 偏振光傳感器的無人船實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺

無人船實(shí)驗(yàn)平臺(如圖6)采用Pixhawk控制平臺，搭載了MPU6000慣性傳感器(包含三軸加速度計、三軸陀螺儀)，并另外集成了L3GD20陀螺儀與LSM303D加速度計，組成雙慣性器件平臺進(jìn)行慣性姿態(tài)、位置及速度的解算，電子羅盤HMC5883L用以提供航向角信息。在無人船頂部安裝有M8N GPS模塊用以提供位置、速度信息；2.4G數(shù)傳模塊用以與地面站實(shí)現(xiàn)信息交互、任務(wù)規(guī)劃；遙控器用以切換偏振光組合導(dǎo)航系統(tǒng)與磁羅盤組合導(dǎo)航系統(tǒng)；在無人船尾部搭載了本課題組研發(fā)的偏振光導(dǎo)航傳感器[29]，用以提供航向角信息，并為其集成云臺底座以提升波動情況下的輸出穩(wěn)定性(如圖7)。無人船實(shí)驗(yàn)平臺采用傳感器精度信息如表1所示。

圖6 無人船實(shí)驗(yàn)平臺Fig.6 USV experimental platform

圖7 偏振光傳感器集成云臺結(jié)構(gòu)Fig.7 Structure diagram of polarization sensor integrated gimbal

表1 組合導(dǎo)航系統(tǒng)傳感器精度信息

3.2 循跡對比實(shí)驗(yàn)

實(shí)驗(yàn)地點(diǎn)為大連海事大學(xué)心海湖(121°31′26″E，38°52′8.4″N)。實(shí)驗(yàn)時間為2019年8月22日17時40分，實(shí)驗(yàn)天氣晴朗無云。實(shí)驗(yàn)預(yù)設(shè)軌跡為15 m的正方形。無人船航行速度設(shè)定為0.5 m/s。

實(shí)驗(yàn)中使無人船沿預(yù)定方形路徑連續(xù)航行兩圈，其中第一圈采用偏振光傳感器/MIMU/GPS組合導(dǎo)航系統(tǒng)，第二圈切換為磁羅盤/MIMU/GPS導(dǎo)航系統(tǒng)。記錄航行軌跡數(shù)據(jù)如圖8所示，航向角數(shù)據(jù)如圖9所示。利用GPS數(shù)據(jù)分段求解實(shí)際軌跡與預(yù)設(shè)軌跡的偏離程度與航向角偏差，結(jié)果如表2所示。位置偏差求解方法：首先利用4個設(shè)定角點(diǎn)坐標(biāo)求解設(shè)定路徑方程的4段線段，隨后將實(shí)際軌跡GPS坐標(biāo)點(diǎn)按對應(yīng)線段分為4組求位置偏差，最后對所有偏差加和求取均值作為偏離程度(由于無人船設(shè)定在航點(diǎn)半徑1 m范圍內(nèi)視為到達(dá)，并向下一航點(diǎn)做轉(zhuǎn)向調(diào)整，故在此范圍內(nèi)不考慮偏差)。航向角偏差求解方法：通過SD卡記錄的航向角與預(yù)設(shè)軌跡航向角做差得到航向角偏差，對所有坐標(biāo)點(diǎn)處航向角偏差求均值作為航向角偏差。

圖8 無人船循跡實(shí)驗(yàn)軌跡Fig.8 Experimental tracks of USVs

圖9 無人船航向角Fig.9 Heading angle of USVs

通過圖8、圖9與表2可以得到，偏振光傳感器/MIMU/GPS組合導(dǎo)航系統(tǒng)軌跡偏離預(yù)設(shè)軌跡平均值為 0.209 m，航向角偏差為6.055°。磁羅盤/MIMS/GPS組合導(dǎo)航系統(tǒng)偏離預(yù)設(shè)軌跡平均值為 0.248 m，航向角偏差為7.897°。在軌跡圖8的(5,18)處、航向角圖9的360°區(qū)域磁羅盤受到干擾精度降低、數(shù)據(jù)波動較大，對比采用偏振光組合導(dǎo)航系統(tǒng)的無人船軌跡具有明顯偏差。在磁羅盤短時間受到干擾的情況下偏振光組合導(dǎo)航系統(tǒng)不受影響，將偏振光導(dǎo)航傳感器融入后導(dǎo)航循跡精度提升了15.7 %；航向角精度提高23.3%。由此可知，偏振光組合導(dǎo)航系統(tǒng)具有較高的魯棒性，且誤差不隨時間累積、實(shí)時性好。

表2 兩種導(dǎo)航方式下誤差對比

3.3 編隊(duì)實(shí)驗(yàn)

實(shí)驗(yàn)地點(diǎn)為大連海事大學(xué)心海湖(121°31′26″E，38°52′8.4″N)。實(shí)驗(yàn)時間為2019年12月13日15時30分，實(shí)驗(yàn)天氣晴朗無云。實(shí)驗(yàn)預(yù)設(shè)軌跡為邊長7.5 m的“W”形。無人船導(dǎo)航系統(tǒng)采用偏振光傳感器/MIMU/GPS系統(tǒng)。

圖10 無人船編隊(duì)Fig.10 Photo of USVs formation experiment

實(shí)驗(yàn)中設(shè)定黑色無人船為領(lǐng)航船，領(lǐng)航速度為0.5 m/s，紅色無人船為跟隨船，跟隨速度為0.7 m/s。領(lǐng)航船領(lǐng)先跟隨船距離1.5 m成跟隨狀編隊(duì)行駛(如圖10)。使領(lǐng)航船沿預(yù)設(shè)軌跡航行一周，并通過2.4G數(shù)傳模塊鏈接跟隨船向其實(shí)時發(fā)送期望坐標(biāo)(期望坐標(biāo)由領(lǐng)航船GPS坐標(biāo)與預(yù)設(shè)隊(duì)形的跟隨船相對位置向量Li矢量加和得到)。跟隨船接收到領(lǐng)航船廣播的期望坐標(biāo)后由該船導(dǎo)航控制器自動導(dǎo)航至期望點(diǎn)。

編隊(duì)實(shí)驗(yàn)航行軌跡如圖11所示。將領(lǐng)航船軌跡與預(yù)設(shè)編隊(duì)矢量Li加和得到跟隨船的期望路徑，由于本實(shí)驗(yàn)設(shè)定為跟隨隊(duì)形故跟隨船理論軌跡與領(lǐng)航船軌跡重合。通過計算得到領(lǐng)航者的軌跡誤差為0.425 m，跟隨者的軌跡誤差為0.707 m。領(lǐng)航船軌跡誤差的計算方法同上述循跡實(shí)驗(yàn)位置誤差計算，區(qū)別是將航點(diǎn)處的位置偏差計算在內(nèi)。跟隨船軌跡誤差的計算方法為：將兩船GPS航點(diǎn)對齊后計算相應(yīng)的位置誤差加和求均值后作為軌跡誤差。

圖11 無人船編隊(duì)實(shí)驗(yàn)軌跡Fig.11 Experimental tracks of USVs formation

在實(shí)際編隊(duì)過程中，由于領(lǐng)航船轉(zhuǎn)向半徑小，跟隨船轉(zhuǎn)彎處仍保持預(yù)設(shè)隊(duì)形，故跟隨船期望點(diǎn)在轉(zhuǎn)彎處偏離領(lǐng)航船軌跡(如圖12)，t1,t2時刻領(lǐng)航者與跟隨者軌跡基本重合，但在t3時刻領(lǐng)航者迅速轉(zhuǎn)向并保持預(yù)設(shè)隊(duì)形，使跟隨者期望坐標(biāo)點(diǎn)偏離領(lǐng)航者軌跡，t4時刻后又重合。t3時刻偏離誤差理論值為：|Li|-R，其中R為領(lǐng)航船轉(zhuǎn)向半徑。在t1，t2，t4直線段時刻的偏離誤差理論值為：|Li|sin(θ)，其中θ為領(lǐng)航船行駛在直線軌跡中的實(shí)時航向角輸出與預(yù)設(shè)軌跡切向的角度差值。

圖12 跟隨者軌跡偏差理論圖Fig.12 Principle diagram of tracking bias

4 結(jié) 論

為了實(shí)現(xiàn)偏振光傳感器在水面波動環(huán)境下及編隊(duì)協(xié)同情境下的應(yīng)用，并提升無人船導(dǎo)航的抗電磁干擾能力。本文設(shè)計了偏振光傳感器/MIMU/GPS組合導(dǎo)航算法，搭建了偏振光無人船導(dǎo)航平臺進(jìn)行導(dǎo)航與編隊(duì)實(shí)驗(yàn)。循跡對比實(shí)驗(yàn)結(jié)果表明：偏振光組合導(dǎo)航系統(tǒng)航向角誤差為6.055°，循跡位置誤差為0.209 m，在磁羅盤短時受到干擾的情況下該組合導(dǎo)航系統(tǒng)不受影響。編隊(duì)實(shí)驗(yàn)結(jié)果表明：領(lǐng)航船循跡誤差為0.425 m，跟隨者編隊(duì)的軌跡誤差為0.707 m，偏振光組合導(dǎo)航系統(tǒng)可應(yīng)用于無人船導(dǎo)航與編隊(duì)。

目前，偏振光無人船導(dǎo)航平臺僅應(yīng)用于湖面環(huán)境，下一步的工作是通過云臺底座的姿態(tài)芯片感知補(bǔ)償后仍存在的小傾斜度誤差并輸入偏振光傳感器的計算芯片，在此基礎(chǔ)上加入補(bǔ)償算法使傳感器輸出更加穩(wěn)定。同時進(jìn)行湖面、河面、海面環(huán)境下的無人船同軌跡循跡對比實(shí)驗(yàn)，進(jìn)一步驗(yàn)證偏振光組合導(dǎo)航系統(tǒng)應(yīng)用于水面導(dǎo)航的可行性。