当前位置: 首页 > 专利查询>李志专利>正文

一种基于地球重力场与地磁场序贯修正的姿态解算方法技术

技术编号:19564367 阅读:39 留言:0更新日期:2018-11-25 01:18
本发明专利技术提供一种基于地球重力场与地磁场序贯修正的姿态解算方法,其步骤包括如下:(1)根据航姿系统中陀螺仪采集的角速率计算系统模型的状态转移矩阵,并利用离散化的四元数微分方程更新载体姿态四元数;(2)根据航姿系统中加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正;(3)根据航姿系统中磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。本发明专利技术利用地球物理场信息对更新得到的姿态四元数进行了两次序贯最优估计,在该算法中,不需向系统提供先验地磁场信息,只需提供先验重力加速度矢量,克服了上述地磁场在应用中的不足。

A Method of Attitude Solution Based on Sequential Modification of Earth's Gravity Field and Geomagnetic Field

The present invention provides an attitude calculation method based on the sequential correction of gravity field and geomagnetic field. The steps include: (1) calculating the state transition matrix of the system model according to the angular rate collected by gyroscope in the attitude system, and updating the carrier attitude quaternion by using discrete Quaternion Differential equation; (2) updating the carrier attitude quaternion according to the attitude system. The attitude quaternion updated in step (1) is corrected by the principle of minimum mean square error; (3) the attitude quaternion corrected in step (2) is corrected by the principle of minimum mean square error according to the vector of carrier geomagnetic field collected by magnetometer in the attitude system. (3) The attitude quaternion corrected in step (2) is corrected by the principle of minimum mean square error. The method uses geophysical field information to estimate the updated attitude quaternion by two sequential optimum estimates. In the algorithm, the prior geomagnetic field information is not needed to the system, but the prior gravity acceleration vector is provided to overcome the shortcomings of the above geomagnetic field in application.

【技术实现步骤摘要】
一种基于地球重力场与地磁场序贯修正的姿态解算方法
本专利技术属于定位导航
,具体涉及一种基于地球重力场与地磁场序贯修正的姿态解算方法。技术背景航姿参考系统(AttitudeandHeadingReferenceSystem,AHRS)中集成了MEMS传感器(陀螺仪、加速度计、磁力计),其有成本低、集成度高以及体积小等优点,但是在进行姿态解算时,却存在输出噪声过大、零点漂移无法完全消除、角速率输出易受干扰等所导致的姿态解算精度低、累计误差大等问题。因此,通常情况下,需要采用多个传感器经过信息融合算法实现高精度的姿态解算。相较于利用欧拉角进行姿态解算,四元数作为一种非常有用的数学工具,由于其参数少且避免了欧拉角产生的奇异,在现代姿态解算应用中,得到了广泛的应用。地磁场作为地球固有的物理特征,在导航中的应用越来越广泛,但是地磁场易受周围建筑物及钢性结构的影响,在地球表面的不同位置,地磁场矢量各不相同且未知,大大限制的地磁场在导航中的应用。
技术实现思路
为解决以上技术问题本专利技术提供一种基于地球重力场与地磁场序贯修正的姿态解算方法。本专利技术的技术方案是:一种基于地球重力场与地磁场序贯修正的姿态解算方法,包括如下三个步骤:(1)根据航姿系统(AHRS)中MEMS陀螺仪采集的角速率计算系统模型的状态转移矩阵,并利用离散化的四元数微分方程更新载体姿态四元数;(2)根据航姿系统(AHRS)中MEMS加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正;(3)根据航姿系统(AHRS)中MEMS磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。详细地,步骤(1)具体包括:a.建立陀螺仪动力学模型:即其中,X=[X0X1X2X3]T表示全局坐标系G到载体坐标系B的旋转四元数,表示在载体坐标系中测量的MEMS陀螺角速度信息,且利用四元数微分方程的比卡求解法,使上式离散化可得其中,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;表示姿态四元数Xk的逆。上式经过变换可得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>0e.计算重力加速度修正的滤波增益KgkKgk=Pk/k-1(Hgk)T(HgkPk/k-1(Hgk)T+Rg′k)-1f.基于重力加速度的姿态四元数状态估计。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+ly2d.根据所述载体地磁场矢量信息及先验地磁场矢量计算量测矩阵e.计算地磁场矢量量测噪声协方差矩阵Rm′k。设磁力计的输出噪声为nmag,则有nmag=m-m°其中,m°表示载体坐标系下加速度计的真实值。实际量测矩阵与理想量测矩阵误差为其中,表示理想量测矩阵。把上式代入步骤a中的磁力计观测模型,可得0=HmkXk-ΔHmkXk-ΔHmkXk变形为其中,令则量测噪声协方差矩阵为其中,Σmag为磁力计噪声协方差矩阵。上式得到的量测噪声协方差矩阵在应用的过程中很有可能是奇异的,为了避免矩阵的奇异,需要对该量测噪声协方差矩阵进行改进,即Rm′k=Rmk+βmIβm>0f.计算地磁场矢量修正的滤波增益Kmk。Kmk=Pgk(Hmk)T(HmkPgk(Hmk)T+Rm′k)-1g.基于地磁场矢量的姿态四元数状态估计。h.基于地磁场矢量的姿态四元数状态估计均方误差Pmk。Pmk=(I-KmkHmk)Pgk其中,I表示单位矩阵。本专利技术的优点是:该算法利用地球物理场(地球重力场、地磁场)信息对更新得到的姿态四元数进行了两次序贯最优估计。在该算法中,不需向系统提供先验地磁场信息,只需提供先验重力加速度矢量,克服了上述地磁场在应用中的不足。附图说明图1是重力加速度矢量在全局坐标系G与载体坐标系B中的示意图;图2是地磁场矢量在全局坐标系G与载体坐标系B中的示意图;图3是全局坐标系G的构建示意图;图4是本专利技术实施例提供的一种基于地球重力场与地磁场序贯修正的姿态解算方法流程图;图5是在ROS验证平台下,利用本专利技术输出的载体姿态四元数波形图;图6是在ROS验证平台下,利用本专利技术输出的载体姿态欧拉角波形图;图7是本专利技术提出的算法与基于互补滤波器、扩展卡尔本文档来自技高网...

【技术保护点】
1.一种基于地球重力场与地磁场序贯修正的姿态解算方法,其特征在于,具体步骤为:(1)根据航姿系统中MEMS陀螺仪采集的角速率计算系统模型的状态转移矩阵,并利用离散化的四元数微分方程更新载体姿态四元数;(2)根据航姿系统中MEMS加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正;(3)根据航姿系统中MEMS磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。

【技术特征摘要】
1.一种基于地球重力场与地磁场序贯修正的姿态解算方法,其特征在于,具体步骤为:(1)根据航姿系统中MEMS陀螺仪采集的角速率计算系统模型的状态转移矩阵,并利用离散化的四元数微分方程更新载体姿态四元数;(2)根据航姿系统中MEMS加速度计采集的载体重力加速度对步骤(1)中更新得到的姿态四元数由均方误差最小原则进行修正;(3)根据航姿系统中MEMS磁力计采集的载体地磁场矢量对步骤(2)中修正得到的姿态四元数由均方误差最小原则进行第二次修正。2.根据权利要求1所述的一种基于地球重力场与地磁场序贯修正的姿态解算方法,其特征在于,步骤(1)的具体过程是:a.建立陀螺仪动力学模型:即其中,X=[X0X1X2X3]T表示全局坐标系G到载体坐标系B的旋转四元数,表示在载体坐标系中测量的MEMS陀螺角速度信息,且利用四元数微分方程的比卡求解法,使上式离散化可得其中,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)中的状态估计均方误差。3.根据权利要求1所述的一种基于地球重力场与地磁场序贯修正的姿态解算方法,其特征在于,步骤(2)的具体过程是:a.建立加速度计观测模型:将三维矢量g与a看作零标量的四元数,则g与a间的变换关系可用四元数乘法表示为其中,g为在全局坐标系G中测量的重力加速度,a为在载体坐标系B中测量的重力加速度,且满足||g||=||a||=1;表示姿态四元数Xk的逆;上式经过变换可得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...

【专利技术属性】
技术研发人员:李志王录汪璟倪瑞
申请(专利权)人:李志王录汪璟倪瑞
类型:发明
国别省市:山西,14

网友询问留言 已有0条评论
  • 还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。

1