CN103499348B - Ahrs高精度姿态数据计算方法 - Google Patents

Abstract

本发明公开了一种AHRS高精度姿态数据计算方法，主要解决了现有技术中存在的由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求的问题。其包括初始化传感器和归一化坐标系转换矩阵；计算载体的姿态数据和此时的坐标系转换矩阵R₁；计算该时刻的坐标系转换矩阵R₂；确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵；根据步骤（4）中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据；返回步骤（2），进行下一时刻的姿态数据计算。通过上述方案，本发明达到了便于实现、精度较高的目的，具有很高的实用价值和推广价值。

Description

AHRS高精度姿态数据计算方法

技术领域

本发明涉及一种导航领域的姿态数据计算方法，具体地说，是涉及一种AHRS高精度姿态数据计算方法。

背景技术

众所周知，惯性导航主要涉及两方面的内容：定位和定姿，定位可通过全球卫星定位***（如GPS、北斗等）完成；定姿主要将加速度计输出的初始角度信息和陀螺仪输出的三轴角速度信息通过积分计算得到当前的姿态数据（主要包括方向角、俯仰角和翻滚角）。由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求。

发明内容

本发明的目的在于提供一种AHRS高精度姿态数据计算方法，主要解决现有技术中存在的由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求问题。

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

AHRS高精度姿态数据计算方法，包括以下步骤：

（1）将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理；

（2）读取AHRS中加速度计和磁力计的数据，计算载体的姿态数据和此时的坐标系转换矩阵R₁；

（3）读取AHRS中陀螺仪的数据，根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R₂；

（4）根据坐标系转换矩阵R₁和坐标系转换矩阵R₂确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵；

（5）根据步骤（4）中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据；

（6）返回步骤（2），进行下一时刻的姿态数据计算。

所述步骤（1）具体包括：

（1a）配置传感器的各项参数；

（1b）根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵R_normalized。

所述步骤（1b）包括以下步骤：

（1b1）从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z；从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z；

（1b2）根据公式计算俯仰角θ；根据公式计算翻滚角φ；根据公式

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ 计算方向角ψ；

相应地，所述归一化坐标系转换矩阵R_normalized通过公式

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ 完成初始化。

所述步骤（2）具体包括以下步骤：

（2a）从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z；从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z；

（2b）根据公式计算当前时刻的俯仰角θ；根据公式计算当前时刻的翻滚角φ；根据公式

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ 计算当前时刻的方向角ψ，其中，θ为当前时刻的俯仰角；φ为当前时刻的翻滚角；

根据公式 $R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ 得出该时刻的坐标系转换矩阵R₁，其中，θ为当前时刻的俯仰角；φ为当前时刻的翻滚角；ψ为当前时刻的方向角。

所述步骤（3）具体包括以下步骤：

（3a）从陀螺仪上读取载体坐标系在X轴向的角速度ω_x、在Y轴向的角速度ω_y、在Z轴向的角速度ω_z；

（3b）根据公式 $\begin{array}{c}{\mathrm{d\θ}}_x={\mathrm{\ω}}_x\mathrm{dt}\\ {\mathrm{d\θ}}_y={\mathrm{\ω}}_y\mathrm{dt}\\ {\mathrm{d\θ}}_z={\mathrm{\ω}}_z\mathrm{dt}\end{array}$ 和公式 $R_2(t+\mathrm{dt})=R_{\mathrm{normalized}}\left(t\right)\left[\begin{array}{ccc}1& -{\mathrm{d\θ}}_z& {\mathrm{d\θ}}_y\\ {\mathrm{d\θ}}_z& 1& -{\mathrm{d\θ}}_x\\ -{\mathrm{d\θ}}_y& {\mathrm{d\θ}}_x& 1\end{array}\right]$ 得出该时刻的坐标系转换矩阵R₂。

所述步骤（4）具体包括以下步骤：

（4a）根据计算出的坐标系转换矩阵R₁和坐标系转换矩阵R₂计算该时刻最终的坐标系转换矩阵R=R₁+λ(R₂-R₁)，其中，λ为坐标系转换矩阵校正因子，且0≤λ≤1；

（4b）根据以下公式：

$R^T=\left[\begin{array}{ccc}X& T& Z\end{array}\right]=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{yx}}& r_{\mathrm{zx}}\\ r_{\mathrm{xy}}& r_{\mathrm{yy}}& r_{\mathrm{zy}}\\ r_{\mathrm{xz}}& r_{\mathrm{yz}}& r_{\mathrm{zz}}\end{array}\right];$ $\mathrm{err}=X\·Y=X^{T}Y=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{xy}}& r_{\mathrm{xz}}\end{array}\right]\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right];$

${\left[\begin{array}{c}{r}_{\mathrm{xx}}\\ r_{\mathrm{xy}}\\ r_{\mathrm{xz}}\end{array}\right]}_{\mathrm{orth}}=X_{\mathrm{orth}}=X-\frac{\mathrm{err}}{2}Y;{\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]}_{\mathrm{orth}}=Y_{\mathrm{orth}}=Y-\frac{\mathrm{err}}{2}X;{\left[\begin{array}{c}{r}_{\mathrm{zx}}\\ r_{\mathrm{zy}}\\ r_{\mathrm{zz}}\end{array}\right]}_{\mathrm{orth}}=Z_{\mathrm{orth}}=X_{\mathrm{orth}}\×Y_{\mathrm{orth}};$

$R_{\mathrm{normalized}}=\left[\begin{array}{ccc}{X}_{\mathrm{normalized}}& Y_{\mathrm{normalized}}& Z_{\mathrm{normalized}}\end{array}\right];X_{\mathrm{normalized}}=\frac{1}{2}(3-X_{\mathrm{orth}}\·X_{\mathrm{orth}})X_{\mathrm{orth}};$

$Y_{\mathrm{normalized}}=\frac{1}{2}(3-Y_{\mathrm{orth}}\·Y_{\mathrm{orth}})Y_{\mathrm{orth}};Z_{\mathrm{normalized}}=\frac{1}{2}(3-Z_{\mathrm{orth}}\·Z_{\mathrm{orth}})Z_{\mathrm{orth}};$ 对该时刻最终的坐标系转换矩阵R进行归一化，得出该时刻归一化坐标系转换矩阵R_normalized。

所述步骤（5）中该时刻最终的姿态数据通过该时刻的最终俯仰角θ、翻滚角φ和方向角ψ确定，该时刻的最终俯仰角θ、翻滚角φ和方向角ψ根据公式

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

以及步骤（4b）计算出的归一化坐标系转换矩R_normalized阵求得。

与现有技术相比，本发明具有以下有益效果：

（1）本发明以加速度计、磁力计和陀螺仪获取的数据源为基础，分别独立计算载体的坐标系转移矩阵，并以加速度计和磁力计计算出的坐标系转移矩阵为基础，通过陀螺仪计算的坐标系转移矩阵对其进行纠偏，以提升坐标转移矩阵的精度，进而大大提升了最终姿态数据的精度，且只使用由陀螺仪计算的坐标系转移矩阵进行辅助纠偏能够有效降低陀螺仪累积误差等带来的影响，更有助于最终姿态数据精度的提升，设计十分巧妙，符合技术需求。

（2）本发明所实现的新型AHRS高精度姿态数据计算方法能够使惯性导航的定姿精度得到明显的提升，达到更佳的惯性导航效果，对将惯性导航应用到更多需要精确姿态数据的领域中作出了重大贡献，具有突出的实质性特点和显著进步，适合大规模推广应用。

附图说明

图1为本发明的实施例中地面坐标系与载体坐标系角度示意图。

图2为本发明的流程示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。

实施例

为了解决现有技术中存在的由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求问题，如图1所示，本发明中采用两个不同的坐标系：地面坐标系(也称东北天坐标系)和载体坐标系。

地面坐标系是指在地球表面距离较近的范围内(如几十公里内)，可将大地水平面近似视为一个平面，从而可以以正东方向为x正轴，正北方向为y正半轴，垂直于地面向上为z正半轴，构成地面坐标系，也就是所谓的东北天坐标系；

载体坐标系是基于载体建立的一种符合右手定则的坐标系，载体坐标系以载体质心为原点，x轴与载体纵轴重合，指向载体头部为正；y轴与载体横轴重合，指向载体右侧为正；z轴垂直于载体平面，指向载体腹部为正。

当载体姿态发生变化时，载体坐标系将相对于地面坐标系发生旋转。如图1所示，(xe，ye，ze)表示地面坐标系，(xb，yb，zb)表示载体坐标系，在图1中，地面坐标系在xeOye平面内旋转ψ度，在xbOze平面内旋转θ度，在ybOzb平面内旋转φ度后，即可得到图1中的载体坐标系，此时，称ψ为方向角，θ为俯仰角，φ为翻滚角。本发明使用这3个角度（方向角、俯仰角和翻滚角）来度量载体的当前姿态，并以此作为姿态数据输出。

设Q_B=[Q_BxQ_ByQ_Bz]^T，Q_E=[Q_ExQ_EyQ_Ez]^T分别表示载体坐标系和地面坐标系下的坐标向量，则有Q_B=RQ_E。其中R称为坐标系转换矩阵，且满足(式1)： $R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ (式1)

本发明中，[·]^T表示矩阵的转置；r_xx为矩阵R^T第1行第1列的元素，r_xy为矩阵R^T第2行第1列的元素，以此类推，r_zz为矩阵R^T第3行第3列的元素；X=[r_xxr_xyr_xz]^T，Y=[r_yxr_yyr_yz]^T，Z=[r_zxr_zyr_zz]^T；X_orth，Y_orth，Z_orth分别是X，Y，Z对应的正交向量；X_normalized，Y_normalized，Z_normalized分别是X，Y，Z对应的归一化向量；X·Y表示向量X与向量Y的点乘，X×Y表示向量X与向量Y的叉乘。

如图2所示，本发明公开了一种新型AHRS高精度姿态数据计算方法，整体方案如下：

步骤一、通过配置传感器各项参数，并根据此时加速度计和磁力计的输出数据，初始化归一化坐标系转换矩阵R_normalized实现传感器及归一化坐标系转换矩阵的初始化；

其中，归一化坐标系转换矩阵R_normalized初始化通过从加速度计读取载体坐标系x轴、y轴和z轴3个方向的加速度accel_x、accel_y和accel_z，从磁力计读取载体坐标系x轴、y轴和z轴三个方向的磁场强度magnetom_x、magnetom_y和magnetom_z，并根据(式2)、(式3)和(式4)计算俯仰角θ、翻滚角φ和方向角ψ，之后将计算出的俯仰角θ、翻滚角φ和方向角ψ代入(式1)完成。(式2)、(式3)和(式4)如下所示：

$\tan \left(\mathrm{\θ}\right)=-\frac{\mathrm{accel}\_x}{\sqrt{\mathrm{accel}\_y^2+\mathrm{accel}\_z^2}}$ (式2)

$\tan \left(\mathrm{\φ}\right)=\frac{\mathrm{accel}\_y}{\mathrm{accel}\_z}$ (式3)

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ (式4)

步骤二、从加速度计读取载体坐标系x轴、y轴和z轴3个方向的加速度accel_x、accel_y和accel_z；从磁力计读取载体坐标系x轴、y轴和z轴3个方向的磁场强度magnetom_x、magnetom_y和magnetom_z；

根据(式2)、(式3)和(式4)初步计算当前时刻的俯仰角θ、翻滚角φ和方向角ψ。并将计算出的俯仰角θ、翻滚角φ和方向角ψ代入(式1)，得到该时刻的坐标系转换矩阵R₁。

步骤三、从陀螺仪读取载体坐标系x轴、y轴和z轴3个方向的角速度ω_x、ω_y和ω_z；根据(式5)和(式6)得到该时刻的坐标系转换矩阵R₂，(式5)和(式6)如下所示：

$\begin{array}{c}{\mathrm{d\θ}}_x={\mathrm{\ω}}_x\mathrm{dt}\\ {\mathrm{d\θ}}_y={\mathrm{\ω}}_y\mathrm{dy}\\ {\mathrm{d\θ}}_z={\mathrm{\ω}}_z\mathrm{dt}\end{array}$ (式5)

$R_2(t+\mathrm{dt})=R_{\mathrm{normalized}}\left(t\right)\left[\begin{array}{ccc}1& {-\mathrm{d\θ}}_z& {\mathrm{d\θ}}_y\\ {\mathrm{d\θ}}_z& 1& -{\mathrm{d\θ}}_x\\ -{\mathrm{d\θ}}_y& {\mathrm{d\θ}}_x& 1\end{array}\right]$ (式6)

步骤四、根据计算得出的坐标系转换矩阵R₁和坐标系转换矩阵R₂，和(式7)计算该时刻最终的坐标系转换矩阵R，(式7)如下所示：

R=R₁+λ(R₂-R₁)(式7)

其中，λ为坐标系转换矩阵校正因子，且0≤λ≤1。

根据(式8)～(式16)对该时刻最终的坐标系转换矩阵R进行归一化，得到该时刻归一化坐标系转换矩阵R_normalized，(式8)～(式16)如下所示：

$R^T=\left[\begin{array}{ccc}X& Y& Z\end{array}\right]=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{yx}}& r_{\mathrm{zx}}\\ r_{\mathrm{xy}}& r_{\mathrm{yy}}& r_{\mathrm{zy}}\\ r_{\mathrm{xz}}& r_{\mathrm{yz}}& r_{\mathrm{zz}}\end{array}\right]$ (式8)

$\mathrm{err}=X\•Y=X^{Y}Y=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{xy}}& r_{\mathrm{xz}}\end{array}\right]\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]$ (式9)

${\left[\begin{array}{c}{r}_{\mathrm{xx}}\\ r_{\mathrm{xy}}\\ r_{\mathrm{xz}}\end{array}\right]}_{\mathrm{orth}}=X_{\mathrm{orth}}=X-\frac{\mathrm{err}}{2}Y$ (式10)

${\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]}_{\mathrm{orth}}=Y_{\mathrm{orth}}=Y-\frac{\mathrm{err}}{2}X$ (式11)

${\left[\begin{array}{c}{r}_{\mathrm{zx}}\\ r_{\mathrm{zy}}\\ r_{\mathrm{zz}}\end{array}\right]}_{\mathrm{orth}}=Z_{\mathrm{orth}}=X_{\mathrm{orth}}\×Y_{\mathrm{orth}}$ (式12)

R_normalized=[X_normalizedY_normalizedZ_normalized](式13)

$X_{\mathrm{normalized}}=\frac{1}{2}(3-X_{\mathrm{orth}}\•X_{\mathrm{orth}})X_{\mathrm{orth}}$ (式14)

$Y_{\mathrm{normalized}}=\frac{1}{2}(3-Y_{\mathrm{orth}}\•Y_{\mathrm{orth}})Y_{\mathrm{orth}}$ (式15)

$Z_{\mathrm{normalized}}=\frac{1}{2}(3-Z_{\mathrm{orth}}\•Z_{\mathrm{orth}})Z_{\mathrm{orth}}$ (式16)

步骤五、根据(式1)对坐标系转换矩阵的定义，及计算出的归一化坐标系转换矩阵R_normalized计算出该时刻的最终俯仰角θ、翻滚角φ和方向角ψ。

按照上述实施例，便可很好地实现本发明。

Claims (1)

1.AHRS高精度姿态数据计算方法，其特征在于，包括以下步骤：

(1)将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理；

(2)读取AHRS中加速度计和磁力计的数据，计算载体的姿态数据和此时的坐标系转换矩阵R₁；

(3)读取AHRS中陀螺仪的数据，根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R₂；

(4)根据坐标系转换矩阵R₁和坐标系转换矩阵R₂确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵；

(5)根据步骤(4)中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据；

(6)返回步骤(2)，进行下一时刻的姿态数据计算；

所述步骤(1)具体包括：

(1a)配置传感器的各项参数；

(1b)根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵R_normalized；

所述步骤(1b)包括以下步骤：

(1b1)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z；从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z；

(1b2)根据公式计算俯仰角θ；根据公式y计算翻滚角φ；根据公式

mag_x=magnetom_x*cos(θ)+magnetom_y*sin(φ)*sin(θ)

+magnetom_z*cos(φ)*sin(θ)

计算方向角ψ；

相应地，所述归一化坐标系转换矩阵R_normalized通过公式

完成初始化；

所述步骤(2)具体包括以下步骤：

(2a)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z；从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z；

(2b)根据公式计算当前时刻的俯仰角θ；根据公式计算当前时刻的翻滚角φ；根据公式

mag_x＝magnetom_x*cos(θ)+magnetom_y*sin(φ)*sin(θ)

+magnetom_z*cos(φ)*sin(θ)

计算当前时刻的方向角ψ；

根据公式得出该时刻的坐标系转换矩阵R₁，其中，θ为当前时刻的俯仰角；φ为当前时刻的翻滚角；ψ为当前时刻的方向角；

所述步骤(3)具体包括以下步骤：

(3a)从陀螺仪上读取载体坐标系在X轴向的角速度ω_x、在Y轴向的角速度ω_y、在Z轴向的角速度ω_z；

(3b)根据公式和公式得出该时刻的坐标系转换矩阵R₂；

所述步骤(4)具体包括以下步骤：

(4a)根据计算出的坐标系转换矩阵R₁和坐标系转换矩阵R₂计算该时刻最终的坐标系转换矩阵R＝R₁+λ(R₂-R₁)，其中，λ为坐标系转换矩阵校正因子，且0≤λ≤1；

(4b)根据以下公式：

对该时刻最终的坐标系转换矩阵R进行归一化，得出该时刻归一化坐标系转换矩阵R_normalized；

所述步骤(5)中该时刻最终的姿态数据通过该时刻的最终俯仰角θ、翻滚角φ和方向角ψ确定，该时刻的最终俯仰角θ、翻滚角φ和方向角ψ根据公式

以及步骤(4b)计算出的归一化坐标系转换矩R_normalized阵求得。