CN113175926A - 一种基于运动状态监测的自适应水平姿态测量方法 - Google Patents

一种基于运动状态监测的自适应水平姿态测量方法 Download PDF

Info

Publication number
CN113175926A
CN113175926A CN202110428645.5A CN202110428645A CN113175926A CN 113175926 A CN113175926 A CN 113175926A CN 202110428645 A CN202110428645 A CN 202110428645A CN 113175926 A CN113175926 A CN 113175926A
Authority
CN
China
Prior art keywords
state
time
horizontal attitude
matrix
moment
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
CN202110428645.5A
Other languages
English (en)
Other versions
CN113175926B (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202110428645.5A priority Critical patent/CN113175926B/zh
Publication of CN113175926A publication Critical patent/CN113175926A/zh
Priority to PCT/CN2022/087805 priority patent/WO2022222938A1/zh
Priority to US17/844,224 priority patent/US20220326017A1/en
Application granted granted Critical
Publication of CN113175926B publication Critical patent/CN113175926B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • 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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于运动状态监测的自适应水平姿态测量方法，基于新建立的状态空间模型，将水平姿态角作为状态变量，补偿随机常值零偏的角速度增量Δωb作为***方程的控制输入，补偿随机常值零偏的比力fb作为量测量，同时对载体机动状态的判断条件进行改善，综合利用MEMS‑IMU输出的加速度信息和角速度信息对载体机动信息进行判断判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对水平姿态解算的影响。该方法对载体机动状态无特殊要求，能够在无外界信息辅助的情况下，保证***在不同运动状态下具有较高的姿态测量精度。

Description

一种基于运动状态监测的自适应水平姿态测量方法
技术领域
本发明涉及一种基于运动状态监测的自适应水平姿态测量方法，属于惯性技术领域。
背景技术
随着微机电***技术的发展，低成本MEMS-IMU在导航领域有着越来越多的应用，通过利用基于微机电***的惯性传感器进行运动参数测量，可以检测船舶在海中复杂的运动状态，实现用户对水面舰船的运动数据采集，这要求MEMS-IMU能够实时准确地输出载体的角运动参数和线运动参数。
微机电陀螺仪具有随机漂移特性，其积分误差随时间累积，加速度计不存在累积误差，但是易受到载体震动影响。常用的将二者数据融合的算法是卡尔曼滤波和互补滤波，例如在专利申请号为201811070907.X，名称为“基于机动状态判断的MEMS惯性导航***水平姿态自修正方法”的专利文件中，通过比较加速度计输出和当地重力加速度幅值，将载体运动分为低、中、高动态。在低和中动态时，实时调整量测噪声矩阵，在高动态时只进行时间更新。但是若载体较长时间处于高动态下，则姿态误差会越来越大。又如在专利申请号为202011092956.0，名称为“基于自适应EKF算法的小型无人机MARG航姿估计方法”的专利文件中，针对外部加速度的自适应滤波算法，通过对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。传统自适应调整的方法大多通过对载体加速度计输出的模值与当地重力加速度进行比较，实现运载体机动状态的判断，未考虑载体角运动对加速度测量的干扰，这对于一些复杂环境来说可能是不充分的。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种基于运动状态监测的自适应水平姿态测量方法，可以提高运载体在机动场景下的水平姿态测量精度，为运载体提供准确的水平姿态信息。
为解决上述技术问题，本发明的一种基于运动状态监测的自适应水平姿态测量方法，包括以下步骤：
步骤1：对捷联惯性导航***进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角θ0和俯仰角φ0，然后进入导航工作模式；
步骤2：初始化卡尔曼滤波器，将步骤1中得到的初始水平姿态角θ0和φ0作为卡尔曼滤波状态量X0＝[θ0 φ0]T的初始值，初始均方误差为P0；
步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
Figure BDA0003030567080000021
和角速度
Figure BDA0003030567080000022
步骤4：利用第k时刻的角速度增量
Figure BDA0003030567080000023
作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测，其中，
Figure BDA0003030567080000024
T为解算周期；
步骤5：利用步骤3中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节，其中N为数据窗的大小；
步骤6：第k时刻比力为
Figure BDA0003030567080000025
选取量测向量Zk＝[fx,k fy,k]T进行量测更新，实现状态量的校正，其中fx,k、fy,k和fz,k分别为比力
Figure BDA0003030567080000026
在载体系x轴、y轴和z轴方向的分量；
步骤7：将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。
本发明还包括：
1.步骤4中卡尔曼滤波一步预测具体为：
Figure BDA0003030567080000027
式中，
Figure BDA0003030567080000028
为k-1时刻状态最优估计，Pk-1为k-1时刻状态估计的均方误差阵，
Figure BDA0003030567080000029
为k时刻状态一步预测，Φk/k-1＝I2为k时刻状态一步转移矩阵，I2为二阶单位阵，
Figure BDA00030305670800000210
为k时刻输入系数矩阵，φk-1、θk-1为k-1时刻水平姿态最优估计，uk-1为第k时刻角速度增量
Figure BDA00030305670800000211
Pk/k-1为k时刻状态一部预测均方误差阵，Qk为***噪声方差阵。
2.步骤5所述利用步骤3中获得的比力fb和角速度ωb对运载体的机动状态进行判别具体为：利用步骤3中获得的k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行计算，求取机动向量Tk，实现机动判断，Tk满足：
Figure BDA00030305670800000212
式中，
Figure BDA0003030567080000031
和
Figure BDA0003030567080000032
分别为对步骤3中第k-N+1时刻到k时刻惯性测量单元输出且补偿随机常值零偏的比力fb和角速度ωb进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σf和σω分别为权重系数；
3.步骤5中对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节具体为：
Figure BDA0003030567080000033
式中，Tk,i为Tk的第i个元素，
Figure BDA0003030567080000034
为加速度计噪声对应方差。
4.步骤6中量测更新的更新方程为：
Figure BDA0003030567080000035
式中，
Figure BDA0003030567080000036
为k时刻量测矩阵，g为当地重力加速度，φk、θk为k时刻水平姿态一步预测值，Kk为k时刻滤波增益，
Figure BDA0003030567080000037
为k时刻状态估计，Pk为k时刻状态估计均方误差阵。
本发明的有益效果：本发明涉及以微机电惯性测量单元(MEMS-IMU)为核心器件的姿态测量单元，本发明建立新的状态空间模型，将水平姿态角作为状态变量，MEMS-IMU输出的角速度增量Δωb作为***方程的控制输入，MEMS-IMU输出的比力fb作为量测量，同时对载体机动判断条件进行改善，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息的判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对对平姿态信息的影响。该方法对载体机动状态无特殊要求，能够在无外界信息辅助的情况下，保证***在不同运动状态下具有较高的姿态测量精度，有一定的工程应用价值。
附图说明
图1为本发明的实现流程图；
图2为本发明的实现算法流程图；
图3为本发明提出算法解算误差。
具体实施方式
下面结合附图和具体实施方式对本发明做进一步说明。
本发明是这样实现的：
对捷联惯性导航***的惯性测量元件进行充分预热，并完成器件随机常值零偏计算和初始水平姿态角的计算，之后可进入导航工作状态；将水平姿态角作为状态变量，MEMS-IMU输出的角速度增量Δωb作为***方程的控制输入，MEMS-IMU输出的比力fb作为量测量，建立卡尔曼滤波方程，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息进行判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对水平姿态解算的影响。具体步骤如下：
步骤1、对捷联惯性导航***的惯性测量元件进行充分预热，并完成器件随机常值零偏计算和初始水平姿态角(横滚角θ0和俯仰角φ0)的计算，使之进入导航工作状态；
步骤2、将步骤1中得到的初始水平姿态角θ0和φ0作为卡尔曼滤波状态量X0＝[θ0φ0]T的初始值，初始均方误差为P0，初始化卡尔曼滤波器；
步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
Figure BDA0003030567080000041
和角速度
Figure BDA0003030567080000042
步骤4、利用第k时刻的角速度增量
Figure BDA0003030567080000043
作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测，其中，
Figure BDA0003030567080000044
T为解算周期；；
步骤5、利用步骤2中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节，其中N为数据窗的大小；
步骤6、利用第k时刻的比力
Figure BDA0003030567080000045
作为量测向量Zk进行量测更新，实现状态量的校正；
步骤7、将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。
步骤4中卡尔曼滤波一步预测相关方程建立：
Figure BDA0003030567080000046
式中，
Figure BDA0003030567080000047
为k-1时刻状态最优估计，Pk-1为k-1时刻状态估计的均方误差阵，
Figure BDA0003030567080000048
为k时刻状态一步预测，Φk/k-1＝I2为k时刻状态一步转移矩阵，I2为二阶单位阵，
Figure BDA0003030567080000051
为k时刻输入系数矩阵，φk-1、θk-1为k-1时刻水平姿态最优估计，uk-1为第k时刻角速度增量
Figure BDA0003030567080000052
Pk/k-1为k时刻状态一部预测均方误差阵，Qk为***噪声方差阵；
步骤5运载体机动状态判断，根据步骤3中获得的比力fb和角速度ωb对运载体的机动状态进行计算，求取机动向量Tk，从而实现机动判断：
Figure BDA0003030567080000053
式中，
Figure BDA0003030567080000054
和
Figure BDA0003030567080000055
分别为对步骤3中第k-N+1时刻到k时刻的比力fb和角速度ωb进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σf和σω分别为权重系数；
步骤5中卡尔曼滤波量测噪声协方差矩阵Rk的自适应规则如下：
Figure BDA0003030567080000056
式中，Tk,i为Tk的第i个元素，
Figure BDA0003030567080000057
为加速度计噪声对应方差；
步骤6中量测更新，利用步骤3中第k时刻的比力
Figure BDA0003030567080000058
作为量测向量Zk＝[fx,k fy,k]T进行量测更新，更新方程如下：
Figure BDA0003030567080000059
式中，
Figure BDA00030305670800000510
为k时刻量测矩阵，φk、θk为k时刻水平姿态一步预测值，Kk为k时刻滤波增益，
Figure BDA00030305670800000511
为k时刻状态估计，Pk为k时刻状态估计均方误差阵。
结合图1和图2，本发明的具体实施方式包括以下步骤：
步骤1、对捷联惯性导航***的惯性测量元件进行充分预热；
步骤2、采集一段时间内静止状态下MEMS-IMU的数据，认为该时间段内MEMS-IMU的输出平均值为器件随机常值零偏，对陀螺仪和加速度计的常值零偏误差进行校正补偿，获得补偿常值零偏误差后比力fb和角速度ωb；
步骤3、对捷联惯性导航***进行初始对准，得到载体系(b系)相对于导航坐标系(n系，本发明中导航坐标系选取地理坐标系)的初始水平姿态角(横滚角θ0和俯仰角φ0)，之后开始进入导航工作状态；
步骤4、建立卡尔曼滤波方程为：
Figure BDA0003030567080000061
式中，Xk为状态向量，Φk/k-1为状态一步转移矩阵，Bk-1为输入系数矩阵，I2为二阶单位阵，uk-1为已知的确定性输入，Zk是量测向量，Hk为量测矩阵，Wk-1是***噪声向量，Vk是量测噪声向量；
步骤5、选取水平姿态角作为状态量X＝[θ φ]T(横滚角θ和俯仰角φ)，根据步骤3中得到的初始水平姿态θ0和φ0作为状态量X0＝[θ0 φ0]T的初始值，初始均方误差为P0，初始化卡尔曼滤波器；
步骤6：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
Figure BDA0003030567080000062
和角速度
Figure BDA0003030567080000063
步骤7、利用步骤6中第k时刻
Figure BDA0003030567080000064
采样间隔内的增量
Figure BDA0003030567080000065
(T为解算周期)作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测：
Figure BDA0003030567080000066
式中，
Figure BDA0003030567080000067
为k-1时刻状态最优估计，Pk-1为k-1时刻状态估计的均方误差阵，
Figure BDA0003030567080000068
为k时刻状态一步预测，Φk/k-1＝I2为k时刻状态一步转移矩阵，
Figure BDA0003030567080000069
为k时刻输入系数矩阵，φk-1、θk-1为k-1时刻水平姿态最优估计，uk-1为第k时刻角速度增量
Figure BDA00030305670800000610
Pk/k-1为k时刻状态一部预测均方误差阵，Qk为***噪声方差阵；
步骤8、根据步骤6中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行计算，求取机动向量Tk，从而实现对卡尔曼滤波量测噪声协方差矩阵Rk的自适应调节：
Figure BDA0003030567080000071
式中，
Figure BDA0003030567080000072
和
Figure BDA0003030567080000073
分别为对对步骤6中第k-N+1时刻到k时刻的比力fb和角速度ωb进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σf为比力的权重系数(对于舰船常取0.5-5，对于车辆常取1-3),σω为角速度的权重系数(对于舰船常取1-5，对于车辆常取0.5-1)；
卡尔曼滤波量测噪声协方差矩阵Rk的自适应调节规则如下：
Figure BDA0003030567080000074
式中，Tk,i为Tk的第i个元素，
Figure BDA0003030567080000075
为加速度计噪声对应方差；
步骤9、利用步骤6中第k时刻的比力
Figure BDA0003030567080000076
作为量测向量Zk＝[fx,kfy,k]T进行量测更新，更新方程如下：
Figure BDA0003030567080000077
式中，
Figure BDA0003030567080000078
为k时刻量测矩阵，g为当地重力加速度，φk、θk为k时刻水平姿态一步预测值，Kk为k时刻滤波增益，
Figure BDA0003030567080000079
为k时刻状态估计，Pk为k时刻状态估计均方误差阵；
步骤10、将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤6-9。
至此就完成了基于运动状态监测的自适应水平姿态测量方法。
为了说明算法的有效性，对算法进行仿真。仿真条件设置如下：设各轴陀螺仪零偏均为1°/h，加速度计零偏均为1×10-4g。设置运动状态如下：运载体处于摇摆运动状态，各轴摇摆均服从正弦函数规律，其中横摇、纵摇与航向的摇摆幅值均为10°，摇摆周期均为10s，初始姿态角和相位角均为0°，仿真结果如附图3所示，水平姿态测量误差较小，精度较高。
尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和附图所公开的内容。

Claims (5)

1.一种基于运动状态监测的自适应水平姿态测量方法，其特征在于，包括以下步骤：
步骤1：对捷联惯性导航***进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角θ0和俯仰角φ0，然后进入导航工作模式；
步骤2：初始化卡尔曼滤波器，将步骤1中得到的初始水平姿态角θ0和φ0作为卡尔曼滤波状态量X0＝[θ0 φ0]T的初始值，初始均方误差为P0；
步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
Figure FDA0003030567070000011
和角速度
Figure FDA0003030567070000012
步骤4：利用第k时刻的角速度增量
Figure FDA0003030567070000013
作为已知的确定性输入uk-1，进行卡尔曼滤波一步预测，其中，
Figure FDA0003030567070000014
T为解算周期；
步骤5：利用步骤3中获得k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节，其中N为数据窗的大小；
步骤6：第k时刻比力为
Figure FDA0003030567070000015
选取量测向量Zk＝[fx,k fy,k]T进行量测更新，实现状态量的校正，其中fx,k、fy,k和fz,k分别为比力
Figure FDA0003030567070000016
在载体系x轴、y轴和z轴方向的分量；
步骤7：将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。
2.根据权利要求1所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤4所述卡尔曼滤波一步预测具体为：
Figure FDA0003030567070000017
式中，
Figure FDA0003030567070000018
为k-1时刻状态最优估计，Pk-1为k-1时刻状态估计的均方误差阵，
Figure FDA0003030567070000019
为k时刻状态一步预测，Φk/k-1＝I2为k时刻状态一步转移矩阵，I2为二阶单位阵，
Figure FDA00030305670700000110
为k时刻输入系数矩阵，φk-1、θk-1为k-1时刻水平姿态最优估计，uk-1为第k时刻角速度增量
Figure FDA00030305670700000111
Pk/k-1为k时刻状态一部预测均方误差阵，Qk为***噪声方差阵。
3.根据权利要求1或2所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤5所述利用步骤3中获得的比力fb和角速度ωb对运载体的机动状态进行判别具体为：利用步骤3中获得的k-N+1时刻到k时刻的比力fb和角速度ωb对运载体的机动状态进行计算，求取机动向量Tk，实现机动判断，Tk满足：
Figure FDA0003030567070000021
式中，
Figure FDA0003030567070000022
和
Figure FDA0003030567070000023
分别为对步骤3中第k-N+1时刻到k时刻的比力fb和角速度ωb进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σf和σω分别为权重系数。
4.根据权利要求3所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤5所述对卡尔曼滤波量测噪声协方差矩阵Rk自适应调节具体为：
Figure FDA0003030567070000024
式中，Tk,i为Tk的第i个元素，
Figure FDA0003030567070000025
为加速度计噪声对应方差。
5.根据权利要求1或2所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤6所述量测更新的更新方程为：
Figure FDA0003030567070000026
式中，
Figure FDA0003030567070000027
为k时刻量测矩阵，g为当地重力加速度，φk、θk为k时刻水平姿态一步预测值，Kk为k时刻滤波增益，
Figure FDA0003030567070000028
为k时刻状态估计，Pk为k时刻状态估计均方误差阵。
CN202110428645.5A 2021-04-21 2021-04-21 一种基于运动状态监测的自适应水平姿态测量方法 Active CN113175926B (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN202110428645.5A CN113175926B (zh) 2021-04-21 2021-04-21 一种基于运动状态监测的自适应水平姿态测量方法
PCT/CN2022/087805 WO2022222938A1 (zh) 2021-04-21 2022-04-20 一种基于运动状态监测的自适应水平姿态测量方法
US17/844,224 US20220326017A1 (en) 2021-04-21 2022-06-20 Self-Adaptive Horizontal Attitude Measurement Method based on Motion State Monitoring

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110428645.5A CN113175926B (zh) 2021-04-21 2021-04-21 一种基于运动状态监测的自适应水平姿态测量方法

Publications (2)

Publication Number Publication Date
CN113175926A true CN113175926A (zh) 2021-07-27
CN113175926B CN113175926B (zh) 2022-06-21

Family

ID=76923958

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110428645.5A Active CN113175926B (zh) 2021-04-21 2021-04-21 一种基于运动状态监测的自适应水平姿态测量方法

Country Status (2)

Country Link
CN (1) CN113175926B (zh)
WO (1) WO2022222938A1 (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111770A (zh) * 2021-11-19 2022-03-01 清华大学 一种水平姿态测量方法、***、处理设备及存储介质
WO2022222938A1 (zh) * 2021-04-21 2022-10-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法

Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040149036A1 (en) * 2000-04-21 2004-08-05 Eric Foxlin Motion-tracking
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
US20070118286A1 (en) * 2005-11-23 2007-05-24 The Boeing Company Ultra-tightly coupled GPS and inertial navigation system for agile platforms
CN101419080A (zh) * 2008-06-13 2009-04-29 哈尔滨工程大学 微型捷联惯性测量***的零速校正方法
CN102508278A (zh) * 2011-11-28 2012-06-20 北京航空航天大学 一种基于观测噪声方差阵估计的自适应滤波方法
CN102607562A (zh) * 2012-04-12 2012-07-25 南京航空航天大学 基于载体飞行模态判别的微惯性参数自适应姿态确定方法
US20130211713A1 (en) * 2010-06-25 2013-08-15 Trusted Positioning Inc. Moving platform ins range corrector (mpirc)
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法
CN104316055A (zh) * 2014-09-19 2015-01-28 南京航空航天大学 一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法
CN106697333A (zh) * 2017-01-12 2017-05-24 北京理工大学 一种航天器轨道控制策略的鲁棒性分析方法
CN109341717A (zh) * 2018-09-13 2019-02-15 红色江山(湖北)导航技术有限公司 基于机动状态判断的mems惯性导航***水平姿态自修正方法
CN110132271A (zh) * 2019-01-02 2019-08-16 中国船舶重工集团公司第七0七研究所 一种自适应卡尔曼滤波姿态估计算法
CN110398257A (zh) * 2019-07-17 2019-11-01 哈尔滨工程大学 Gps辅助的sins***快速动基座初始对准方法
CN110440756A (zh) * 2019-06-28 2019-11-12 中国船舶重工集团公司第七0七研究所 一种惯导***姿态估计方法
CN110702143A (zh) * 2019-10-19 2020-01-17 北京工业大学 基于李群描述的sins捷联惯性导航***动基座快速初始对准方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111623779A (zh) * 2020-05-20 2020-09-04 哈尔滨工程大学 一种适用于噪声特性未知的时变***自适应级联滤波方法
CN112254723A (zh) * 2020-10-13 2021-01-22 天津津航计算技术研究所 基于自适应ekf算法的小型无人机marg航姿估计方法
CN112378401A (zh) * 2020-08-28 2021-02-19 中国船舶重工集团公司第七0七研究所 一种惯导***运动加速度估计方法
CN112504275A (zh) * 2020-11-16 2021-03-16 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100552688B1 (ko) * 2003-09-08 2006-02-20 삼성전자주식회사 관성 항법 시스템의 자세 보상 방법 및 장치, 이를 이용한관성 항법 시스템의 위치 계산 방법 및 장치
CN102096086B (zh) * 2010-11-22 2012-09-05 北京航空航天大学 一种基于gps/ins组合导航***不同测量特性的自适应滤波方法
CN108844536B (zh) * 2018-04-04 2020-07-03 中国科学院国家空间科学中心 一种基于量测噪声协方差矩阵估计的地磁导航方法
CN109163721B (zh) * 2018-09-18 2020-06-09 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法

Patent Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040149036A1 (en) * 2000-04-21 2004-08-05 Eric Foxlin Motion-tracking
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
US20070118286A1 (en) * 2005-11-23 2007-05-24 The Boeing Company Ultra-tightly coupled GPS and inertial navigation system for agile platforms
CN101419080A (zh) * 2008-06-13 2009-04-29 哈尔滨工程大学 微型捷联惯性测量***的零速校正方法
US20130211713A1 (en) * 2010-06-25 2013-08-15 Trusted Positioning Inc. Moving platform ins range corrector (mpirc)
CN102508278A (zh) * 2011-11-28 2012-06-20 北京航空航天大学 一种基于观测噪声方差阵估计的自适应滤波方法
CN102607562A (zh) * 2012-04-12 2012-07-25 南京航空航天大学 基于载体飞行模态判别的微惯性参数自适应姿态确定方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法
CN104316055A (zh) * 2014-09-19 2015-01-28 南京航空航天大学 一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法
CN106697333A (zh) * 2017-01-12 2017-05-24 北京理工大学 一种航天器轨道控制策略的鲁棒性分析方法
CN109341717A (zh) * 2018-09-13 2019-02-15 红色江山(湖北)导航技术有限公司 基于机动状态判断的mems惯性导航***水平姿态自修正方法
CN110132271A (zh) * 2019-01-02 2019-08-16 中国船舶重工集团公司第七0七研究所 一种自适应卡尔曼滤波姿态估计算法
CN110440756A (zh) * 2019-06-28 2019-11-12 中国船舶重工集团公司第七0七研究所 一种惯导***姿态估计方法
CN110398257A (zh) * 2019-07-17 2019-11-01 哈尔滨工程大学 Gps辅助的sins***快速动基座初始对准方法
CN110702143A (zh) * 2019-10-19 2020-01-17 北京工业大学 基于李群描述的sins捷联惯性导航***动基座快速初始对准方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111623779A (zh) * 2020-05-20 2020-09-04 哈尔滨工程大学 一种适用于噪声特性未知的时变***自适应级联滤波方法
CN112378401A (zh) * 2020-08-28 2021-02-19 中国船舶重工集团公司第七0七研究所 一种惯导***运动加速度估计方法
CN112254723A (zh) * 2020-10-13 2021-01-22 天津津航计算技术研究所 基于自适应ekf算法的小型无人机marg航姿估计方法
CN112504275A (zh) * 2020-11-16 2021-03-16 哈尔滨工程大学 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
JOSÉ F. VASCONCELOS等: "Discrete-Time Complementary Filters for Attitude and Position Estimation: Design, Analysis and Experimental Validation", 《IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY》 *
QIMIN XU等: "Enhancing Localization Accuracy of MEMS-INS/GPS/In-Vehicle Sensors Integration During GPS Outages", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 *
WEI GAO等: "Performance Comparison of Two Filtering Approaches for INS Rapid Transfer Alignment", 《2007 INTERNATIONAL CONFERENCE ON MECHATRONICS AND AUTOMATION》 *
任旭阳: "SINS/GPS/OD容错组合导航***研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
奔粤阳: "高动态环境捷联惯导算法研究", 《中国博士学位论文全文数据库 工程科技Ⅱ辑》 *
苑艳华等: "基于卡尔曼滤波器的航姿***测姿算法研究", 《传感技术学报》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022222938A1 (zh) * 2021-04-21 2022-10-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN114111770A (zh) * 2021-11-19 2022-03-01 清华大学 一种水平姿态测量方法、***、处理设备及存储介质
CN114111770B (zh) * 2021-11-19 2024-04-09 清华大学 一种水平姿态测量方法、***、处理设备及存储介质

Also Published As

Publication number Publication date
WO2022222938A1 (zh) 2022-10-27
CN113175926B (zh) 2022-06-21

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN113175926B (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN110231029B (zh) 一种水下机器人多传感器融合数据处理方法
CN110702113B (zh) 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法
CN112857398B (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN109086250B (zh) 适用于带斜置光纤陀螺的mems惯组的数据融合方法
CN111121820B (zh) 基于卡尔曼滤波的mems惯性传感器阵列融合方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN113434806B (zh) 一种抗差自适应多模型滤波方法
CN110375773B (zh) Mems惯导***姿态初始化方法
CN110736459B (zh) 惯性量匹配对准的角形变测量误差评估方法
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN110471293B (zh) 一种估计时变角速度的z轴陀螺仪滑模控制方法
US20220326017A1 (en) Self-Adaptive Horizontal Attitude Measurement Method based on Motion State Monitoring
CN111290279B (zh) 一种基于误差转换函数的神经网络滑模控制方法
CN109631895B (zh) 一种物体的位姿估计方法和装置
CN116972833A (zh) 一种基于自适应增量式卡尔曼滤波的水平姿态测量***及方法

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