偏振光傳感器的無人船導航與編隊應用

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

(大連理工大學 機械工程學院，遼寧 大連 116024)

1 引 言

地球約三分之二的面積被海洋覆蓋，但僅有為數不多的海域被完全探索[1]。氣候變化、環境異常、資源探索、國家安全等問題都引發了商業、科學和軍事領域對自主水面航行器(Unmanned Surface Vehicle,USV)即無人船的強烈需求。因此，無人船得到了極大的發展，具有目標探測、環境感知與自主航行等功能。但單個無人船的作業能力、搭載資源有限，難以執行復雜的規模性任務，通過控制多無人船編隊協同執行任務，擴大了其作業范圍，提高了作業效率與任務復雜度[2]。

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

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

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

2 偏振光組合導航與編隊原理

2.1 偏振光導航原理

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

圖1 偏振光導航原理Fig.1 Principle diagram of polarized light navigation

圖2 偏振光傳感器坐標系Fig.2 Coordinate system of polarization sensor

(1)

(2)

太陽矢量OS在世界坐標系的表示為：

OSn=

(3)

由瑞利散射規律得到：

En=OSn×OPn.

(4)

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

(5)

2.2 偏振光組合導航算法

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

圖3 偏振光傳感器/MIMU/GPS組合導航系統框圖Fig.3 Block diagram of polarization sensor /MIMU/GPS integrated navigation system

卡爾曼濾波器狀態方程：

X′=FX+W,

(6)

Z=HX+V.

(7)

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

Z=

(8)

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

(9)

H為狀態量與觀測量轉換矩陣:

(10)

(11)

其中KP,KI為比例、積分系數，下標k代表時刻。

2.3 編隊控制策略

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

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

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

圖5 無人船預設隊形定義Fig.5 Definition of USV preset formation

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

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

(12)

(13)

若實現隊形保持即有：

(14)

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

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

3.1 實驗平臺

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

圖6 無人船實驗平臺Fig.6 USV experimental platform

圖7 偏振光傳感器集成云臺結構Fig.7 Structure diagram of polarization sensor integrated gimbal

表1 組合導航系統傳感器精度信息

3.2 循跡對比實驗

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

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

圖8 無人船循跡實驗軌跡Fig.8 Experimental tracks of USVs

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

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

表2 兩種導航方式下誤差對比

3.3 編隊實驗

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

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

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

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

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

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

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

4 結 論

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

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