CN107525503A - 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 - Google Patents

基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 Download PDF

Info

Publication number
CN107525503A
CN107525503A CN201710731641.8A CN201710731641A CN107525503A CN 107525503 A CN107525503 A CN 107525503A CN 201710731641 A CN201710731641 A CN 201710731641A CN 107525503 A CN107525503 A CN 107525503A
Authority
CN
China
Prior art keywords
mtd
msub
mrow
mover
mtr
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.)
Granted
Application number
CN201710731641.8A
Other languages
English (en)
Other versions
CN107525503B (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.)
Individual
Original Assignee
Individual
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 Individual filed Critical Individual
Priority to CN201710731641.8A priority Critical patent/CN107525503B/zh
Publication of CN107525503A publication Critical patent/CN107525503A/zh
Application granted granted Critical
Publication of CN107525503B publication Critical patent/CN107525503B/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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Magnetic Resonance Imaging Apparatus (AREA)

Abstract

基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，本发明涉及自适应级联卡尔曼滤波方法。本发明为了解决现有技术计算量大以及在大失准角情况下与GPS失锁过程中的测姿精度低的问题。本发明通过采用自适应姿态滤波并串联一个速度位置滤波器提高姿态、速度、位置等导航信息的精度。利用修正后的姿态信息以及惯导系统与GPS提供的速度、位置信息之间的误差量，建立基于速度、位置误差方程的速度位置滤波器。对速度、位置误差以及加速度计的漂移特性进行估计，改善速度位置等导航信息。本发明通过降低维数减小计算负担，有效克服了航向大失准角以及GPS失锁情况下对载体姿态估计偏差大的缺陷。本发明用于组合导航领域。

Description

基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法
技术领域
本发明涉及组合导航领域，特别涉及双天线GPS和MIMU组合导航领域。
背景技术
基于MEMS(Micro-Electro-Mechanical Systems)的惯性测量单元(MicroInertial Measurement Unit,MIMU)具有体积小、成本低等一系列优势，使得其应用前景越来越广阔。然而MEMS惯性器件精度较低，导致在短时间内引起较大的估计偏差，因此通常采用GPS/MEMS组合导航，从而获得长时间的精确导航信息。
针对基于MEMS工艺的惯性器件无法实现自对准，为获得初始航向角，通常采用双天线GPS与MIMU组合，为载体在静态与动态情况下，提供较为准确的航向信息；由于MIMU精度较低，通常将姿态角误差考虑为大方位角误差，因此基于双天线GPS/MIMU组合导航系统，通常采用非线性模型和相应的非线性卡尔曼滤波方法。如文献(SAMAN M S.IntegratedNavigation and Self alignment using Square Root Unscented Kalman Filtering[C]International Bhurban Conference on Applied Science&Technology(IBCAST).2013:73-76.)采用基于姿态角、速度、位置误差方程的非线性滤波算法，这种方法取得了较好的滤波效果，但双天线GPS提供的航向信息没有得到充分运用；文献(WU Z W,YAO M L,MA HG.Low-cost attitude estimation with MIMU and two-antenna GPS for Satcom-on-the-move[J].GPS Solutions,2013,17:75-87.)提出了基于姿态四元数微分方程的非线性KF算法，通过对四元数估计，从而校正陀螺漂移，这种算法充分利用了双天线GPS航向信息，但由于直接采取将导航参数作为状态量的直接法滤波，这些状态变量的数值量级大、变化快，而且会严重影响各个状态的估计精度，造成滤波精度下降甚至发散的问题；文献(李开龙，胡柏青,常路宾.改进四元数无味卡尔曼滤波算法[J].系统工程与电子技术,2016,38(6):1399-1404.)介绍了基于四元数误差方程的非线性算法，通过对四元数误差估计，该方法利用间接滤波的方法，适用于三个姿态误差角是任意角的情况，并充分利用了双天线GPS提供的航向信息；对于上述文献所涉及的算法中，由于不仅引入非线性关系、而且状态维数高，给计算带来了巨大负担。同时这些方法在GPS失效，无法提供速度位置等量测量时，便无法进行滤波处理，给导航精度带来很大影响。
针对GPS失锁情况下，改善导航精度问题，文献(CHEN C,LI X,SONG X,et al.Anovel fusion methodology to bridge GPS outages for land vehicle positioning[J].Measure Science and Technology,2015,26(7):561-568.)将机器学习算法应用于解决GPS失锁问题，取得了较好的滤波效果，但是需要长时间的学习以及大量的训练样本。目前在GPS失锁情况下，利用磁强计代替GPS对航向信息进行修正，由于磁强计解算的航向角存在较大误差，同时易受周围环境磁场的干扰，噪声较大，给滤波带来较大难度。
发明内容
本发明的目的是为了解决现有技术计算量大以及在大失准角情况下与GPS失锁过程中的测姿精度低的问题，而提出基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法。
一种基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法包括以下步骤：
本发明的目的是在降低计算量的同时，提高在大失准角情况下与GPS失锁过程中的测姿精度。本发明提出一种自适应串联的卡尔曼滤波算法。该方法包括自适应姿态滤波器与速度位置滤波器。其中，姿态滤波利用加性四元数误差与姿态误差角之间的等价方程,以及陀螺仪、加速度计、GPS获取的姿态信息对四元数误差进行修正，从而提高测姿精度,获取较高精度的姿态信息与姿态矩阵。利用修正后的姿态矩阵，结合惯性导航系统与GPS提供的速度位置信息，建立基于速度位置误差的滤波算法，对速度位置误差进行估计，对速度位置进行反馈校正，从而获取准确的速度位置信息。
为充分利用MEMS与双天线GPS提供的导航信息，减少计算负担，改善方位大失准角以及GPS失锁造成的导航精度降低等问题，本发明提出一种自适应姿态卡尔曼与速度位置卡尔曼级联滤波的算法。
步骤一：利用加性四元数误差的线性微分方程，建立基于加性四元数误差的状态方程；
步骤二：利用惯性导航系统解算的载体的姿态角，并将载体的姿态角与加速度计和GPS提供的姿态角作差，得到载体的姿态误差角；所述载体为车辆、飞机和船舶等运载体，姿态角包括俯仰角、横滚角和航向角；加速度计解算出俯仰角与横滚角，GPS提供航向角；
步骤三：根据姿态误差角与加性四元数误差之间的转换关系，建立观测方程；
步骤四：根据GPS工作状态对卡尔曼滤波算法做出自适应调整，调整后的卡尔曼滤波算法对加性四元数误差进行估计，对步骤二中惯性导航系统解算的载体的姿态角进行修正，得到修正后的姿态角；
步骤五：根据载体的速度和位置误差方程，建立基于速度和位置的误差量的状态方程；
步骤六：将惯性导航系统解算的速度和位置与GPS提供的速度和位置的误差量作为观测量，建立观测方程；
步骤七：利用步骤五中建立的状态方程和步骤六中建立的观测方程，根据卡尔曼滤波算法对载体速度和位置的误差进行估计，校正载体速度与位置信息。
所述MIMU为惯性测量单元。
本发明的有益效果为：
1、与传统的基于双天线GPS/MIMU组合的滤波算法相比，串联的自适应滤波算法在保证精确的导航精度的同时极大地减少了运算负担。卡尔曼滤波算法的计算量正比于n3+n2m，其中n为卡尔曼滤波算法中的状态量维数，m为卡尔曼滤波算法中的观测量维数，本发明所设计的级联卡尔曼滤波算法中各自的状态量维数分别为7维和9维，量测维数为4维和6维，根据计算量正比于的计算公式，本发明的算法计算量正比于1754；相比于传统线性卡尔曼滤波算法的15维状态量和6维观测量，其计算量正比于4725；若采用非线性卡尔曼滤波算法，计算量会更大。因此，采用本发明算法的计算量会大大减小，能够有效地降低运算时间、提高运算速度。
2、本发明所设计的算法适用于失准角是航向大失准角的情况。通过仿真实验验证，在航向失准角为20°的情况下，经过算法的信息融合，能够使得在30s～60s将航向大失准角由20°降到0.04°。
3、由于姿态滤波器于速度位置滤波器相对独立，即使在GPS失锁之后，也能对姿态角进行滤波处理，使得运载体无论在直线运动还是转弯运动情况下，姿态信息能够保持较好的精度。避免了在GPS失锁阶段，纯惯导系统的导航信息快速发散的问题。通过跑车试验，在GPS失锁100秒内、运动状态包含直行与转弯的情况下，经过本算法滤波后，俯仰角误差、横滚角误差、航向角误差峰值分别小于0.5°、0.5°、0.1°，而不使用滤波处理，仅依靠纯惯导解算姿态角，其俯仰角误差、横滚角误差、航向角误差峰值均超过1°。
附图说明
图1是仿真运动轨迹；
图2是仿真条件下不同子系统单独解算俯仰角的效果图；
图3是仿真条件下不同子系统单独解算横滚角的效果图；
图4是仿真条件下不同子系统单独解算航向角的效果图；
图5是仿真条件下双天线GPS/MIMU组合滤波后俯仰角效果图；
图6是仿真条件下滤波后俯仰角误差图；
图7是仿真条件下双天线GPS/MIMU组合滤波后横滚角效果图；
图8是仿真条件下滤波后横滚角误差图；
图9是仿真条件下双天线GPS/MIMU组合滤波后航向角效果图；
图10是仿真条件下滤波后航向角误差图；
图11是仿真条件下滤波后东向速度误差曲线；
图12是仿真条件下滤波后北向速度误差曲线；
图13是仿真条件下滤波后天向速度误差曲线；
图14是仿真条件下滤波后东向位置误差曲线；
图15是仿真条件下滤波后北向位置误差曲线；
图16是仿真条件下滤波后天向位置误差曲线；
图17是仿真条件下航向大失准角时航向角误差图；
图18是仿真条件下GPS失锁条件下俯仰角误差图；
图19是仿真条件下GPS失锁条件下横滚角误差图；
图20是仿真条件下GPS失锁条件下航向角误差图；
图21是跑车实验运动轨迹；
图22是跑车条件下不同子系统单独解算俯仰角的效果图；
图23是跑车条件下不同子系统单独解算横滚角的效果图；
图24是跑车条件下不同子系统单独解算航向角的效果图；
图25是跑车条件下双天线GPS/MIMU组合滤波后俯仰角效果图；
图26是跑车条件下滤波后俯仰角误差图；
图27是跑车条件下双天线GPS/MIMU组合滤波后横滚角效果图；
图28是跑车条件下滤波后横滚角误差图；
图29是跑车条件下双天线GPS/MIMU组合滤波后航向角效果图；
图30是跑车条件下滤波后航向角误差图；
图31是跑车条件下滤波后东向速度误差曲线；
图32是跑车条件下滤波后北向速度误差曲线；
图33是跑车条件下滤波后天向速度误差曲线；
图34是跑车条件下滤波后东向位置误差曲线；
图35是跑车条件下滤波后北向位置误差曲线；
图36是跑车条件下滤波后天向位置误差曲线；
图37是跑车条件下GPS失锁时俯仰角误差图；
图38是跑车条件下GPS失锁时横滚角误差图；
图39是跑车条件下GPS失锁时航向角误差图。
具体实施方式
具体实施方式一：基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法包括以下步骤：
步骤一：利用加性四元数误差的线性微分方程，建立基于加性四元数误差的状态方程；
步骤二：利用惯性导航系统解算的载体的姿态角，并将载体的姿态角与加速度计和GPS提供的姿态角作差，得到载体的姿态误差角；所述载体为车辆、飞机和船舶等运载体，姿态角包括俯仰角、横滚角和航向角；加速度计解算出俯仰角与横滚角，GPS提供航向角；
步骤三：根据姿态误差角与加性四元数误差之间的转换关系，建立观测方程；
步骤四：根据GPS工作状态对卡尔曼滤波算法做出自适应调整，调整后的卡尔曼滤波算法对加性四元数误差进行估计，对步骤二中惯性导航系统解算的载体的姿态角进行修正，得到修正后的姿态角；
步骤五：根据载体的速度和位置误差方程，建立基于速度和位置的误差量的状态方程；
步骤六：将惯性导航系统解算的速度和位置与GPS提供的速度和位置的误差量作为观测量，建立观测方程；
步骤七：利用步骤五中建立的状态方程和步骤六中建立的观测方程，根据卡尔曼滤波算法对载体速度和位置的误差进行估计，校正载体速度与位置信息。
本发明为了解决目前非线性滤波算法计算量大以及航向大失准角或GPS失效情况下测姿精度低的问题。通过采用自适应姿态滤波并串联一个速度位置滤波器提高姿态、速度、位置等导航信息的精度。姿态滤波利用四元数误差与姿态角误差之间的转换方程构造观测量，对四元数误差以及陀螺漂移进行估计，从而提高测姿精度；利用修正后的姿态信息以及惯导系统与GPS提供的速度、位置信息之间的误差量，建立基于速度、位置误差方程的速度位置滤波器。对速度、位置误差以及加速度计的漂移特性进行估计，从而改善速度位置等导航信息。本发明通过降低维数减小就了计算负担，同时可以提供高精度的姿态信息，有效克服了航向大失准角以及GPS失锁情况下对载体姿态估计偏差大的缺陷。
具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中利用加性四元数误差的线性微分方程，建立基于加性四元数误差的状态方程的具体过程为：
定义δQ为加性四元数误差，满足式(1)：
式中为计算四元数为计算四元数的标量，为计算四元数的矢量，Q＝[q0 q1 q2 q3]T为真实四元数，q0为真实四元数的标量，q1、q2、q3为真实四元数的矢量，δq0为计算四元数与真实四元数标量的差值，δq1、δq2、δq3为计算四元数与真实四元数矢量的差值；由加性四元数的线性微分方程可知：
式中为加性四元数误差的一阶导数，ωx、ωy、ωz分别为陀螺仪的x、y、z轴测量的数据，ωE、ωN、ωU分别为利用东向速度、北向速度、天向速度与位置计算得到的导航坐标系相对于惯性坐标系的角速度，为陀螺仪的随机常值漂移， 分别为陀螺仪的x、y、z轴的随机常值漂移；
选取作为基于加性四元数误差的状态量，根据式(2)建立基于加性四元数误差的状态方程为：
式中其中04×3表示3行4列的零矩阵，03×3表示3行3列的零矩阵，w为基于加性四元数误差的系统噪声矢量，t是时刻；
其它步骤及参数与具体实施方式一相同。
具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤三中根据姿态误差角与加性四元数误差之间的转换关系，建立观测方程的具体过程为：
定义Qe为乘性四元数误差，满足式(4)：
式中为四元数乘法运算法则，并且Qe满足qe0为乘性四元数误差的标量，qe1、qe2、qe3为乘性四元数误差的矢量；
将(1)式与(4)式联立得：
将导航坐标系n定义为东北天地理坐标系，由于初始偏差、测量误差、计算误差等影响，导致实际的导航坐标系C与真实的导航坐标系n之间存在着角度偏差，称为平台误差角，定义平台误差角φx为C坐标系与真实导航坐标系n的x轴的偏差角，φy为C坐标系与真实导航坐标系n的y轴的偏差角，φz为C坐标系与真实导航坐标系n的z轴的偏差角，C坐标系与真实导航坐标系n之间的变换矩阵为
设载体真实姿态角为θn为载体真实俯仰角，γn为载体真实横滚角，为载体真实航向角，GPS和MIMU组合导航系统解算的姿态角为θc为GPS和MIMU组合导航系统解算的俯仰角，γc为GPS和MIMU组合导航系统解算的横滚角，为GPS和MIMU组合导航系统解算的航向角，由于计算误差、测量误差等影响，载体真实姿态角与解算的姿态角之间存在误差，称为真实姿态误差角，定义真实姿态误差角为：
δθ＝θc-θn
δγ＝γc-γn
其中δθ为真实俯仰误差角，δγ为真实横滚误差角，为真实航向误差角；
根据平台误差角与真实姿态误差角之间的转换方程，则有：
在实际应用中，利用惯性导航系统解算的载体的三个姿态角与加速度计和GPS提供的三个姿态角作差，得到载体的姿态误差角，利用式(6)得到解算过程中的平台误差角；下面阐述平台误差角与乘性四元数之间的等价关系。
在基于四元数法的姿态解算方法中，平台误差角与乘性四元数误差之间的关系如式(7)：
Φ为平台误差角模值；
将步骤二得到的姿态误差角转换为平台误差角，将转换后的平台误差角转换为乘性四元数误差，取Z＝Qe-[1 0 0 0]T作为基于加性四元数误差的观测量，则观测方程为：
其中v为量测噪声矢量；X为基于加性四元数误差的状态量，t为时刻。
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤四中根据GPS工作状态对卡尔曼滤波算法做出自适应调整，调整后的卡尔曼滤波算法对加性四元数误差进行估计，对步骤二中惯性导航系统解算的载体的姿态角进行修正，对姿态角进行修正的具体过程为：
首先，根据GPS的不同工作状态，进行分析，在GPS正常工作的情况下，按照上述建立的状态方程与观测方程便可以对加性四元数误差进行估计，从而对姿态角进行修正；当双天线GPS无法提供航向角时，存在两种情况：一种是双天线中的一个天线无法正常工作，变为单天线模式，此时GPS可以提供速度、位置与速度航向角等信息，另一种情况为双天线GPS处于失锁的状态下，无法正常工作，即无法提供导航信息。
针对第一种情况，可以利用速度航向角代替航向角的方法，即有：
式中为速度航向角，Vx为C坐标系的x轴上的速度分量，Vy为C坐标系的y轴上的速度分量；利用速度航向角代替航向角后，便可以按照上述的状态方程与观测方程对加性四元数误差进行估计，从而对姿态角进行修正；
针对第二种情况，当GPS无法提供导航信息时，采取只利用俯仰角与横滚角求取相应的观测量，并对观测方程作出调整的方法，方法如下：
由于俯仰角误差与横滚角误差均为小角度误差，因此有：
结合式(5)、(10)可得：
根据式(11)，建立调整后的观测方程为：
式中Z2(t)为调整后的观测量；
综上，对于双天线GPS处于不同的工作状态，所对应的处理方法不同以及观测方程中的观测噪声方差也不同。因此，对于不同GPS工作状态下的观测量以及观测量噪声作出自适应调整，调整规则如下：
式中，σ2表示对应量的方差值，为乘性四元数误差的方差值，为x轴平台误差角的方差值，为y轴平台误差角的方差值，s表示GPS当前导航状态，s＝1表示双天线GPS可以提供完整的导航信息，即GPS航向角能够直接输出；s＝2表示双天线GPS无法输出航向信息，通过速度分解求得速度航向角，利用速度航向角代替航向角；s＝3表示双天线GPS无法提供导航信息。ωz(t)为MEMS陀螺仪输出的z轴角速度，κ阈值，取值在0.025至0.04之间，对运载体的运动状态进行判断；m为调整噪声方差参数，是一常数。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤五中根据载体的速度和位置误差方程，建立基于速度和位置的误差量的状态方程具体为：
其中ωvp(t)为状态噪声，Fvp为状态转移矩阵，Xvp为基于速度和位置的误差的状态量，为Xvp的一阶导数；
其中δVE为载体东向速度误差，δVN为载体北向速度误差，δVU为载体天向速度误差，δL为载体纬度误差，δλ为载体经度误差，δh为载体高度误差，为加速度计x轴偏置，为加速度计y轴偏置，为加速度计z轴偏置。
Tt为修正后的姿态捷联矩阵。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤六中将惯性导航系统解算的速度和位置与GPS提供的速度和位置的误差量作为观测量，建立观测方程具体为：
Zvp(t)＝Hvp(t)Xvp(t)+νvp(t) (15)
其中νvp(t)为量测噪声，Hvp(t)为观测矩阵，Zvp(t)为基于速度和位置的误差的观测量。
Hvp＝[I6×6 06×3]
其它步骤及参数与具体实施方式一至五之一相同。
实施例一：
为了验证本发明所提算法的有效性，对算法进行仿真实验。为模拟MEMS级别精度的惯导水平，将惯导陀螺漂移设置为20°/h，陀螺一阶马尔科夫过程相关时间180s,等效加速度零偏设置为10-3g。速度随机游走系数设置为运动轨迹如图1所示，具体为：30s静止→10s加速→30s匀速→10s转弯→30s匀速→10s转弯→30s匀速→10s转弯→30s匀速→10s转弯→30s匀速→10s减速→10s静止。
真实俯仰角为1°，真实横滚角为-1°，初始航向角为90°，初始失准角φx、φy和φz均为0.1°。GPS速度误差为0.05m/s位置误差为5m。航向角误差为0.1°。仿真时间为250s。
实验一：双天线GPS正常工作。即对在正常情况下对本发明的算法进行验证。对姿态角、速度、位置的估计精度进行验证。
实验二：双天线GPS失锁情况下。即在实验一的基础上，在60s～80s时间段认为GPS失锁，此阶段不提供GPS的任何相关信息，其他条件保持不变。此过程中包含10s匀速直线运动与10s转弯运动。
实验三：初始航向角误差为大失准角。即在实验一的基础上，将航向角的初始误差角设定为20。，其它条件保持不变。
一、仿真条件与内容：
为了验证本发明所提算法的有效性，对算法进行仿真实验。为模拟MEMS级别精度的惯导水平，将惯导陀螺漂移设置为20°/h，陀螺一阶马尔科夫过程相关时间180s,等效加速度零偏设置为10-3g。速度随机游走系数设置为运动轨迹为：30s静止→10s加速→30s匀速→10s转弯→30s匀速→10s转弯→30s匀速→10s转弯→30s匀速→10s转弯→30s匀速→10s减速→10s静止。
真实俯仰角为1°，真实横滚角为-1°，初始航向角为90°，初始失准角φx、φy和φz均为0.1°。GPS速度误差为0.05m/s位置误差为5m。航向角误差为0.1°。仿真时间为250s。
实验一：双天线GPS正常工作。即对在正常情况下对本发明的算法进行验证。对姿态角、速度、位置的估计精度进行验证。
实验二：双天线GPS失锁情况下。即在实验一的基础上，在60s～80s时间段认为GPS失锁，此阶段不提供GPS的任何相关信息，其他条件保持不变。此过程中包含10s匀速直线运动与10s转弯运动。
实验三：初始航向角误差为大失准角。即在实验一的基础上，将航向角的初始误差角设定为20。，其它条件保持不变。
二、仿真实验效果与分析：
从图2、图3和图4中可以看出，无论是单纯的惯导解算姿态或加速度计解算姿态，都无法给出长时间高精度的姿态信息。而双天线GPS在正常工作状态下，可以提供高精度的航向信息。但是当周围环境存在干扰，导致收到的卫星数目不足或不同步时，GPS给出的航向角的误差会迅速增大甚至无法提供航向角信息。
根据实验一，即在双天线GPS正常工作情况下，按照本发明方法进行仿真验证，获得的姿态效果如图5、图7和图9所示，滤波后的三个姿态角与理论值的误差如图6、图8和图10所示，速度滤波效果如图11、图12和图13所示，位置滤波效果如图14、图15和图16所示。在通过本发明的滤波方法之后，相对比于单独子系统解算姿态方法，姿态精度得到极大提高，并能够保持收敛。其中，俯仰角、横滚角、航向角的均方根误差(RMS)分别为0.063、0.056、0.012，具有较高的测姿精度。在速度位置滤波过程中，速度与位置均保持收敛，并能达到较高的定位测速精度。三个速度误差之和优于GPS设定的0.05m/s误差，位置误差优于GPS预设的5m误差。
根据实验二，即在GPS信号失锁的情况下，根据本发明算法，做出相应的自适应调整，即只使用水平姿态误差角作为观测量，进行自适应姿态卡尔曼滤波处理。
从图18、图19和图20中可以看出，GPS失锁过程中，没有滤波仅依靠惯导解算姿态的情况下，俯仰角、横滚角以及航向角都出现了不同程度上的发散。而根据本发明的滤波算法信息融合之后，俯仰角和横滚角可以实现收敛的效果；即使缺少航向角相应的观测量，也能在很大程度上抑制航向角的发散。
根据实验三，即在航向大失准角的情况下，按照本发明算法进行信息融合。由图17可知：在方位失准角为20°的情况下，根据本发明算法的信息融合后，可以有效地提高在方位大失准角情况下的姿态精度，并能在较短的时间内达到收敛并稳定。通过分析滤波后所得到的姿态数据可知：在30s时间内将大方位失准角由20°降到0.04°。
三、跑车实验与分析：
为验证算法在实际场景中的准确性与可靠性，通过跑车试验验证算法在实际中所能达到的滤波效果。通过双天线GPS与型号为ADIS16488的MEMS惯性传感器件组合进行算法验证，以光纤陀螺与双天线GPS组合作为对照，提供参考姿态、速度与位置等信息。光纤陀螺与双天线GPS组合提供的姿态误差优于0.01°，速度误差优于0.02m/s，位置误差优于2m。
为保证实验的准确性，在某处空旷地带进行车载实验，运动轨迹如图21所示，过程包括：静止阶段与五次绕圈运动阶段，时长约22min。主要针对GPS正常工作情况下与GPS失锁情况下，根据基于本发明算法的滤波效果对算法进行验证与分析。
实验一：双天线GPS正常工作
在运动过程中，双天线GPS全程保持正常工作状态，可以提供可靠高精度的速度、位置、航向等导航信息。
首先，利用不同传感器的信息分别独立解算姿态，效果如图22、图23和图24所示，以证明不同传感器之间数据融合的必要。从图22、图23和图24中可以看出：陀螺解算的三个姿态角随时间而发散严重；加速度计在没有剔除有害加速度干扰的情况下，存在很大的测姿误差。双天线GPS提供的航向角信息在正常工作模式下可以保持较好的测姿精度，但容易受到周围环境因素的影响，在周围存在干扰的情况下，GPS的测姿精度误差会迅速增大，甚至无法提供包括航向角在内的导航信息。
根据本发明所提供的姿态滤波算法进行信息融合，效果如图25、图27和图29所示，三个姿态角与光纤提供的姿态角之间的误差如图26、图28和图30所示。在剔除相关有害加速度之后，经过信息融合，使得姿态保持较好的导航精度。俯仰角、横滚角、航向角的RMS值分别为0.145、0.170、0.026。由于双天线GPS精度较高，航向信息具有较高的准确性，在转弯阶段，由于GPS数据更新较慢，与MEMS惯性传感器件信息不同步，造成了在转弯过程中，航向信息误差较大，造成在图30中航向误差角出现尖峰的情况。滤波后的速度误差如图31、图32和图33所示，滤波后的位置误差如图34、图35和图36所示，根据速度与位置的效果图中可以看出：速度位置信息得到有效地校正，并能够保持收敛，具有较高的可靠性与准确性。
实验一：双天线GPS失锁情况下
为验证本发明算法在GPS完全失锁情况下的滤波效果，在400秒至520秒的时间段内，进行两次转弯和一段直行运动。在此过程中屏蔽GPS相关的导航信息，对采集到的传感器信息进行数据融合。对比使用滤波与否的效果，如图37、图38和图39所示。从图37、图38和图39所示中可以看出，在没有滤波的情况下，三个姿态角都有不同程度的发散趋势，其中航向角发散最为严重；经过滤波处理后，俯仰角和横滚角得到收敛，而航向信息即使在缺乏相应的观测信息的情况下，发散趋势得到有效遏制。在GPS失效的120秒时间内，本发明提出的算法可以提供较好的姿态信息。
通过理论分析、仿真实验与实际跑车试验验证：通过将两个卡尔曼滤波器级联，单独的卡尔曼滤波器的维数比较低，相比于传统滤波器的15维方法，计算量大大减少，从而有效地降低运算时间、提高运算速度。同时，级联的两个卡尔曼滤波器的模型均为线性模型，因此不必使用具有复杂计算的非线性滤波算法，在一定程度上减少了计算量。同时，滤波算法精度具有较好的准确性与可靠性。此外，由于姿态滤波与速度位置滤波相对独立，即使在GPS失效后，也能对姿态进行滤波处理，使得姿态能够保持较好的精度，有效抑制了姿态发散的情况。
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (6)

1.基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，其特征在于：所述自适应级联卡尔曼滤波方法包括以下步骤：
步骤一：利用加性四元数误差的线性微分方程，建立基于加性四元数误差的状态方程；
步骤二：利用惯性导航系统解算的载体的姿态角，并将载体的姿态角与加速度计和GPS提供的姿态角作差，得到载体的姿态误差角；所述载体为车辆、飞机和船舶，姿态角包括俯仰角、横滚角和航向角；加速度计解算出俯仰角与横滚角，GPS提供航向角；
步骤三：根据姿态误差角与加性四元数误差之间的转换关系，建立观测方程；
步骤四：根据GPS工作状态对卡尔曼滤波算法做出自适应调整，调整后的卡尔曼滤波算法对加性四元数误差进行估计，对步骤二中惯性导航系统解算的载体的姿态角进行修正，得到修正后的姿态角；
步骤五：根据载体的速度和位置误差方程，建立基于速度和位置的误差量的状态方程；
步骤六：将惯性导航系统解算的速度和位置与GPS提供的速度和位置的误差量作为观测量，建立观测方程；
步骤七：利用步骤五中建立的状态方程和步骤六中建立的观测方程，根据卡尔曼滤波算法对载体速度和位置的误差进行估计，校正载体速度与位置信息。
2.根据权利要求1所述的基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，其特征在于：所述步骤一中利用加性四元数误差的线性微分方程，建立基于加性四元数误差的状态方程的具体过程为：
定义δQ为加性四元数误差，满足式(1)：
<mrow> <mi>&amp;delta;</mi> <mi>Q</mi> <mo>=</mo> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mo>-</mo> <mi>Q</mi> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
式中为计算四元数 为计算四元数的标量，为计算四元数的矢量，Q＝[q0 q1 q2 q3]T为真实四元数，q0为真实四元数的标量，q1、q2、q3为真实四元数的矢量，δq0为计算四元数与真实四元数标量的差值，δq1、δq2、δq3为计算四元数与真实四元数矢量的差值；由加性四元数的线性微分方程可知：
<mrow> <mi>&amp;delta;</mi> <mover> <mi>Q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>(</mo> <mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mi>x</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mi>z</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mi>y</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mi>z</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mi>y</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>E</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>N</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>U</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mi>E</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>U</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>&amp;omega;</mi> <mi>N</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mi>N</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>U</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>E</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;omega;</mi> <mi>U</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>N</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mi>E</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> <mo>)</mo> <mi>&amp;delta;</mi> <mi>Q</mi> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <msup> <mi>&amp;epsiv;</mi> <mi>b</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
式中为加性四元数误差的一阶导数，ωx、ωy、ωz分别为陀螺仪的x、y、z轴测量的数据，ωE、ωN、ωU分别为利用东向速度、北向速度、天向速度与位置计算得到的导航坐标系相对于惯性坐标系的角速度，为陀螺仪的随机常值漂移，分别为陀螺仪的x、y、z轴的随机常值漂移；
选取作为基于加性四元数误差的状态量，根据式(2)建立基于加性四元数误差的状态方程为：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>w</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
式中其中04×3表示3行4列的零矩阵，03×3表示3行3列的零矩阵，w为基于加性四元数误差的系统噪声矢量，t是时刻。
3.根据权利要求2所述的基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，其特征在于：所述步骤三中根据姿态误差角与加性四元数误差之间的转换关系，建立观测方程的具体过程为：
定义Qe为乘性四元数误差，满足式(4)：
<mrow> <mi>Q</mi> <mo>=</mo> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mo>&amp;CircleTimes;</mo> <msub> <mi>Q</mi> <mi>e</mi> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
式中为四元数乘法运算法则，并且Qe满足qe0为乘性四元数误差的标量，qe1、qe2、qe3为乘性四元数误差的矢量；
将(1)式与(4)式联立得：
<mrow> <msub> <mi>Q</mi> <mi>e</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
将导航坐标系n定义为东北天地理坐标系，实际的导航坐标系C与真实的导航坐标系n之间存在角度偏差，称为平台误差角，定义平台误差角φx为C坐标系与真实导航坐标系n的x轴的偏差角，φy为C坐标系与真实导航坐标系n的y轴的偏差角，φz为C坐标系与真实导航坐标系n的z轴的偏差角，C坐标系与真实导航坐标系n之间的变换矩阵为
<mrow> <msubsup> <mi>C</mi> <mi>n</mi> <mi>c</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
设载体真实姿态角为θn为载体真实俯仰角，γn为载体真实横滚角，为载体真实航向角，GPS和MIMU组合导航系统解算的姿态角为θc为GPS和MIMU组合导航系统解算的俯仰角，γc为GPS和MIMU组合导航系统解算的横滚角，为GPS和MIMU组合导航系统解算的航向角，载体真实姿态角与解算的姿态角之间存在误差，称为真实姿态误差角，定义真实姿态误差角为：
δθ＝θc-θn
δγ＝γc-γn
其中δθ为真实俯仰误差角，δγ为真实横滚误差角，为真实航向误差角；
得到平台误差角与真实姿态误差角之间的转换方程，即有：
在基于四元数法的姿态解算方法中，平台误差角与乘性四元数误差之间的关系如式(7)：
<mrow> <mi>&amp;Phi;</mi> <mo>=</mo> <msqrt> <mrow> <msubsup> <mi>&amp;phi;</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;phi;</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;phi;</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> </mrow>
<mrow> <msub> <mi>Q</mi> <mi>e</mi> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> <mtd> <mrow> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mi>&amp;Phi;</mi> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> <mtd> <mrow> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> <mi>&amp;Phi;</mi> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> <mtd> <mrow> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> <mi>&amp;Phi;</mi> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
Φ为平台误差角模值；
将步骤二得到的姿态误差角转换为平台误差角，将转换后的平台误差角转换为乘性四元数误差，取Z＝Qe-[1 0 0 0]T作为基于加性四元数误差的观测量，则观测方程为：
<mrow> <mi>Z</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> </msub> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>3</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mi>&amp;Phi;</mi> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> <mi>&amp;Phi;</mi> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> <mi>&amp;Phi;</mi> </mfrac> <mi>sin</mi> <mfrac> <mi>&amp;Phi;</mi> <mn>2</mn> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>v</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
其中v为量测噪声矢量，t为时刻。
4.根据权利要求3所述的基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，其特征在于：所述步骤四中根据GPS工作状态对卡尔曼滤波算法做出自适应调整，调整后的卡尔曼滤波算法对加性四元数误差进行估计，对步骤二中惯性导航系统解算的载体的姿态角进行修正，对姿态角进行修正的具体过程为：
当双天线中的一个天线无法正常工作，变为单天线模式，利用速度航向角代替航向角的方法，即有：
式中为速度航向角，Vx为C坐标系的x轴上的速度分量，Vy为C坐标系的y轴上的速度分量；利用速度航向角代替航向角后，按照公式(3)和公式(8)建立的状态方程与观测方程对加性四元数误差进行估计，对姿态角进行修正；
当双天线GPS处于失锁的状态下，GPS无法提供导航信息时，采取只利用俯仰角与横滚角求取相应的观测量，并对观测方程作出调整的方法，方法如下：
由于俯仰角误差与横滚角误差均为小角度误差，因此有：
<mrow> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mn>2</mn> </mfrac> <mo>,</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>=</mo> <mfrac> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> <mn>2</mn> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
结合式(5)、(10)可得：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mn>2</mn> <mo>*</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;q</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
根据式(11)，建立调整后的观测方程为：
<mrow> <msub> <mi>Z</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mn>2</mn> <mo>*</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>v</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
式中Z2(t)为调整后的观测量；
对于不同GPS工作状态下的观测量以及观测量噪声作出自适应调整，调整规则如下：
其中σ2表示对应量的方差值，为乘性四元数误差的方差值，为x轴平台误差角的方差值，为y轴平台误差角的方差值，s表示GPS当前导航状态，s＝1表示双天线GPS可以提供完整的导航信息，即GPS航向角能够直接输出；s＝2表示双天线GPS无法输出航向信息，通过速度分解求得速度航向角，利用速度航向角代替航向角；s＝3表示双天线GPS无法提供导航信息；ωz(t)为MEMS陀螺仪输出的z轴角速度，κ为阈值，m为调整噪声方差参数。
5.根据权利要求4所述的基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，其特征在于：所述步骤五中根据载体的速度和位置误差方程，建立基于速度和位置的误差量的状态方程具体为：
<mrow> <msub> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>v</mi> <mi>p</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>F</mi> <mrow> <mi>v</mi> <mi>p</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msub> <mi>X</mi> <mrow> <mi>v</mi> <mi>p</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>v</mi> <mi>p</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
其中ωvp(t)为系统噪声，Fvp为状态转移矩阵，Xvp为基于速度和位置的误差的状态量，为Xvp的一阶导数；
<mrow> <msub> <mi>X</mi> <mrow> <mi>v</mi> <mi>p</mi> </mrow> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>E</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>N</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>U</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>&amp;lambda;</mi> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>h</mi> </mrow> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中δVE为载体东向速度误差，δVN为载体北向速度误差，δVU为载体天向速度误差，δL为载体纬度误差，δλ为载体经度误差，δh为载体高度误差，为加速度计x轴偏置，为加速度计y轴偏置，为加速度计z轴偏置。
6.根据权利要求5所述的基于双天线GPS和MIMU组合的自适应级联卡尔曼滤波方法，其特征在于：所述步骤六中将惯性导航系统解算的速度和位置与GPS提供的速度和位置的误差量作为观测量，建立观测方程具体为：
Zvp(t)＝Hvp(t)Xvp(t)+νvp(t) (15)
其中νvp(t)为量测噪声，Hvp(t)为观测矩阵，Zvp(t)为基于速度和位置的误差的观测量。
CN201710731641.8A 2017-08-23 2017-08-23 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 Active CN107525503B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710731641.8A CN107525503B (zh) 2017-08-23 2017-08-23 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710731641.8A CN107525503B (zh) 2017-08-23 2017-08-23 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法

Publications (2)

Publication Number Publication Date
CN107525503A true CN107525503A (zh) 2017-12-29
CN107525503B CN107525503B (zh) 2020-09-11

Family

ID=60682071

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710731641.8A Active CN107525503B (zh) 2017-08-23 2017-08-23 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法

Country Status (1)

Country Link
CN (1) CN107525503B (zh)

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108896044A (zh) * 2018-07-23 2018-11-27 湖南格纳微信息科技有限公司 一种基于惯性导航和卫星导航的定位方法及装置
CN109489653A (zh) * 2018-10-24 2019-03-19 中国人民解放军海军工程大学 一种基于无线通信的便携式组合导航系统及方法
CN109724596A (zh) * 2018-12-07 2019-05-07 江苏大学 一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法
CN109764870A (zh) * 2019-01-17 2019-05-17 上海华测导航技术股份有限公司 基于变换估计量建模方案的载体初始航向估算方法
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法
CN110988950A (zh) * 2018-10-03 2020-04-10 古野电气株式会社 导航装置、航行辅助信息的生成方法及存储介质
CN111189443A (zh) * 2020-01-14 2020-05-22 电子科技大学 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法
CN112083465A (zh) * 2020-09-18 2020-12-15 德明通讯（上海）有限责任公司 位置信息获取系统及方法
CN112504275A (zh) * 2020-11-16 2021-03-16 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
WO2021120525A1 (zh) * 2019-12-19 2021-06-24 北京三快在线科技有限公司 无人设备的导航
CN113375694A (zh) * 2021-05-25 2021-09-10 南京航空航天大学 一种静基座条件下低成本陀螺全部零偏快速估计方法
CN113391336A (zh) * 2021-06-17 2021-09-14 上海联适导航技术股份有限公司 一种航向角的检测方法、装置、设备及可读存储介质
CN113405553A (zh) * 2020-11-30 2021-09-17 辽宁工业大学 一种无人驾驶特种车辆姿态测量方法
CN113607167A (zh) * 2021-03-15 2021-11-05 南京航空航天大学 一种用于航姿系统的自适应姿态估计方法及其平滑切换算法
CN114111771A (zh) * 2021-11-25 2022-03-01 九江中船仪表有限责任公司（四四一厂） 一种双轴稳定平台的动态姿态测量方法
CN114167458A (zh) * 2021-12-07 2022-03-11 中国船舶重工集团公司第七0七研究所 一种gnss航迹角降噪计算方法
CN114485641A (zh) * 2022-01-24 2022-05-13 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN114543795A (zh) * 2021-12-31 2022-05-27 文远苏行(江苏)科技有限公司 双天线航向角的安装误差估计方法和调整方法及相关设备
CN113776529B (zh) * 2021-09-13 2024-04-19 中国人民解放军海军工程大学 一种基于载体系四元数姿态误差的非线性误差模型

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060161329A1 (en) * 2005-01-14 2006-07-20 Robert Crane System and method for advanced tight coupling of GPS and inertial navigation sensors
CN101067657A (zh) * 2007-02-28 2007-11-07 北京航空航天大学 一种机载双天线双测量装置干涉sar基线运动测量方法
CN101464152A (zh) * 2009-01-09 2009-06-24 哈尔滨工程大学 一种sins/gps组合导航系统自适应滤波方法
CN103245963A (zh) * 2013-05-09 2013-08-14 清华大学 双天线gnss/ins深组合导航方法及装置
CN204347258U (zh) * 2014-08-18 2015-05-20 北京七维航测科技股份有限公司 双天线gnss/ins组合导航系统
CN107037469A (zh) * 2017-04-11 2017-08-11 北京七维航测科技股份有限公司 基于安装参数自校准的双天线组合惯导装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060161329A1 (en) * 2005-01-14 2006-07-20 Robert Crane System and method for advanced tight coupling of GPS and inertial navigation sensors
CN101067657A (zh) * 2007-02-28 2007-11-07 北京航空航天大学 一种机载双天线双测量装置干涉sar基线运动测量方法
CN101464152A (zh) * 2009-01-09 2009-06-24 哈尔滨工程大学 一种sins/gps组合导航系统自适应滤波方法
CN103245963A (zh) * 2013-05-09 2013-08-14 清华大学 双天线gnss/ins深组合导航方法及装置
CN204347258U (zh) * 2014-08-18 2015-05-20 北京七维航测科技股份有限公司 双天线gnss/ins组合导航系统
CN107037469A (zh) * 2017-04-11 2017-08-11 北京七维航测科技股份有限公司 基于安装参数自校准的双天线组合惯导装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张京等: "农机INS/GNSS组合导航系统航向信息融合方法", 《农业机械学报》 *
杨钊等: "卡尔曼滤波在无人水面艇双天线GPS定位测向系统中的应用", 《计算机测量与控制》 *

Cited By (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108896044A (zh) * 2018-07-23 2018-11-27 湖南格纳微信息科技有限公司 一种基于惯性导航和卫星导航的定位方法及装置
CN108896044B (zh) * 2018-07-23 2021-09-10 湖南格纳微信息科技有限公司 一种基于惯性导航和卫星导航的定位方法及装置
CN110988950A (zh) * 2018-10-03 2020-04-10 古野电气株式会社 导航装置、航行辅助信息的生成方法及存储介质
CN109489653A (zh) * 2018-10-24 2019-03-19 中国人民解放军海军工程大学 一种基于无线通信的便携式组合导航系统及方法
CN109724596A (zh) * 2018-12-07 2019-05-07 江苏大学 一种基于最小二乘法和卡尔曼滤波的智能拖拉机定位方法
CN109764870B (zh) * 2019-01-17 2023-04-25 上海华测导航技术股份有限公司 基于变换估计量建模方案的载体初始航向估算方法
CN109764870A (zh) * 2019-01-17 2019-05-17 上海华测导航技术股份有限公司 基于变换估计量建模方案的载体初始航向估算方法
CN109916399A (zh) * 2019-04-04 2019-06-21 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法
CN109916399B (zh) * 2019-04-04 2019-12-24 中国人民解放军火箭军工程大学 一种阴影下的载体姿态估计方法
WO2021120525A1 (zh) * 2019-12-19 2021-06-24 北京三快在线科技有限公司 无人设备的导航
CN111189443A (zh) * 2020-01-14 2020-05-22 电子科技大学 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法
CN112083465A (zh) * 2020-09-18 2020-12-15 德明通讯（上海）有限责任公司 位置信息获取系统及方法
CN112504275B (zh) * 2020-11-16 2022-09-02 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN112504275A (zh) * 2020-11-16 2021-03-16 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN113405553A (zh) * 2020-11-30 2021-09-17 辽宁工业大学 一种无人驾驶特种车辆姿态测量方法
CN113405553B (zh) * 2020-11-30 2023-05-26 辽宁工业大学 一种无人驾驶特种车辆姿态测量方法
CN113607167B (zh) * 2021-03-15 2024-07-05 南京航空航天大学 一种用于航姿系统的自适应姿态估计方法及其平滑切换算法
CN113607167A (zh) * 2021-03-15 2021-11-05 南京航空航天大学 一种用于航姿系统的自适应姿态估计方法及其平滑切换算法
CN113375694A (zh) * 2021-05-25 2021-09-10 南京航空航天大学 一种静基座条件下低成本陀螺全部零偏快速估计方法
CN113391336A (zh) * 2021-06-17 2021-09-14 上海联适导航技术股份有限公司 一种航向角的检测方法、装置、设备及可读存储介质
CN113776529B (zh) * 2021-09-13 2024-04-19 中国人民解放军海军工程大学 一种基于载体系四元数姿态误差的非线性误差模型
CN114111771A (zh) * 2021-11-25 2022-03-01 九江中船仪表有限责任公司（四四一厂） 一种双轴稳定平台的动态姿态测量方法
CN114167458A (zh) * 2021-12-07 2022-03-11 中国船舶重工集团公司第七0七研究所 一种gnss航迹角降噪计算方法
CN114543795A (zh) * 2021-12-31 2022-05-27 文远苏行(江苏)科技有限公司 双天线航向角的安装误差估计方法和调整方法及相关设备
CN114543795B (zh) * 2021-12-31 2024-01-02 文远苏行(江苏)科技有限公司 双天线航向角的安装误差估计方法和调整方法及相关设备
CN114485641B (zh) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN114485641A (zh) * 2022-01-24 2022-05-13 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置

Also Published As

Publication number Publication date
CN107525503B (zh) 2020-09-11

Similar Documents

Publication Publication Date Title
CN107525503A (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN103759742B (zh) 基于模糊自适应控制技术的捷联惯导非线性对准方法
CN103557871B (zh) 一种浮空飞行器捷联惯导空中初始对准方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
CN101246011B (zh) 一种基于凸优化算法的多目标多传感器信息融合方法
Li et al. Observability analysis of non-holonomic constraints for land-vehicle navigation systems
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN103575299A (zh) 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
CN105865452B (zh) 一种基于间接卡尔曼滤波的移动平台位姿估计方法
CN101979277A (zh) 卫星磁测磁控系统的全实物验证平台与工作方法
CN109471144A (zh) 基于伪距/伪距率的多传感器紧组合列车组合定位方法
Wang et al. A new magnetic compass calibration algorithm using neural networks
CN101900573B (zh) 一种实现陆用惯性导航系统运动对准的方法
CN102654406A (zh) 基于非线性预测滤波与求容积卡尔曼滤波相结合的动基座初始对准方法
CN103017787A (zh) 适用于摇摆晃动基座的初始对准方法
CN109000639B (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
CN106767925A (zh) 带双轴转位机构的惯导系统三位置参数辨识对准方法
CN110672095A (zh) 一种基于微惯导的行人室内自主定位算法
Luo et al. An on-line full-parameters calibration method for SINS/DVL integrated navigation system
Cheng et al. A polar integrated alignment assisted by dvl under large azimuth misalignment
Zhao et al. Integrated navigation error analysis based on Kalman filter of INS error compensation
CN102914318B (zh) 非完全自由度惯性平台关键参数多位置加权自主检测方法
Wang et al. An adaptive cascaded Kalman filter for two-antenna GPS/MEMS-IMU integration
Huang et al. Attitude determination for underwater gliders using unscented Kalman filter based on smooth variable algorithm

Legal Events

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