CN103557866B - 一种基于地磁技术的虚拟陀螺仪及算法的方法 - Google Patents

Abstract

本发明公开了一种基于地磁技术的虚拟陀螺仪及算法的方法，基于MEMS技术，使用三轴地磁传感器和三轴加速度传感器组合模拟一个虚拟陀螺仪，解决了现有陀螺仪功耗大成本高等问题。并实现6轴传感器簇代替9轴传感器簇的惯性导航技术方案。

Description

一种基于地磁技术的虚拟陀螺仪及算法的方法

技术领域

本发明属于MEMS惯性导航领域,具体涉及一种基于地磁技术的虚拟陀螺仪及算法的方法。

背景技术

现在应用于消费电子领域的陀螺仪的功耗大，成本高。

发明内容

本发明针对上述问题，提供一种基于地磁技术的虚拟陀螺仪及算法的方法，得到载体的姿态信息和角速度信息。对基于椭球模型假设的软磁硬磁干扰参数进行估计并补偿，利用这些参数对经过低通滤波处理后的传感器数据进行校正，并采用双矢量定姿法计算姿态信息，为了适应外界磁场的变化，采用带有渐消因子的递推最小二乘法对椭球模型参数进行实时估计，使得系统能够自适应地调节以适应环境变化。根据姿态信息，计算得到载体的角速度信息，实现了陀螺的功能，即虚拟陀螺。

本发明解决上述问题所采用的技术方案是：一种基于地磁技术的虚拟陀螺仪,包括：6-DOF传感器簇和数据预处理模块；所述6-DOF传感器簇通过输出端连接数据预处理模块。

进一步地，所述6-DOF传感器簇包括三轴地磁传感器和三轴加速度传感器；所述三轴地磁传感器和三轴加速度传感器通过输出端连接6-DOF传感器簇外部。

更进一步地，所述数据预处理模块包括低通滤波模块、模数转换模块、校准模块及均值滤波模块；所述校准模块通过输入端连接低通滤波模块和模数转换模块；所述均值滤波模块通过输入端连接低通滤波模块和模数转换模块。

更进一步地，所述基于地磁技术的虚拟陀螺仪包括：电子罗盘算法的方法和虚拟陀螺算法的方法。

更进一步地，所述电子罗盘算法的方法包括以下步骤：

(1)利用校正后的磁传感器数据和加速度传感器数据，根据双矢量定姿原理计算载体实时的姿态信息；

(2)根据姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角。

更进一步地，其中的利用校正后的磁传感器数据和加速度传感器数据，根据双矢量定姿原理计算载体实时的姿态信息包括以下步骤：

重力矢量在导航坐标系n系为地磁矢量在导航坐标系n系为加速度计和磁传感器敏感到的分别是重力和地磁矢量在载体坐标系b系的投影：和则根据惯性导航原理有：

分别取他们的叉乘矢量，有：

和

因此得到：

故姿态矩阵

更进一步地，根据姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角，包括以下步骤：

载体的三个姿态角Y,P,R看做一组欧拉角，经过三次旋转可以将导航坐标系n系转换到载体坐标系b系，得到转移矩阵三次转动的顺序为：绕z轴转Y角，绕x轴转P角，再绕y轴转R角。三次转动对应的变换矩阵分别为

$C_Y=\left[\begin{array}{ccc}\cos Y& \sin Y& 0\\ -\sin Y& \cos Y& 0\\ 0& 0& 1\end{array}\right]$

$C_P=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos P& -\sin P\\ 0& \sin P& \cos P\end{array}\right]$

$C_R=\left[\begin{array}{ccc}\cos R& 0& -\sin R\\ 0& 1& 0\\ \sin R& 0& \cos R\end{array}\right]$

所以转换矩阵为：

$C_n^b=\left[\begin{array}{ccc}\cos Y\cos R+\sin R\sin P\sin Y& \cos R\sin Y-\sin R\cos Y\sin P& -\sin R\cos P\\ -\sin Y\cos P& \cos Y\cos P& -\sin P\\ \sin R\cos Y+\cos R\sin Y\sin P& \sin R\sin Y+\cos R\cos Y\sin P& \cos P\cos R\end{array}\right]$

令

$C_b^n={\left(C_n^b\right)}^T={\[T_{ij}\]}_{3\×3}$

更进一步地，所述虚拟陀螺算法的方法包括以下步骤：

根据动力学原理，利用姿态信息计算载体的角速率信息，实现虚拟陀螺的功能。

更进一步地，其中的根据动力学原理，利用姿态信息计算载体的角速率信息，实现虚拟陀螺的功能包括以下步骤：

由惯性导航原理,有：

vb和vn分别为载体的速度在载体坐标系b系和导航坐标系n系的投影。

对其求导，忽略线加速度有：

由于有

由动力学知识有：

${\stackrel{\·}{v}}_n={\stackrel{\·}{v}}_b-\mathrm{\ω}\×v_b$

其中，为b系相对于n系的转动角速度。

${\stackrel{\·}{v}}_n=0,{\stackrel{\·}{v}}_b=\mathrm{\ω}\×v_b$

所以有：

$\[\mathrm{\ω}\×\]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_x& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_x& 0& -{\mathrm{\ω}}_z\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_z& 0\end{array}\right]$

离散化后有

$\[\mathrm{\ω}\×\]=\frac{C_{nk+1}^b-C_{nk}^b}{\mathrm{\Δ}t}{\left(C_n^b\right)}^{-1}$

由于是正交矩阵,有:

${\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T$

$\[\mathrm{\ω}\×\]=\frac{C_{nk+1}^b-C_{nk}^b}{\mathrm{\Δ}t}{\left(C_n^b\right)}^T=\frac{C_{nk+1}^b\·{\left(C_n^b\right)}^T-I}{\mathrm{\Δ}t}$

令

ω_x＝(1/2Δt)(W₃₂-W₂₃)

ω_y＝(1/2Δt)(W₁₃-W₃₁)

ω_z＝(1/2Δt)(W₂₁-W₁₂)

计算角速度结束。

本发明的优点是：

1、使用MEMS地磁传感器虚拟陀螺仪，从而为客户提供角速度和姿态信息，并且价格低，功耗低，体积小，集成方便；

2、实现6轴传感器簇代替9轴传感器簇的惯性导航系统。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

在附图中：

图1为本发明的原理框图；

图2为本发明的6轴传感器簇等价于9轴传感器簇原理图。

具体实施方式

下面将参考附图并结合实施例，来详细说明本发明。

参考图1和图2，如图1和图2所示的一种基于地磁技术的虚拟陀螺仪，包括：6-DOF(DegreesofFreedom)传感器簇和数据预处理模块；所述6-DOF传感器簇通过输出端连接数据预处理模块。

所述6-DOF传感器簇包括三轴地磁传感器和三轴加速度传感器，所述三轴地磁传感器和三轴加速度传感器通过输出端连接6-DOF传感器簇外部。

所述数据预处理模块包括低通滤波模块、模数转换模块、校准模块及均值滤波模块；所述校准模块通过输入端连接低通滤波模块和模数转换模块；所述均值滤波模块通过输入端连接低通滤波模块和模数转换模块。

所述数据预处理模块功能：

(1)磁传感器数据经过低通滤波器去噪声，再经过校正模块消除软磁硬磁干扰，将磁数据映射到空间的标准球上；

(2)加速度数据经过低通滤波器去除噪声，滤除高频的震动信息，再经过均值平滑器滤除有害加速度的影响。

所述基于地磁技术的虚拟陀螺仪算法的方法包括：电子罗盘算法的方法和虚拟陀螺算法的方法。

更进一步地，所述电子罗盘算法的方法包括以下步骤：

(1)利用校正后的磁传感器数据和加速度传感器数据，根据双矢量定姿原理计算载体实时的姿态信息；

(2)根据姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角。

其中的利用校正后的磁传感器数据和加速度传感器数据，根据双矢量定姿原理计算载体实时的姿态信息包括以下步骤：

重力矢量在导航坐标系n系为地磁矢量在导航坐标系n系为加速度计和磁传感器敏感到的分别是重力和地磁矢量在b系(载体坐标系)的投影：和则根据惯性导航原理有：

分别取他们的叉乘矢量，有：

和

因此得到：

故姿态矩阵

其中的姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角，包括以下步骤：

载体的三个姿态角Y,P,R看做一组欧拉角，经过三次旋转可以将导航坐标系n系转换到载体坐标系b系，得到转移矩阵三次转动的顺序为：绕z轴转Y角，绕x轴转P角，再绕y轴转R角。三次转动对应的变换矩阵分别为

$C_Y=\left[\begin{array}{ccc}\cos Y& \sin Y& 0\\ -\sin Y& \cos Y& 0\\ 0& 0& 1\end{array}\right]$

$C_P=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos P& -\sin P\\ 0& \sin P& \cos P\end{array}\right]$

$C_R=\left[\begin{array}{ccc}\cos R& 0& -\sin R\\ 0& 1& 0\\ \sin R& 0& \cos R\end{array}\right]$

所以转换矩阵为：

令

$C_b^n={\left(C_n^b\right)}^T={\[T_{ij}\]}_{3\×3}$

所述虚拟陀螺算法的方法包括以下步骤：

根据动力学原理，利用姿态信息计算载体的角速率信息，实现虚拟陀螺的功能。

其中的根据动力学原理，利用姿态信息计算载体的角速率信息，实现虚拟陀螺的功能包括以下步骤：

由惯性导航原理,有：

vb和vn分别为载体的速度在载体坐标系b系和导航坐标系n系的投影。

对其求导，忽略线加速度有：

由于有

由动力学知识有：

${\stackrel{\·}{v}}_n={\stackrel{\·}{v}}_b-\mathrm{\ω}\×v_b$

其中，ω为b系相对于n系的转动角速度。

${\stackrel{\·}{v}}_n=0,{\stackrel{\·}{v}}_b=\mathrm{\ω}\×v_b$

所以有：

$\[\mathrm{\ω}\×\]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_x& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_x& 0& -{\mathrm{\ω}}_z\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_z& 0\end{array}\right]$

离散化后有

$\[\mathrm{\ω}\×\]=\frac{C_{nk+1}^b-C_{nk}^b}{\mathrm{\Δ}t}{\left(C_n^b\right)}^{-1}$

由于是正交矩阵,有:

${\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T$

$\[\mathrm{\ω}\×\]=\frac{C_{nk+1}^b-C_{nk}^b}{\mathrm{\Δ}t}{\left(C_n^b\right)}^T=\frac{C_{nk+1}^b\·{\left(C_n^b\right)}^T-I}{\mathrm{\Δ}t}$

令

ω_x＝(1/2Δt)(W₃₂-W₂₃)

ω_y＝(1/2Δt)(W₁₃-W₃₁)

ω_z＝(1/2Δt)(W₂₁-W₁₂)

计算出角速度结束。

综上所述，三轴MEMS(Micro-Electro-MechanicalSystems)地磁传感器和虚拟陀螺仪算法的方法求出了陀螺仪角速率，地磁传感器最终输出航向信息和角速度信息。根据姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角。如图1所示，此方案解决在消费电子产品领域内的惯性导航问题，比如玩具车和玩具飞机姿态导航，手机导航的姿态和航向。

进一步，因为加速度传感器感知线性运动，经过导航算法的方法计算输出横滚和俯仰姿态信息；地磁感知航向，经过导航算法的方法计算输出航向信息；陀螺仪感知角运动，根据上述的虚拟陀螺算法的方法，使用地磁来虚拟陀螺仪，并计算出角速度。因此，如图2所示，加速度传感器和地磁传感器可以在惯性导航系统中实现6轴传感器簇来代替9轴传感器簇的航姿参考系统

(AHRS)。

以上所述仅为本发明的优选实施例而已，本发明包括但不限于本实例，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于地磁技术的虚拟陀螺仪算法的方法，其特征在于，包括：电子罗盘算法的方法和虚拟陀螺算法的方法，

所述虚拟陀螺算法的方法包括以下步骤：根据动力学原理，利用姿态信息计算载体的角速率信息，实现虚拟陀螺的功能，

所述根据动力学原理，利用姿态信息计算载体的角速率信息，实现虚拟陀螺的功能包括以下步骤：

由惯性导航原理,有：

vb和vn分别为载体的速度在载体坐标系b系和导航坐标系n系的投影，对其求导，忽略线加速度有：

由于有

由动力学知识有：

${\stackrel{\·}{v}}_n={\stackrel{\·}{v}}_b-\mathrm{\ω}\×v_b$

其中，为b系相对于n系的转动角速度，

${\stackrel{\·}{v}}_n=0,{\stackrel{\·}{v}}_b=\mathrm{\ω}\×v_b$

所以有：

$\[\mathrm{\ω}\×\]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_x& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_x& 0& -{\mathrm{\ω}}_z\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_z& 0\end{array}\right]$

离散化后有

$\[\mathrm{\ω}\×\]=\frac{C_{nk+1}^b-C_{nk}^b}{\mathrm{\Δ}t}{\left(C_n^b\right)}^{-1}$

由于是正交矩阵,有:

${\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T$

$\[\mathrm{\ω}\×\]=\frac{C_{nk+1}^b-C_{nk}^b}{\mathrm{\Δ}t}{\left(C_n^b\right)}^T=\frac{C_{nk+1}^b\·{\left(C_n^b\right)}^T-I}{\mathrm{\Δ}t}$

令

ω_x＝(1/2Δt)(W₃₂-W₂₃)

ω_y＝(1/2Δt)(W₁₃-W₃₁)

ω_z＝(1/2Δt)(W₂₁-W₁₂)

计算角速度结束，

所述电子罗盘算法的方法包括以下步骤：

(1)利用校正后的磁传感器数据和加速度传感器数据，根据双矢量定姿原理计算载体实时的姿态信息；

(2)根据姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角，

其中的利用校正后的磁传感器数据和加速度传感器数据，根据双矢量定姿原理计算载体实时的姿态信息包括以下步骤：

重力矢量在导航系n系为地磁矢量在导航坐标系n系为加速度计和磁传感器敏感到的分别是重力和地磁矢量在载体坐标系b系的投影：和则根据惯性导航原理有：

分别取他们的叉乘矢量，有：

和

因此得到：

故姿态矩阵

根据姿态角与姿态矩阵的关系计算出航向角、横滚角和俯仰角包括以下步骤：

载体的三个姿态角Y,P,R看做一组欧拉角，经过三次旋转可以将导航坐标系n系转换到载体坐标系b系，得到转移矩阵三次转动的顺序为：绕z轴转Y角，绕x轴转P角，再绕y轴转R角，三次转动对应的变换矩阵分别为

$C_Y=\left[\begin{array}{ccc}\cos Y& \sin Y& 0\\ -\sin Y& \cos Y& 0\\ 0& 0& 1\end{array}\right]$

$C_P=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos P& -\sin P\\ 0& \sin P& \cos P\end{array}\right]$

$C_R=\left[\begin{array}{ccc}\cos R& 0& -\sin R\\ 0& 1& 0\\ \sin R& 0& \cos R\end{array}\right]$

所以转换矩阵为：

$C_n^b=\left[\begin{array}{ccc}\cos Y\cos R+\sin R\sin P\sin Y& \cos RsinY-\sin R\cos Y\sin P& -\sin R\cos P\\ -\sin Y\cos P& \cos Y\cos P& -\sin P\\ \sin R\cos Y+\cos R\sin Y\sin P& \sin R\sin Y+\cos R\cos Y\sin P& \cos P\cos R\end{array}\right]$

令