CN103712622A - 基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置 - Google Patents

Abstract

基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置，属于惯性导航技术领域。该装置由惯性测量单元、旋转平台、控制单元和计算单元构成；惯性测量单元通过集成的陀螺仪和加速度计分别测量角速率和加速度；计算单元通过角速率和加速度分别计算惯性测量单元的倾角数值；旋转平台控制惯性测量单元旋转以改变姿态；通过旋转前后不同姿态下的倾角数值和陀螺漂移的数学关系实现陀螺漂移的估计，并补偿漂移以提高姿态定位精度。本发明的特点在于：无需磁场计或GPS等外部传感器的辅助，仅通过控制惯性测量单元旋转即可估计陀螺仪的漂移，减少长时间内姿态角尤其是航向角的积累误差。

Description

基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置

技术领域

本发明属于惯性导航技术领域，特别涉及一种基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置，适合于移动机器人在室内、强磁干扰等作业环境下利用惯性测量单元进行姿态定位。

背景技术

惯性导航法是一种常见的姿态测量方法。通过对三轴陀螺仪所测量的运动角速度进行积分，能够计算得到姿态信息。但是由于陀螺仪存在漂移问题，长时间累积计算会产生较大误差，需要利用其他传感器获得的信息对陀螺仪的漂移进行估计和误差补偿。

已有陀螺仪漂移估计补偿方法，如专利“用于对位置测量装置中的漂移进行补偿方法”（CN103109159A）采用低通过滤加速度计信号来补偿陀螺仪漂移造成的误差，但对于漂移造成的平行于铅垂线方向上的误差，需要通过磁罗盘来进行补偿。该方法的不足之处在于：不适合在周围有强磁场干扰的环境下应用。如专利“车辆用陀螺仪的角速度误差修正装置、修正方法”（CN103206965A），采用GPS接收器检测的车辆速度和防伪对陀螺仪传感器输出的角速度误差进行修正，该方法的不足之处在于：不适合在封闭空间下GPS信号微弱的环境下应用，例如室内、煤矿等场合。

已有的单轴或多轴旋转惯性器件的装置及方法，如专利“一种双轴旋转光纤捷联惯性导航装置”（CN102980578），这类装置及方法的不足之处在于：不具有漂移估计的功能，当旋转运动无法将陀螺仪的漂移互相抵消时，装置及方法应用受到限制。

专利“Inertial measurement system with self correction”（US8010308）提出了利用加速度计估计陀螺处于铅垂方向时的漂移以修正航向角的方法。该方法需要确保旋转轴绕水平方向以保证修正精度，不适用于姿态测量系统的姿态角处于任意值的情形。

综上所述，目前对陀螺仪漂移进行估计的方法，都需要在磁场计或GPS等外部传感器的辅助下实现。而当作业环境中磁强计和GPS信号不可靠时，这些附加传感器以及相关的陀螺仪漂移估计方法应用受到限制。例如一种面向大型水轮机叶片表面检测的爬壁移动机器人，由于机器人的坑内作业环境为封闭空间并有强磁场干扰，现有的姿态定位方法无法应用。因此，迫切需要不依靠GPS、磁强计等外界辅助方式，仅利用惯性测量元件自身集成的陀螺仪和加速度计来估计陀螺仪的漂移以减小姿态定位误差的方法。

发明内容

本发明的目的是针对已有技术的不足之处，提供一种利用惯性测量单元（InertialMeasurement Unit，简称IMU）旋转实现对陀螺漂移估计并补偿的方法及装置，克服常见的惯性导航方法需要磁场计或GPS等外部传感器辅助的问题。

为实现上述目的，本发明的技术方案如下：

本发明所述的基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于该方法包括以下步骤：

1)通过惯性测量单元所集成的陀螺仪和加速度计测量角速率和加速度；

2)通过陀螺仪测量的角速率计算时间间隔Δt内惯性测量单元的倾角的变化数值：

${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \cos \mathrm{\φ}& 0& -\cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)\mathrm{\Δt}---\left(1\right)$

式中 $\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)$ 是陀螺仪测量得到的角速率，φ为横滚角，θ为俯仰角， ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ 为通过角速率计算的倾角变化值；

3)通过加速度计测量的加速度利用公式（2）计算惯性测量单元的倾角，并获得时间间隔Δt内倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a:$

$\left(\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right)=\left(\begin{array}{c}a\tan 2(-A_{\mathrm{mx}},-A_{\mathrm{mz}})\\ \arcsin \left(\frac{A_{\mathrm{my}}}{\left|\right|A_m\left|\right|}\right)\end{array}\right)---\left(2\right)$

式中是A_mx、A_my、A_mz分别为三轴加速度计测量的加速度，A_m=[A_mx  A_my  A_mz]， ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ 为通过加速度计算的倾角变化值；

4)计算2）获得的倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ 和3）获得的倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ 的差值r：

$r={\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g-{\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a---\left(3\right)$

5)确定横滚角为φ、俯仰角为θ下，倾角变化的差值r和陀螺漂移b的数学关系式：

r=Vb   (4)

式中， $V=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \sin \mathrm{\φ}\sec \mathrm{\θ}& 0& \cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right);$

6)控制惯性测量单元旋转，旋转轴方向的单位向量为u，旋转角度为Δα，旋转后的新姿态下的横滚角为φ′、俯仰角为θ′；

7)重复步骤1)～5)，确定横滚角为φ′、俯仰角为θ′下，倾角变化的差值r’和陀螺漂移b的数学关系式：

r′=V′b   (5)

式中， $V^{\′}=\left(\begin{array}{ccc}\sin {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}& 1& -\cos {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}\\ \sin {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}& 0& \cos {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}\end{array}\right);$

8)联立旋转前后不同姿态下的数学关系式，通过最小二乘法计算陀螺仪的漂移并补偿陀螺仪角速率，输出校正后的姿态测量结果。

本发明所述的基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于所述的欧拉角采用如下定义：

导航坐标系定义为o_nx_ny_nz_n；惯性测量单元坐标系定义为o_bx_by_bz_b；导航坐标系o_nx_ny_nz_n绕z_n轴转(-ψ)角得ox₃y₃z₃，ox₃y₃z₃绕x₃轴转θ角得ox₂y₂z₂，ox₂y₂z₂再绕y₂轴转φ角，则得惯性测量单元坐标系o_bx_by_bz_b；从导航坐标系到惯性测量单元坐标系的转换矩阵R就是惯性测量单元的姿态矩阵：

$R=\left[\begin{array}{ccc}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\sin \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(5\right)$

式中φ为横滚角，θ为俯仰角，ψ为航向角。

本发明所述的基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于所述的新姿态下的横滚角φ′、俯仰角θ′的计算方法为：

1)根据u和Δα确定惯性测量单元所在坐标系的旋转矩阵S为：

$S=I\cos \mathrm{\Δ\α}+\sin \mathrm{\Δ\α}{\left[u\right]}_{\×}+(1-\cos \mathrm{\Δ\α})u\⊗u---\left(6\right)$

式中：[u]_×为u的反对称矩阵，I是单位矩阵，

为点积；

2)根据旋转矩阵S，由旋转前的姿态矩阵通过公式(6)计算旋转后新的姿态矩阵R’：

R′=S^TR   (6)

3)根据姿态矩阵R’得到惯性测量单元在新姿态下的横滚角φ′、俯仰角θ′。

本发明提供的一种基于惯性测量单元旋转的陀螺仪漂移估计补偿装置，其特征在于：该装置包括惯性测量单元、旋转平台、控制单元和计算单元；所述的旋转平台，包括基座、旋转架和旋转电磁设备；所述的惯性测量单元固接于旋转架上，所述的旋转架套固在旋转电磁设备的旋转轴上，所述的旋转电磁设备固接在基座上；所述的控制单元包括驱动电路和控制器；所述的旋转电磁设备通过驱动电路与控制器的信号输出端相连；所述的控制器通过驱动电路控制旋转电磁设备实现惯性测量单元的旋转运动；所述的惯性测量单元和计算单元的信号输入端相连，所述的计算单元可获取惯性测量单元采集的运动信号并做实时计算。

本发明与现有技术相比，具有以下优点和显著进步：

本发明提出的基于惯性测量单元旋转估计并补偿陀螺漂移的方法，能为长时间的姿态测量提供较为精确的信息，尤其是减小航向角的定位误差，不受室内、磁干扰等环境的限制。

附图说明

图1是本发明方法的流程示意图。

图2是本发明提供的基于惯性测量单元旋转的陀螺仪漂移估计补偿装置实施例的结构连接框图。

图3是旋转平台（旋转电磁铁、底座、旋转架）和惯性测量单元的连接示意图。

图4是方法对陀螺仪X轴漂移的估计结果的实验曲线。

图5是补偿漂移前后航向角的定位误差的对比实验曲线。

在图1至图5中：

1－惯性测量单元，         2－旋转平台，         3－控制单元，

4－计算单元，             21－基座，            22－旋转架，

23－旋转电磁设备，        31－驱动电路，        32－控制器，

231－旋转轴。

具体实施方式

下面结合附图及实施例进一步详细说明本发明工作原理、具体结构的内容。

本发明所述的基于旋转惯性测量单元的陀螺漂移估计补偿方法的一种实施例，如图1、图2、图3所示。图1示意了本发明的基于旋转惯性测量单元的陀螺漂移估计补偿方法的流程图。具体包括了以下步骤：本发明所述的基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于该方法包括以下步骤：

1)通过惯性测量单元所集成的三轴陀螺仪和三轴加速度计分别测量运动的角速率和加速度。

2)通过陀螺仪测量的角速率计算时间间隔Δt内惯性测量单元的倾角的变化数值：

${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \cos \mathrm{\φ}& 0& -\cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)\mathrm{\Δt}---\left(1\right)$

式中 $\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)$ 是陀螺仪测量得到的角速率，φ为横滚角，θ为俯仰角， ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ 为通过角速率计算的倾角变化值；

3)通过加速度计测量的加速度利用公式（2）计算惯性测量单元的倾角，并获得时间间隔Δt内倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a:$

$\left(\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right)=\left(\begin{array}{c}a\tan 2(-A_{\mathrm{mx}},-A_{\mathrm{mz}})\\ \arcsin \left(\frac{A_{\mathrm{my}}}{\left|\right|A_m\left|\right|}\right)\end{array}\right)---\left(2\right)$

式中是A_mx、A_my、A_mz分别为三轴加速度计测量的加速度，A_m=[A_mx  A_my  A_mz]， ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ 为通过加速度计算的倾角变化值；；

4)计算2）获得的倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ 和3）获得的倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ 的差值r：

$r={\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g-{\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a---\left(3\right)$

5)确定横滚角为φ、俯仰角为θ下，倾角变化的差值r和陀螺漂移b的数学关系式：

r=Vb   (4)

式中， $V=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \sin \mathrm{\φ}\sec \mathrm{\θ}& 0& \cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right);$

6)控制惯性测量单元绕单轴旋转一个确定角度Δα，旋转轴方向的单位向量为u，旋转后的新姿态下的横滚角为φ′、俯仰角为θ′；

7)重复步骤1)～5)，确定横滚角为φ′、俯仰角为θ′下，倾角变化的差值r’和陀螺漂移b的数学关系式：

r′=V′b   (5)

式中， $V^{\′}=\left(\begin{array}{ccc}\sin {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}& 1& -\cos {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}\\ \sin {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}& 0& \cos {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}\end{array}\right);$

8)联立旋转前后不同姿态下的数学关系式，获得一个以陀螺仪的漂移b为未知量的方程组：

$\hat{r}=\hat{V}b---\left(6\right)$

式中， $\hat{r}={\left[\begin{array}{ccc}{r}_1& ...& r_n\end{array}\right]}^T,\hat{V}={\left[\begin{array}{ccc}{V}_1& ....& V_n\end{array}\right]}^T;$

由于陀螺仪的漂移是一个缓慢变化的随机过程，b短时间内可以认为不变，因此(6)为超定方程组，通过最小二乘法可以计算陀螺仪的漂移：

$b={\left({\hat{V}}^T\hat{V}\right)}^{-1}{\hat{V}}^T\hat{r}---\left(7\right)$

然后通过所估计的漂移对陀螺仪角速率进行补偿：

$\left(\begin{array}{c}{\mathrm{\ω}}_x^{\′}\\ {\mathrm{\ω}}_y^{\′}\\ {\mathrm{\ω}}_z^{\′}\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)-b---\left(8\right)$

式中 $\left(\begin{array}{c}{\mathrm{\ω}}_x^{\′}\\ {\mathrm{\ω}}_y^{\′}\\ {\mathrm{\ω}}_z^{\′}\end{array}\right)$ 是补偿后的角速率；

利用补偿后的角速率，根据公式(9)计算校正后的欧拉角：

$\left(\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right)=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \cos \mathrm{\φ}& 0& \sin \mathrm{\φ}\\ \sin \mathrm{\φ}\sec \mathrm{\θ}& 0& -\cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_x^{\′}\\ {\mathrm{\ω}}_y^{\′}\\ {\mathrm{\ω}}_z^{\′}\end{array}\right)---\left(9\right)$

输出校正后的姿态测量结果。

本实施例中，所述的欧拉角采用如下定义：

导航坐标系定义为o_nx_ny_nz_n；惯性测量单元坐标系定义为o_bx_by_bz_b；导航坐标系o_nx_ny_nz_n绕z_n轴转(-ψ)角得ox₃y₃z₃，ox₃y₃z₃绕x₃轴转θ角得ox₂y₂z₂，ox₂y₂z₂再绕y₂轴转φ角，则得惯性测量单元坐标系o_bx_by_bz_b；从导航坐标系到惯性测量单元坐标系的转换矩阵R就是惯性测量单元的姿态矩阵：

$R=\left[\begin{array}{ccc}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\sin \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(5\right)$

式中φ为横滚角，θ为俯仰角，ψ为航向角。

本实施例中，所述的新姿态下的横滚角φ′、俯仰角θ′的计算方法为：

1)根据u和Δα确定惯性测量单元所在坐标系的旋转矩阵S为：

$S=I\cos \mathrm{\Δ\α}+\sin \mathrm{\Δ\α}{\left[u\right]}_{\×}+(1-\cos \mathrm{\Δ\α})u\⊗u---\left(6\right)$

式中：[u]_×为u的反对称矩阵，I是单位矩阵，

为点积；

2)根据旋转矩阵S，由旋转前的姿态矩阵通过公式(6)计算旋转后新的姿态矩阵R’：

R′=S^TR   (6)

3)根据姿态矩阵R’得到惯性测量单元在新姿态下的横滚角φ′、俯仰角θ′。

基于惯性测量单元旋转的陀螺仪漂移估计补偿装置包括惯性测量单元1、旋转平台2、控制单元3和计算单元4；所述的旋转平台，包括基座21、旋转架22和旋转电磁设备23；所述的惯性测量单元固接于旋转架上，所述的旋转架套固在旋转电磁设备的旋转轴上，所述的旋转电磁设备固接在基座上；所述的控制单元包括驱动电路31和控制器32；所述的旋转电磁设备通过驱动电路与控制器的信号输出端相连；所述的控制器通过驱动电路控制旋转电磁设备实现惯性测量单元的旋转运动；所述的惯性测量单元和计算单元的信号输入端相连，所述的计算单元可获取惯性测量单元采集的运动信号并做实时计算。

本实施例所述的控制器和计算单元采用带有相关接口电路的计算机或微控制器。

本实施例的各部件之间的连接方式如图2所示。图3是图2所示实施例的惯性测量单元、旋转电磁铁、旋转架和基座的连接方式示意图。

本实施例中，所述的惯性测量单元采用Xsens公司的MEMS惯性测量单元MTI-100，应用中也可以采用其他集成了三轴陀螺仪和三轴加速度计的惯性测量单元产品。

本实施例中，所述的旋转设备采用旋转电磁铁，控制器采用DSP，通过方波信号驱动电磁铁，实现往复旋转摆动，摆动角度为90°。

由于本发明无需磁场计或GPS等外部传感器的辅助，仅利用惯性测量单元所集成的陀螺仪和加速度计来估计陀螺仪的漂移并进行补偿，实现提高姿态定位精度尤其是减小航向角的定位误差的功能，适用于移动机器人在室内、强磁干扰等作业环境下利用惯性测量单元进行姿态定位。

上述实施例仅用于说明本发明，其中旋转平台的结构、旋转设备的选择以及旋转方式的实现都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变化和改进，均不应排除在本发明的保护范围之外。

Claims (4)

1.一种基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于该方法包括以下步骤：

1)通过惯性测量单元所集成的陀螺仪和加速度计测量角速率和加速度；

通过陀螺仪测量的角速率计算时间间隔Δt内惯性测量单元的倾角的变化数值 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g:$

${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \cos \mathrm{\φ}& 0& -\cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)\mathrm{\Δt}---\left(1\right)$

式中 $\left(\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right)$ 是陀螺仪测量得到的角速率，φ为横滚角，θ为俯仰角， ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ 为通过角速率计算的倾角变化值；

2)通过加速度计测量的加速度利用公式（2）计算惯性测量单元的倾角，并获得时间间隔Δt内倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a:$

$\left(\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right)=\left(\begin{array}{c}a\tan 2(-A_{\mathrm{mx}},-A_{\mathrm{mz}})\\ \arcsin \left(\frac{A_{\mathrm{my}}}{\left|\right|A_m\left|\right|}\right)\end{array}\right)---\left(2\right)$

3)式中是A_mx、A_my、A_mz分别为三轴加速度计测量的加速度，A_m=[A_mx  A_my  A_mz]， ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ 为通过加速度计算的倾角变化值；

4)计算2）获得的倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g$ 和3）获得的倾角变化 ${\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a$ 的差值r：

$r={\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_g-{\left(\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\Δ\θ}\end{array}\right)}_a---\left(3\right)$

5)确定横滚角为φ、俯仰角为θ时倾角变化的差值r和陀螺漂移b的数学关系式：

r=Vb   (4)

式中， $V=\left(\begin{array}{ccc}\sin \mathrm{\φ}\tan \mathrm{\θ}& 1& -\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \sin \mathrm{\φ}\sec \mathrm{\θ}& 0& \cos \mathrm{\φ}\sec \mathrm{\θ}\end{array}\right);$

6)控制惯性测量单元旋转，旋转轴方向的单位向量为u，旋转角度为Δα，旋转后的新姿态下的横滚角为φ′、俯仰角为θ′；

7)重复步骤1)～5)，确定横滚角为φ′、俯仰角为θ′时倾角变化的差值r’和陀螺漂移b的数学关系式：

r′=V′b   (5)

式中， $V^{\′}=\left(\begin{array}{ccc}\sin {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}& 1& -\cos {\mathrm{\φ}}^{\′}\tan {\mathrm{\θ}}^{\′}\\ \sin {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}& 0& \cos {\mathrm{\φ}}^{\′}\sec {\mathrm{\θ}}^{\′}\end{array}\right);$

8)联立旋转前后不同姿态下的数学关系式，通过最小二乘法计算陀螺仪的漂移并补偿陀螺仪角速率，输出校正后的姿态测量结果。

2.如权利要求1所述的一种基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于：所述的欧拉角采用如下定义：

导航坐标系定义为o_nx_ny_nz_n；惯性测量单元坐标系定义为o_bx_by_bz_b；导航坐标系o_nx_ny_nz_n绕z_n轴转(-ψ)角得ox₃y₃z₃，ox₃y₃z₃绕x₃轴转θ角得ox₂y₂z₂，ox₂y₂z₂再绕y₂轴转φ角，则得惯性测量单元坐标系o_bx_by_bz_b；从导航坐标系到惯性测量单元坐标系的转换矩阵R就是惯性测量单元的姿态矩阵：

$R=\left[\begin{array}{ccc}\cos \mathrm{\φ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\φ}\sin \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(5\right)$

式中φ为横滚角，θ为俯仰角，ψ为航向角。

3.如权利要求1所述的一种基于惯性测量单元旋转的陀螺漂移估计补偿方法，其特征在于：所述的新姿态下的横滚角φ′、俯仰角θ′的计算方法为：

1)根据u和Δα计算惯性测量单元所在坐标系的旋转矩阵S：

$S=I\cos \mathrm{\Δ\α}+\sin \mathrm{\Δ\α}{\left[u\right]}_{\×}+(1-\cos \mathrm{\Δ\α})u\⊗u---\left(6\right)$

式中：[u]_×为u的反对称矩阵，I是单位矩阵，

为点积；

2)根据旋转矩阵S，由旋转前的姿态矩阵R通过公式(6)计算旋转后新的姿态矩阵R’：

R′=S^TR   (6)

3)根据姿态矩阵R’得到惯性测量单元在新姿态下的横滚角φ′、俯仰角θ′。

4.实现如权利要求1～3所述方法的一种基于惯性测量单元旋转的陀螺仪漂移估计补偿装置，其特征在于：该装置包括惯性测量单元（1）、旋转平台（2）、控制单元（3）和计算单元（4）；所述的旋转平台包括基座（21）、旋转架（22）和旋转电磁设备（23）；所述的惯性测量单元固接于旋转架上，所述的旋转架套固在旋转电磁设备的旋转轴上，所述的旋转电磁设备固接在基座（1）上；所述的控制单元（4）包括驱动电路（31）和控制器（32）；所述的旋转电磁设备通过驱动电路与控制器的信号输出端相连；所述的控制器通过驱动电路控制旋转电磁设备实现惯性测量单元的旋转运动；所述惯性测量单元的输出端通过信号线和计算单元的输入端相连，所述的计算单元获取惯性测量单元采集的运动信号并做实时计算。

Images