基于天空光偏振模式的导航姿态最优化解算方法

金仁成 谢林达 褚金奎 蔚彦昭 魏巍

摘 要：针对传统导航系统自主性差、抗电磁干扰能力弱的问题，本文提出一种基于天空光偏振模式的仿生导航方案。利用自主研发的偏振光传感器实现导航定向功能，并结合惯性测量单元设计了一种导航姿态最优化解算方法。现有的姿态最优化求解策略一般是采用步长固定的搜索算法，但是其直接影响了运动状态下姿态估计的准确性，为此本文采取动态步长搜索机制。实验结果表明：本文提出的算法能有效抑制陀螺仪漂移误差，而且对高频噪声干扰有明显滤除效果，表现出良好的静态性能;同时，本文提出的方法较常规算法具有更高的动态精度，进一步提高了偏振光导航系统的稳定性与可靠性，为无人机飞行控制提供更准确的参数信息。

关键词：偏振模式;仿生导航;优化;动态步长;姿态解算

中图分类号：TJ765;V249 文献标识码：A文章编号：1673-5048（2020）01-0039-07

0 引言

在晴朗无云的天气条件下，太阳发出的自然光被大气层中的粒子散射后，整个天宇范围内会形成相对稳定的大气偏振模式[1-2]。自然界中的许多生物能够通过自身的偏振视觉系统感知天空中的偏振模式实现导航。比如沙蚁离开巢穴觅食后，能够利用偏振光信息找到归巢的最短路线[3-4]。一些候鸟可以利用天空偏振光来矫正自身的生物磁罗盘完成迁徙活动[5-6]。偏振光导航作为一种新型的仿生导航技术，不仅比卫星导航有着更强的抗电磁干扰能力，而且与惯性导航相比，具有误差不随时间积累的优势。

目前，国内外学者在偏振光导航方面做了大量研究工作。20世纪90年代，国外学者Lambrinos 等利用偏振光导航机理，设计出仿生偏振光导航传感器并应用于机器人中，通过实验证明了将偏振光信息用于导航定向的可行性[7]。2012年澳大利亚Chahl 等研究人员仿照蜻蜓利用偏振光导航的过程，通过简化飞行模型将偏振光导航传感器应用于无人机中完成初步航向测量[8]。大连理工大学褚金奎教授团队率先在国内对偏振光导航机理展开研究，通过分析昆虫复眼偏振敏感单元设计了偏振光导航传感器，并在地面机器人实验中成功应用[9]。同时，北京大学、哈尔滨工业大学、国防科技大学等国内高校对偏振光的散射特性以及传输特性开展了大量研究[10-12]。

现阶段，偏振光传感器在地面导航中已得到成功应用，但是在无人机上的应用仍处于起步阶段。事实上，目前的偏振光导航策略主要有两种：

一是利用偏振光传感器实现完全的自主定姿过程[13];二是基于卡尔曼滤波的偏振光辅助定姿方法[14]。然而这两种方法均存在一定局限性：方法一在航向信息解算过程中存在偏振方位角模糊性问题，而且导航姿态解算精度与飞行器水平姿态的误差精度密切相关，水平倾角只有在小角度范围内变化时才能达到较高解算精度[13]，因此极大限制了飞行器的空间姿态解算。方法二利用卡尔曼滤波模型实现偏振光传感器与惯性测量单元输出值的数据融合。然而在每次迭代过程中其计算量相对较大，实时性难以保证，同时偏振光传感器的模型误差受多种因素影响[14]，无法确保卡尔曼滤波器中误差项的准确性，进一步制约了导航系统性能的改善。

本文基于天空光偏振模式提出了一种姿态最优化解算方法，并且融合偏振光传感器和惯性测量单元的量测数据实现了动态步长搜索功能，最后通过实验验证了所提出的方法能够有效提高偏振光导航系统的稳定性和精度，为无人机完成实际飞行任务提供了可靠保证。

1 偏振光组合导航系统定姿过程

1.1 基于四元数的姿态更新

常见的姿态解算方法主要有欧拉角法、方向余弦法和四元数法等。欧拉角法利用坐标轴的有序转动实现姿态变换，虽然计算量小但方程可能出现奇异值，不能全姿态解算;方向余弦法和四元数法在解算过程中不会存在奇异解，但是方向余弦法需要计算旋转矩阵的9个参数，计算量通常较大;相比之下，四元数法的更新方程只需求解4维数据，计算量大幅下降[15]，因此本文采用四元数作为姿态解算的數学依据。

本文定义姿态四元数为Q，且有

本文设计的无人机导航系统主要涉及载体坐标系b系和导航坐标系n系，其坐标系间的位置关系如图1所示。图中，基于地球坐标系OeXeYeZe确定b系和n系的空间位置关系。其中，b系原点与机体质心重合、Xb轴沿载体纵轴向前、Yb轴沿载体横轴向右，Zb轴与Xb和Yb轴构成右手笛卡尔坐标系。n系各轴的选取规则类似，其中Xn轴、Yn轴和Zn轴分别指向地理的北向、东向和地心方向。

无人机在飞行过程中姿态不断变化，载体坐标系相对导航坐标系的旋转过程可由四元数表示。同时，将导航坐标系到载体坐标系的姿态矩阵定义为Cbn，其表达式为

再根据姿态矩阵与四元数的对应关系，全姿态信息的解算结果为

1.2 偏振光传感器导航机理

太阳发出的自然光经过空气分子的散射作用会形成稳定的大气偏振分布模式[16]。根据瑞利散射模型原理，偏振光E矢量方向总是垂直于太阳入射光与观察者所确定的平面，因此可以利用这种方位信息进行导航定向，同时利用天空光偏振分布模式，建立了基于偏振光传感器的导航定向模型，如图2所示。

为说明偏振光传感器的空间位置，这里引入偏振光测量坐标系m系。图2中，O点为偏振光传感器所在位置，Xm轴为偏振光传感器自身参考轴方向，Zm轴为偏振光传感器观测方向，Ym轴与二者构成右手笛卡尔坐标系。O点到太阳投影点S的连线表示太阳矢量，Hs与As分别为太阳高度角和太阳方位角，二者与时间和位置信息相关，计算公式为

偏振光传感器输出值为偏振方位角α，是传感器参考轴方向与偏振光E矢量方向的夹角。同时，根据偏振光传感器量测输出，可将偏振光E矢量在m系中的投影坐标表示为

根据瑞利散射模型，偏振光E矢量与观测矢量和太阳矢量形成几何上的垂直关系，由此计算偏振光E矢量在m系中的映射形式为

2 基于仿生偏振光的姿态最优化求解方法

梯度优化模型利用负梯度方向搜索寻优，并保证每次迭代更新后目标函数逐步减小，通常用于求解函数最优解[17-18]。本文通过偏振光传感器和加速度计量测数据，构建基于四元数的目标误差函数模型，并求解误差函数最小值，以获得偏振光导航系统的最优姿态四元数，从而求解姿态最优估计值。

2.1 基于偏振光传感器探测值的误差模型

本文利用偏振光E矢量在不同坐标系中投影值的确定关系，构建偏振误差矢量模型求解偏振梯度搜索方向，以矫正偏振方位角的量测偏差。

采集偏振光传感器输出数据，则偏振光E矢量在m系中的表达式为

查询天文年历和经纬度信息，按式（5）计算太阳高度角Hs和太阳方位角As，以确定太阳矢量的投影位置Sn，再按式（7）计算偏振矢量在m系中的映射值Ems。此时利用m系中偏振光传感器的测量值Emc与瑞利散射模型计算出的偏振矢量Ems构造偏振误差矢量：

为建立偏振梯度搜索模型，对偏振误差矢量求导，获得相应的雅可比矩阵，即

2.2 基于加速度计量测数据的误差模型

加速度计具有良好的静态特性，长时间使用不会引入积分误差。本文利用重力矢量在载体坐标系与导航坐标系之间的旋转变换关系建立重力误差矢量模型，通过重力梯度搜索方向矫正加速度计的输出偏差。

假设重力矢量垂直向下，则导航坐标系中归一化后表示为

同时利用旋转四元数矩阵将Ang转换到载体坐标系下，则有

加速度计在载体坐标系下实际测得的重力矢量表示为

为建立重力梯度搜索模型，对重力误差矢量Emp求导，获得相应的雅可比矩阵：

2.3 最优化模型中的动态步长机制

根据偏振误差矢量和重力误差矢量构建偏振光导航系统的姿态误差矢量：

對e（Q）取欧式范数的平方，从而得到梯度寻优目标误差函数：

此时，对姿态四元数的优化转换为求目标误差函数F（Q）的最小值。为求解误差最小值的姿态四元数，将姿态误差矢量的雅可比矩阵定义为

将目标误差函数的梯度及其一阶偏导数定义为

根据最优化精确搜索理论，可推导出梯度优化模型的动态步长因子为

传统的梯度模型需要多次迭代才能获得最优解，但是在姿态解算中，每个解算周期只需进行一次迭代即可获得最优姿态四元数。本文通过旋转四元数微分方程建立偏振光传感器、加速度计与陀螺仪之间的数据融合关系。利用动态步长λ计算姿态四元数最优解：

3 实验测试及结果分析

本文搭建了偏振导航实验平台，所使用的飞控板上集成了惯性测量单元MPU6050和电子罗盘HMC5883L等设备。飞控板内部采用精度较高的卡尔曼滤波算法求解姿态角，可作为姿态输出参考值。为了评测算法的性能，分别通过静态实验和动态实验与步长固定的梯度搜索方法对比分析，以验证本文设计的动态步长梯度优化方法的可行性和有效性。

3.1 静态实验测试及分析

静态实验于2018年5月25日17点20分开展，实验场地选在大连理工大学机械工程学院室外。通过查询天文年历表，可以获得太阳位置相关信息，其中，太阳高度角为20.54°，太阳方位角为-100.89°。

为了保证实验结果的可靠性，将偏振光导航实验平台静止放置约400 s，经过不同方法解算的姿态角如图3所示。

图3中，本文设计的动态步长梯度优化方法估计的姿态角不仅消除了陀螺仪积分漂移问题，而且与飞控板参考算法相比，获得的姿态信息具有更好的稳定性以及更高的静态精度。同时考虑到动态步长梯度优化方法的优势主要体现在运动环境中，因此在静态实验中，与固定步长梯度优化方法估计的姿态角具有相当的解算精度。

3.2 动态实验测试及分析

动态实验通过变化飞控板姿态，以验证优化方法的运动适应性。选取的实验地点不变，实验时间为2018年5月25日18点，此时查询天文年历表，可以获得太阳高度角为12.98°，太阳方位角为-106.62°。经过不同方法解算的姿态角如图4所示。同时，两种梯度优化方法的姿态估计偏差曲线如图5所示。

根据图4结果可知，本文设计的动态步长梯度优化方法能够很好贴合姿态变化过程，通过融合偏振光传感器数据实现导航姿态的动态解算。同时从图5的姿态估计偏差曲线可以清晰看到，在姿态变化的全过程，动态步长方法的姿态偏差值要明显小于固定步长方法，从而表明本文提出的动态步长机制能够保证运动环境下姿态解算的准确性，有效提高了偏振光导航姿态估计的动态精度。

4 结论

本文从偏振光导航机理入手，通过引入仿生导航传感器设计偏振光组合导航系统，以弥补现有导航方式不足，从而提供一种自主性好、抗电磁干扰能力强、无误差累积效应的新型导航方案。同时，为提高偏振光导航系统的姿态估计精度，设计了基于天空光偏振模式的导航姿态最优化解算方法，并分别通过静态实验与动态实验测试分析。

从实验结果可以看到，采用动态步长机制的梯度优化方法不仅具有良好的静态稳定性，而且在运动环境中的姿态估计精度要明显优于固定步长梯度优化法，表明该方法能有效提高偏振光导航系统姿态解算的动态精度，可以进一步增强导航平台的可靠性与稳定性。但偏振光传感器的精度与天气条件关系紧密，下一步工作将考虑大气湍流、雾霾、烟尘等不同天气因素对偏振光导航姿态估计精度的影响，从而进一步优化现有姿态解算模型，以提高偏振光导航系统的鲁棒性能。

参考文献：

[1] El Jundi B，Smolka J，Baird E，et al. Diurnal Dung Beetles Use the Intensity Gradient and the Polarization Pattern of the Sky for Orientation[J]. Journal of Experimental Bio-logy，2014，217（13）：2422-2429.

[2] El Jundi B，Pfeiffer K，Heinze S，et al. Integration of Polarization and Chromatic Cues in the Insect Sky Compass[J]. Journal of Comparative Physiology A，2014，200（6）：575-589.

[3] Lebhardt F，Ronacher B. Transfer of Directional Information between the Polarization Compass and the Sun Compass in Desert Ants[J]. Journal of Comparative Physiology A，2015，201（6）：599-608.

[4] Lebhardt F，Koch J，Ronacher B. The Polarization Compass Dominates over Idiothetic Cues in Path Integration of Desert Ants[J]. Journal of Experimental Biology，2012，215（3）：526-535.

[5] Melnikov V，Leskinen M，Koistinen J. Doppler Velocities at Orthogonal Polarizations in Radar Echoes from Insects and Birds[J]. IEEE Geoscience & Remote Sensing Letters，2014，11（3）：592-596.

[6] Muheim R. Behavioural and Physiological Mechanisms of Polarized Light Sensitivity in Birds[J]. Philosophical Transactions of the Royal Society of London，2011，366（1565）：763-771.

[7] Lambrinos D，Mller R，Labhart T，et al. A Mobile Robot Employing Insect Strategies for Navigation[J]. Robotics and Autonomous Systems，2000，30（1）：39-64.

[8] Chahl J，Mizutani A . Biomimetic Attitude and Orientation Sensors[J]. IEEE Sensors Journal，2012，12（2）：289-297.

[9] Chu Jinkui，Wang Hongqing，Chen Wenjing，et al. Application of a Novel Polarization Sensor to Mobile Robot Navigation[C]∥International Conference on Mechatronics and Automation，Changchun，2009：3750-3755.

[10] 晏磊，關桂霞，陈家斌，等. 基于天空偏振光分布模式的仿生导航定向机理初探[J]. 北京大学学报：自然科学版，2009，45（4）：616-620.

Yan Lei，Guan Guixia，Chen Jiabin，et al. The Bionic Orientation Mechanism in the Skylight Polarization Pattern[J]. Acta Scientiarum Naturalium Universitatis Pekinensis，2009，45（4）：616-620. （in Chinese）

[11] 黄显林，姜肖楠，卢鸿谦，等. 自主视觉导航方法综述[J]. 吉林大学学报：信息科学版，2010，28（2）：158-165.

Huang Xianlin，Jiang Xiaonan，Lu Hongqian，et al.Survey of Vision for Autonomous Navigation[J]. Journal of Jilin University：Information Science Edition，2010，28（2）：158-165.（in Chinese）

[12] 范晨，胡小平，何晓峰，等. 天空偏振模式对仿生偏振光定向的影响及实验[J]. 光学精密工程，2015，23（9）：2429-2437.

Fan Chen，Hu Xiaoping，He Xiaofeng，et al.Influence of Skylight Polarization Pattern on Bionic Polarized Orientation and Corresponding Experiments[J]. Optics and Precision Engineering，2015，23（9）：2429-2437. （in Chinese）

[13] 王玉杰，胡小平，练军想，等. 仿生偏振光定向算法及误差分析[J].宇航学报，2015，36（2）：211-216.

Wang Yujie，Hu Xiaoping，Lian Junxiang，et al.Algorithms and Error Analysis of Bionic Orientation Based on Polarized Light[J]. Journal of Astronautics，2015，36（2）：211-216. （in Chinese）

[14] 盧鸿谦，黄显林，尹航. 三维空间中的偏振光导航方法[J]. 光学技术，2007，33（3）：412-415.

Lu Hongqian，Huang Xianlin，Yin Hang. Polarized-Light-Based Navigation in Three-Dimensional Space[J]. Optical Technique，2007，33（3）：412-415. （in Chinese）

[15] Fresk E，Nikolakopoulos G . Full Quaternion Based Attitude Control for a Quadrotor[C]∥ 2013 European Control Conference，Zurich，2013：3864-3869.

[16] Gál J，Horváth G，Meyer-Rochow B V，et al. Polarization Patterns of the Summer Sky and Its Neutral Points Measured by Full-Sky Imaging Polarimetry in Finnish Lapland North of the Arctic Circle[J]. Proceedings of the Royal Society A：Mathematical，Physical and Engineering Sciences，2001，457（2010）：1385-1399.

[17] 黄平. 最优化理论与方法[M]. 北京：清华大学出版社，2009：82-84.

Huang Ping. Optimal Theories and Methods[M]. Beijing：Tsinghua University Press，2009：82-84. （in Chinese）

[18] Basak R，Das A，Sanyal A. Cost Optimal Design of a Power Inductor by Sequential Gradient Search[J]. Journal of the Institution of Engineers （India）：Series B，2018，99（4）：413-418.