本发明属于定位导航技术领域，具体涉及一种基于地球重力场与地磁场序贯修正的姿态解算方法。
技术背景
航姿参考系统(attitudeandheadingreferencesystem，ahrs)中集成了mems传感器(陀螺仪、加速度计、磁力计)，其有成本低、集成度高以及体积小等优点，但是在进行姿态解算时，却存在输出噪声过大、零点漂移无法完全消除、角速率输出易受干扰等所导致的姿态解算精度低、累计误差大等问题。因此，通常情况下，需要采用多个传感器经过信息融合算法实现高精度的姿态解算。相较于利用欧拉角进行姿态解算，四元数作为一种非常有用的数学工具，由于其参数少且避免了欧拉角产生的奇异，在现代姿态解算应用中，得到了广泛的应用。地磁场作为地球固有的物理特征，在导航中的应用越来越广泛，但是地磁场易受周围建筑物及钢性结构的影响，在地球表面的不同位置，地磁场矢量各不相同且未知，大大限制的地磁场在导航中的应用。
技术实现要素：
为解决以上技术问题本发明提供一种基于地球重力场与地磁场序贯修正的姿态解算方法。
本发明的技术方案是：一种基于地球重力场与地磁场序贯修正的姿态解算方法，包括如下三个步骤：
(1)根据航姿系统(ahrs)中mems陀螺仪采集的角速率计算系统模型的状态转移矩阵，并利用离散化的四元数微分方程更新载体姿态四元数；
(2)根据航姿系统(ahrs)中mems加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正；
(3)根据航姿系统(ahrs)中mems磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。
详细地，步骤(1)具体包括：
a.建立陀螺仪动力学模型：
即
其中，x＝[x0x1x2x3]t表示全局坐标系g到载体坐标系b的旋转四元数，
利用四元数微分方程的比卡求解法，使上式离散化可得
其中，xk-1为tk-1时刻全局坐标系g到载体坐标系b的旋转四元数，xk为tk时刻全局坐标系g到载体坐标系b的旋转四元数，
且
b.获得载体坐标系b下的载体角速度信息
c.根据所述载体角速度信息计算系统状态转移矩阵
c.载体姿态四元数状态一步预测
d.计算系统噪声协方差矩阵qk。
对于mems陀螺仪在微小时间间隔δt，其真实的角增量δθ°是无法测量的，假设在tk-1时刻到tk时刻的时间间隔内，陀螺仪的实际输出与真实角增量的误差为δθk/k-1，有
其中，δθk/k-1表示陀螺仪的实际角增量，
相应的，实际状态转移矩阵与真实状态转移矩阵的误差为δφk/k-1，则有
将上式代入步骤a中陀螺仪离散化的动力学模型中，可得
-δθk/k-1xk-1＝-ξ(xk-1)δθk/k-1
式中
设陀螺仪的输出噪声为ngyro,则有
则系统噪声协方差矩阵为：
其中，σgyro为陀螺仪噪声协方差矩阵。
f.计算姿态四元数状态一步预测均方误差pk/k-1。
其中，pk-1表示步骤(3)中的状态估计均方误差。
详细地，步骤(2)具体包括：
a.建立加速度计观测模型：
将三维矢量g与a看作零标量的四元数，则g与a间的变换关系可用四元数乘法表示为
其中，g为在全局坐标系g中测量的重力加速度，a为在载体坐标系b中测量的重力加速度，且满足||g||＝||a||＝1；
上式经过变换可得
m′(a)xk-m(g)xk＝0
其中，m′(a)与m(g)表示由零标量的四元数g与a构成的矩阵，如下
b.获得载体坐标系b下的载体重力加速度信息a＝[axayaz]t；
c.根据所述载体重力加速度信息及先验重力加速度计算量测矩阵
d.计算重力加速度量测噪声协方差矩阵rg′k
设加速度计的输出噪声为nacc，则有
nacc＝a-a°
其中，a°表示载体坐标系下加速度计的真实值。
实际量测矩阵与理想量测矩阵误差为
其中，
将上式代入步骤a中的加速度计观测模型，可得
0＝hgkxk-δhgkxk
-δhgkxk变形为
其中，
令
其中，σacc为加速度计噪声协方差矩阵。
上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的，为了避免矩阵的奇异，需要对该量测噪声协方差矩阵进行改进，即
rg′k＝rgk+βgiβg>0
e.计算重力加速度修正的滤波增益kgk
kgk＝pk/k-1(hgk)t(hgkpk/k-1(hgk)t+rg′k)-1
f.基于重力加速度的姿态四元数状态
g.基于重力加速度的姿态四元数状态估计均方误差pgk。
pgk＝(i-kgkhgk)pk/k-1
详细地，步骤(3)具体包括：
a.建立磁力计观测模型：
把三维矢量h与m看作零标量的四元数，则h与m间的变换关系可用四元数乘法表示为
其中，h为在全局坐标系g中测量的地磁场矢量，m为在载体坐标系b中测量的地磁场矢量，且满足||g||＝||a||＝1。
上式经过变换可得
m′(m)xk-m(h)xk＝0
其中，m′(m)与m(h)表示由零标量的四元数h与m构成的矩阵，如下
b.获得载体坐标系b下的载体地磁场矢量信息m＝[mxmymz]t；
c.计算先验地磁场矢量h。
全局参考坐标系g的gz轴选取重力加速度g的反方向，gx轴选取地磁北极，gy按照右手法则选取，则先验地磁场矢量h与全局参考坐标系g的gxogz面重合即h＝[hx0hy]t，则有
其中，l＝[lxlylz]t表示带有误差的先验地磁场矢量。
则有
式中
γ＝lx2+ly2
d.根据所述载体地磁场矢量信息及先验地磁场矢量计算量测矩阵
e.计算地磁场矢量量测噪声协方差矩阵rm′k。
设磁力计的输出噪声为nmag，则有
nmag＝m-m°
其中，m°表示载体坐标系下加速度计的真实值。
实际量测矩阵与理想量测矩阵误差为
其中，
把上式代入步骤a中的磁力计观测模型，可得
0＝hmkxk-δhmkxk
-δhmkxk变形为
其中，
令
其中，σmag为磁力计噪声协方差矩阵。
上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的，为了避免矩阵的奇异，需要对该量测噪声协方差矩阵进行改进，即
rm′k＝rmk+βmiβm>0
f.计算地磁场矢量修正的滤波增益kmk。
kmk＝pgk(hmk)t(hmkpgk(hmk)t+rm′k)-1
g.基于地磁场矢量的姿态四元数状态
h.基于地磁场矢量的姿态四元数状态估计均方误差pmk。
pmk＝(i-kmkhmk)pgk
其中，i表示单位矩阵。
本发明的优点是：该算法利用地球物理场(地球重力场、地磁场)信息对更新得到的姿态四元数进行了两次序贯最优估计。在该算法中，不需向系统提供先验地磁场信息，只需提供先验重力加速度矢量，克服了上述地磁场在应用中的不足。
附图说明
图1是重力加速度矢量在全局坐标系g与载体坐标系b中的示意图；
图2是地磁场矢量在全局坐标系g与载体坐标系b中的示意图；
图3是全局坐标系g的构建示意图；
图4是本发明实施例提供的一种基于地球重力场与地磁场序贯修正的姿态解算方法流程图；
图5是在ros验证平台下，利用本发明输出的载体姿态四元数波形图；
图6是在ros验证平台下，利用本发明输出的载体姿态欧拉角波形图；
图7是本发明提出的算法与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角的俯仰角输出波形对比图；
图8是本发明提出的算法与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角的横滚角输出波形对比图；
图9是本发明提出的算法与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角的航向角输出波形对比图。
具体实施方式
下面结合附图对本发明做清楚完整的描述，以使本领域的技术人员在不需要作出创造性劳动的条件下，能够充分实施本发明。
如图1-4所示，本发明实施例提供的一种基于地球重力场与地磁场序贯修正的姿态解算方法，使用ah-100bahrs传感器输出的三轴陀螺、三轴加速度、三轴磁场计数据，分别为ω、a和m，包括以下几个步骤：
步骤1：载体初始姿态四元数初始化，初始姿态四元数均方误差初始化；
步骤2：读取陀螺仪数据，并进行载体姿态四元数更新；
步骤3：读取加速度计数据，并对步骤2中更新的姿态四元数进行第一次修正；
步骤4：读取磁力计数据，并对步骤3中一次修正的姿态四元数进行第二次修正。
作为本发明的一个实施例，步骤1中姿态四元数与均方误差矩阵初始化为
作为本发明的一个实施例，步骤2中读取陀螺仪数据，并进行载体姿态四元数更新，具体通过以下步骤实现：
(2a)在载体坐标系b，下从陀螺仪中获得载体角速度信息
(2b)根据所述载体角速度信息计算系统状态转移矩阵
(2c)陀螺仪噪声协方差矩阵设置
(2d)计算系统噪声协方差矩阵
(2e)载体姿态四元数状态一步预测
(2f)姿态四元数状态一步预测均方误差
作为本发明的一个实施例，步骤3中读取加速度计数据，并对步骤2中更新的姿态四元数进行第一次修正，具体通过以下步骤实现：
(3a)在载体坐标系b下从加速度计中获得载体角速度信息a＝[axayaz]t；
(3b)根据所述载体重力加速度信息及先验重力加速度计算量测矩阵
(3c)加速度计噪声协方差矩阵设置
(3d)计算重力加速度量测噪声协方差矩阵
(3e)计算重力加速度修正的滤波增kgk＝pk/k-1(hgk)t(hgkpk/k-1(hgk)t+rg′k)-1；
(3f)估计基于重力加速度修正的姿态四元数
(3g)基于重力加速度的姿态四元数状态估计均方误差pgk＝(i-kgkhgk)pk/k-1。
作为本发明的一个实施例，步骤4中读取磁力计数据，并对步骤3中一次修正的姿态四元数进行第二次修正，具体通过以下步骤实现：
(4a)在载体坐标系b下从磁力计中获得载体角速度信息m＝[mxmymz]t；
(4b)计算先验地磁场矢量h；
(4c)根据所述载体地磁场矢量信息及先验地磁场矢量计算量测矩阵
(4d)磁力计噪声协方差矩阵设置
(4e)计算地磁场矢量量测噪声协方差矩阵
(4f)计算地磁场矢量修正的滤波增kmk＝pgk(hmk)t(hmkpgk(hmk)t+rm′k)-1；
(4g)估计基于地磁场矢量的姿态四元数状态
(4h)基于地磁场矢量的姿态四元数状态估计均方误差pmk＝(i-kmkhmk)pgk。
利用该姿态解算算法解算出的载体姿态四元数输出波形如图5所示，其w、x、y分量的精度为0.001，z分量的精度为0.002。载体姿态欧拉角输出波形如图6所示，其横滚与俯仰分量的精度为0.1°，航向分量的精度为0.2°。
将本发明与基于互补滤波器、扩展卡尔曼滤波器、自适应扩展卡尔曼滤波器姿态解算算法的姿态欧拉角输出波形进行对比，对比图如图7-9所示。
以上对本发明的较佳实施例进行了描述，需要指出的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。