偏振光传感器的无人船导航与编队应用

褚金奎，林木音，王寅龙，李金山，郭晓庆

(大连理工大学 机械工程学院，辽宁 大连 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，偏振光组合导航系统可应用于无人船导航与编队。

目前，偏振光无人船导航平台仅应用于湖面环境，下一步的工作是通过云台底座的姿态芯片感知补偿后仍存在的小倾斜度误差并输入偏振光传感器的计算芯片，在此基础上加入补偿算法使传感器输出更加稳定。同时进行湖面、河面、海面环境下的无人船同轨迹循迹对比实验，进一步验证偏振光组合导航系统应用于水面导航的可行性。