基于偏振光的导航定姿自适应滤波算法

金仁成，谢林达，蔚彦昭，魏 巍

(大连理工大学 机械工程学院，辽宁 大连 116024)

0 引言

航姿参考系统(attitude and heading reference system, AHRS)中的惯性测量单元(inertial measurement unit, IMU)由角速度陀螺仪和加速度计组成。通过对陀螺仪输出角速度积分可以获得小型无人机的姿态角，虽然高成本陀螺仪精度更高、可靠性更好，但是由于其成本高昂,不适合于小型无人机中使用。同时，由于高斯误差、漂移误差等干扰因素存在，低成本陀螺仪计算的姿态角随时间不断发散，解算精度较低[1]。为了解决上述问题，传统方法是将惯导/磁力计/GPS构成组合导航系统[2]。虽然磁力计能在一定程度上修正陀螺仪量测信息，获得精度较高的姿态角，但是其易受电磁场干扰、机载控制电路等环境因素影响，在实际飞行中的应用性能大幅降低。同时在高机动情况下由于卫星信号传输频率与飞行器运动频率的不一致性显著增强，导致GPS丢星现象严重，影响航向信息的精确解算。考虑到传统导航方式的局限性，仿生导航逐渐成为研究热点。

近年来，人们发现蝴蝶、蜜蜂等生物能利用天空中偏振光进行导航寻迹[3-5]，对其生理结构分析后，进一步发现了大气偏振分布模式的存在。20世纪90年代，LAMBRINOS等[6]利用偏振光导航机理，设计出偏振光传感器实验样机，并通过机器人实验平台验证了将偏振光信息用于导航定向的可行性。大连理工大学褚金奎教授团队[7-10]率先对偏振光导航进行了一系列科学研究，成功研制出偏振光传感器并应用到移动机器人导航平台中，同时在利用偏振光信息实现导航定位方面进行了创新性探索研究[11]。随后，卢鸿谦等[12-13]利用卡尔曼滤波算法仿真验证了基于偏振光传感器辅助定姿的解算原理，然而其模型中对于偏振光传感器的噪声影响只考虑了理想测量噪声情形，并未考虑测量干扰等因素的不良影响。

综合上述分析，偏振光传感器在地面导航环境中已得到广泛应用，但在无人机上的应用仍处于探索阶段。王玉杰等[14]基于标准大气偏振模型推导了载体倾斜时的航向角计算方法，但是定向误差与载体的水平角倾斜误差密切相关，只能在小范围内倾斜才能保证定向精度。支炜等[15]设计了偏振光与红外传感器的辅助惯导定姿方法，通过设计卡尔曼滤波器仿真分析其可行性，但是仿真条件比较理想化，而且未充分考虑姿态解算的实时性要求。

本文总结了现阶段基于偏振光传感器导航的优势与不足，通过采用偏振光传感器和惯性测量单元组合导航的方式克服单独利用偏振光传感器存在的空间位置约束问题，并设计了一种自适应互补滤波姿态估计算法，在降低计算量满足姿态解算实时性要求基础上，采取自适应机制以提高复杂环境下导航系统的姿态解算精度，最后进行了对比实验，其结果表明该方法能获得实时性更强、精度更高的姿态信息，满足小型无人机的实际飞行需求。

1 偏振光传感器测角原理

太阳射出的自然光在传输过程中，受到大气微粒分子的散射而产生偏振现象，会在天空中形成稳定分布的偏振模式。本文利用偏振光传感器检测天空光的偏振分布模式，通过计算偏振光传感器参考方向与太阳子午线之间的夹角，即偏振方位角δ，以实现偏振光导航定向功能。

(1)

式中：α为太阳赤纬；β为观测点纬度；t为太阳时角，与当地经度有关。

图1 偏振光传感器测量模型Fig.1 Measurement model of polarization sensor

根据偏振光传感器量测输出，将偏振光E矢量投影到m系中，并表示为

pm(S)=[sinδcosδ0]T

(2)

根据瑞利散射模型，偏振光E矢量与观测矢量和太阳矢量所确定的平面构成几何上的垂直关系，由此得到的E矢量在m系中投影表示为

(3)

2 自适应互补滤波算法设计

通过对角速率陀螺仪的量测输出积分可以获得当前姿态角，然而陀螺仪测量值中含有零偏误差、高斯误差，以及随机误差等干扰因素，这样陀螺仪单独解算的姿态角随时间呈现发散趋势。从物理意义角度分析，加速度计输出值是重力矢量在载体坐标系下的投影值，据此可计算横滚与俯仰的水平姿态信息，虽然其计算角度不存在误差累积现象，但是电机振动等高频噪声会对其性能造成不利影响。同时，本文采用的偏振光传感器也具有低频特性良好、易受高频干扰的特点。因此，利用它们各自在频域上的优势，设计了互补滤波算法融合惯导与偏振光传感器的量测值以解算姿态角信息。然而在实际应用中，运动加速度的干扰，以及天空中云雾等水汽因素都会严重影响姿态解算精度，因此本文进一步采取自适应互补滤波机制，以提高偏振光导航系统的鲁棒性和稳定性，并分层次建立偏振光自适应互补滤波模型。

2.1 加速度计/陀螺仪互补滤波器

(4)

同时，将导航坐标系下的重力矢量投影到载体坐标系并表示为

(5)

将gb(f)与gb(R)的叉积gb(×)定义为加速度计纠偏矢量并修正陀螺仪输出值。不同于传统的PID控制，本文的互补滤波算法采用比例-积分(PI)补偿策略，其中比例参数体现了互补频率的影响，主要衡量加速度计和陀螺仪的置信度，积分参数体现了对陀螺仪误差的补偿性能，且两种参数的选择与滤波器截止频率密切相关。工程经验表明提高截止频率可使系统静态输出更稳定，但动态输出滞后；降低截止频率能提高系统动态特性，但会增加漂移误差。因此PI参数的选择需综合考虑上述各因素并通过实验验证其有效性。本文利用加速度计对陀螺仪的补偿过程表示为

ωg/a=-kap(gb(×))-kai(gb(×))

(6)

式中：ωg/a是角速度矢量加速度计补偿项；kap是加速度计比例参数；kai是加速度计积分参数。然而由式(5)可知，利用加速度计量测信息只能补偿陀螺仪解算的横滚角和俯仰角信息，航向角仍处于发散状态，因此需引入其他传感器辅助修正航向误差。

2.2 偏振光传感器/陀螺仪互补滤波器

在标准大气偏振模型中，偏振光传感器测量的偏振方位角无误差累积效应，可以为陀螺仪航向信息的补偿提供一种行之有效的解决方案。同时考虑到传感器电路干扰因素以及机体振动等高频噪声影响，本文采用低通滤波的方式实现对偏振光传感器输出信息的降噪处理。

通过查询天文年历获得观测点的位置和时间信息，并由式(3)计算得到偏振光E矢量理论参考值在模块坐标系下的投影pm(R)。再利用式(2)求解偏振光E矢量实际探测值在模块坐标系下投影值pm(S)。

为有效纠正陀螺仪累积误差，定义pm(R)与pm(S)的叉积pm(×)为偏振纠偏矢量，并作为航向角补偿项。这里采用和上文一致的比例-积分补偿策略，其对陀螺仪的补偿过程表示为

ωg/p=-kpp(pm(×))-kpi(pm(×))

(7)

式中：ωg/p是角速度矢量偏振光补偿项；kpp是偏振光比例参数；kpi是加速度计积分参数。

2.3 自适应调节机制

事实上，在无人机实际飞行过程中会存在复杂运动状态，其中加减速运动是常见情形，此时如果依然采用常规互补滤波算法，由于加速度计无法区分重力加速度和运动加速度，会降低算法的补偿精度，尤其是机体运动加速度较大时，姿态解算的准确性会受到严重影响。因此在出现这类情况时，应使互补滤波器的PI控制参数动态可变。基于此目的，进一步设计了相应的自适应调整方案。

从实时性角度考虑，本文方案设定积分参数kai保持不变，通过动态调整比例参数kap以适应复杂的加减速环境，从而保证机动状态下全姿态信息的解算精度。本文算法中的自适应滤波系数选取规则为

(8)

太阳光经大气中粒子散射后在整个天宇范围内形成稳定的大气偏振模式，相比传统导航方式，偏振光导航具有自主性好、抗电磁干扰能力强以及误差不随时间累积等诸多优势。然而，偏振光传感器的探测性能与天气状况密切相关，本文针对观测方向存在异常水汽干扰情形采取相应措施。利用pm(×)的欧式范数与设定阈值threshold进行比较分析，作为判断偏振光传感器是否受到异常水汽干扰的关键因素，通过动态调整比例参数kpp，提高偏振光导航系统对异常天气情况的适应程度。其表达式为

(9)

根据上述分析，本文设计的自适应互补滤波姿态估计算法流程如图2所示。

图2 自适应互补滤波算法流程图Fig.2 Flow chart of adaptive complementary filter algorithm

3 实验结果及分析

3.1 实验平台

搭建了偏振光导航实验平台，采用基于ARM Cortex-M3内核的飞控板，其主频为72 MHz，并集成惯性测量单元MPU-6050、3轴磁力计HMC5883和气压计MS5611等设备。将飞控板输出的姿态信息作为参考值以验证基于偏振光的自适应滤波算法。通过静态实验、机动实验以及存在异常水汽干扰实验全面验证算法有效性，实验结果表明本文设计的自适应滤波算法具有良好的抗干扰能力，有效提高了偏振光导航系统的稳定性。

3.2 静态实验

实验地点选取大连理工大学机械工程学院室外，地理位置信息为东经121.527°、北纬38.879°，实验时间为2018年5月8日，天气晴朗无云，从当天17：00开始将偏振光导航实验平台静止放置约375 s。陀螺仪量测姿态角如图3所示。由图3可知，单独使用陀螺仪解算的姿态角随时间偏离参考值，呈现出明显的发散现象，无法满足姿态解算的精度要求。然而，通过采用本文设计的互补融合策略修正效果显著。静态姿态角对比如图4所示。从图4中可以清晰看到，本文算法不仅修正了陀螺仪的测量偏差，而且其融合滤波效果较飞控板参考输出值的静态稳定性更优，表明本文算法对机载电路噪声的抗干扰能力更强。由于静态实验中运动加速度始终保持零值，因此基于偏振光的自适应滤波值与基于偏振光的常规互补滤波算法解算结果相当。

图3 陀螺仪量测姿态角Fig.3 Attitude angles measured by gyroscope

图4 静态姿态角对比图Fig.4 Static attitude angles comparison diagram

图5 加速度计输出值Fig.5 Acceleration measured by accelerometer

3.3 机动实验

无人机起飞50 s左右逐渐增大加速度值，稳定一段时间后再逐渐减速。其中，加速度计各轴输出值的变化趋势如图5所示。机动姿态角对比如图6所示。结果表明，在存在较大运动加速度情况下，如果仍采用常规互补滤波方案，会造成很大的姿态角误差。此时，虽然横滚角仍可以满足精度要求，但是俯仰角最大偏差达到16.25°，而且航向角最大偏差也接近1.58°。当采用本文提出的自适应滤波算法后，姿态角偏差明显降低，误差均在±0.3°范围内，具有较高的解算精度，提高了偏振光导航系统的鲁棒性能。

图6 机动姿态角对比Fig.6 Dynamic attitude angles comparison diagram

3.4 天空水汽异常实验

偏振光传感器视场范围有限，对视线方向水汽分布敏感。如果水汽异常情况显著，则偏振光传感器性能会大幅降低，因此在利用偏振光信息导航时应及时监测异常并采取相应措施。

事实上，当天空水汽分布异常时，采用常规互补滤波算法会融入相应测量误差，严重影响航向角解算精度。本文选取晴间多云天气以验证偏振光自适应滤波算法有效性。异常水汽影响下航向角对比如图7所示。由图可见，在175 s到200 s期间，偏振光传感器观测方向存在少量云雾。实验结果表明，受观测点水汽异常影响，偏振光传感器存在短时间内测量偏差较大情况，此时基于偏振光修正航向信息的能力大幅降低，图中常规互补滤波算法没有异常处理机制，导致航向角计算偏差为30°～40°，精度严重降低。为了改善异常状态下航向测量准确性，本文利用pm(×)的欧式范数作为判断异常水汽存在的标准，通过动态调整相应参数来提高偏振光导航系统自适应能力。从实验结果可以看出，采用本文自适应滤波算法后，能够很好克服少量云雾干扰情形，进一步提高了偏振光导航系统鲁棒性。

图7 异常水汽影响下航向角对比图Fig.7 Yaw attitude angle measured in the state of abnormal water vapor comparison diagram

4 结论

为提高导航系统姿态解算精度，设计了基于偏振光传感器的自适应互补滤波算法，完成了静态、机动以及天空存在异常水汽干扰等实验。由于存在高斯误差、漂移误差等干扰因素，单独利用低成本陀螺仪计算的姿态角会随时间逐渐发散。在采用本文自适应互补滤波策略后，通过融合偏振光传感器和加速度计的量测数据，能有效抑制陀螺仪发散问题，保证了姿态解算的收敛性，并提高了偏振导航系统的姿态解算精度，为无人机导航领域提供了一种全新方案，具有广阔的应用前景。

考虑到基于偏振光的导航精度与传感器的数据采集精度密切相关，后续研究中将优化信号滤波方法，减少噪声信号对采集到偏振光信息的干扰，进一步提高偏振光导航系统的定姿精度。