一种应用于三轴稳定器中的陀螺仪漂移抑制方法

程小辉，张 宇

（桂林理工大学a.广西嵌入式技术与智能系统重点实验室；b.信息科学与工程学院，广西桂林541006）

0 引 言

科学技术的进步推动了微机电系统（Micro Electro Mechanical System，MEMS）的快速发展，而人工智能的浪潮也为MEMS 器件的发展带来新的方法。由于MEMS器件的特征长度在微米级甚至纳米级，且质量轻、功耗低、可靠性高、灵敏性好、易于集成，因此逐渐取代了传统的机械传感器。其中，微机械陀螺仪即MEMS陀螺仪作为一种灵敏可靠的惯性器件被广泛用于无人机、惯性导航等运动传感器中。

传统机械陀螺仪主要利用的是角动量守恒原理来维持机械的平衡；而MEMS 陀螺仪主要利用科里奥利力的原理来维持平衡。MEMS陀螺仪精度的高低决定了它所应用控制装置的稳定性，因此，用现有的人工智能技术提高陀螺仪的精度是不可忽视的方法。陀螺仪的精度主要由误差来决定，即陀螺仪的漂移值。陀螺仪的漂移误差分为确定性漂移误差和随机漂移误差［1］，确定性漂移误差是由系统干扰力矩和陀螺仪自转轴偏离给定方向的进动累计等等各种确定性的因素造成的，通过建立确定误差的数学模型进行相应的误差补偿可以基本消除确定性漂移误差。而陀螺仪的随机漂移是由干扰力矩中随机性部分引起的，随机漂移误差变化规律没有确定的形式，也无法用确定的数学函数来加以描述，所以不能用常规方法加以补偿。因此只能从算法方向入手，通过算法来对随机误差进行分析补偿，才能削弱随机漂移误差，抑制陀螺仪的漂移误差，提高陀螺仪的精度。

针对陀螺仪的漂移问题，车晓蕊等［2］通过单轴正反连续旋转的解调技术和卡尔曼滤波减小了姿态角误差，提高了系统的精度。张佳舒等［3］通过RBF算法将机械臂的控制误差收敛到零，实验表明，该方法在控制误差收敛的问题上有较好的效果。Wang 等［4］将基于相空间的线性分解和卡尔曼滤波算法应用于单轴的捷联惯导系统能够有效地降低陀螺仪的噪声干扰。徐韩等［5］通过阿伦方差法验证了改进的卡尔曼滤波可以有效地对MEMS 陀螺进行误差补偿。Peng 等［6］用Allan方差法对陀螺仪的随机误差进行了分析。吴鹏等［7］将卡尔曼滤波应用与惯性系统中，得到了预期的成果。还有很多学者用了不同的方法对MEMS 陀螺仪的漂移进行抑制［8-13］。

RBF神经网络具有非线性变换的特性和高度并行的运算能力，在理论上可以以任意精度逼近非线性函数的非线性函数［14-15］。本文根据陀螺的随机漂移误差需要预测补偿的特点，通过RBF神经网络进行补偿，并在之后进行卡尔曼滤波算法可以很好地补偿陀螺仪具有慢时变、非线性的特点的随机漂移误差累计，对随机误差进行消除。因此通过神经网络RBF 算法与卡尔曼滤波相结合对MEMS 陀螺的随机漂移误差进行建模补偿，可以抑制陀螺仪的随机误差漂移，达到预期效果。

1 模型的建立

1.1 三轴稳定器模型

三轴平衡稳定器的模型如图1 所示，手持握把中嵌入有控制芯片，控制稳定器3 个不同位置中的航向电机、横滚电机和俯仰电机。每个电机外部都有可控的旋转轴。使用时，将相机正向推入卡座中与稳定器通过端口相连，打开开关，稳定器的俯仰轴、横滚轴、航向轴就会在内部算法的指令下对相机位置进行稳定调整。当相机位置发生变化时，芯片监测到相机位置变化，会通过电路驱动电机控制外部旋转轴来对这些不平衡量进行调整，通过陀螺仪、加速度计等内部模块调整系统参数从而控制3 个电机的位置使得相机始终处于平衡位置，达到相机拍摄画面平稳的目的。

稳定器旋转的方向按右手法则定义，即右手大拇指指向轴向，四指弯曲的方向就是该轴旋转的方向。X轴角度即为绕X轴旋转方向的角度，Y轴角度即为绕Y轴旋转方向的角度，Z轴角度即为绕Z轴旋转方向的角度。

图1 三轴稳定器坐标示意图

1.2 陀螺仪姿态线性模型

通过对采集的数据进行分析，根据载体坐标下陀螺仪存在的漂移和高斯白噪声误差，建立陀螺仪数学模型［16］

式中：φg（t）为陀螺仪的输出；φ（t）为输出角速度；ε（t）为随机漂移误差

vg为测量白噪声；M1、M2和T是根据陀螺仪的实际输出得到。将式（2）进行微分可得：

将式（3）离散后得到差分模型：

式中，Ts是采样时间。

2 稳定器中陀螺仪的误差补偿

2.1 径向基函数（RBF）神经网络原理

在RBF网络中，输入层到隐含层的映射是非线性的，输入层仅仅起到传输信号作用，因此输入层和隐含层之间是连接权值为1 的连接。而隐含层到输出层的映射是线性的。隐含层到输出层采用线性优化策略因而学习速度较快［17］。

RBF的神经网络模型如图2 所示。图中：x1（k），x2（k），…，xn（k）表示输入变量，在本文中代表图1 中三轴稳定器横滚轴、俯仰轴和航向轴3 个轴的角速度输入经建模处理后的输入值；R1，R2，…，Rn表示的是输入的径向基函数；ωh1，ωh2，…，ωhn表示的是隐含层每个神经元的权值；n表示输入的节点数。应用于RBF算法的流程图如图3 所示。

图3 RBF算法流程图

因为高斯基函数有表示形式简单、径向对称、光滑性好、任意阶导数都存在的特点，因此RBF 神经网络的激活函数选择了高斯函数：

RBF神经网络的输出为

设d是样本的期望输出值，则基函数的方差可表示为

算法步骤如下：

基于K-均值聚类的方法求解基函数中心c。

（1）网络初始化。随机选取j个训练样本作为聚类中心ci（i＝1，2，…，j）。

（2）将输入的训练样本集合按最近邻规则分组。按照xp与中心ci之间的欧式距离，将xp分配到输入样本的各个聚类集合中。

（3）重新调整聚类中心。计算新的聚类中心ci，ci是聚类集合中训练样本的平均值，如果ci不变，则为RBF最终的基中心，否则再从第（1）步开始重新求解。

（4）求解方差σi。因为选择了高斯函数作为基函数，因此，方差求解为

式中，cmax为选取中心之间的最大距离。

（5）计算隐含层和输出层之间的权值。通过最小二乘法计算得到隐含层至输出层之间的神经元连接权值。

2.2 卡尔曼滤波的基本原理

卡尔曼滤波是一种时域滤波方法，在线性高斯的假设下，采用空间状态方法描述系统，用递推形式实现算法可以直接获得后验概率的解析方程的解，是一种在惯性系统和导航系统等稳定系统中被广泛应用的最优估计理论方法［12］。

应用于陀螺仪卡尔曼滤波算法流程如图4 所示。

图4 卡尔曼滤波算法流程图

（1）状态预测。通过状态转移概率及上一时刻的后验概率p（xk｜yk-1）得出一步预测概率分布，从而得到状态预测的均值和方差。

目标状态预测的均值：

目标状态预测的方差：

（2）由归一化系数p（yk｜yk-1）似然函数与一步状态预测概率的乘积中的状态进行积分，可以得到观测转移的概率分布，从而得到目标观测的均值和方差，并可算出卡尔曼增益，卡尔曼增益可以用来权衡预测与观测对状态滤波的贡献。

目标观测预测的均值：

目标观测预测的方差：

卡尔曼增益

（3）利用递推贝叶斯公式p（xk｜yk）算得状态的后验概率，从而得到目标状态的均值和方差。

目标状态（后验）的均值：

目标状态（后验）的方差：

3 实验结果

综合考虑三轴稳定器的性能和成本，本实验的MCU 选用ARM Cortex-M 内核的32 位微控制器STM32，传感器选用WT931 模块，模块内封装内集成了高精度陀螺仪、加速度计和地磁场传感器。它能通过I2C接口连接MCU，以数字量的形式输出加速度信号和角速度信号，该模块输出频率为500 Hz，波特率为921.600 Mb/s，工作电压3.3 V。数据在稳定器锁定模式下进行采集，采集时间为5 min。将数据通过Matlab进行实验仿真，并输出数据图。图5 是陀螺仪在未经过RBF神经网络估测和卡尔曼滤波的情况下的角速度输出数据。

图5 原始状态下陀螺仪的输出信号

对神经网络进行训练的过程如下，首先通过MATLAB工具箱构建一个输入层为3 并含有5 个神经元的隐含层神经网络，其中，输入层与隐含层的连接权值为1，陀螺仪取前950 000 个漂移数据为训练样本，训练的误差精度设为3.8 ×10-6，对经过神经网络训练和卡尔曼滤波的陀螺仪角速度进行输出，输出数据如图6 ～11 所示。

图6 锁定状态下MEMS陀螺仪x轴的输出图

图7 改进后的锁定状态下x轴的输出

图8 锁定状态下MEMS陀螺仪y轴的输出

图9 改进后的锁定状态下y轴的输出

图10 锁定状态下MEMS陀螺仪z轴的输出

图11 改进后的锁定状态下z轴的输出

图6、7 分别是未经神经网络估测和卡尔曼滤波的陀螺仪轴输出曲线图与通过训练和滤波的陀螺仪轴的输出曲线图。图8 ～11 分别是y轴和z轴改进前与改进后的输出曲线图。通过实验结果对比图可以看出，该方法在抑制陀螺仪的漂移问题上真实有效，可以达到陀螺仪的漂移角速度在0.1°/s的范围内。

图12、13 是在锁定状态下航向角的输出图。由图可见，经过神经网络预测补偿和卡尔曼滤波后的航向角输出。未改进前，航向角的漂移是一直在增加的，改进后航向角的漂移被抑制在±5°之间，说明此改进方法真实有效。

图12 锁定状态下原始航向角输出

图13 改进后的航向角输出

4 结 语

本文通过RBF 神经网络方法对陀螺仪的输出进行控制，结合卡尔曼滤波算法对陀螺仪的随机漂移进行抑制，减小惯性误差，提高系统的精度，来实现三轴平衡稳定器精确维持相机画面稳定的目的。选择径向基函数RBF神经网络的优点是，将原本陀螺仪漂移的非线性问题变为线性可分，可以快速地估测陀螺仪的误差并准确的预测补偿，而使用卡尔曼滤波算法进行误差补偿的优点是，不需要设计任何数学模型，只需要根据过去的经验来学习、处理数据，用于数据预测、分类、模式识别等各种数据处理场合。大多数情况下，使用卡尔曼滤波算法得到的结果要优于传统的数学统计方法，能够适用于影响因素多、变化大的场合。

通过仿真实验可以说明使用RBF 神经网络算法与卡尔曼滤波算法相结合可以对陀螺仪的随机漂移误差进行抑制，能够实现预期功能，达到三轴平衡稳定器在摄影者拍摄时保持拍摄画面稳定的目的。