CN107202578B - 一种基于mems技术的捷联式垂直陀螺仪解算方法 - Google Patents

Abstract

本发明公开了一种基于MEMS技术的捷联式垂直陀螺仪解算方法，具体按照以下步骤实施：步骤1，互补对称加速度传感器的布局与各角度解算，步骤2，进行捷联解算，在步骤1中解算出的俯仰角和横滚角基础上，把三轴陀螺仪测得载体的角速度信息按俯仰轴和横滚轴二次旋转，变换到水平地理坐标系，按捷联算法求出俯仰角和横滚角，步骤3，利用Kalman滤波器进行信息融合，解决了现有机械式垂直陀螺仪在载体加速过程中的测量不准确问题和捷联式惯导系统的高成本问题。

Description

一种基于MEMS技术的捷联式垂直陀螺仪解算方法

技术领域

本发明属于电子信息和导航技术领域，涉及一种基于MEMS技术的捷联式垂直陀螺仪解算方法。

背景技术

垂直陀螺仪(Vertical Gyroscope,VG)是无人机、稳定云台和“动中通”天线等重要的姿态传感器，其在载体运动过程中实时获取载体的姿态信息,即俯仰角和横滚角。

机械式垂直陀螺仪：机械式垂直陀螺仪是一个框架式双自由度陀螺和液体摆构成的闭环控制系统，使陀螺的自转轴稳定在当地地垂线方向上，它的原理就是利用陀螺的定轴性和进动性可测量载体的俯仰角和横滚角。机械螺仪由于其结构复杂、制造困难、漂移误差大和使用环境要求高等，现在已经很少应用。

捷联式惯导系统：捷联式惯导系统不采用实体平台，把三轴加速度计与三轴陀螺仪直接固连在载体上，利用计算机实时计算姿态矩阵，通过姿态矩阵把加速度计测得的机体的加速度信息，变化到导航坐标系，然后进行导航解算，同时从姿态矩阵的元素中提取航向角和姿态角信息。缺点一是捷联式惯导系统依靠高精度的陀螺和加速度计，才能在初始对准过程中，通过敏感当地的地球自传角速度，才能给出航向角信息；二是由于陀螺仪和加速度计长时间会产生漂移，需要外部信息修正，比如GPS和北斗信息。

发明内容

本发明的目的是提供一种基于MEMS技术的捷联式垂直陀螺仪解算方法，解决了现有机械式垂直陀螺仪在载体加速过程中的测量不准确问题和捷联式惯导系统的高成本问题。

本发明所采用的技术方案是，一种基于MEMS技术的捷联式垂直陀螺仪解算方法，基于如下定义的坐标系：垂直陀螺仪的导航坐标系取为地理坐标系，建立载体所在位置的地理坐标系为北-东-地，即N-E-D，固连在载体上的载体坐标系为X-Y-Z，X处于载体对称平面内，由质心指向载体运动前向；Y垂直于载体对称平面并指向右方；Z在载体对称平面内且垂直于X轴指向下方，其特征在于，具体按照以下步骤实施：

步骤1，互补对称加速度传感器的布局与各角度解算，

步骤2，进行捷联解算，在步骤1中解算出的俯仰角和横滚角基础上，把三轴陀螺仪测得载体的角速度信息按俯仰轴和横滚轴二次旋转，变换到水平地理坐标系，按捷联算法求出俯仰角和横滚角，

步骤3，利用Kalman滤波器进行信息融合。

本发明的特点还在于，

所述的步骤1具体为，

步骤1.1，互补对称加速度传感器的布局，将在载体的X-Z和Y-Z平面沿X轴和Y轴方向，且与X轴和Y轴的夹角分别为α角度分别布置2对加速度传感器，在X-Y-Z轴三个方向上，各布置一个陀螺仪；

步骤1.2，俯仰角度的解算，

设载体X轴方向有加速度x_a和俯仰角度x_t时，从两个加速度传感器上采集到的加速度的输出值ax1和ax2分别为：

经过推导，可以求得：

其中中间变量：

步骤1.3，横滚角的解算，

当载体Y轴方向有加速度y_a和横滚角度y_t时，利用同样的原理，借助公式(1)、(2)在求出横滚角度y_t的基础之上除以cos(x_t)即可。

所述的步骤2具体为：

设载体坐标系下的俯仰角增量θ、横滚角增量γ，载体坐标系与地理坐标系经过两次旋转变换，变换阵为：

载体坐标系上，按步骤1.1方法布置的三个陀螺的测量值分别为：ω_x、ω_y和ω_z，把载体系测量得到的三维角速度转换到地理坐标系，得到：

式中

和

为水平地理系下对应的角速度，

对应横滚轴；

对应俯仰轴；

角速度在采样周期内积分，根据(4),结合初始条件可以得到水平地理坐标系下的俯仰角增量θ和横滚角增量γ分别为：

式中T为解算周期。

步骤3具体为，以俯仰角误差φ_x＝x_t-θ和横滚角误差φ_y＝y_t-γ以及三个漂移误差作为Kalman滤波器的状态变量；

步骤3.1，建立系统状态方程，

取垂直陀螺仪系统的状态量X为：

X＝[φ_x φ_y ε_x ε_y ε_z]^T，

式中：φ_x是俯仰角测量误差，φ_y是横滚角的测量误差；ε_x、ε_y和ε_z为三轴陀螺仪的漂移误差，

这三个误差事前通过静态测试得到，用一阶马尔科夫过程描述，则垂直陀螺仪的状态方程为：

式中：系统状态转移矩阵

系统噪声驱动矩阵：

系统激励噪声矩阵W＝[w_x w_y w_z]^T，

τ_x、τ_y、τ_z和w_x、w_y、w_z分别为X、Y和Z轴陀螺的一阶马尔科夫时间和方差。

步骤3.2，建立系统量测方程，

以互补对称加速度传感器解算的角度x_t、y_t和捷联解算得到的角度θ、γ之差作为观测量：

所以量测方程为：Z＝HX+V

式中：量测矩阵

V为加速度测量角度的方差。

本发明的有益效果是，通过传感器的布局、姿态角度的捷联式算法、载体加速度对角度解算误差的消除和捷联式解算数据与倾角解算的数据融合等技术，最终给出动态环境下的载体俯仰角和横滚角的测量值。

附图说明

图1是本发明基于MEMS技术的捷联式垂直陀螺仪解算方法中的互补对称加速度传感器的布置图；

图2是本发明基于MEMS技术的捷联式垂直陀螺仪解算方法中的三轴陀螺仪的布置图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

一种基于MEMS技术的捷联式垂直陀螺仪解算方法，基于如下定义的坐标系：垂直陀螺仪的导航坐标系取为地理坐标系，建立载体所在位置的地理坐标系为北-东-地(即N-E-D)、固连在载体上的载体坐标系为X-Y-Z，X处于载体对称平面内，由质心指向载体运动前向；Y垂直于载体对称平面并指向右方；Z在载体对称平面内且垂直于X轴指向下方，

具体按照以下步骤实施：

步骤1，互补对称加速度传感器的布局与各角度解算，

步骤1.1，互补对称加速度传感器的布局，按照如图1所示，将在载体的X-Z和Y-Z平面沿X轴和Y轴方向，且与X轴和Y轴的夹角分别为α角度分别布置2对加速度传感器，在X-Y-Z轴三个方向上，各布置一个陀螺仪；

步骤1.2，俯仰角度的解算，

设载体X轴方向有加速度x_a和俯仰角度x_t时，从两个加速度传感器上采集到的加速度的输出值ax1和ax2分别为：

经过推导，可以求得：

其中中间变量：

步骤1.3，横滚角的解算，

当载体Y轴方向有加速度y_a和横滚角度y_t时，利用同样的原理，借助公式(1)、(2)在求出横滚角度y_t的基础之上除以cos(x_t)即可。

步骤2，进行捷联解算，在步骤1.2中解算出的俯仰角和横滚角基础上，把三轴陀螺仪测得载体的角速度信息按俯仰轴和横滚轴二次旋转，变换到水平地理坐标系，按捷联算法求出俯仰角和横滚角，设此方法解算出的俯仰角为θ，横滚角为γ，与之前解算出的俯仰角x_t和横滚角y_t代表的是同一角度，只是表示方法不同。

步骤2.1，设载体坐标系下的俯仰角增量θ、横滚角增量γ，载体坐标系与地理坐标系经过两次旋转变换，变换阵为：

载体坐标系上，按步骤1.1方法布置的三个陀螺的测量值分别为：ω_x、ω_y和ω_z，把载体系测量得到的三维角速度转换到地理坐标系，得到：

式中

和

为水平地理系下对应的角速度，

对应横滚轴；

对应俯仰轴。

角速度在采样周期内积分，根据(4),结合初始条件可以得到水平地理坐标系下的俯仰角增量θ和横滚角增量γ分别为

式中T为解算周期。

步骤3，利用Kalman滤波器进行信息融合，以俯仰角误差φ_x＝x_t-θ和横滚角误差φ_y＝y_t-γ以及三个漂移误差作为Kalman滤波器的状态变量。

步骤3.1，建立系统状态方程，

取垂直陀螺仪系统的状态量X为：

X＝[φ_x φ_y ε_x ε_y ε_z]^T

式中：φ_x是俯仰角测量误差，φ_y是横滚角的测量误差；ε_x、ε_y和ε_z为三轴陀螺仪的漂移误差,这三个误差事前通过静态测试得到，用一阶马尔科夫过程描述，则垂直陀螺仪的状态方程为：

式中：系统状态转移矩阵

系统噪声驱动矩阵：

系统激励噪声矩阵W＝[w_x w_y w_z]^T，

τ_x、τ_y、τ_z和w_x、w_y、w_z分别为X、Y和Z轴陀螺的一阶马尔科夫时间和方差。

步骤3.2，建立系统量测方程，

以互补对称加速度传感器解算的角度x_t、y_t和捷联解算得到的角度θ、γ之差作为观测量：

所以量测方程为：Z＝HX+V

式中：量测矩阵

V为加速度测量角度的方差。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明可借助三轴MEME技术的陀螺仪和四个MEMS技术的加速度传感器，在ARM或DSP硬件平台实现。

本发明的有益效果是：

(1)本发明的垂直陀螺仪，利用了对称互补的加速度原理，可以有效消除载体加速度信息对角度解算的影响。

(2)本发明利用三轴陀螺仪按捷联算法解算，可以与加速度解算出的角度信息形成互补滤波，防止角度解算出错，同时也系统的相应带宽。

(3)本发明以加速度传感器解算出的角度信息作为Kalman滤波器的观测量，设计Kalman滤波器，通过滤波后可以消除角度和陀螺仪的漂移误差。

(4)适用范围广泛，适用于无人机和稳定平台。

本发明利用低成本的MEMS技术的三轴陀螺仪和四轴的加速度，三轴陀螺仪按照载体的OXYZ轴安装，四个加速度计分别安装在OXZ两个和OYZ平面两个，两个加速度计对称安装，与各自的X轴和Y轴成统一的对称角度，这样就能够克服载体加速度和载体非直线运动时离心加速度信息对姿态角度信息解算带来的影响。同时提出了一种基于捷联解算角度信息与姿态角的信息融合方法。一方面，该算法能够克服姿态角度解算动态和静态测量精度不足问题；另一方面利用陀螺仪解算能够克服载体非直线运动离心加速度对姿态角解算精度的影响问题，最后该方法不需要任何外部信息，利用自身的姿态角度信息利用Kalman滤波器对陀螺的漂移误差进行修正，能够保证垂直陀螺仪的长时间精度。

Claims (3)

1.一种基于MEMS技术的捷联式垂直陀螺仪解算方法，基于如下定义的坐标系：垂直陀螺仪的导航坐标系取为地理坐标系，建立载体所在位置的地理坐标系为北-东-地，即N-E-D，固连在载体上的载体坐标系为X-Y-Z，X处于载体对称平面内，由质心指向载体运动前向；Y垂直于载体对称平面并指向右方；Z在载体对称平面内且垂直于X轴指向下方，其特征在于，具体按照以下步骤实施：

步骤1，互补对称加速度传感器的布局与各角度解算，具体为，

步骤1.1，互补对称加速度传感器的布局，将在载体的X-Z和Y-Z平面沿X轴和Y轴方向，且与X轴和Y轴的夹角分别为α角度分别布置2对加速度传感器，在X-Y-Z轴三个方向上，各布置一个陀螺仪；

步骤1.2，俯仰角度的解算，

设载体X轴方向有加速度x_a和俯仰角度x_t时，从两个加速度传感器上采集到的加速度的输出值ax1和ax2分别为：

经过推导，可以求得：

其中中间变量：

步骤1.3，横滚角的解算，

当载体Y轴方向有加速度y_a和横滚角度y_t时，利用同样的原理，借助公式(1)、(2)在求出横滚角度y_t的基础之上除以cos(x_t)即可，

步骤2，进行捷联解算，在步骤1中解算出的俯仰角和横滚角基础上，把三轴陀螺仪测得载体的角速度信息按俯仰轴和横滚轴二次旋转，变换到水平地理坐标系，按捷联算法求出俯仰角和横滚角，

步骤3，利用Kalman滤波器进行信息融合。

2.根据权利要求1所述的基于MEMS技术的捷联式垂直陀螺仪解算方法，其特征在于，所述的步骤2具体为：

设载体坐标系下的俯仰角增量θ、横滚角增量γ，载体坐标系与地理坐标系经过两次旋转变换，变换阵为：

载体坐标系上，按步骤1.1方法布置的三个陀螺的测量值分别为：ω_x、ω_y和ω_z，把载体系测量得到的三维角速度转换到地理坐标系，得到：

式中

和

为水平地理系下对应的角速度，

对应横滚轴；

对应俯仰轴；

角速度在采样周期内积分，根据(4),结合初始条件可以得到水平地理坐标系下的俯仰角增量θ和横滚角增量γ分别为：

式中T为解算周期。

3.根据权利要求1所述的基于MEMS技术的捷联式垂直陀螺仪解算方法，其特征在于，所述的步骤3具体为，以俯仰角误差φ_x＝x_t-θ和横滚角误差φ_y＝y_t-γ以及三个漂移误差作为Kalman滤波器的状态变量；

步骤3.1，建立系统状态方程，

取垂直陀螺仪系统的状态量X为：

X＝[φ_x φ_y ε_x ε_y ε_z]^T，

式中：φ_x是俯仰角误差，φ_y是横滚角误差；ε_x、ε_y和ε_z为三轴陀螺仪的漂移误差，

这三个误差事前通过静态测试得到，用一阶马尔科夫过程描述，则垂直陀螺仪的状态方程为：

式中：系统状态转移矩阵

系统噪声驱动矩阵：

系统激励噪声矩阵W＝[w_x w_y w_z]^T，

τ_x、τ_y、τ_z和w_x、w_y、w_z分别为X、Y和Z轴陀螺的一阶马尔科夫时间和方差；

步骤3.2，建立系统量测方程，

以互补对称加速度传感器解算的角度x_t、y_t和捷联解算得到的俯仰角增量θ、横滚角增量γ之差作为观测量：

所以量测方程为：Z＝HX+V

式中：量测矩阵

V为加速度测量角度的方差。

Images