偏振光傳感器的無人船導(dǎo)航與編隊?wèi)?yīng)用

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

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

1 引 言

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

然而，導(dǎo)航與編隊協(xié)同以個體的導(dǎo)航信息準(zhǔn)確性為基石，無人船的傳感器受環(huán)境的干擾會導(dǎo)致個體的失控與編隊的失穩(wěn)。無人船采用的慣性導(dǎo)航、地磁導(dǎo)航等傳統(tǒng)導(dǎo)航方式存在誤差累積、易受干擾等缺點[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)航，證實了利用天空偏振光信息導(dǎo)航的可行性。2008年，褚金奎等人[13]搭建了國內(nèi)首個六通道偏振光傳感器，并實現(xiàn)了無人車的偏振光自主導(dǎo)航[14]。2012年，Chahl等人[15]將偏振光傳感器搭載于無人機(jī)測量其航向角。2017年，胡小平等人[16]提出一種偏振光組合導(dǎo)航方案并通過跑車實驗證實了可行性。2018年，Julien等人[17]利用兩通道紫外偏振光傳感器實現(xiàn)了六足機(jī)器人的循跡。偏振光傳感器同時也被廣泛研究應(yīng)用于定位[18-21]、定姿[22]與組合導(dǎo)航中[23-25]。2018年，陳永臺等人[26]搭建了全天空偏振成像探測器。2019年，萬振華等人[27]對成像式偏振光傳感器進(jìn)行了誤差模型的建立與驗證。上述文獻(xiàn)的偏振光導(dǎo)航應(yīng)用環(huán)境均為平穩(wěn)環(huán)境，而在波動水面的非穩(wěn)定環(huán)境下載體無法始終保持水平狀態(tài)，從而影響偏振光傳感器的測量精度。支煒等人[22]對偏振光傳感器傾斜的問題提出了補償算法。盧鴻謙等人[23]推導(dǎo)了偏振光在三維環(huán)境組合導(dǎo)航的應(yīng)用原理并進(jìn)行了仿真分析。

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

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

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

太陽輻射的無偏振態(tài)自然光在穿過大氣層時被大氣中的粒子吸收和散射，發(fā)生瑞利散射和米氏散射，從而在天空中產(chǎn)生穩(wěn)定且規(guī)律分布的偏振光。天空中任意一點偏振光的偏振方向(E矢量)總是垂直于由太陽、觀測點和被觀測點組成的平面。基于天空偏振光分布模式，本文采用的偏振光導(dǎo)航傳感器通過測量其體軸與天頂點入射光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慣性解算作為補償，以補償后的姿態(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 編隊控制策略

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

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

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

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

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

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

(12)

(13)

若實現(xiàn)隊形保持即有：

(14)

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

3 偏振光傳感器的無人船實驗

3.1 實驗平臺

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

圖6 無人船實驗平臺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 循跡對比實驗

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

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

圖8 無人船循跡實驗軌跡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)具有較高的魯棒性，且誤差不隨時間累積、實時性好。

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

3.3 編隊實驗

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

圖10 無人船編隊Fig.10 Photo of USVs formation experiment

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

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

圖11 無人船編隊實驗軌跡Fig.11 Experimental tracks of USVs formation

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

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

4 結(jié) 論

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

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