MIMU/偏振光拟牛顿优化航姿求解方法

金仁成 魏巍 孙庆飞 洪金辉 姜佳旭

摘要：针对传统磁力计/MIMU组合导航系统存在的抗电磁干扰能力弱、适应性差等问题，提出了一种基于大气偏振分布模式的无人机导航航姿无约束优化求解方法。融合陀螺仪、加速度计、偏振光传感器测量数据，构建基于改进拟牛顿法的姿态最优化解算模型，其中，该模型以最优化理论为基础，引入实时四元数微分构建动态步长因子，预测姿态更新趋势;设计状态判断阈值，自适应判断飞行状态，提高解算精度。实验结果表明，这种方法得到的姿态角静态性能稳定，同时，在动态环境中，该方法得到的姿态角误差值最小，较梯度下降法精度提高50%，较共轭梯度法精度提高37%～50%，表现出较好的动态收敛性与误差稳定性，动态精度较高，能够较准确跟踪姿态变化，可以有效提高偏振光辅助的捷联惯导组合导航系统航姿估计的准确性与可靠性。

关键词：偏振光;MIMU;航姿解算;拟牛顿

中图分类号：V249             文献标志码：A

文章编号：1009-9492（2021）12-0023-06

开放科学（资源服务）标识码（OSID）：

MIMU/ Polarized Light Quasi-newtonian Optimization Method for Attitude Solving

Jin Rencheng ，Wei Wei ，Sun Qingfei ，Hong Jinhui ，Jiang Jiaxu

（School of Mechanical Engineering， Dalian University of Technology， Dalian， Liaoning 116024， China）

Abstract： Aiming at the problems of the traditional magnetometer /MIMU integrated navigation system， such as weak anti-electromagnetic interference  ability  and  poor  adaptability，  an  unconstrained  optimization  method  for  UAV  navigation  attitude  was  proposed  based  on atmospheric polarization distribution mode. The attitude optimization model based on the improved quasi-Newton method was constructed by integrating the measurement data of gyroscope， accelerometer and polarized light sensor. Based on the optimization theory， the dynamic step factor was constructed by introducing real-time quaternion differential to predict the attitude updating trend. The threshold value of state judgment was designed， and the flight state was judged adaptively to improve the precision of solution. Attitude angle of the experimental results show that the approach of static performance is stable， at the same time， in a dynamic environment， the method to get the minimum value attitude angle error， the gradient descent method precision is increased by 50%， the conjugate gradient method precision is increased by 37%～50%， show a good dynamic stability， convergence and error dynamic accuracy is higher， can accurately track the attitude change， it can effectively improve the accuracy and reliability of attitude estimation of strapdown inertial navigation system assisted by polarized light.     Key words： polarized light; MIMU; navigation position calculating; quasi-newton

0 引言

隨着微机电系统（MEMS）工艺和计算机技术的发展，小型无人机多采用低成本的 MEMS传感器（主要有陀螺仪、加速度计、磁力计）作为姿态估计设备。其中，陀螺仪是姿态角度估计的关键传感器，但由于其固有误差，长时间累计积分运算，会存在严重漂移现象，通常利用加速度计补偿陀螺仪的俯仰角和横滚角偏差，利用磁强计补偿航向角偏差。但是磁强计易受地磁场与电磁场的干扰，不能用于某些特殊环境。因此，进一步研究高精度、高可靠性的组合导航系统成为了无人机导航控制领域的研究热点。

通过观察一些鸟类和昆虫的自主导航行为，科学家发现某些生物能够通过感知自然光经过大气散射后形成的偏振光用于导航。近年来，国内外研究人员对偏振光在导航中的应用开展了各种研究[1-5]，2011年，大连理工大学的褚金奎课题组将偏振光传感器运用于移动机器人上，实现移动导航。2012年，德国学者 Wolfgang以及澳大利亚学者 Javaan Chahl ，分别尝试将偏振光传感器搭载于四轴无人机与固定翼无人机上进行实验。2014年，中国科技大学通过与中科院先进制造所合作，将成像式偏振光导航传感器应用到了汽车导航系统。2019年，西北工业大学的周军等提出一种基于偏振光传感器和磁力计的姿态确定方法，验证了其估计卫星姿态信息的可行性。

现阶段关于偏振光导航的研究主要集中在地面二维平面以及偏振光原理研究上，在无人机导航等三维空间的应用仍处于起步阶段。目前偏振光在组合导航系统中多以辅助惯性导航系统定姿为主，主要是通过卡尔曼滤波算法或互补滤波算法实现姿态估计。但卡尔曼滤波算法受误差估计模型影响，易发生滤波精度下降甚至发散的情况，计算精度受噪声影响明显。互补滤波算法需要对比例参数与积分参数进行估计，参数估计随机性会影响补偿的效果[6-9]。

针对现阶段偏振光辅助导航系统的优势与不足，本文基于大气偏振光分布模式提出了一种航姿无约束优化求解方法，设计动态步长因子与状态阈值，提高了姿态求解的精度与效率，通过实验验证了该方法可以提高基于偏振光辅助的航姿参考系统的姿态估计准确性与可靠性，为无人机仿生导航提供了一种可靠的姿态求解方案。

1 偏振光传感器辅助定姿原理

1.1 姿态坐标系描述

飞行器的空间姿态本质上是机体坐标系（b 系）与导航坐标系（ n 系）的姿态关系，本文导航坐标系原点选在飞行器质心，Xn 轴指向地理北向（north）， Yn 轴指向地理东向（east）， Zn 轴指向地心方向。机体坐标系原点选在飞行器质心，Xb 轴沿机体纵轴指前， Yb 轴沿机体横轴指右，Zb 轴指向机体下方，坐标关系如图1所示。

1.2 四元數姿态更新

目前对于无人机姿态更新主要有欧拉角法、四元数法和方向余弦法等。欧拉角法形式直观、易于理解，但存在90°奇异性问题。方向余弦法采用方向余弦矩阵存储姿态信息，每次更新需计算9个参数，计算量大。四元数法求解不存在奇异性问题，只需计算4个参数，方法简单，计算时间短[10-12]。因此本文采用四元数法描述姿态更新过程。

四元数的物理意义表示了转轴方向与转角大小，其三角式表达为：

四元数微分方程为：

其矩阵表达为：

其中，四元数为 Q =q0 q1 q2 q3T  ，载体角速度为ωb =ω bx   ω by   ω bzT 。

无人机飞行姿态的变化可以理解为载体坐标系（b系）相对于导航坐标系（ n 系）的旋转，本文将导航坐标系到载体坐标系的姿态矩阵定义为 Cnb ，同时用四元数进行表示，即：

通过将三姿态角表示的姿态旋转矩阵与用四元数表示的姿态矩阵对应可用四元数对3个姿态角进行表示，即：

式中：γ为横滚角;θ为俯仰角;ψ为航向角。

1.3 偏振光辅助导航定姿模型

太阳光遇到大气微粒分子发生散射会产生光的偏振，并形成稳定的大气偏振分布模式，本文利用偏振光传感器检测天空中偏振光分布模式[13]。由 Rayleigh 散射原理可知，最大偏振光矢量（E ）总是垂直与太阳矢量（S）与观测矢量（Z ）所成平面，由此可建立偏振光传感器理论测量模型，如图2所示。

为方便说明，引入偏振光传感器测量坐标系 m 系和太阳坐标系p 系，其中，原点为偏振光传感器质心，Xm 轴为偏振传感器参考轴方向，Zm 轴为偏振光传感器观测偏振矢量方向， Ym 轴与前二轴构成右手笛卡尔坐标系，太阳坐标系三轴则分别指向地理的西向、南向、天向。

太阳在太阳坐标系中的位置可以由太阳高度角和太阳方位角表示，二者满足：

式中：φ为地理纬度;δ为太阳的赤纬;τ为太阳的时角。

三者可通过查询天文历获得，太阳矢量在p 系中可表示为：

根据模型可得到 m 系下理论最大偏振矢量 E′的表达式：

式中： [W m ×]为观测矢量的反对称矩阵; Cb m 为载体坐标系到偏振光测量坐标系的转换矩阵; Cnb Q 为四元数表示的导航坐标系到载体坐标系的转换矩阵; Cp n 为太阳坐标系到偏振光测量坐标系的转换矩阵。

本文中，偏振光传感器输出值为最大偏振矢量与偏振光传感器参考轴的夹角α，忽略偏振光传感器输出误差，偏振光传感器实测偏振矢量可以表示为：

2 基于偏振模式的组合导航航姿最优化方法求解

共轭梯度法是求解大型非线性无约束优化问题最常用的算法之一，通常以迭代点处的负梯度方向与共轭方向的线性组合来确定较优搜索方向，通过迭代逐渐逼近最优解[14-15]。本文利用加速度计传感器，偏振光传感器实测数据与理论数据的误差构建目标误差函数模型，并通过求解误差函数最小值，获得偏振光辅助航姿解算系统的最优姿态四元数，进而得到最优航姿姿态估计值。

2.1 重力矢量误差函数模型

参考1.2节中的偏振光理论测量坐标系模型，单位重力矢量在导航坐标系中为：

将重力矢量转换到载体坐标系下：

式中： Ag（b）（Q ）为载体坐标系中四元数表示的理论重力矢量;  Cnb （Q ）为四元数表示的导航坐标系到载体坐标系的转换矩阵。

在静止与飞行中，MEMS加速度计传感器输出量可以认为是重力在载体坐标系中的投影，加速度计输出量为：

式中： abx 、 ay（b）、  abz 为加速计三轴输出量。

载体坐标系中，加速度计的输出量与理论重力矢量的差值反应的而是姿态矩阵估计的误差值，因此建立重力矢量误差模型：

对四元数 Q =q0   q 1   q2   q3T 求微分得到重力矢量误差模型的 Jacobian矩阵 Jεb Q ：

2.2 偏振矢量误差函数模型

参考1.2节中的偏振光理论测量坐标系模型，查询天文年历和经纬度信息，根据偏振矢量与太阳矢量及观测矢量组成的平面成垂直关系，由式（15）得到四元数表示的偏振光测量坐标系下的理论偏振矢量 Esm Q ：

式中： vx（n）、  vy（n）、  vz（n）为太阳矢量Vs p 在导航坐标系下投影 Vs n 的三轴分量。

偏振光传感器的输出量为偏振光测量坐标系中偏振矢量与传感器参考轴的夹角，测量偏振矢量可表示为：

由理论偏振矢量與测量偏振矢量可得到偏振矢量误差模型：

对四元数 Q =q0   q 1   q2   q3T 求微分得到重力矢量误差模型的 Jacobian矩阵 Jεm Q ：

2.3 动态步长因子设计

根据2.1节中建立的重力矢量误差模型 AbεQ 及其对应的 Jacobian矩阵 Jεb Q ，2.2节中建立的偏振矢量误差模型 Eεm Q 及其对应的 Jacobian 矩阵 Jεb Q ，组建偏振光辅助航姿解算系统的综合矢量误差模型 F Q 及其对应的 Jacobian矩阵 J Q ：

由无约束优化方法中的梯度优化理论，得到综合矢量误差函数的梯度：

为减小计算量，定义步长为：

式中：k 为姿态四元数第 k 次迭代;?F Q k 为第 k 次迭代的梯度; Dk Q 为第 k 次迭代的搜索方向，以负梯度方向-?F Q k 与黑塞矩阵的近似矩阵 Bk Q 通过线性组合得到。

规定搜索方向：

其中， Bk Q 通过 BFGS修正公式获得：

根据载体的运动状态变化引入动态步长因子，当姿态变化较小时，动态步长因子减小，反之，动态步长因子增大，提高解算精度，加快收敛速度，动态步长因子αk   （可变化）定义为：

式中： Q?k 为第 k 次迭代的四元数微分，通常共轭梯度优化方法需通过多次迭代来获得最优解，但在航姿求解中，每一次迭代都可以得到姿态四元数的最优解。

由于重力加速度计输出为重力加速度与运动加速度的矢量和，当运动加速度过大时会影响模型解算精度，因此设计状态阈值 c ，当加速度计输出大于设定值ε时，运动加速度较大应减小搜索步长，提高搜索精度。

本方法通过求解综合姿态矢量误差函数最小值，求解最优四元数，对陀螺仪数据进行补偿，完成最优姿态四元数的解算，过程如下：

式中： ω b =ωbx   ωy（b）  ωbz为陀螺仪测得的角速度矢量;?为四元数乘法运算; ΔT 为姿态四元数更新周期; P Q k 为最优搜素方向。

3 实验验证与结果分析

基于偏振光辅助的航姿解算系统主要包括惯性测量单元与偏振光传感器。本实验使用 Paparazzi uav开源项目中 Lisa/M2.0飞控系统（集成惯性测量单元MPU6000和地磁传感器 HMC5883L）中惯性测量传感器输出数据与偏振光传感器输出数据进行数据融合估计姿态并将飞控系统输出作为实验参考值，该系统使用惯性测量单元与地磁传感器作为姿态估计传感器，图 3所示为飞控系统输出精度测试结果。

考虑到飞控板安装误差的存在可以认定飞控系统输出具有一定精度可以作为参考值。为了评估提出的航姿解算方法，分别在静态与动态实验环境下进行实验验证，并与飞控系统实验参考值，梯度下降优化法与共轭梯度优化法进行对比分析，以验证本文中基于动态步长因子与状态阈值的拟牛顿优化航姿解算算法的可靠性和准确性。

3.1 静态实验测试及分析

静态实验地点为大连理工大学机械东楼楼顶（121.528255° E ，38.8814192° N），海拔35 m ，时间是2021年1月17日上午10：34，天气晴朗，根据天文历表可知太阳高度角为26.12°，太阳方位角为-0.8°。由于将实验平台静止放置在地面一段时间，实验数据如图4所示。图中展示了静态实验中本文设计的基于动态步长因子的拟牛顿优化方法与梯度下降法、共轭梯度法的姿态对比曲线和状态变化曲线。图中状态变化曲线平缓，符合静态实验的特点。

为进一步说明不同方法之间的差异，表1所示为静态实验姿态角均方差以及平均值对比数据。由表可知，横滚角中，拟牛顿法最接近参考值，俯仰角中，共轭梯度法与拟牛顿法最接近参考值，误差相同，航向角中，3种优化方法平均值相同，可以看出3种优化方法在静态时没有明显区别。与参考值相比较，在可接受误差范围内，具有一定静态精度与稳定性。

3.2 动态实验测试及分析

选取地点不变，时间是2021年1月16日16：33，根据天文历表可知太阳高度角为2.64°，太阳方位角为-143.16°。将偏振光导航实验平台在三维空间中，实验数据如图5所示。由图可知，在姿态变化时，本文的拟牛顿优化方法可以很好地跟踪姿态角的变化。状态变化的曲线，符合实验的动态特点，对复杂运动状态下的姿态误差起到了调控作用。

为了更直观地比较姿态解算方法的差异，图 6所示为不同方法获得的姿态角的偏差曲线图。由图可知，在姿态变化时，基于动态步长因子与状态阈值的拟牛顿优化方法的收敛速度快于其他方法，姿态估计角度误差较小。

为进一步说明其他方法之间的差异，表2～3所示为动态实验姿态角最大绝对值误差以及误差均方差对比数据。由表可知，基于动态步长因子与状态阈值的拟牛顿优化航姿解算的姿态角误差最小，较梯度下降法精度提高50%，较共轭梯度法精度提高37%～50%，说明基于动态步长因子的共轭梯度法与其他方法相比，在误差最小的情况下误差波动最小，表现出更好的动态收敛性，动态精度更高，验证了对于偏振光辅助的捷联惯导导航系统共轭梯度优化方法有利于飞控系统的航姿估计。

4 结束语

本文将点源式偏振光传感器引入传统惯导系统替代磁强计，构成新的航姿参考系统（陀螺仪、加速度计、偏振光传感器），利用误差最小原理搭建算法模型，通过动态步长因子优化的拟牛顿法进行最优解求解，通过状态阈值抑制运动加速度对加速度计的干扰，实现姿态解算，为偏振光导航提供了一种新的航姿求解方案。实验结果表明，拟牛顿优化的航姿解算方法在静态环境下的具有一定精度与平稳性，而且由于动态步长因子与状态阈值的设计，在动态环境下也表现出较好的姿态跟踪能力，与梯度下降法、共轭梯度优化的滤波算法的对比结果表明，拟牛顿优化方法的解算收敛速度更快，姿态误差更小，说明本文提出的偏振光/惯导组合导航的拟牛顿优化航姿算法能够有效提高偏振光辅助导航系统的姿态跟踪能力与解算效率。

在建立重力矢量模型时，忽略了运动加速度对加速度计输出的影响，虽然使用状态阈值进行抑制，并且优化搜索本身基于误差最小情况进行，但在运动加速度过大时仍会影响姿态解算，下一步工作将考虑建立运动加速度计误差补偿，从而进一步优化现有姿态解算模型，以提高偏振光/惯导组合导航系统的鲁棒性能。

参考文献： [1] 褚金奎， 王洪青， 戎成功. 基于偏振光传感器的导航系统实验测试[J]. 宇航学报， 2011， 32（3）：489-494.

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

[3] W Stürzl， Carey N. A Fisheye Camera System for Polarisation Detection on UAVs[C]//European Conference on Computer Vi? sion， 2012.

[4] D Wang， H Liang， H Zhu. A bionic camera-based polarization navigation sensor[J]. Sensors，2014，14（7）：13006-13023.

[5] Huang H， Yang Z， Zhou J. Microsatellite attitude determination based on skylight polarization and geomagnetic measurement[J]. Optik-International Journal for Light and Electron Optics， 2018 （178）：1177-1184.

[6] 褚金奎， 林木音， 王寅龍，等. 偏振光传感器的无人船导航与编队应用[J]. 光学精密工程， 2020（8）：1661-1669.

[7] 曾云豪，杜涛，星艳.基于惯性/偏振光/光流的六足步行机器人自主导航方法研究[J]. 导航定位与授时， 2020，39（6）：108-114.

[8] He X， Zhang L， Fan C， et al. A MIMU/Polarized Camera/GNSS Integrated Navigation Algorithm for UAV Application[C]// 2019 DGON Inertial Sensors and Systems （ISS）. IEEE， 2019.

[9] 金仁成，陈文，孙会生，等. MIMU/偏振光互补滤波组合导航算法研究[J].燕山大学学报，2016， 40（3）：276-282.

[10] Qian H， Qiu Z. Iterated Unscented Kalman Filter for Spacecraft Attitude Estimation[C]//中国控制会议，2018.

[11] Feng L， Yue B， Qu X. Airborne platform estimation based on the algorithm of Unscented Kalman filter quaternion[C]//2016 IEEE Chinese Guidance， Navigation and Control Conference （CGNCC）. IEEE， 2017.

[12] 陈国通， 范圆圆， 孙敬. 四元数算法在姿态矩阵解算中的研究[J]. 信息通信， 2019（3）：46-48.

[13] 金仁成， 孙加亮， 谢林达，等. 基于偏振光传感器的全姿态角解算方法研究[J]. 单片机与嵌入式系统应用， 2018（6）：47-50.

[14] 孙金秋，游有鹏，傅忠云.基于共轭梯度法和互补滤波相结合的姿态解算算法[J].传感技术学报，2014，27（4）：524-528.

[15] 曾聪，章政，王龙.基于共轭梯度的EKF姿态估计算法[J].计算机工程与设计，2018，39（10：）3118-3128.

第一作者简介：金仁成（1969-），男，吉林长春人，博士，副教授，研究领域为无线传感器网络与无人机自主导航技术，已发表论文70余篇。

（编辑：王智圣）