CN103344226B - 一种基于mems旋转技术的寻北系统及方法 - Google Patents

Abstract

本发明公开了一种基于MEMS旋转技术的寻北系统，系统分为导航模块和转台模块，导航模块包括：MEMS组件、MEMS数据采集模块、导航计算机；转台模块包括：转动台面、转台微控制器、电机驱动模块、电机、测角模块、测角元件信号调理电路、导电滑环；电机与电机驱动模块相连接，转动台面固定在电机上；测角模块通过导电滑环与转动台面的转轴连接，控制与转动台面的转轴连接的转台微控制器及测角元件信号调理电路，调节转动台面的转角。本发明还公开了一种基于MEMS寻北系统的寻北方法。相对于现有技术本发明结构更加紧凑、体积小、重量轻。根据旋转运动的工作模式，构建动态寻北方法，提高寻北精度，缩短寻北时间，具有精度高、携带方便等优点。

Description

一种基于MEMS旋转技术的寻北系统及方法

技术领域

本发明公开了一种基于MEMS旋转技术的寻北系统及其对应的寻北方法，涉及惯性技术领域。

背景技术

寻北是建立真北方向的技术，目前高精度的寻北方法主要有惯性法、天文观测法、大地测量法、卫星定位法、参照物法等多种寻北方法。但是，在一些有遮挡的复杂地形和天侯等条件下，天文观测法、大地测量法、卫星定位法和参照物法都会受到不同程度的制约，难于实现高精度的寻北功能。相比较而言，惯性法才能够不受自然条件、环境的干扰，独立完成寻北任务，具有连续工作时间长、精度高等特点，是常用的寻北方式。

一般地，基于惯性法的寻北系统，其组成部件主要有陀螺、加速度计、I/F转换电路、直流电源等，通过测量当地地理水平面上地球转速分量的方法，推算当地真北方向，因此，陀螺是系统的关键部件。在不同的时期，寻北仪的研制与陀螺的发展密切相关，目前常用的陀螺有液浮陀螺、动力调谐陀螺、静电陀螺、光纤陀螺等，精度较高，但是体积和成本也很高。而MEMS陀螺虽然精度低，但是体积和成本很小，可以作为寻北系统的研制方向。

为了实现提高系统精度的同时，压低成本，国内外专家学者都着手研究新型的惯性导航系统，即基于旋转调制技术的惯性导航系统。这种旋转调制技术能够通过规则、周期运动改变惯性组件的信息输出，实现提高系统精度的目标。

发明内容

本发明所要解决的技术问题是设计一种基于MEMS旋转技术的寻北系统及其对应的寻北算法，将旋转调制技术应用与MEMS寻北系统中，结合旋转调制技术本身的优点，改善MEMS器件精度低的弊端，研制小体积、低成本、高精度的旋转的MEMS寻北系统。

本发明为解决上述技术问题采用以下技术方案：

一种基于MEMS旋转技术的寻北系统，包括导航模块和转台模块，

所述导航模块包括：MEMS组件、MEMS数据采集模块、导航计算机；

所述转台模块包括：转动台面、转台微控制器、电机驱动模块、电机、测角模块、测角元件信号调理电路、导电滑环；

MEMS组件设置在转动台面上，MEMS数据采集模块检测MEMS组件的初始方向信息，传输给导航计算机；电机驱动模块与MEMS数据采集模块相连接，接受MEMS数据采集模块的数据反馈；

转动台面的转轴处设置导电滑环，测角模块通过导电滑环采集转动台面的转角数据，转角数据经过测角元件信号调理电路传输至导航计算机；

导航计算机处理接收到的转动台面的转角数据和MEMS数据采集模块采集到的MEMS组件的初始方向信息，经过寻北解算发送控制信号至转台微控制器，转台微控制器将控制信号传输至电机驱动模块，电机驱动模块控制电机转动，进而带动固定在电机上的转动台面转动，将MEMS组件转动至真北方向。

进一步的，所述MEMS组件为三个正交安装的陀螺和三个正交安装的加速度计。

与所述基于MEMS旋转技术的寻北系统所对应的寻北算法具体如下：

（1）建立陀螺的等效输出模型：

${\mathrm{\ω}}^b=C_s^b{\mathrm{\ω}}^s=\left[\begin{array}{ccc}\cos \left(\mathrm{\Ωt}\right)& -\sin \left(\mathrm{\Ωt}\right)& 0\\ \sin \left(\mathrm{\Ωt}\right)& \cos \left(\mathrm{\Ωt}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\cos \left(\mathrm{\Ωt}\right)-{\mathrm{\ω}}_y\sin \left(\mathrm{\Ωt}\right)\\ {\mathrm{\ω}}_x\sin \left(\mathrm{\Ωt}\right)+{\mathrm{\ω}}_y\cos \left(\mathrm{\Ωt}\right)\\ {\mathrm{\ω}}_z\end{array}\right]$

其中，s为旋转坐标系，b为机体坐标系，C表示旋转矩阵，Ω为旋转角速率，t为旋转时间，ω_x、ω_y、ω_z分别为陀螺输出的x、y、z轴分量；

（2）通过将步骤（1）的模型联立如下公式，求得陀螺初始方向与真北方向的夹角θ：

$\mathrm{\θ}={\tan }^{-1}({\mathrm{\ω}}_x/{\mathrm{\ω}}_y)$

其中，k为陀螺标度因数，为当地地理纬度，ω_e为地球坐标系相对惯性坐标系的角速率，ε为相应的陀螺误差。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：选用MEMS陀螺和加速度计组成惯性测量设备，极大地减小了系统的体积和重量，更容易携带。利用旋转技术，调制了MEMS陀螺和加速度计的误差形式，与MEMS系统的常用形式相比，使系统的精度得到了近一个数量级。在构建转动机构时，采用了传输与控制相分离的模式，使系统结构更加紧凑、体积小、重量轻。根据旋转运动的工作模式，构建动态寻北方法，提高寻北精度，缩短寻北时间，具有精度高、携带方便等优点。

附图说明

图1是本发明MEMS旋转系统的方案设计图。

图2是MEMS旋转技术的寻北系统示意图，

其中：1.MEMS组件,2.转动台面，3.导航计算机,4.MEMS数据采集模块,5.测角模块,6.电机驱动模块。

图3是MEMS旋转技术寻北系统的分体模块示意图。

图4是本发明的逻辑流程图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

如图1所示，本发明公开的一种基于MEMS旋转技术的寻北系统主要分为导航模块以及转台模块，导航模块包括MEMS组件、MEMS数据采集模块、导航计算机；转台模块包括转动台面、测角模块、转台微控制器、电机驱动模块。

所述基于MEMS旋转技术的寻北系统，其系统示意图和分体模块示意图分别如图2、图3所示：

导航模块包括：MEMS组件、MEMS数据采集模块、导航计算机，上述设备都安装在与转台模块固定连接的支架上；MEMS组件包括三个正交安装的陀螺和三个正交安装的加速度计；MEMS数据采集模块与MEMS组件连接，检测MEMS组件的信息，传输给导航计算机；

转台模块包括：转动台面、转台微控制器、电机驱动模块、电机、测角模块、测角元件信号调理电路、导电滑环；电机与电机驱动模块相连接，电机上固定转动台面；测角模块通过导电滑环与转动台面的转轴连接，控制与转动台面的转轴连接的转台微控制器及测角元件信号调理电路，调节转动台面的转角。

基于MEMS旋转技术的寻北系统通过MEMS组件与转动台面相连，转动台面的正反、转停等转动带动MEMS组件转动，电机驱动模块与测角模块、MEMS采集模块、导航计算机固连，并固定在外壳上，通过导电滑环与MEMS组件及转动台面进行信号传输。采用信号传输与监控分离的模式，将转动机构的转台微控制器、电机驱动模块和测角元件信号调理电路构造成一个整体，与MEMS组件的后台控制系统相结合，做成一个独立的控制系统；将三维正交的MEMS陀螺和加速度计放置在转动机构的旋转台面，两者通过信号传输接口相联系。

本发明的逻辑流程图如图4所示，其具体实施步骤如下：

一、构建MEMS惯性组件的信号采集及控制系统。

MEMS旋转惯性组件主要实现的功能包含：数据采集、初始对准、误差标定、寻北。所以，包含的软、硬件设备有MEMS惯性组件、导航计算机、输入输出接口、电源、系统解算软件。

具体的工作流程如下：

1.系统的启动和自检测，系统启动后，通过自检测程序检测各个部分的工作是否正常。它是保证系统能进入正常工作状态，并提高系统可靠性的措施。

2.系统的初始化，系统的初始化包括：导航平台的初始对准，在计算机中用对准程序确定姿态矩阵的初始值；惯性仪表的校准，它是提高系统精度的重要保证。

由于MEMS器件的精度较低，通过旋转方式的调制，可以使器件的精度提高一个数量级左右。具体的实现过程如下：

由周期性旋转运动得旋转坐标轴相对机体轴的坐标转换矩阵为

$C_s^b=\left[\begin{array}{ccc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}\cos \left(\mathrm{\Ωt}\right)& -\sin \left(\mathrm{\Ωt}\right)& 0\\ \sin \left(\mathrm{\Ωt}\right)& \cos \left(\mathrm{\Ωt}\right)& 0\\ 0& 0& 1\end{array}\right]$

其中，s为旋转坐标系，b为机体坐标系，θ为旋转角度，C表示旋转矩阵，Ω为旋转角速率，t为旋转时间。

根据MEMS陀螺的零偏矩阵、安装误差矩阵和标度因数误差矩阵公式：

（1）零偏矩阵B_G

B_G＝[B_gxB_gyB_gz]^T

其中，B_gx为x轴向陀螺的零偏量，B_gy为y轴向陀螺的零偏量，B_gz为z轴向陀螺的零偏量。

（2）安装误差矩阵θ_G

${\mathrm{\θ}}_G=\left[\begin{array}{ccc}0& {\mathrm{\θ}}_{\mathrm{gxz}}& {-\mathrm{\θ}}_{\mathrm{gxy}}\\ {-\mathrm{\θ}}_{\mathrm{gyz}}& 0& {\mathrm{\θ}}_{\mathrm{gyx}}\\ {\mathrm{\θ}}_{\mathrm{gzy}}& {-\mathrm{\θ}}_{\mathrm{gzx}}& 0\end{array}\right]$

其中，θ_gxz为z轴向陀螺的安装误差系数在x轴向的分量，θ_gxy为y轴向陀螺的安装误差系数在x轴向的分量，θ_gyz为z轴向陀螺的安装误差系数在y轴向的分量，θ_gyx为x轴向陀螺的安装误差系数在y轴向的分量，θ_gzy为y轴向陀螺的安装误差系数在z轴向的分量，θ_gzx为x轴向陀螺的安装误差系数在z轴向的分量。

（3）标度因数误差矩阵K_G

K_G＝diag[k_gxk_gyk_gz]

其中，k_gx为x轴向陀螺的标度因数误差系数，k_gy为y轴向陀螺的标度因数误差系数，k_gz为z轴向陀螺的标度因数误差系数。

得到MEMS陀螺在旋转坐标系下的输出为：

ω_m＝(I+K_G)(I+θ_G)ω+B_G+δω≈(I+K_G+θ_G)ω+B_G+δω

同理，加速度计在旋转坐标系下的输出为；

f_m＝(I+K_A)(I+θ_A)f+B_A+δf≈(I+K_A+θ_A)f+B_A+δf

其中，I为陀螺和加速度计玲偏常值的单位系数矩阵；ω_m、f_m为陀螺和加速度计在旋转系下的实际输出，K_G和K_A为陀螺和加速度计的标度因数误差系数，θ_G和θ_A为陀螺和加速度计的安装误差系数，B_G和B_A为陀螺和加速度计的零偏，δω和δf为陀螺和加速度计的随机性误差。

根据坐标转换矩阵得陀螺和加速度计在机体系下的等效输出为：

$\mathrm{\Δ}{\mathrm{\ω}}^b=C_s^b\mathrm{\Δ}{\mathrm{\ω}}^s=\left[\begin{array}{ccc}\cos \left(\mathrm{\Ωt}\right)& -\sin \left(\mathrm{\Ωt}\right)& 0\\ \sin \left(\mathrm{\Ωt}\right)& \cos \left(\mathrm{\Ωt}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δ\ω}}_x\\ {\mathrm{\Δ\ω}}_y\\ {\mathrm{\Δ\ω}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\Δ\ω}}_x\cos \left(\mathrm{\Ωt}\right)-{\mathrm{\Δ\ω}}_y\sin \left(\mathrm{\Ωt}\right)\\ {\mathrm{\Δ\ω}}_x\sin \left(\mathrm{\Ωt}\right)+{\mathrm{\Δ\ω}}_y\cos \left(\mathrm{\Ωt}\right)\\ {\mathrm{\Δ\ω}}_z\end{array}\right]$

${\mathrm{\Δf}}^b=C_s^b{\mathrm{\Δf}}^s=\left[\begin{array}{ccc}\cos \left(\mathrm{\Ωt}\right)& -\sin \left(\mathrm{\Ωt}\right)& 0\\ \sin \left(\mathrm{\Ωt}\right)& \cos \left(\mathrm{\Ωt}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δf}}_x\\ {\mathrm{\Δf}}_y\\ {\mathrm{\Δf}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\Δf}}_x\cos \left(\mathrm{\Ωt}\right)-{\mathrm{\Δf}}_y\sin \left(\mathrm{\Ωt}\right)\\ {\mathrm{\Δf}}_x\sin \left(\mathrm{\Ωt}\right)+{\mathrm{\Δf}}_y\cos \left(\mathrm{\Ωt}\right)\\ {\mathrm{\Δf}}_z\end{array}\right]$

由上述两式可知，旋转运动调制了x轴和y轴上惯性器件的误差，改变了误差输出形式，将无规律性的误差调制为具有规则的余弦形式的误差输出，在周期积分解算中，可以部分地被消除。

3.惯性仪表的误差补偿，为了保证系统的精度，必须通过实际模型和经验参数对惯性仪表进行补偿。

4.寻北功能实现，设置MEMS惯性组件绕其中心轴连续恒速旋转，使得陀螺的非随机误差得到周期性调制。

5.信息输出与控制，系统的控制系统包含PC104主板、电源模块、串口扩展板、接口转换模块、转动机构的驱动器等。通过主板和驱动器向转动机构发送指令，控制转动机构周期性的工作，并将MEMS组件的采集信号传输给PC104主板，完成信息的解算和寻北功能。

二、构建基于MEMS的小型转动机构。该小型转动机构包含：转动台面；微控制器单元；电机驱动模块；电动机；测角元件；测角元件信号调理电路；导电滑环。可以使转动机构保持稳定的周期运动。

采用信号传输与监控分离的模式，将转动机构的微控制器单元、电机驱动模块和测角元件信号调理电路构造成一个整体，与MEMS组件的后台控制系统相结合，做成一个独立的控制系统；将三维正交的MEMS陀螺和加速度计放置在转动机构的旋转台面。两者通过信号传输接口相联系，一方面可以节省空间，另一方面方便了转动机构与后台控制系统间的信号传输。

三、构建基于MEMS旋转技术的寻北算法。

陀螺的等效输出模型为：

${\mathrm{\ω}}^b=C_s^b{\mathrm{\ω}}^s=\left[\begin{array}{ccc}\cos \left(\mathrm{\Ωt}\right)& -\sin \left(\mathrm{\Ωt}\right)& 0\\ \sin \left(\mathrm{\Ωt}\right)& \cos \left(\mathrm{\Ωt}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\cos \left(\mathrm{\Ωt}\right)-{\mathrm{\ω}}_y\sin \left(\mathrm{\Ωt}\right)\\ {\mathrm{\ω}}_x\sin \left(\mathrm{\Ωt}\right)+{\mathrm{\ω}}_y\cos \left(\mathrm{\Ωt}\right)\\ {\mathrm{\ω}}_z\end{array}\right]$

通过陀螺敏感轴的初始方向与真北方向的夹角θ计算，即如下公式：

结合上述两式，得到陀螺的动态输出为：

则为：

写成矩阵形式为：

ω'＝AX

由矩阵求解及最小二乘估计，得到计算公式为：

θ＝arctan(X(1,1)/X(2,1))。

其中，ω_ie为地球坐标系相对惯性坐标系的角速率，k为陀螺标度因数，为当地地理纬度，Ωt为t时刻的转过的角度，ε、ε'分别陀螺的实际误差和等效误差，ω′为地球坐标系相对惯性坐标系的角速率矩阵，A为公式(1)的系数矩阵，由坐标转换获得，X为公式(1)中北向夹角求解矩阵。

Claims (1)

1.一种基于MEMS旋转技术的寻北系统，其特征在于包括导航模块和转台模块，

所述导航模块包括：MEMS组件、MEMS数据采集模块、导航计算机；

所述转台模块包括：转动台面、转台微控制器、电机驱动模块、电机、测角模块、测角元件信号调理电路、导电滑环；

MEMS组件设置在转动台面上，MEMS数据采集模块检测MEMS组件的初始方向信息，传输给导航计算机；电机驱动模块与MEMS数据采集模块相连接，接受MEMS数据采集模块的数据反馈；

转动台面的转轴处设置导电滑环，测角模块通过导电滑环采集转动台面的转角数据，转角数据经过测角元件信号调理电路传输至导航计算机；

导航计算机处理接收到的转动台面的转角数据和MEMS数据采集模块采集到的MEMS组件的初始方向信息，经过寻北解算发送控制信号至转台微控制器，转台微控制器将控制信号传输至电机驱动模块，电机驱动模块控制电机转动，进而带动固定在电机上的转动台面转动，将MEMS组件转动至真北方向；

所述MEMS组件为三个正交安装的陀螺和三个正交安装的加速度计，寻北系统的寻北方法具体如下：

(1)建立陀螺的等效输出模型为：

${\mathrm{\ω}}^b=C_s^b{\mathrm{\ω}}^s=\left[\begin{array}{ccc}\cos \left(\mathrm{\Ω}t\right)& -\sin \left(\mathrm{\Ω}t\right)& 0\\ \sin \left(\mathrm{\Ω}t\right)& \cos \left(\mathrm{\Ω}t\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_x\cos \left(\mathrm{\Ω}t\right)-{\mathrm{\ω}}_y\sin \left(\mathrm{\Ω}t\right)\\ {\mathrm{\ω}}_x\sin \left(\mathrm{\Ω}t\right)+{\mathrm{\ω}}_y\cos \left(\mathrm{\Ω}t\right)\\ {\mathrm{\ω}}_z\end{array}\right]$

其中，s为旋转坐标系，b为机体坐标系，C表示旋转矩阵，Ω为旋转角速率，t为旋转时间，ω_x、ω_y、ω_z分别为陀螺输出的x、y、z轴分量；

(2)通过将步骤(1)的模型联立如下公式，

得到陀螺的动态输出为：

将公式(1)写成矩阵形式为：ω′＝AX

由矩阵求解及最小二乘估计，得到陀螺初始方向与真北方向的夹角θ：

θ＝arctan(X(1,1)/X(2,1))

其中，k为陀螺标度因数，为当地地理纬度，ω_e为地球坐标系相对惯性坐标系的角速率，ε、ε′分别为陀螺的实际误差和等效误差，ω′为地球坐标系相对惯性坐标系的角速率矩阵，A为公式(1)的系数矩阵，由坐标转换获得；X为公式(1)中北向夹角求解矩阵。