CN106052685B - 一种两级分离融合的姿态和航向估计方法 - Google Patents

一种两级分离融合的姿态和航向估计方法 Download PDF

Info

Publication number
CN106052685B
CN106052685B CN201610450750.8A CN201610450750A CN106052685B CN 106052685 B CN106052685 B CN 106052685B CN 201610450750 A CN201610450750 A CN 201610450750A CN 106052685 B CN106052685 B CN 106052685B
Authority
CN
China
Prior art keywords
equation
angle
attitude
vector
course
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201610450750.8A
Other languages
English (en)
Other versions
CN106052685A (zh
Inventor
张生志
刘超军
罗璋
余帅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan Yuansheng Innovative Technology Co Ltd
Original Assignee
Wuhan Yuansheng Innovative Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhan Yuansheng Innovative Technology Co Ltd filed Critical Wuhan Yuansheng Innovative Technology Co Ltd
Priority to CN201610450750.8A priority Critical patent/CN106052685B/zh
Publication of CN106052685A publication Critical patent/CN106052685A/zh
Application granted granted Critical
Publication of CN106052685B publication Critical patent/CN106052685B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Abstract

本发明公开了一种两级分离融合的姿态和航向估计方法，包括：建立系统的卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；f为前后时刻状态向量之间的传递函数；解算上述卡尔曼滤波方程得到状态向量根据计算得到俯仰角θ和横滚角φ。本发明方法将姿态角和航向角的解算进行两级分离计算，构建的线性滤波方程可以避免扩展卡尔曼滤波和无迹卡尔曼滤波等非线性滤波方法的使用，将极大降低系统计算复杂度。同时，第一级卡尔曼滤波负责利用陀螺仪和加速度计进行姿态角的解算，磁传感器不参与第一级滤波，排除了磁传感器所受干扰对于姿态角估计的影响。

Description

一种两级分离融合的姿态和航向估计方法
技术领域
本发明属于惯性导航技术领域，更具体地，涉及一种两级分离融合的姿态和航向估计方法。
背景技术
在航空航天、车辆导航、机器人和平台稳定控制等诸多领域，航向与姿态是实现运动物体控制的关键参数。对于飞机，没有准确的实时航向与姿态信息，飞机就会失去控制，不能平稳而安全地到达目的地；对于卫星，没有准确的实时姿态信息，就不能按照程序打开太阳能电池帆板，严重影响卫星的定位；对于机器人，需要及时获取自身的姿态信息，并反馈给“大脑”(机器人控制程序)，进而按照程序执行任务，例如越过障碍或者做出特定的动作。航向姿态参考系统(Attitude and heading reference system，AHRS)利用惯性传感器，如陀螺仪、加速度计等，测量物体的惯性参数，通过特定的数据融合算法得到运动物体的姿态和航向参数。航姿参考系统本身不对外发送任何光、电、声等信号，也不接收外界信号，属于完全自主工作，隐蔽性好，抗干扰能力强，可全地域、全天时、全天候工作。
以往的平台式惯性导航系统需要复杂的物理平台，体积大，重量大，可靠性差且不易于维护。捷联惯性导航系统依靠设定的导航坐标系构建姿态解算算法，省去了复杂的物理结构平台。并且随着微机电系统(Micro-Electro-Mechanical System，MEMS)技术的不断发展，基于MEMS技术的陀螺仪、加速度计等惯性传感器的精度越来越高，体积越来越小而且成本越来越低，使得捷联惯导系统的发展更加迅速，应用范围更加广泛。
姿态更新解算是捷联惯导系统的关键算法，传统的姿态更新算法有欧拉角法、方向余弦法和四元数法。其中四元数法由于算法简单、计算量小被广泛应用于工程实际当中。MEMS陀螺仪存在固有零漂，长时间进行积分运算存在累积误差，不便于单独使用。加速度计和地磁传感器组成的电子罗盘可以解算准静态下物体的欧拉姿态角，由于此方法不需要进行积分运算，因此不存在累积误差，但是解算的姿态角动态响应差，无法在动态条件下应用。目前国内外已经开发多种将两类器件相结合的数据融合方法，出现了包括扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、模糊算法、互补算法在内的多种数据融合方法，使得可以通过MEMS陀螺仪、加速度计和地磁传感器测量数据融合得到精确的被测物体姿态和航向。
如附图1所示，传统的航姿参考系统(AHRS)通常采用非线性的扩展卡尔曼滤波(EKF)算法对陀螺仪、加速度计和磁传感器的输出进行数据融合，为控制和导航系统提供运动物体相对于惯性坐标系的姿态和航向信息。相比于线性卡尔曼滤波算法，EKF对非线性函数的Taylor展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性应用于非线性系统估计中。然而该方法也带来了两个缺点：首先，当状态方程和测量方程属于强非线性时EKF违背局部线性假设，Taylor展开式中被忽略的高阶项将带来大的误差，此时EKF算法可能会使滤波发散；另外，由于EKF在线性化处理时需要用雅克比(Jacobian)矩阵，并且涉及高阶矩阵求逆，其繁琐的计算过程导致算法实现相对困难。EKF算法中的这些复杂运算增加了系统航姿信息的解算时间，降低了系统航姿信息的更新计算频率，对系统硬件中信号处理单元的处理能力有较高的要求。
此外，现有技术在进行航向和姿态估计时，将陀螺仪、速度计及磁传感器同时用于航向和姿态解算，而姿态角(俯仰角和横滚角)的解算并不需要磁传感器，仅解算航向角的时候需要用到磁传感器。这无疑增加了系统的数据处理负荷。而且，姿态系统中集成的磁传感器很容易受到周围附加磁场的干扰，将加速度计和磁传感器的输出值同时带入滤波器融合，会对姿态角的解算产生不利影响，降低估计精度。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提供了一种两级分离融合的姿态和航向估计方法。其目的在于通过构建线性的卡尔曼滤波方程，降低系统计算复杂度、减少时间消耗，由此解决现有估计方法中计算复杂度较高、计算时间消耗较多的的技术问题。
为实现上述目的，本发明提供了一种两级分离融合的姿态和航向估计方法，采用两级分离的卡尔曼滤波进行陀螺仪和加速度计的数据融合，包括：
建立航姿参考系统的线性卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示航姿参考系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；f为前后时刻状态向量之间的传递函数；
解算上述卡尔曼滤波方程得到状态向量
根据上述状态向量计算得到俯仰角θ和横滚角φ，其中：
特别注意的是，本发明的一个实施例中，所述卡尔曼滤波状态方程和观测方程均为线性方程。
本发明的一个实施例中，所述方法还包括：
结合磁传感器测量值得到航向角ψ，其中
本发明的一个实施例中，将时序k对应的载体坐标系下重力分量作为状态向量xk。
本发明的一个实施例中，将时序k对应的加速度计输出向量选定为观测向量yk。
与现有技术普遍采用的EKF和UKF方法相比，本发明具有以下有益效果：
1、采用线性卡尔曼滤波方法进行姿态解算，避免高阶矩阵的求逆运算或者无迹变换，降低系统计算复杂度，提高系统姿态更新频率；
2、相比于使用EKF或者UKF同时求解姿态和航向角，本发明技术方案将姿态角和航向角分离的两级滤波方法排除了磁场干扰对姿态角的影响，提高姿态角解算精度；
3、两级分离计算方法提高了系统配置的灵活性，可根据是否需要系统提供航向信息输出而选择是否将磁传感器集成到系统；
综上所述，本发明提供了一种两级分离融合的姿态和航向估计方法，将姿态角和航向角的解算进行两级分离计算，可以避免EKF和UKF等非线性滤波方法的使用，将极大降低系统计算复杂度。同时，第一级卡尔曼滤波负责利用陀螺仪和加速度计进行姿态角的解算，磁传感器不参与第一级滤波，排除了磁传感器测量值对于姿态角估计的影响。将得到的姿态角带入下一级的航向角解算，在保证航向角解算精度的同时，极大地减少计算时间消耗，有效提高系统的航姿信息更新计算频率。
附图说明
图1是现有技术中采用非线性的扩展卡尔曼滤波算法进行数据融合得到姿态和航向信息的原理示意图；
图2是本发明实施例中地理坐标系与载体坐标系以及三轴姿态角的关系示意图；
图3为本发明实施例中一种两级分离融合的姿态和航向估计方法的原理示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
目前已有的一种用于MEMS陀螺仪、加速度计和磁传感器的数据融合EKF方法的主要原理如下：
首先建立四元数形式的陀螺仪运动方程：
其中，ω＝[ωx ωy ωz]为陀螺仪三轴输出值，q＝[q0 q1 q2 q3]T为姿态四元数,为q的导数。将四元数向量x＝[q0 q1 q2 q3]T作为扩展卡尔曼滤波的状态向量，四元数微分方程作为扩展卡尔曼滤波状态方程。
进一步地，利用载体坐标系b与导航坐标系n的变换关系：
其中，(xb yb zb)为载体坐标系b下的物理量，(xn yn zn)为导航坐标系n下的物理量，为两坐标系间的方向余弦矩阵。
如附图2所示地理坐标系(NED系)与载体坐标系(XbYbZb系)以及三轴姿态角[φθψ]的关系图，载体坐标系为XbYbZb，导航坐标系为XnYnZn，加速度计和磁传感器测量值满足：
其中，和分别为加速度计和磁传感器的观测向量，和分别为重力矢量和地磁场矢量。将加速度计和磁传感器测量值作为观测向量y，建立卡尔曼滤波测量方程。得到航向姿态参考系统扩展卡尔曼滤波方程如下：
其中，为状态向量x的导数，矩阵A为状态向量传递矩阵，h为状态向量与观测向量之间的传递函数，w、v分别为过程噪声和测量噪声。对于实时计算，状态方程和测量方程离散化为：
其中，f为状态向量间的传递函数。通过扩展卡尔曼滤波器迭代得到实时姿态四元数。用四元数表示坐标变换矩阵为：
最终得到被测物体航向姿态信息：
其中Ci,j为的第i行、j列元素，φ,θ,ψ分别表示运动物体的横滚角、俯仰角和航向角。
从上述已有的技术方案来看，目前的航姿参考系统主要采用EKF和UKF等非线性滤波进行陀螺仪、加速度计和磁传感器数据融合，增加了系统复杂度，消耗更多的计算时间。同时磁传感器受到的干扰也会被引入到姿态角解算，降低了姿态角解算精度。针对以上缺点，本发明的目的是通过构建线性的卡尔曼滤波方程，降低系统计算复杂度，减少时间消耗。同时，姿态角和航向角分离的两级计算排除了受干扰的磁传感器测量值对姿态角的影响，提高了姿态角解算精度。并且增加了系统设计的灵活性，可根据是否需要系统提供航向信息输出而选择是否将磁传感器集成到系统。
如图3所示，本发明技术方案采用两级分离的卡尔曼滤波进行陀螺仪、加速度计和磁传感器的数据融合。具体实施过程为：利用载体坐标系b与导航坐标系n之间物理量的变换关系
其中，和分别为导航坐标系n与载体坐标系b中的物理量。
将式(1)两边求导得到
其中，和分别为和的导数，为的导数。
根据捷联惯性导航基本理论，坐标变换矩阵满足
其中，为物体运动角速度构成的反对称矩阵
将式(3)带入式(2)得到
则将载体坐标系下重力分量作为状态向量x，满足式(5)
其中，为载体坐标系b下的重力分量，为的导数。
将加速度计输出向量选定为观测向量y
根据式(6)和式(7)建立系统的卡尔曼滤波状态方程和测量方程：
x和y分别表示系统的状态向量和观测向量。w和v分别为状态方程过程噪声和观测方程测量噪声。对于实时计算，状态方程和测量方程离散化为：
其中xk和yk分别表示航姿参考系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；A表示状态向量之间的传递矩阵。
与已有方法不同的是，式(9)得到系统的线性卡尔曼滤波方程，通过解算得到状态向量进而得到俯仰角θ和横滚角φ：
结合磁传感器测量值得到航向角：
可以看出本发明将姿态和航向进行分离估计，排除了传统卡尔曼滤波融合算法中磁传感器受到干扰后对姿态估计的不利影响。磁场畸变仅仅影响航向估计，对姿态角估计没有影响，提高了姿态角的解算精度。同时，两级分离计算中涉及的矩阵的最高阶数为三阶，降低了系统迭代计算的复杂度。而且三阶矩阵求逆可以使用简单公式，提高了姿态估计的计算速度。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种两级分离融合的姿态和航向估计方法，其特征在于，采用两级分离的卡尔曼滤波进行陀螺仪和加速度计的数据融合，第一级卡尔曼滤波负责利用陀螺仪和加速度计进行姿态角的解算，磁传感器不参与第一级滤波，包括：
建立航向和姿态参考系统的卡尔曼滤波状态方程和测量方程：其中xk和yk分别表示航姿参考系统的状态向量和观测向量，wk和vk分别为状态方程过程噪声和观测方程测量噪声，k表示本次计算的时序；A表示状态向量之间的传递矩阵；
解算上述卡尔曼滤波方程得到状态向量 为载体坐标系b下的重力分量；
根据上述状态向量计算得到俯仰角θ和横滚角φ，其中：
结合磁传感器测量值得到航向角ψ，其中
2.如权利要求1所述的方法，其特征在于，卡尔曼滤波状态方程和观测方程均为线性方程。
3.如权利要求1或2所述的方法，其特征在于，将时序k对应的载体坐标系下的重力分量作为状态向量xk。
4.如权利要求1或2所述的方法，其特征在于，将时序k对应的加速度计输出向量选定为观测向量yk。
CN201610450750.8A 2016-06-21 2016-06-21 一种两级分离融合的姿态和航向估计方法 Active CN106052685B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610450750.8A CN106052685B (zh) 2016-06-21 2016-06-21 一种两级分离融合的姿态和航向估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610450750.8A CN106052685B (zh) 2016-06-21 2016-06-21 一种两级分离融合的姿态和航向估计方法

Publications (2)

Publication Number Publication Date
CN106052685A CN106052685A (zh) 2016-10-26
CN106052685B true CN106052685B (zh) 2019-03-12

Family

ID=57169188

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610450750.8A Active CN106052685B (zh) 2016-06-21 2016-06-21 一种两级分离融合的姿态和航向估计方法

Country Status (1)

Country Link
CN (1) CN106052685B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106419930A (zh) * 2016-11-30 2017-02-22 深圳市酷浪云计算有限公司 运动健康管理平台及运动智能装备
CN106885568B (zh) * 2017-02-21 2020-09-01 北京京东尚科信息技术有限公司 无人机数据处理方法和装置
CN109211226A (zh) * 2017-06-29 2019-01-15 沈阳新松机器人自动化股份有限公司 一种基于mems运动传感器二维姿态解算的方法及装置
US10462608B1 (en) 2017-07-31 2019-10-29 Agero, Inc. Estimating orientation of a mobile device with respect to a vehicle using global displacement information and local motion information
CN107607113B (zh) * 2017-08-02 2020-03-17 华南农业大学 一种两轴姿态倾角测量方法
CN107860382B (zh) * 2017-11-07 2021-04-06 吉林大学 一种在地磁异常情况下应用ahrs测量姿态的方法
CN108068823A (zh) * 2017-12-06 2018-05-25 上海评驾科技有限公司 一种车辆驾驶行为检测方法
CN108519090B (zh) * 2018-03-27 2021-08-20 东南大学—无锡集成电路技术研究所 一种基于优化的ukf算法的双通道组合定姿算法的实现方法
CN108592917B (zh) * 2018-04-25 2021-02-23 珠海全志科技股份有限公司 一种基于失准角的卡尔曼滤波姿态估计方法
CN110873889B (zh) * 2019-11-19 2021-09-24 中国人民解放军海军工程大学 贯穿辐射应急探测模拟系统及方法
CN111045040A (zh) * 2019-12-09 2020-04-21 北京时代民芯科技有限公司 一种适用于动态弱信号的卫星导航信号跟踪系统及方法
CN112504275B (zh) * 2020-11-16 2022-09-02 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN113625214B (zh) * 2021-08-10 2023-08-29 广东工业大学 一种磁力计电磁干扰诊断滤波方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000245A (zh) * 2007-01-10 2007-07-18 北京航空航天大学 一种sins/gps/磁罗盘组合导航系统的数据融合方法
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN101839719A (zh) * 2010-05-16 2010-09-22 中北大学 一种基于陀螺、地磁传感器的惯性测量装置
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN104406586A (zh) * 2014-12-04 2015-03-11 南京邮电大学 基于惯性传感器的行人导航装置和方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR2961897B1 (fr) * 2010-06-25 2012-07-13 Thales Sa Filtre de navigation pour un systeme de navigation par correlation de terrain
FR3028031B1 (fr) * 2014-10-29 2019-09-20 Safran Electronics & Defense Procede d'estimation d'un etat de navigation contraint en observabilite

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000245A (zh) * 2007-01-10 2007-07-18 北京航空航天大学 一种sins/gps/磁罗盘组合导航系统的数据融合方法
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN101839719A (zh) * 2010-05-16 2010-09-22 中北大学 一种基于陀螺、地磁传感器的惯性测量装置
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN104406586A (zh) * 2014-12-04 2015-03-11 南京邮电大学 基于惯性传感器的行人导航装置和方法

Also Published As

Publication number Publication date
CN106052685A (zh) 2016-10-26

Similar Documents

Publication Publication Date Title
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN102654404B (zh) 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN107478223A (zh) 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN108225308A (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN103512584A (zh) 导航姿态信息输出方法、装置及捷联航姿参考系统
CN105929836B (zh) 用于四旋翼飞行器的控制方法
CN110017837A (zh) 一种姿态抗磁干扰的组合导航方法
CN109084760B (zh) 一种楼宇间导航系统
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN109764870B (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN106840151A (zh) 基于时延补偿的无模型船体变形测量方法
CN109000639B (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
Gao et al. An integrated land vehicle navigation system based on context awareness
CN108592943A (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
Wang et al. Attitude determination method by fusing single antenna GPS and low cost MEMS sensors using intelligent Kalman filter algorithm
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN104634348B (zh) 组合导航中的姿态角计算方法
Weibel et al. Small unmanned aerial system attitude estimation for flight in wind
CN110375773B (zh) Mems惯导系统姿态初始化方法
Huang et al. Study on INS/DR integration navigation system using EKF/RK4 algorithm for underwater gliders
CN103616026A (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant