基于偏振光与MEMS 陀螺的航向角测量系统设计*

支 炜，褚金奎，王寅龙

(大连理工大学 精密与特种加工教育部重点实验室，辽宁 大连116024)

0 引 言

在飞行器的飞行控制过程中，实时准确地获得飞行器在空中的航向和位置是保证飞行器安全的重要前提。尽管角速度陀螺可以进行单独航向角度测量，但是由于飞行器体积与承载能力的限制，飞行器中大多采用MEMS 角速度陀螺，但此种陀螺精度较低，系统误差较大，并且由于被测角度与陀螺输出存在积分关系，角度会随时间积累而不断增大［1］，不适用于长时间载体航向角的确定。因此，如何采用低精度陀螺传感器进行长时间高精度导航成为目前导航领域的热点。

目前用来修正角速度陀螺的传感器有很多种，文献［2］采用磁强计与角速度陀螺组合测量角度，但由于强磁计受周围环境中磁场的影响较大，稳定性不强，并且在地球两极地区地球磁场几乎垂直于地面，因此，在很多情况采用此种方法测算水平航向角并不可靠。文献［3］采用差分GPS 修正角速度陀螺的测量值，但由于差分GPS 需要配置多天线，要求飞行载体具有较大的安装空间，同时GPS 信号本身受制于人，属于被动式导航，因此，在某些需要保密的场合并不适用。

本文采用自主式导航的偏振光传感器修正角速度陀螺随机误差，并建立航向角度测量特征模型，采用直接式卡尔曼滤波方法对角速度陀螺和偏振光传感器的输出信号进行融合，算法简单实用，能够获得实时高精度无漂移的航向角。

1 偏振光测量原理［4］

偏振光导航传感器是根据天空中的偏振光分布模式为依据来进行导航的传感器，偏振光传感器模仿沙蚁头背部的偏振敏感复眼感知天空偏振光模式，确定体轴与太阳子午线的夹角，可以直接输出水平面内全方位的角度信号，并且精度不随温度、时间变化而产生累计误差。偏振光传感器输出信号是关于偏振方位角、偏振度的函数，其关系满足以下方程

式中 d 为天空偏振光的偏振程度，α 为偏振方位角，是传感器的参考方向与太阳子午线之间的夹角，p1(α)，p2(α)，p3(α)为3 个偏振光检测单元输出的带有天空偏振光信息的电压信号。经过公式变换整理后得到如下公式

进一步可以得到航向角计算公式

本文采用的偏振光传感器经室外精密转台的测试表明，晴朗天空时该传感器单周期内定向误差［5］为±0.2°。

2 卡尔曼滤波模型

采用卡尔曼滤波算法进行数据融合，首先建立系统的状态方程和测量方程。

MEMS 陀螺输出载体的角速度，在采样时间为T 的情况下，航向角测量值与陀螺输出的关系［6］可用公式(8)所示

其中，˙φ 为陀螺输出，φk+1与φk分别为k+1 时刻与k 时刻的航向角。

由于偏振光传感器的角度输出与陀螺的角速度输出存在导数关系，状态方程直接选取真实航向角作为状态向量，并将陀螺常值偏差扩展为状态变量，以便在滤波过程中直接估计陀螺的常值漂移。偏振光传感器输出直接作为量测值，与系统变量呈线性关系。建立状态方程和观测方程如下

简写成

如果要估算k 时刻的实际角度，还需已知系统过程噪声协方差阵Q 和测量误差的协方差矩阵R，这2 个矩阵分别由系统状态变量和测量变量的测量噪声来决定。Q 与R矩阵的形式如下

式中 Qgyro和Qε分别是陀螺测量和陀螺测量常值误差的协方差，Rpol是偏振光传感器测量的方差，其数值代表卡尔曼滤波器对其传感器数据的信任程度，值越小，表明信任程度越高。在该系统中偏振光传感器的值更为接近准确值，因此，取Rpol的值小于Q 的值。

数据融合过程如图1 所示，陀螺输出角速度经过积分后进入卡尔曼滤波器进行状态更新，当发现新的量测值时，再进行量测更新。状态更新频率与量测更新频率可以不相等，在实际运用中设系统状态采样时间为T1，量测值采样时间为T2，一般T2会大于T1，当没有新的量测值、或者不需要高频率的量测矫正时，系统只进行时间更新，当发现新的量测值时，再进行量测更新。这样设计的最大优点是能够充分利用系统资源，提高运算速度。卡尔曼滤波器输出的陀螺常值漂移闭环反馈给角速度陀螺，以便得到更高精度的输入。

图1 数据融合算法Fig 1 Algorithm of data fusion

3 实验与分析

为了验证卡尔曼滤波信号融合方法的有效性，在室内进行了测试实验，卡尔曼滤波算法参数设置如表1 所示。

表1 滤波参数Tab 1 Parameters of filtering

实验过程中，采用北京润诚时代科技公司的FMRS 型旋转定位台输出作为测量标准来验证本文提出的算法。此转台带有反馈数显装置，可以直接读取测量值，定位精度为3'，重复定位精度为2″，最大转速为10°/s。实验装置如图2所示，MEMS 陀螺型号为L3G4200D。首先，将实验采用与偏振光传感器的中心重合，固定于高精度转台之上。之后，转台带动陀螺/偏振光导航系统以10°/s 的转速，在水平360°范围内进行匀速旋转，每次采样周期T 大约为52 ms，实际采样时间由采样处理器计时器精确计时。历时大约300 s，采集约6 000 组数据。

图2 实验装置Fig 2 Experimental device

图3 表示实验结果，从图中可以看出:角速度陀螺单独输出的数据积分值随着时间的推移误差逐渐增大，280 s 后测量误差达到36.27°，但经过偏振光传感器作为量测值的滤波校正后，估计角度(▲标记)的偏差降为6.05°。

图3 最优估计航向角Fig 3 Heading angle of the optimal estimation

图4 表示在滤波过程中估计出的角速度陀螺的常值漂移，将此常值漂移反馈到角速度陀螺中，即可在线及时修正陀螺输出。

图4 角速度陀螺的常值漂移Fig 4 Constant value drift of angle velocity gyro

4 结束语

本文研究了采用偏振光导航传感器对角速度陀螺随机漂移的矫正方法，在动态数据采集实验的基础上，通过直接卡尔曼滤波算法，有效地补偿传感器漂移与测量噪声等因素对陀螺的影响。实验结果表明:实验进行280 s 后，测量误差由原来的36.27°下降为6.05°。该方法算法简单实用，对微处理器的资源占用率低，为进一步设计微小型机器人和飞行器组合导航控制系统提供有力依据。

［1］ 吉训生，王寿荣.MEMS 陀螺仪随机漂移误差研究［J］.宇航学报，2006，27(4):640－642.

［2］ 黄 旭，王常虹.磁强计和微机械陀螺/加速度计组合定姿的扩展卡尔曼滤波器设计［J］.黑龙江大学自然科学学报，2005(4):454－458.

［3］ 曹 超，马 磊，赵 舵，等.基于ARM 的移动机器人组合导航系统设计与实现［J］.传感器与微系统，2013，32(2):102－104.

［4］ 张 强，赵开春，褚金奎，等.一种嵌入式仿生测角传感器的设计与实现［J］.传感器与微系统，2008，27(9):106－111.

［5］ 褚金奎，王洪青，戎成功，等.基于偏振光传感器的导航系统实验测试［J］.宇航学报，2011，32(3):489－494.

［6］ 冯智勇，曾 瀚.基于陀螺仪及加速度计信号融合的姿态角度测量［J］.西南师范大学学报:自然科学版，2011，36(4):137－141.