CN101915580B - 一种基于微惯性和地磁技术的自适应三维姿态定位方法 - Google Patents

Abstract

本发明公开一种基于微惯性和地磁技术的自适应三维姿态定位方法，包括：1)利用微惯性测量装置中的传感器感应载体的运动姿态；2)设置装置的初始姿态及全局坐标系下的加速度和地磁场信息；3)对装置的姿态值进行求解；4)同时利用三轴微陀螺传感器数据对装置姿态值进行预测；5)对三轴微加速度传感器和三轴磁场传感器数据进行置信判定，检测周围环境干扰，设置自适应参数；6)利用步骤5处理后的三轴微加速度传感器数据和三轴磁场传感器数据获得装置姿态误差值；7)对步骤4得到的姿态预测值以及步骤6得到的更正信息以及步骤5得到的自适应参数进行融合，获得装置姿态值；8)输出姿态信息。

Description

一种基于微惯性和地磁技术的自适应三维姿态定位方法

技术领域

本发明属于三维姿态控制技术领域，涉及一种基于微惯性和地磁技术的高效自适应三维姿态定位方法。

背景技术

随着微电子机械系统(MEMS)的发展，特别是微惯性技术的发展，载体的位姿获取已经从原来的平台惯导发展到捷联惯导和组合导航技术，其系统的体积、重量和成本都大大降低，因此采用惯性/地磁技术进行姿态的跟踪检测有着广泛的前景。目前，国内外已经设计出了相当多的微惯性姿态测量装置用于姿态测量跟踪，本申请人在申请号为“200810114391.4”的中国专利“基于ZigBee无线单片机的微惯性测量装置”中提供了一种可用于运动载体姿态测量的装置，该专利在本申请中引入作为参考。在该申请中，采用六轴微惯性传感器(集成了三轴微陀螺传感器和三轴微加速度传感器)和三轴磁场传感器来测量运动载体的姿态，通过基于ZigBee无线单片机对所测得的信号进行姿态解算，并将解算得到的姿态信息以无线方式传送给其他系统或者上位机。

惯性跟踪技术的基本原理在于在已知初始位置和初始姿态等初始信息的基础上，依据惯性原理，对三轴微陀螺传感器获取的角速度信息进行积分获得姿态；对三轴维加速度传感器获取的加速度信息进行两次积分获取位置。但是由于存在积分累计误差效应，需要结合其他传感器对其误差值进行实时更正，从而保证其数据的准确性。基于三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器进行姿态定位的方法，其姿态值往往用四元数进行描述，四元数的引入不仅简化了计算，而且避免了欧拉角系统的万向节死锁问题。三轴微陀螺传感器的一次积分获得预测的姿态值，三轴微加速度传感器和三轴磁场传感器对预测的姿态值进行实时更正。目前的主流姿态跟踪算法为卡尔曼滤波：通过三轴微陀螺传感器的预测以及三轴微加速度传感器和三轴磁场传感器的更正来获取姿态信息。但是该算法复杂，卡尔曼滤波涉及到的观测噪声以及系统噪声对结果精度影响较大，而周围的噪声往往可能不是假定的白噪声，且对噪声参数的获取往往较为困难。

Bachmann提出了一种基于四元数的“互补“滤波的方法(见Bachmann，E.R.，Duman，I.，Usta，U.Y.，McGhee，R.B.，Yun，X.P.，Zyda，M.J.，“Orientation Tracking for Humans and Robots Using InertialSensors，″Proc.of 1999 Symposium on Computational Intelligence inRobotics & Automation，Monterey，CA，November 1999)。该方法通过使用高斯-牛顿迭代方法，利用三轴微加速度传感器和三轴磁场传感器的数据获得当前姿态的最佳误差值，然后与三轴微陀螺传感器的数据融合，从而获得姿态值。与卡尔曼滤波相比，该算法大大降低了计算复杂性，并能取得较好的姿态准确性。然而该算法没有考虑到噪声干扰等问题。在此基础上，Jung Keun Lee提出了利用虚拟旋转的概念来避免高斯-牛顿算法中的矩阵逆运算(见Jung Keun Lee；Park，E.J，”A FastQuaternion-Based Orientation Optimizer via Virtual Rotation forHuman Motion Tracking”，Biomedical Engineering，IEEE Transactions onVolume 56，Issue 5，May 2009 Page(s)：1574-1582)，并提出了一种通过检测三轴微加速度传感器和三轴磁场传感器噪声是否超出阈值来确定某时刻获取的加速传感器和磁场传感器数据是否有效。但是该方法对于当加速度传感器和磁场传感器数据有效时的存在的噪声没有进行考虑。国内方面也有一些关于利用三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器来获取姿态的相关专利，本申请人在申请号为“200810224898.5”的中国专利“考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法”中提供一种对加速度进行补偿的无迹卡尔曼滤波惯性位姿跟踪方法，该专利在本申请中引入作为参考。

发明内容

为了解决现有技术的问题，本发明的目的是充分考虑计算复杂度以及运动载体瞬时加速度或周围环境电磁场的干扰对三轴微加速度传感器和三轴磁场传感器的影响，提出一种考虑微惯性组合测量装置瞬时加速度和周围环境电磁场干扰的基于降阶高斯-牛顿方法的快速自适应三维姿态的定位方法。

为了达到上述的发明目的，本发明提出了一种基于微惯性和地磁技术的自适应三维姿态定位方法，该方法包括以下步骤：

步骤S1：利用集成了三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态：三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；

步骤S2：设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据：在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i＝0时的四元数形式姿态值q0＝[1 0 0 0]^T、三轴微加速度传感器矢量ⁿY_A＝[a_0x a_0y a_0z]^T和三轴磁场传感器矢量ⁿY_M＝[m_0x m_0y m_0z]^T，对三轴微加速度传感器矢量ⁿY_A和三轴磁场传感器矢量ⁿY_M进行单位化，得到单位三轴微加速度传感器矢量ⁿS_A和单位三轴磁场传感器矢量ⁿS_M，所述单位化公式为：

ⁿS_A＝ⁿY_A/||ⁿY_A||和ⁿS_M＝ⁿY_M/||ⁿY_M||，

式中：a_0x、a_0y、a_0z表示初始时刻即i＝0时的全局坐标系下的三轴微加速度传感器的加速度数据，m_0x、m_0y、m_0z表示初始时刻即i＝0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标n表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；

步骤S3：对微惯性组合测量装置的姿态值进行求解：利用当前时刻即i＝t时的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值q_t以及步骤2获得的初始姿态值q₀、三轴微加速度传感器的加速度数据ⁿY_A、ⁿS_A和三轴磁场传感器的地球磁场数据ⁿY_M、ⁿS_M，来获取微惯性组合测量装置下一时刻即i＝t₊Δt时的姿态值q_t+Δt；

步骤S4：利用三轴微陀螺传感器数据对i＝t₊Δt时刻微惯性组合测量装置的姿态值进行预测：将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数Q_G＝[0 ω_tx ω_ty ω_tx]^T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的磁场强度数据分别为^bY_A＝[a_tx a_ty a_tz]^T和^bY_M＝[m_tx m_ty m_tz]^T，其中G、b为标记号，ω_tx、ω_ty、ω_tz表示的是当前时刻t的三轴陀螺传感器的角速度数据，a_tx、a_ty、a_tz表示的是当前时刻t的三轴微加速度传感器的加速度数据，m_tx、m_ty、m_tz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；

利用微惯性组合测量装置当前时刻t的姿态值q_t和当前时刻t的三轴微陀螺传感器的旋转角速度四元数Q_G，对i＝t₊Δt时刻的微惯性组合测量装置的姿态值

进行预测，由于两个时刻之间的间隔Δt较小，假设认为t和t₊Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测

方程：

${\hat{q}}_{t+\mathrm{\Δt}}=q_t+{\stackrel{\·}{q}}_t\mathrm{\Δt}---\left(1\right)$

式中 ${\stackrel{\·}{q}}_t=\frac{1}{2}{q}_t\⊗Q_G---\left(2\right)$

对预测后的四元数进行单位化：

${\hat{q}}_{t+\mathrm{\Δt}}^{\′}={\hat{q}}_{t+\mathrm{\Δt}}/\left|\right|{\hat{q}}_{t+\mathrm{\Δt}}\left|\right|---\left(3\right)$

式中，q_t为当前时刻t的微惯性组合测量装置的姿态值，

为下一时刻预测姿态值，Δt为采样时间间隔，

为四元数形式的当前时刻t全局姿态下的旋转角速度；

步骤S5：对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子，该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小，从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置如下：

$S_M^b=\left\{\begin{array}{ccc}Y_M^b/\left|\right|Y_M^b\left|\right|& \mathrm{if}& (k_M=1-|\left|\right|Y_M^b\left|\right|-q_t^{-1}\&CircleTimes;Y_M^n\&CircleTimes;q_t\left|\right||/||q_t^{-1}\&CircleTimes;Y_M^n\&CircleTimes;q_t||<{\mathrm{\&epsiv;}}_M)\\ q_t^{-1}\&CircleTimes;S_M^n\&CircleTimes;q_t& \mathrm{else}& k_{M=1}\end{array}\right.---\left(4\right)$

$S_A^b=\left\{\begin{array}{ccc}Y_A^b/\left|\right|Y_A^b\left|\right|& \mathrm{if}& (k_A=1-|\left|\right|Y_A^b\left|\right|-q_t^{-1}\&CircleTimes;Y_A^n\&CircleTimes;q_t\left|\right||/||q_t^{-1}\&CircleTimes;Y_A^n\&CircleTimes;q_t||<{\mathrm{\&epsiv;}}_A)\\ q_t^{-1}\&CircleTimes;S_A^n\&CircleTimes;q_t& \mathrm{else}& k_{A=1}\end{array}\right.$

式中：^bS_M为调整后的三轴磁场传感器矢量、^bS_A为调整后的三轴微加速度传感器矢量；k_A为三轴微加速度传感器自适应因子，k_M为三轴磁场传感器自适应因子；ε_A为三轴微加速度传感器噪声上界，ε_M为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界ε_A或三轴磁场传感器噪声上界ε_M，ε_A、ε_M的取值范围是(0-1)，周围环境噪声越大，该噪声上界ε_A、ε_M值应取值越小，具体取值应依据在现场环境中的实验决定；

步骤S6：利用步骤S5调整后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值

该姿态误差值通过高斯-牛顿方法获得：

${\stackrel{\&RightArrow;}{q}}_e=-\frac{1}{2}R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right){\mathrm{\&Xi;}}^{-1}R_n^b{\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)}^T({\left[R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)S_A^n\right]}^{\&times;}S_A^b+{\left[R_n^b{\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)}^n{S}_M\right]}^{\&times;}S_M^b)---\left(16\right)$

式中：

$\mathrm{\&Xi;}=2I-(S_A^{n}S_A^T{}_n+S_M^{n}S_M^T{}_n)---\left(17\right)$

$R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)=2\left[\begin{array}{ccc}0.5-q_2^2-q_3^2& q_1{q}_2+q_0{q}_3& q_1{q}_3-q_0{q}_2\\ q_1{q}_2-q_0{q}_3& 0.5-q_1^2-q_3^2& q_2{q}_3+q_0{q}_1\\ q_1{q}_3+q_0{q}_2& q_2{q}_3-q_0{q}_1& 0.5-q_1^2-q_2^2\end{array}\right]---\left(18\right)$

${\left[\mathrm{\&lambda;}\right]}^{\&times;}=\left[\begin{array}{ccc}0& -{\mathrm{\&lambda;}}_z& {\mathrm{\&lambda;}}_y\\ {\mathrm{\&lambda;}}_z& 0& -{\mathrm{\&lambda;}}_x\\ -{\mathrm{\&lambda;}}_y& {\mathrm{\&lambda;}}_x& 0\end{array}\right]---\left(19\right)$

${\left[\mathrm{\&mu;}\right]}^{\&times;}=\left[\begin{array}{ccc}0& -{\mathrm{\&mu;}}_z& {\mathrm{\&mu;}}_y\\ {\mathrm{\&mu;}}_z& 0& -{\mathrm{\&mu;}}_x\\ -{\mathrm{\&mu;}}_y& {\mathrm{\&mu;}}_x& 0\end{array}\right]---\left(20\right)$

式中：λ表示为

μ表示为λ_x、λ_y、λ_z为向量λ的三个分量，μ_x、μ_y、μ_z为向量μ的三个分量，q₀、q₁、q₂、q₃为四元数的四个分量，

为四元数

转换成的旋转矩阵；矩阵Ξ只与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据有关，所以在姿态方法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；

步骤S7：基于步骤S4得到的姿态预测信息、步骤S6得到微惯性组合测量装置的姿态误差值和步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值：

$q_{t+\mathrm{\&Delta;t}}={\hat{q}}_{t+\mathrm{\&Delta;t}}+{\hat{q}}_{t+\mathrm{\&Delta;t}}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]k_A{k}_M\mathrm{\&Delta;t}---\left(21\right)$

然后再对其单位化q_t+Δt＝q_t+Δt/||q_t+Δt||；其中

表示的是四元数的乘法运算；

步骤S8：输出姿态信息：根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义的、自适应三维姿态的俯仰角α、横滚角β和航向角γ如下：

$\left\{\begin{array}{cc}\mathrm{\&alpha;}=\mathrm{arctg}\left(\frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)& \mathrm{\&alpha;}\&Element;(-\mathrm{180,180}]\\ \mathrm{\&beta;}=\arcsin (-2(q_1{q}_3+q_0{q}_2))& \mathrm{\&beta;}\&Element;(-\mathrm{90,90}]\\ \mathrm{\&gamma;}=\mathrm{arctg}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2+q_1^2-q_2^2-q_3^2}\right)& \mathrm{\&gamma;}\&Element;(-\mathrm{180,180}]\end{array}\right.---\left(22\right).$

其中：利用三轴微陀螺传感器的预测以及三轴微加速度传感器和三轴磁场传感器数据的更正获取姿态信息。

其中：微惯性组合测量装置的初始姿态标定需要进行一次矩阵逆求解，在预测过程和更正过程所有的姿态信息均以四元数的形式进行表示和运算。

其中：考虑周围环境中的电磁场对地球磁场的干扰和瞬时加速度的影响，如果干扰变化超过阈值，则三轴微加速度传感器和三轴磁场传感器的数据进行舍弃；如果干扰变化没超过阈值，则按照干扰的大小设置三轴微加速度传感器和三轴磁场传感器数据的自适应因子。

本发明的有益效果：采用本发明方法可以得到如下优点：

一方面，本发明与传统的卡尔曼滤波算法相比，大大降低了算法的复杂性，而算法的准确性与其他算法相比却没有明显的降低，甚至有些情况下与传统算法相比计算精度有了提高。从图4、图5、图6和表7中能够看到本发明方法与主流的姿态解算算法的输出没有差别，表1中列出了与主流的姿态解算算法的比较数据。

另一方面，本发明方法中的自适应性使得在周围环境存在电磁场干扰以及瞬时加速度的情况下，降低了干扰对装置姿态值准确性的影响。

如图4示出为静止、周围环境的无磁干扰的情况下，本发明方法与主流姿态传感器MTi结果的比较。其中本发明方法的姿态输出为实线表示，MTi传感器的输出为虚线表示。图4中含有的(a)图为MTi传感器与四阶龙格库塔方法的航向角比较；图4中含有的(b-d)图为MTi传感器和本算法的横滚角、航向角和俯仰角的比较；

如图5示出为静止，周围环境存在磁干扰的情况下，本发明方法与主流姿态传感器MTi结果的比较。其中本发明方法的姿态输出为实线表示，MTi传感器的输出为虚线表示。图5中含有的(a-c)图三个坐标轴的测得的地磁场变化；图5中含有的(d-e)图为MTi传感器和本算法的横滚角、俯仰角和航向角的比较；在图5的情况下，当装置静止时，本发明方法和传统方法都可以准确得出当前姿态值，但是对于周围环境突然的磁场干扰，传统方法已经无法准确计算姿态值，而本发明方法却不受到相应的干扰。

如图6示出为周围环境比较理想，瞬时加速情况下，本发明方法与主流姿态传感器MTi结果的比较。其中本发明方法的姿态输出为实线表示，MTi传感器的输出为虚线表示。图6中含有的(a-c)图为三个坐标轴的测得的加速度的变化；图6中含有的(d-e)图为MTi传感器和本发明方法的横滚角、俯仰角和航向角的比较；在图6情况下，对于瞬时的加速度变化，可以看到本发明方法提出的自适应因子能够缓和姿态值的突然的较大波动，从而提高了姿态值的准确性。

如下示出表1为在图4和图6测试环境下，MTi和本发明方法的姿态准确性的比较，RMSE表示为均方误差大小。

	MTi	本发明	差异
				RMSEroll	0.40/0.35	0.38/0.20	-0.02/-0.15
RMSEpitch	0.31/4.81	0.14/4.64	-0.17/-0.17
				RMSEyaw	0.18/0.64	0.44/0.88	0.26/0.24

附图说明

图1为本发明所适用的惯性/地磁姿态跟踪装置的结构图；

图2为本发明的一种基于微惯性/地磁技术的快速自适应算法的结构框图；

图3进行高斯-牛顿迭代次数为1次，2次，3次的误差更正值的模值变化；

图4为静止，无周围环境的磁干扰的情况下，本发明与主流姿态传感器MTi结果的比较(内部为卡尔曼滤波算法)。

图5为静止，存在周围磁干扰的情况下，本发明与主流姿态传感器MTi结果的比较(内部为卡尔曼滤波算法)。

图6为周围环境比较理想时，突然加速情况下，本发明与主流姿态传感器MTi结果的比较(内部为卡尔曼滤波算法)。

图7为三维姿态定位算法的软件流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

如图1示出利用本发明三维微惯性和地磁姿态跟踪装置，在三轴坐标系的X轴、Y轴和Z轴上分别设有由微加速度传感器、微陀螺传感器和磁场传感器组成的一组传感器，图1中“□”表示微加速度传感器、“○”表示微陀螺传感器、

表示磁场传感器。该惯性和地磁姿态跟踪装置以正交方式集成了三个微加速度传感器、三个微陀螺传感器和三个磁场传感器，三组传感器所在的正交轴构成了装置X轴、Y轴和Z轴的三轴坐标系。

如图2所示，本发明一种基于微惯性和地磁技术的自适应三维姿态的定位方法，在此仅对本发明方法相关步骤中的具体实现细节进行如下说明：

本发明所描述的算法的采样周期Δt依据不同的运行环境而不同。

步骤S1：利用集成的三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态：三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；

步骤S2：设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据：在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i＝0时的四元数形式姿态值q₀＝[1 0 0 0]^T、三轴微加速度传感器矢量ⁿY_A＝[a_0x a_0y a_0z]^T和三轴磁场传感器矢量ⁿY_M＝[m_0x m_0y m_0z]^T，对三轴微加速度传感器矢量ⁿY_A和三轴磁场传感器矢量ⁿY_M进行单位化，得到单位三轴微加速度传感器矢量ⁿS_A和单位三轴磁场传感器矢量ⁿS_M，所述单位化公式为：

ⁿS_A＝ⁿY_A/||ⁿY_A||和ⁿS_M＝ⁿY_M/||ⁿY_M||，

其中：a_0x、a_0y、a_0z表示初始时刻即i＝0时的全局坐标系下的三轴微加速度传感器的加速度数据，m_0x、m_0y、m_0z表示初始时刻即i＝0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标n表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；

步骤S3：对微惯性组合测量装置的姿态值进行求解：利用当前时刻i＝t的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值q_t以及步骤2设定的初始姿态值q₀、三轴微加速度传感器的加速度数据ⁿY_A、ⁿS_A和三轴磁场传感器的地球磁场数据ⁿY_M、ⁿS_M，来获取微惯性组合测量装置下一时刻即i＝t₊Δt时的姿态值q_t+Δt；

步骤S4：利用三轴微陀螺传感器数据对i＝t₊Δt时刻微惯性组合测量装置的姿态值进行预测：将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数Q_G＝[0 ω_tx ω_ty ω_tz]^T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据分别为^bY_A＝[a_tx a_ty a_tz]^T和^bY_M＝[m_tx m_ty m_tz]^T，其中G、b为标记号，ω_tx、ω_ty、ω_tz表示的是当前时刻t的三轴陀螺传感器的角速度数据，a_tx、a_ty、a_tz表示的是当前时刻t的三轴微加速度传感器的加速度数据，m_tx、m_ty、m_tz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；

利用微惯性组合测量装置当前时刻t的姿态值q_t和当前时刻t的三轴微陀螺传感器的旋转角速度四元数Q_G，对i＝t+Δt时刻的微惯性组合测量装置的姿态值进行预测，由于两个时刻之间的间隔Δt较小，假设认为t和t₊Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测

方程：

${\hat{q}}_{t+\mathrm{\&Delta;t}}=q_t+{\stackrel{\&CenterDot;}{q}}_t\mathrm{\&Delta;t}---\left(1\right)$

其中 ${\stackrel{\&CenterDot;}{q}}_t=\frac{1}{2}{q}_t\&CircleTimes;Q_G---\left(2\right)$

对预测后的四元数进行单位化：

${\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}={\hat{q}}_{t+\mathrm{\&Delta;t}}/\left|\right|{\hat{q}}_{t+\mathrm{\&Delta;t}}\left|\right|---\left(3\right)$

其中，q_t为当前时刻t的微惯性组合测量装置的姿态值，

为下一时刻预测姿态值，Δt为采样时间间隔，

为四元数形式的当前时刻t全局姿态下的旋转角速度；

步骤S5：对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子(该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小)从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置；置信判断标准如下：假设目前时刻的姿态数据准确，将初始标定时刻获得的三轴微加速度传感器和三轴磁场传感器数据结合当前的姿态数据预测出当前测得的三轴微加速度传感器和三轴磁场传感器数据，通过将预测的三轴微加速度传感器和三轴磁场传感器数据与当前时刻的三轴微加速度传感器和三轴磁场传感器数据进行模值比较：如果不存在任何干扰和噪声，二者应该大小一致；如果二者之间的差值超过了设定的阈值，则认为当前测得的三轴微加速度传感器或是三轴磁场传感器数据不准确，应该予以舍弃，并以预测的三轴微加速度传感器和三轴磁场传感器的数据为准；如果二者之间的差值在设定的阈值内，则表明当前时刻测得的三轴微加速度传感器和三轴磁场传感器不存在干扰，只存在噪声，并且差值的大小表明了噪声的强弱，对差值信息进行处理，从而获得三轴微加速度传感器和三轴磁场传感器的自适应因子，具体的实现公式如下：

$S_M^b=\left\{\begin{array}{ccc}Y_M^b/\left|\right|Y_M^b\left|\right|& \mathrm{if}& (k_M=1-|\left|\right|Y_M^b\left|\right|-q_t^{-1}\&CircleTimes;Y_M^n\&CircleTimes;q_t\left|\right||/||q_t^{-1}\&CircleTimes;Y_M^n\&CircleTimes;q_t||<{\mathrm{\&epsiv;}}_M)\\ q_t^{-1}\&CircleTimes;S_M^n\&CircleTimes;q_t& \mathrm{else}& k_{M=1}\end{array}\right.---\left(4\right)$

$S_A^b=\left\{\begin{array}{ccc}Y_A^b/\left|\right|Y_A^b\left|\right|& \mathrm{if}& (k_A=1-|\left|\right|Y_A^b\left|\right|-q_t^{-1}\&CircleTimes;Y_A^n\&CircleTimes;q_t\left|\right||/||q_t^{-1}\&CircleTimes;Y_A^n\&CircleTimes;q_t||<{\mathrm{\&epsiv;}}_A)\\ q_t^{-1}\&CircleTimes;S_A^n\&CircleTimes;q_t& \mathrm{else}& k_{A=1}\end{array}\right.$

其中：^bS_M为调整后的三轴磁场传感器矢量、^bS_A为调整后的三轴微加速度传感器矢量；k_A为三轴微加速度传感器自适应因子，k_M为三轴磁场传感器自适应因子；ε_A为三轴微加速度传感器噪声上界，ε_M为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界ε_A或三轴磁场传感器噪声上界ε_M，ε_A、ε_M的取值范围是(0-1)，周围环境噪声越大，该噪声上界ε_A、ε_M值应取值越小，实施时三轴微加速度传感器的噪声上界ε_A选取可为0.8；三轴磁场传感器噪声上界ε_M选取可为0.9。噪声上界ε_A、ε_M的数值选择，不仅仅限于实施例的数据，噪声上界ε_A、ε_M的具体取值应依据在现场环境中的实验决定；

步骤S6：利用步骤S5处理后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值

该误差值基于高斯-牛顿迭代法获得。利用高斯牛顿方法来获得误差更正值的原理如下：给定预测的单位化的姿态值

初始标定的三轴微加速度传感器和三轴磁场传感器的矢量ⁿS_A和ⁿS_M，以及对干扰判定后的三轴微加速度传感器和三轴磁场传感器的矢量^bS_A，^bS_M，可以使用高斯-牛顿迭代来寻求当前最优的四元数，从而满足如下公式：

其中α，β为三轴微加速度传感器和三轴磁场传感器的在高斯牛顿方法中的姿态更正的影响权重，一般设置为1。高斯-牛顿方法可以找到使得误差值最小的最优姿态四元数

一般情况下，预测值与最优姿态四元数之间只存在一个较小的旋转误差q_e：

$\hat{q}={\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;q_e---\left(6\right)$

$q_e=\frac{\cos \left(\frac{{\mathrm{\&theta;}}_e}{2}\right)+w\sin \left(\frac{{\mathrm{\&theta;}}_e}{2}\right)}{\cos \left(\frac{{\mathrm{\&theta;}}_e}{2}\right)}---\left(7\right)$

$=1+w\tan \left(\frac{{\mathrm{\&theta;}}_e}{2}\right)=\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]$

其中θ_e表示旋转的角度误差，w表示的是旋转轴。从而高斯-牛顿迭代转化为寻求更正误差值

$\min \mathrm{\&epsiv;}\left({\stackrel{\&RightArrow;}{q}}_e\right)=\left[\begin{array}{c}\mathrm{\&alpha;}(s_A^b-{({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right])}^{-1}\&CircleTimes;s_A^n({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]))\\ \mathrm{\&beta;}(s_M^b-{({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right])}^{-1}\&CircleTimes;s_M^n\&CircleTimes;({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]))\end{array}\right]$

${\left[\begin{array}{c}\mathrm{\&alpha;}(s_A^b-{({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right])}^{-1}\&CircleTimes;s_A^n({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]))\\ \mathrm{\&beta;}(s_M^b-{({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right])}^{-1}\&CircleTimes;s_M^n\&CircleTimes;({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]))\end{array}\right]}^T---\left(8\right)$

假定初始更正误差值为0，则一次迭代后的更正误差值为：

${\stackrel{\&RightArrow;}{q}}_e={\stackrel{\widehat{\&RightArrow;}}{q}}_e-{\left({\left(\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}\right)}^T\left(\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}\right)\right)}^{-1}{\left(\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}\right)}^T\mathrm{\&epsiv;}\left({\stackrel{\widehat{\&RightArrow;}}{q}}_e\right)---\left(9\right)$

${\stackrel{\&RightArrow;}{q}}_e=\mathrm{\&Delta;}{\stackrel{\&RightArrow;}{q}}_e=-{\left({\left(\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}\right)}^T\left(\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}\right)\right)}^{-1}{\left(\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}\right)}^T\mathrm{\&epsiv;}\left({\stackrel{\widehat{\&RightArrow;}}{q}}_e\right)---\left(10\right)$

其中，高斯-牛顿迭代的雅各比矩阵为：

$J\left({\stackrel{\widehat{\&RightArrow;}}{q}}_e\right)=\frac{\&PartialD;\mathrm{\&epsiv;}}{\&PartialD;{\stackrel{\widehat{\&RightArrow;}}{q}}_e}=-2\left[\begin{array}{c}\mathrm{\&alpha;}{\left[R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)S_A^n\right]}^{\&times;}\\ \mathrm{\&beta;}[R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right){S_M^n]}^{\&times;}\end{array}\right]---\left(11\right)$

经过相应的矩阵化简运算：

$J^{T}J=4{\left[\begin{array}{c}\mathrm{\&alpha;}{\left[R_{\left(\hat{q}\right)S_A^n}^n{}_b\right]}^{\&times;}\\ \mathrm{\&beta;}{\left[R_n^b\left(\hat{q}\right)S_M^n\right]}^{\&times;}\end{array}\right]}^T\left[\begin{array}{c}\mathrm{\&alpha;}{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}\\ \mathrm{\&beta;}{R_n^b\left(\hat{q}\right)S_M^n]}^{\&times;}\end{array}\right]$

$=4\left[\begin{array}{cc}-a{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}& -\mathrm{\&beta;}{R_n^b\left(\hat{q}\right)S_M^n]}^{\&times;}\end{array}\right]\left[\begin{array}{c}\mathrm{\&alpha;}{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}\\ \mathrm{\&beta;}{\left[R_n^b\left(\hat{q}\right)S_M^n\right]}^{\&times;}\end{array}\right]$

$=-4({\mathrm{\&alpha;}}^2{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}\mathrm{\&alpha;}{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}+{\mathrm{\&beta;}}^2{\left[R_n^b\left(\hat{q}\right)S_M^n\right]}^{\&times;}{\left[R_n^b\left(\hat{q}\right)S_M^n\right]}^{\&times;})$

$=4\left({\mathrm{\&alpha;}}^2{(\left(R\left(\hat{q}\right)S_A^n_n^b\right)}^T\right[R_n^b\left(\hat{q}\right)S_A^n]I-\left(R_n^b\left(\hat{q}\right)S_A^n\right){\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^T)---\left(12\right)$

$+{\mathrm{\&beta;}}^2{(\left(R\left(\hat{q}\right)S_M^n_n^b\right)}^T\left[R_n^b\left(\hat{q}\right)S_M^n\right]I-\left(R_n^b\left(\hat{q}\right)S_M^n\right){\left[R_n^b\left(\hat{q}\right)S_M^n\right]}^T))$

$=4({\mathrm{\&alpha;}}^2+{\mathrm{\&beta;}}^2-{\mathrm{\&alpha;}}^{2}R_n^b\left(\hat{q}\right)S_A^{n}S_A^T{}_{n}R_n^b{\left(\hat{q}\right)}^T-{\mathrm{\&beta;}}^{2}R_n^b\left(\hat{q}\right)S_M^{n}S_M^T{}_{n}R_n^b{\left(\hat{q}\right)}^T)$

$=4(({\mathrm{\&alpha;}}^2+{\mathrm{\&beta;}}^2)I-R_n^b({\mathrm{\&alpha;}}^{2}S_A^{n}S_A^{n}T+{\mathrm{\&beta;}}^{2}S_M^{n}S_M^T{}_n)R_T^n{}_b)$

其中，J为

的缩写，

为

的缩写。从而得到了下面表达式：

${\left(J^{T}J\right)}^{-1}=\frac{1}{4}R_n^b{\mathrm{\&Xi;}}^{-1}R_T^n{}_b---\left(13\right)$

其中， $\mathrm{\&Xi;}=({\mathrm{\&alpha;}}^2+{\mathrm{\&beta;}}^2)I-({\mathrm{\&alpha;}}^{2}S_A^{n}S_A^T{}_n+{\mathrm{\&beta;}}^{2}S_M^{n}S_M^T{}_n).$ 又由于：

$J^T\mathrm{\&epsiv;}\left({\stackrel{\&RightArrow;}{q}}_e\right)=2({\mathrm{\&alpha;}}^2{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}S_A^b+{\mathrm{\&beta;}}^2{\left[R\left(\hat{q}\right)_n^{b}S_M^n\right]}^{\&times;}S_M^b)---\left(14\right)$

从而得到一次迭代的更正误差值为：

$\mathrm{\&Delta;}{\stackrel{\&RightArrow;}{q}}_e=-\frac{1}{2}R_n^b{\mathrm{\&Xi;}}^{-1}R_T^n{}_b({\mathrm{\&alpha;}}^2{\left[R_n^b\left(\hat{q}\right)S_A^n\right]}^{\&times;}S_A^b+{\mathrm{\&beta;}}^2{\left[R_n^b\left(\hat{q}\right)S_M^n\right]}^{\&times;}S_M^b)---\left(15\right)$

其中α，β设置为1，从而获得更正误差值为：

${\stackrel{\&RightArrow;}{q}}_e=\mathrm{\&Delta;}{\stackrel{\&RightArrow;}{q}}_e=-\frac{1}{2}R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right){\mathrm{\&Xi;}}^{-1}R_n^b{\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)}^T({\left[R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)S_A^n\right]}^{\&times;}S_A^b+{\left[R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)S_M^n\right]}^{\&times;}S_M^b)---\left(16\right)$

其中： $\mathrm{\&Xi;}=2I-(S_A^{n}S_A^T{}_n+S_M^{n}S_M^T{}_n)---\left(17\right)$

$R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)=2\left[\begin{array}{ccc}0.5-q_2^2-q_3^2& q_1{q}_2+q_0{q}_3& q_1{q}_3-q_0{q}_2\\ q_1{q}_2-q_0{q}_3& 0.5-q_1^2-q_3^2& q_2{q}_3+q_0{q}_1\\ q_1{q}_3+q_0{q}_2& q_2{q}_3-q_0{q}_1& 0.5-q_1^2-q_2^2\end{array}\right]---\left(18\right)$

${\left[\mathrm{\&lambda;}\right]}^{\&times;}=\left[\begin{array}{ccc}0& -{\mathrm{\&lambda;}}_z& {\mathrm{\&lambda;}}_y\\ {\mathrm{\&lambda;}}_z& 0& -{\mathrm{\&lambda;}}_x\\ -{\mathrm{\&lambda;}}_y& {\mathrm{\&lambda;}}_x& 0\end{array}\right]---\left(19\right)$

${\left[\mathrm{\&mu;}\right]}^{\&times;}=\left[\begin{array}{ccc}0& -{\mathrm{\&mu;}}_z& {\mathrm{\&mu;}}_y\\ {\mathrm{\&mu;}}_z& 0& -{\mathrm{\&mu;}}_x\\ -{\mathrm{\&mu;}}_y& {\mathrm{\&mu;}}_x& 0\end{array}\right]---\left(20\right)$

其中：λ表示为

μ表示为λ_x、λ_y、λ_z为向量λ的三个分量，μ_x、μ_y、μ_z为向量μ的三个分量，q₀、q₁、q₂、q₃为四元数

的四个分量，为四元数

转换成的旋转矩阵；因为矩阵Ξ只是与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据有关，所以在算法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；

对于高斯-牛顿迭代次数问题，通过实验——迭代一次，两次，三次后姿态误差值的模值变化，见附图3中含有的(a)图、(b)图、(c)图，得知迭代次数的增加对姿态的准确性没有明显的变化(姿态误差值模值没有明显的减小)，在本方法中只需进行一次高斯-牛顿迭代即可获得较为准确的姿态误差值；

步骤S7：对步骤S4得到的姿态预测信息以及步骤S6得到微惯性组合测量装置的姿态误差值以及步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值：

$q_{t+\mathrm{\&Delta;t}}={\hat{q}}_{t+\mathrm{\&Delta;t}}+{\hat{q}}_{t+\mathrm{\&Delta;t}}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]k_A{k}_M\mathrm{\&Delta;t}---\left(21\right)$

然后再对其单位化q_t+Δt＝q_t+Δt/||q_t+Δt||；其中，

为步骤S3获取，k_Ak_M为步骤S5获取，

表示的是四元数的乘法运算；

步骤S8：输出姿态信息：根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义、自适应三维姿态的的俯仰角α、横滚角β和航向角γ如下：

$\left\{\begin{array}{cc}\mathrm{\&alpha;}=\mathrm{arctg}\left(\frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)& \mathrm{\&alpha;}\&Element;(-\mathrm{180,180}]\\ \mathrm{\&beta;}=\arcsin (-2(q_1{q}_3+q_0{q}_2))& \mathrm{\&beta;}\&Element;(-\mathrm{90,90}]\\ \mathrm{\&gamma;}=\mathrm{arctg}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2+q_1^2-q_2^2-q_3^2}\right)& \mathrm{\&gamma;}\&Element;(-\mathrm{180,180}]\end{array}\right.---\left(22\right).$

如图7所示，本实施例的工作流程如下：

1)系统初始化模块：

姿态跟踪设备一般在静态情况下启动。首先是进行系统初始化，如对各个传感器的信号的检测，处理器芯片的初始化设置，通信系统的初始化以及自适应三维姿态方法的步骤S2的初始信息的设定。

2)坐标轴校准

通过相应的转动平台等设备，检测三轴微陀螺传感器，三轴微加速度传感器和三轴磁场传感器的坐标轴校准情况，对于三个坐标轴之间的校准偏差，可以通过相应的旋转矩阵来进行补偿。如当某传感器坐标轴与设定的标准坐标轴之间存在横滚角γ，俯仰角θ，和航向角ψ的偏差时，可以通过如下公式进行传感器数据的标定：

$s^{\&prime;}=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]s---\left(23\right)$

其中s是某传感器的原始数据，s′为坐标轴对准后的传感器的数据。

3)传感器数据的采集

采集三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的数据，为下一步的做准备。

4)自适应三维姿态定位方法

对于采集到的传感器数据，基于自适应三维姿态定位方法的步骤S4，S5，S6，S7，S8进行装置姿态值的计算。

5)姿态参数输出

对自适应三维姿态定位算法得到的姿态信息，通过相应的通信接口与外接的显示设备或是其他姿态处理设备进行通信输出。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的权利要求书的保护范围之内。

Claims (4)

1.一种基于微惯性和地磁技术的自适应三维姿态定位方法，其特征在于，包括以下步骤：

步骤S1：利用集成了三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器的微惯性组合测量装置来感应运动载体的运动姿态：三轴微陀螺传感器感应运动载体沿轴向的角速度信号，三轴微加速度传感器感应运动载体加速度信号，三轴磁场传感器感应地球磁场信号；

步骤S2：设置微惯性组合测量装置的初始姿态值和全局坐标系下的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据：在确保三轴微陀螺传感器、三轴微加速度传感器和三轴磁场传感器坐标系对准的情况下，保持微惯性组合测量装置固定不动，采集三轴微加速度传感器的加速度数据和三轴磁场传感器感应的地球磁场数据；设置微惯性组合测量装置初始时刻即i＝0时的四元数形式姿态值q₀＝[1 0 0 0]^T、三轴微加速度传感器矢量ⁿY_A＝[a_0x a_0y a_0z]^T和三轴磁场传感器矢量ⁿY_M＝[m_0xm_0ym_0z]^T，对三轴微加速度传感器矢量ⁿY_A和三轴磁场传感器矢量ⁿY_M进行单位化，得到单位三轴微加速度传感器矢量ⁿS_A和单位三轴磁场传感器矢量ⁿS_M，所述单位化公式为：

ⁿS_A＝ⁿY_A/||ⁿY_A||和ⁿS_M＝ⁿY_M/||ⁿY_M||，

式中，a_0x、a_0y、a_0z表示初始时刻即i＝0时的全局坐标系下的三轴微加速度传感器的加速度数据，m_0x、m_0y、m_0z表示初始时刻即i＝0时的全局坐标系下的是三轴磁场传感器的地球磁场数据，下标0表示的是初始时刻，下标x、y、z表示的是每个传感器坐标系下的三个坐标轴；上标n表示的是全局坐标系下的传感器数据、下标A和M分别表示的是三轴微加速度传感器和三轴磁场传感器的数据；S表示的是单位化后相应传感器数据；上标T表示矢量的转置；

步骤S3：对微惯性组合测量装置的姿态值进行求解：利用当前时刻即i＝t时的三轴微陀螺传感器的角速度数据、三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据，当前时刻t的姿态值q_t以及步骤2获得的初始姿态值q₀、三轴微加速度传感器的加速度数据ⁿY_A、ⁿS_A和三轴磁场传感器的地球磁场数据ⁿY_M、ⁿS_M，来获取微惯性组合测量装置下一时刻即i＝t₊Δt时的姿态值q_t+Δt；

步骤S4：利用三轴微陀螺传感器数据对i＝t₊Δt时刻微惯性组合测量装置的姿态值进行预测：将当前时刻t的三轴微陀螺传感器的角速度数据表示为当前姿态下的旋转角速度四元数Q_G＝[0 ω_tx ω_ty ω_tz]^T，当前时刻t的三轴微加速度传感器的加速度数据和三轴磁场传感器的磁场强度数据分别为^bY_A＝[a_tx a_ty a_tz]^T和^bY_M＝[m_tx m_ty m_tz]^T，其中G、b为标记号，ω_tx、ω_ty、ω_tz表示的是当前时刻t的三轴陀螺传感器的角速度数据，a_tx、a_ty、a_tz表示的是当前时刻t的三轴微加速度传感器的加速度数据，m_tx、m_ty、m_tz表示的是当前时刻t的三轴磁场传感器的地球磁场数据；

利用微惯性组合测量装置当前时刻t的姿态值q_t和当前时刻t的三轴微陀螺传感器的旋转角速度四元数Q_G，对i＝t₊Δt时刻的微惯性组合测量装置的姿态值

进行预测，由于两个时刻之间的间隔Δt较小，假设认为t和t₊Δt两个时刻之间三轴微陀螺传感器输出没有变化，从而获得姿态预测

方程：

${\hat{q}}_{t+\mathrm{\&Delta;t}}=q_t+{\stackrel{\&CenterDot;}{q}}_t\mathrm{\&Delta;t}---\left(1\right)$

式中 ${\stackrel{\&CenterDot;}{q}}_t=\frac{1}{2}{q}_t\&CircleTimes;Q_G---\left(2\right)$

对预测后的四元数进行单位化：

${\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}={\hat{q}}_{t+\mathrm{\&Delta;t}}/\left|\right|{\hat{q}}_{t+\mathrm{\&Delta;t}}\left|\right|---\left(3\right)$

式中，q_t为当前时刻t的微惯性组合测量装置的姿态值，

为下一时刻预测姿态值，Δt为采样时间间隔，

为四元数形式的当前时刻t全局姿态下的旋转角速度；

步骤S5：对步骤S4获得的三轴微加速度传感器的加速度数据和三轴磁场传感器地球磁场数据进行置信判断，判定周围环境是否存在电磁场干扰或瞬时加速度干扰；若干扰过大，则放弃该时刻采集的三轴磁场传感器的地球磁场数据或三轴微加速度传感器的加速度数据，否则以测得的周围环境噪声的大小设置自适应因子，该自适应因子是表征不存在较大的电磁干扰或是瞬时加速度情况下的三轴微加速度传感器和三轴磁场传感器的噪声大小，从而基于该自适应因子对三轴微加速度传感器的加速度数据或是三轴磁场传感器的地球磁场数据的影响进行权重设置如下：

$S_M^b=\left\{\begin{array}{ccc}Y_M^b/\left|\right|Y_M^b\left|\right|& \mathrm{if}& (k_M=1-|\left|\right|Y_M^b\left|\right|-\left|\right|q_t^{-1}\&CircleTimes;Y_M^n\&CircleTimes;q_t\left|\right||/q_t^{-1}\&CircleTimes;Y_M^n\&CircleTimes;q_t||<{\mathrm{\&epsiv;}}_M)\\ q_t^{-1}\&CircleTimes;S_M^n\&CircleTimes;q_t& \mathrm{else}& k_M=1\end{array}\right.---\left(4\right)$

$S_A^b=\left\{\begin{array}{ccc}Y_A^b/\left|\right|Y_A^b\left|\right|& \mathrm{if}& (k_A=1-|\left|\right|Y_A^b\left|\right|-q_t^{-1}\&CircleTimes;Y_A^n\&CircleTimes;q_t\left|\right||/||q_t^{-1}\&CircleTimes;Y_A^n\&CircleTimes;q_t||<{\mathrm{\&epsiv;}}_A)\\ q_t^{-1}\&CircleTimes;S_A^n\&CircleTimes;q_t& \mathrm{else}& k_{A=1}\end{array}\right.$

式中，^bS_M为调整后的三轴磁场传感器矢量、^bS_A为调整后的三轴微加速度传感器矢量；k_A为三轴微加速度传感器自适应因子，k_M为三轴磁场传感器自适应因子；ε_A为三轴微加速度传感器噪声上界，ε_M为三轴磁场传感器噪声上界；所述干扰过大是超过公式(4)中的三轴微加速度传感器噪声上界ε_A或三轴磁场传感器噪声上界ε_M，ε_A、ε_M的取值范围是(0-1)，周围环境噪声越大，该噪声上界ε_A、ε_M值应取值越小，具体取值应依据在现场环境中的实验决定；

步骤S6：利用步骤S5调整后的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据获得微惯性组合测量装置的姿态误差值

该姿态误差值通过高斯-牛顿方法获得：

${\stackrel{\&RightArrow;}{q}}_e=-\frac{1}{2}R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right){\mathrm{\&Xi;}}^{-1}R_n^b{\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)}^T({\left[R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)S_A^n\right]}^{\&times;}S_A^b+{\left[R_n^b{\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)}^n{S}_M\right]}^{\&times;}S_M^b)---\left(16\right)$

式中： $\mathrm{\&Xi;}=2I-(S_A^{n}S_A^T{}_n+S_M^{n}S_M^T{}_n)---\left(17\right)$

$R_n^b\left({\hat{q}}_{t+\mathrm{\&Delta;t}}^{\&prime;}\right)=2\left[\begin{array}{ccc}0.5-q_2^2-q_3^2& q_1{q}_2+q_0{q}_3& q_1{q}_3-q_0{q}_2\\ q_1{q}_2-q_0{q}_3& 0.5-q_1^2-q_3^2& q_2{q}_3+q_0{q}_1\\ q_1{q}_3+q_0{q}_2& q_2{q}_3-q_0{q}_1& 0.5-q_1^2-q_2^2\end{array}\right]---\left(18\right)$

${\left[\mathrm{\&lambda;}\right]}^{\&times;}=\left[\begin{array}{ccc}0& -{\mathrm{\&lambda;}}_z& {\mathrm{\&lambda;}}_y\\ {\mathrm{\&lambda;}}_z& 0& -{\mathrm{\&lambda;}}_x\\ -{\mathrm{\&lambda;}}_y& {\mathrm{\&lambda;}}_x& 0\end{array}\right]---\left(19\right)$

${\left[\mathrm{\&mu;}\right]}^x=\left[\begin{array}{ccc}0& -{\mathrm{\&mu;}}_z& {\mathrm{\&mu;}}_y\\ {\mathrm{\&mu;}}_z& 0& -{\mathrm{\&mu;}}_x\\ -{\mathrm{\&mu;}}_y& {\mathrm{\&mu;}}_x& 0\end{array}\right]---\left(20\right)$

式中：λ表示为

μ表示为

λ_x、λ_y、λ_z为向量λ的三个分量，μ_x、μ_y、μ_z为向量μ的三个分量，q₀、q₁、q₂、q₃为四元数

的四个分量，

为四元数

转换成的旋转矩阵；矩阵Ξ只与初始标定时刻的三轴微加速度传感器的加速度数据和三轴磁场传感器的地球磁场数据有关，所以在姿态方法中只要进行一次标定，获取Ξ矩阵及其逆矩阵，无需在其他时刻进行矩阵逆运算；

步骤S7：基于步骤S4得到的姿态预测信息、步骤S6得到微惯性组合测量装置的姿态误差值和步骤S5得到的自适应因子进行融合，获得t+Δt时刻微惯性组合测量装置姿态值：

$q_{t+\mathrm{\&Delta;t}}={\hat{q}}_{t+\mathrm{\&Delta;t}}+{\hat{q}}_{t+\mathrm{\&Delta;t}}\&CircleTimes;\left[\begin{array}{cc}1& {\stackrel{\&RightArrow;}{q}}_e\end{array}\right]k_A{k}_M\mathrm{\&Delta;t}---\left(21\right)$

然后再对其单位化q_t+Δt＝q_t+Δt/||q_t+Δt||；其中

表示的是四元数的乘法运算；

步骤S8：输出姿态信息：根据四元数和欧拉角之间的转换关系，将四元数转换为具有直观意义的、自适应三维姿态的俯仰角α、横滚角β和航向角γ如下：

$\left\{\begin{array}{cc}\mathrm{\&alpha;}=\mathrm{arctg}\left(\frac{2(q_2{q}_3-q_0{q}_1)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)& \mathrm{\&alpha;}\&Element;(-\mathrm{180,180}]\\ \mathrm{\&beta;}=\arcsin (-2(q_1{q}_3+q_0{q}_2))& \mathrm{\&beta;}\&Element;(-\mathrm{90,90}]\\ \mathrm{\&gamma;}=\mathrm{arctg}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2+q_1^2-q_2^2-q_3^2}\right)& \mathrm{\&gamma;}\&Element;(-\mathrm{180,180}]\end{array}\right.---\left(22\right).$

2.根据权利要求1中所述的自适应三维姿态定位方法，其特征在于：利用三轴微陀螺传感器的预测以及三轴微加速度传感器和三轴磁场传感器数据的更正获取姿态信息。

3.根据权利要求1中所述自适应三维姿态定位方法，其特征在于：微惯性组合测量装置的初始姿态标定需要进行一次矩阵逆求解，在预测过程和更正过程所有的姿态信息均以四元数的形式进行表示和运算。

4.据权利要求1中所述的自适应三维姿态定位方法，其特征在于：考虑周围环境中的电磁场对地球磁场的干扰和瞬时加速度的影响，如果干扰变化超过阈值，则三轴微加速度传感器和三轴磁场传感器的数据进行舍弃；如果干扰变化没超过阈值，则按照干扰的大小设置三轴微加速度传感器和三轴磁场传感器数据的自适应因子。

Images