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

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

Info

Publication number
WO2022222938A1
WO2022222938A1 PCT/CN2022/087805 CN2022087805W WO2022222938A1 WO 2022222938 A1 WO2022222938 A1 WO 2022222938A1 CN 2022087805 W CN2022087805 W CN 2022087805W WO 2022222938 A1 WO2022222938 A1 WO 2022222938A1
Authority
WO
WIPO (PCT)
Prior art keywords
time
state
horizontal attitude
angular velocity
matrix
Prior art date
Application number
PCT/CN2022/087805
Other languages
English (en)
French (fr)
Inventor
赵玉新
李帅阳
奔粤阳
吴磊
李倩
周广涛
臧新乐
魏廷枭
王健成
周一帆
Original Assignee
哈尔滨工程大学
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 哈尔滨工程大学 filed Critical 哈尔滨工程大学
Priority to US17/844,224 priority Critical patent/US20220326017A1/en
Publication of WO2022222938A1 publication Critical patent/WO2022222938A1/zh

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

Definitions

  • the invention relates to an adaptive horizontal attitude measurement method based on motion state monitoring, and belongs to the field of inertial technology.
  • MEMS-IMU With the development of MEMS technology, low-cost MEMS-IMU has more and more applications in the field of navigation.
  • inertial sensors based on MEMS to measure motion parameters
  • the complex motion state of ships in the sea can be detected, and the realization of The user collects the motion data of the surface ship, which requires the MEMS-IMU to be able to accurately output the angular motion parameters and linear motion parameters of the carrier in real time.
  • the MEMS gyroscope has the characteristics of random drift, and its integral error accumulates with time.
  • the accelerometer has no accumulated error, but it is easily affected by the vibration of the carrier.
  • the commonly used algorithms to fuse the two data are Kalman filtering and complementary filtering.
  • Kalman filtering titled "The Self-correction Method of Horizontal Attitude of MEMS Inertial Navigation System Based on Maneuver State Judgment"
  • the carrier motion is classified into low, medium, and high dynamics.
  • the measurement noise matrix is adjusted in real-time at low and medium dynamics, and only time-updated at high dynamics.
  • the technical problem to be solved by the present invention is to provide an adaptive horizontal attitude measurement method based on motion state monitoring, which can improve the horizontal attitude measurement accuracy of the carrier in the maneuvering scene, and provide an accurate horizontal attitude for the carrier. posture information.
  • an adaptive horizontal attitude measurement method based on motion state monitoring of the present invention comprises the following steps:
  • Step 1 Initially align the strapdown inertial navigation system, complete the calculation of the random constant zero bias of the device and the calculation of the initial horizontal attitude angle, including the roll angle ⁇ 0 and the pitch angle ⁇ 0 , and then enter the navigation working mode;
  • Step 3 Sampling the MEMS-IMU data at the k-th time, and compensate its random constant zero offset to obtain the compensated specific force and angular velocity
  • Step 4 Use the angular velocity increment at the kth time As a known deterministic input u k-1 , a Kalman filter one-step prediction is performed, where, T is the solution period;
  • Step 5 Use the specific force f b and angular velocity ⁇ b obtained from time k-N+1 to time k in step 3 to discriminate the maneuvering state of the carrier, and adaptively adjust the Kalman filter measurement noise covariance matrix R k , where N is the size of the data window;
  • Step 7 Take the estimated value of the state quantity at the kth moment as the initial value of the state quantity at the next moment, and repeat steps 3-6 until the navigation working state ends.
  • the present invention also includes:
  • step 5 using the specific force f b and angular velocity ⁇ b obtained in step 3 to judge the maneuvering state of the carrier is specifically: using the specific force f obtained in step 3 from time k-N+1 to time k b and the angular velocity ⁇ b calculate the maneuvering state of the carrier, and obtain the maneuvering vector T k to realize maneuver judgment.
  • T k satisfies:
  • step 5 the adaptive adjustment of the Kalman filter measurement noise covariance matrix R k is as follows:
  • T k,i is the ith element of T k , corresponds to the variance of the accelerometer noise.
  • g is the local gravitational acceleration
  • ⁇ k is the one-step prediction values of the horizontal attitude at time k
  • K k is the filter gain at time k
  • P k is the mean square error matrix of state estimation at time k.
  • the present invention relates to an attitude measurement unit with a micro-electromechanical inertial measurement unit (MEMS-IMU) as the core device, the present invention establishes a new state space model, takes the horizontal attitude angle as a state variable, and the output of the MEMS-IMU The angular velocity increment ⁇ b is used as the control input of the system equation, and the specific force f b output by the MEMS-IMU is used as the quantity measurement.
  • the carrier maneuvering judgment conditions are improved, and the acceleration information and angular velocity information output by the MEMS-IMU are comprehensively used to evaluate the carrier maneuvering information.
  • the method has no special requirements for the carrier's maneuvering state, and can ensure that the system has high attitude measurement accuracy in different motion states without the assistance of external information, and has certain engineering application value.
  • Fig. 1 is the realization flow chart of the present invention
  • Fig. 2 is the realization algorithm flow chart of the present invention
  • FIG. 3 is an algorithm for solving the error proposed by the present invention.
  • the present invention is realized in this way:
  • Step 1 Fully preheat the inertial measurement element of the strapdown inertial navigation system, and complete the calculation of the random constant zero bias of the device and the calculation of the initial horizontal attitude angle (roll angle ⁇ 0 and pitch angle ⁇ 0 ) to enter the Navigation working status;
  • Step 3 Sampling the MEMS-IMU data at the k-th time, and compensate its random constant zero offset to obtain the compensated specific force and angular velocity
  • Step 4 Use the angular velocity increment at the kth time as a known deterministic input u k-1 , a Kalman filter one-step prediction is performed, where, T is the solution period;
  • Step 5 Use the specific force f b and angular velocity ⁇ b obtained in step 2 from time k-N+1 to time k to discriminate the maneuvering state of the carrier, and adjust the Kalman filter measurement noise covariance matrix R k adaptively. , where N is the size of the data window;
  • Step 6 Use the comparative force at the kth time As the measurement vector Z k , the measurement is updated to realize the correction of the state quantity;
  • Step 7 Use the estimated value of the state quantity at the kth moment as the initial value of the state quantity at the next moment, and repeat steps 3-6 until the navigation working state ends.
  • step 4 the Kalman filter one-step prediction correlation equation is established:
  • step 5 the maneuvering state of the carrier is judged, and the maneuvering state of the carrier is calculated according to the specific force f b and angular velocity ⁇ b obtained in step 3, and the maneuvering vector T k is obtained, thereby realizing maneuvering judgment:
  • the size of the data window is N
  • g is the local acceleration of gravity
  • ⁇ f and ⁇ ⁇ are the weight coefficients, respectively;
  • step 5 the adaptive rule of Kalman filtering to measure the noise covariance matrix R k is as follows:
  • T k,i is the ith element of T k , is the variance corresponding to the noise of the accelerometer
  • Measurement update in step 6 using the specific force at the kth time in step 3
  • the measurement update is performed, and the update equation is as follows:
  • ⁇ k and ⁇ k are the one-step prediction values of the horizontal attitude at time k
  • K k is the filter gain at time k
  • P k is the mean square error matrix of state estimation at time k.
  • Step 1 Fully preheat the inertial measurement element of the strapdown inertial navigation system
  • Step 2 Collect the data of the MEMS-IMU in a static state for a period of time, consider the average output value of the MEMS-IMU in this period of time as the random constant zero bias of the device, and correct the constant zero bias error of the gyroscope and accelerometer Compensation, obtain the specific force f b and angular velocity ⁇ b after compensating the constant zero bias error;
  • Step 3 perform initial alignment on the strapdown inertial navigation system, and obtain the initial horizontal attitude angle (roll angle) of the carrier system (b system) relative to the navigation coordinate system (n system, the navigation coordinate system in the present invention selects the geographic coordinate system). ⁇ 0 and pitch angle ⁇ 0 ), and then start to enter the navigation working state;
  • Step 4 Establish the Kalman filter equation as:
  • X k is the state vector
  • ⁇ k/k-1 is the state one-step transition matrix
  • B k-1 is the input coefficient matrix
  • I 2 is the second-order identity matrix
  • u k-1 is the known deterministic input
  • Z k is the measurement vector
  • H k is the measurement matrix
  • W k-1 is the system noise vector
  • V k is the measurement noise vector
  • Step 6 Sampling the MEMS-IMU data at the k-th time, and compensating for its random constant zero offset to obtain the compensated specific force and angular velocity
  • Step 7 Use the kth moment in step 6 Increment within sample interval (T is the solution cycle) as the known deterministic input u k-1 , and perform one-step prediction by Kalman filter:
  • Step 8 Calculate the maneuvering state of the carrier according to the specific force f b and the angular velocity ⁇ b obtained from the time k-N+1 to the time k in step 6, and obtain the maneuver vector T k , so as to realize the measurement of the Kalman filter.
  • Adaptive adjustment of the noise covariance matrix R k Adaptive adjustment of the noise covariance matrix R k :
  • the size of the data window is N
  • g is the local gravitational acceleration
  • ⁇ f is the ratio
  • ⁇ is the weight coefficient of angular velocity (1-5 for ships and 0.5-1 for vehicles);
  • T k,i is the ith element of T k , is the variance corresponding to the noise of the accelerometer
  • Step 10 Use the estimated value of the state quantity at the kth time as the initial value of the state quantity at the next time, and repeat steps 6-9.
  • the algorithm is simulated.
  • the simulation conditions are set as follows: the zero bias of each axis of the gyroscope is set to be 1°/h, and the zero bias of the accelerometer is set to be 1 ⁇ 10 -4 g.
  • the motion state is set as follows: the carrier is in a rocking motion state, and the rocking of each axis obeys the law of a sine function.
  • the rocking amplitudes of roll, pitch and heading are all 10°, and the rocking period is all 10s.
  • the initial attitude angle and phase angle are all 10°. Both are 0°, and the simulation results are shown in Figure 3.
  • the horizontal attitude measurement error is small and the accuracy is high.

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作为系统方程的控制输入，补偿随机常值零偏的比力f b作为量测量，同时对载体机动状态的判断条件进行改善，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息进行判断，实现对滤波器量测噪声阵的自动调整，降低载体机动对水平姿态解算的影响。基于运动状态监测的自适应水平姿态测量方法对载体机动状态无特殊要求，能够在无外界信息辅助的情况下，保证系统在不同运动状态下具有较高的姿态测量精度。

Description

一种基于运动状态监测的自适应水平姿态测量方法 技术领域
本发明涉及一种基于运动状态监测的自适应水平姿态测量方法，属于惯性技术领域。
背景技术
随着微机电系统技术的发展，低成本MEMS-IMU在导航领域有着越来越多的应用，通过利用基于微机电系统的惯性传感器进行运动参数测量，可以检测船舶在海中复杂的运动状态，实现用户对水面舰船的运动数据采集，这要求MEMS-IMU能够实时准确地输出载体的角运动参数和线运动参数。
微机电陀螺仪具有随机漂移特性，其积分误差随时间累积，加速度计不存在累积误差，但是易受到载体震动影响。常用的将二者数据融合的算法是卡尔曼滤波和互补滤波，例如在专利申请号为201811070907.X，名称为“基于机动状态判断的MEMS惯性导航系统水平姿态自修正方法”的专利文件中，通过比较加速度计输出和当地重力加速度幅值，将载体运动分为低、中、高动态。在低和中动态时，实时调整量测噪声矩阵，在高动态时只进行时间更新。但是若载体较长时间处于高动态下，则姿态误差会越来越大。又如在专利申请号为202011092956.0，名称为“基于自适应EKF算法的小型无人机MARG航姿估计方法”的专利文件中，针对外部加速度的自适应滤波算法，通过对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。传统自适应调整的方法大多通过对载体加速度计输出的模值与当地重力加速度进行比较，实现运载体机动状态的判断，未考虑载体角运动对加速度测量的干扰，这对于一些复杂环境来说可能是不充分的。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种基于运动状态监测的自适应水平姿态测量方法，可以提高运载体在机动场景下的水平姿态测量精度，为运载体提供准确的水平姿态信息。
为解决上述技术问题，本发明的一种基于运动状态监测的自适应水平姿态测量方法，包括以下步骤：
步骤1：对捷联惯性导航系统进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角θ 0和俯仰角φ 0，然后进入导航工作模式；
步骤2：初始化卡尔曼滤波器，将步骤1中得到的初始水平姿态角θ 0和φ 0作为卡尔曼滤波状态量X 0＝[θ 0 φ 0] T的初始值，初始均方误差为P 0；
步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的 比力
Figure PCTCN2022087805-appb-000001
和角速度
Figure PCTCN2022087805-appb-000002
步骤4：利用第k时刻的角速度增量
Figure PCTCN2022087805-appb-000003
作为已知的确定性输入u k-1，进行卡尔曼滤波一步预测，其中，
Figure PCTCN2022087805-appb-000004
T为解算周期；
步骤5：利用步骤3中获得k-N+1时刻到k时刻的比力f b和角速度ω b对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵R k自适应调节，其中N为数据窗的大小；
步骤6：第k时刻比力为
Figure PCTCN2022087805-appb-000005
选取量测向量Z k＝[f x,k f y,k] T进行量测更新，实现状态量的校正，其中f x,k、f y,k和f z,k分别为比力
Figure PCTCN2022087805-appb-000006
在载体系x轴、y轴和z轴方向的分量；
步骤7：将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。
本发明还包括：
1.步骤4中卡尔曼滤波一步预测具体为：
Figure PCTCN2022087805-appb-000007
式中，
Figure PCTCN2022087805-appb-000008
为k-1时刻状态最优估计，P k-1为k-1时刻状态估计的均方误差阵，
Figure PCTCN2022087805-appb-000009
为k时刻状态一步预测，Φ k/k-1＝I 2为k时刻状态一步转移矩阵，I 2为二阶单位阵，
Figure PCTCN2022087805-appb-000010
为k时刻输入系数矩阵，φ k-1、θ k-1为k-1时刻水平姿态最优估计，u k-1为第k时刻角速度增量
Figure PCTCN2022087805-appb-000011
P k/k-1为k时刻状态一部预测均方误差阵，Q k为系统噪声方差阵。
2.步骤5所述利用步骤3中获得的比力f b和角速度ω b对运载体的机动状态进行判别具体为：利用步骤3中获得的k-N+1时刻到k时刻的比力f b和角速度ω b对运载体的机动状态进行计算，求取机动向量T k，实现机动判断，T k满足：
Figure PCTCN2022087805-appb-000012
式中，
Figure PCTCN2022087805-appb-000013
和
Figure PCTCN2022087805-appb-000014
分别为对步骤3中第k-N+1时刻到k时刻惯性测量单元输出且补偿随机常值零偏的比力f b和角速度ω b进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σ f和σ ω分别为权重系数；
3.步骤5中对卡尔曼滤波量测噪声协方差矩阵R k自适应调节具体为：
Figure PCTCN2022087805-appb-000015
式中，T k,i为T k的第i个元素，
Figure PCTCN2022087805-appb-000016
为加速度计噪声对应方差。
4.步骤6中量测更新的更新方程为：
Figure PCTCN2022087805-appb-000017
式中，
Figure PCTCN2022087805-appb-000018
为k时刻量测矩阵，g为当地重力加速度，φ k、θ k为k时刻水平姿态一步预测值，K k为k时刻滤波增益，
Figure PCTCN2022087805-appb-000019
为k时刻状态估计，P k为k时刻状态估计均方误差阵。
本发明的有益效果：本发明涉及以微机电惯性测量单元(MEMS-IMU)为核心器件的姿态测量单元，本发明建立新的状态空间模型，将水平姿态角作为状态变量，MEMS-IMU输出的角速度增量Δω b作为系统方程的控制输入，MEMS-IMU输出的比力f b作为量测量，同时对载体机动判断条件进行改善，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息的判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对对平姿态信息的影响。该方法对载体机动状态无特殊要求，能够在无外界信息辅助的情况下，保证系统在不同运动状态下具有较高的姿态测量精度，有一定的工程应用价值。
附图说明
图1为本发明的实现流程图；
图2为本发明的实现算法流程图；
图3为本发明提出算法解算误差。
具体实施方式
下面结合附图和具体实施方式对本发明做进一步说明。
本发明是这样实现的：
对捷联惯性导航系统的惯性测量元件进行充分预热，并完成器件随机常值零偏计算和初始水平姿态角的计算，之后可进入导航工作状态；将水平姿态角作为状态变量，MEMS-IMU输出的角速度增量Δω b作为系统方程的控制输入，MEMS-IMU输出的比力f b作为量测量，建立卡尔曼滤波方程，综合利用MEMS-IMU输出的加速度信息和角速度信息对载体机动信息进行判断，实现对滤波器量测噪声阵的自动调整，有效降低了载体机动对水平姿态解算的影响。具体步骤如下：
步骤1、对捷联惯性导航系统的惯性测量元件进行充分预热，并完成器件随机常值零偏计算和初始水平姿态角(横滚角θ 0和俯仰角φ 0)的计算，使之进入导航工作状态；
步骤2、将步骤1中得到的初始水平姿态角θ 0和φ 0作为卡尔曼滤波状态量X 0＝[θ 0 φ 0] T的初始值，初始均方误差为P 0，初始化卡尔曼滤波器；
步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
Figure PCTCN2022087805-appb-000020
和角速度
Figure PCTCN2022087805-appb-000021
步骤4、利用第k时刻的角速度增量
Figure PCTCN2022087805-appb-000022
作为已知的确定性输入u k-1，进行卡尔曼滤波一步预测，其中，
Figure PCTCN2022087805-appb-000023
T为解算周期；；
步骤5、利用步骤2中获得k-N+1时刻到k时刻的比力f b和角速度ω b对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵R k自适应调节，其中N为数据窗的大小；
步骤6、利用第k时刻的比力
Figure PCTCN2022087805-appb-000024
作为量测向量Z k进行量测更新，实现状态量的校正；
步骤7、将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。
步骤4中卡尔曼滤波一步预测相关方程建立：
Figure PCTCN2022087805-appb-000025
式中，
Figure PCTCN2022087805-appb-000026
为k-1时刻状态最优估计，P k-1为k-1时刻状态估计的均方误差阵，
Figure PCTCN2022087805-appb-000027
为k时刻状态一步预测，Φ k/k-1＝I 2为k时刻状态一步转移矩阵，I 2为二阶单位阵，
Figure PCTCN2022087805-appb-000028
为k时刻输入系数矩阵，φ k-1、θ k-1为k-1时刻水平姿态最优估计，u k-1为第k时刻角速度增量
Figure PCTCN2022087805-appb-000029
P k/k-1为k时刻状态一部预测均方误差阵，Q k为系统噪声方差阵；
步骤5运载体机动状态判断，根据步骤3中获得的比力f b和角速度ω b对运载体的机动状态进行计算，求取机动向量T k，从而实现机动判断：
Figure PCTCN2022087805-appb-000030
式中，
Figure PCTCN2022087805-appb-000031
和
Figure PCTCN2022087805-appb-000032
分别为对步骤3中第k-N+1时刻到k时刻的比力f b和角速度ω b进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σ f和σ ω分别为权重系数；
步骤5中卡尔曼滤波量测噪声协方差矩阵R k的自适应规则如下：
Figure PCTCN2022087805-appb-000033
式中，T k,i为T k的第i个元素，
Figure PCTCN2022087805-appb-000034
为加速度计噪声对应方差；
步骤6中量测更新，利用步骤3中第k时刻的比力
Figure PCTCN2022087805-appb-000035
作为量测向量Z k＝[f x,k f y,k] T进行量测更新，更新方程如下：
Figure PCTCN2022087805-appb-000036
式中，
Figure PCTCN2022087805-appb-000037
为k时刻量测矩阵，φ k、θ k为k时刻水平姿态一步预测值，K k为k时刻滤波增益，
Figure PCTCN2022087805-appb-000038
为k时刻状态估计，P k为k时刻状态估计均方误差阵。
结合图1和图2，本发明的具体实施方式包括以下步骤：
步骤1、对捷联惯性导航系统的惯性测量元件进行充分预热；
步骤2、采集一段时间内静止状态下MEMS-IMU的数据，认为该时间段内MEMS-IMU 的输出平均值为器件随机常值零偏，对陀螺仪和加速度计的常值零偏误差进行校正补偿，获得补偿常值零偏误差后比力f b和角速度ω b；
步骤3、对捷联惯性导航系统进行初始对准，得到载体系(b系)相对于导航坐标系(n系，本发明中导航坐标系选取地理坐标系)的初始水平姿态角(横滚角θ 0和俯仰角φ 0)，之后开始进入导航工作状态；
步骤4、建立卡尔曼滤波方程为：
Figure PCTCN2022087805-appb-000039
式中，X k为状态向量，Φ k/k-1为状态一步转移矩阵，B k-1为输入系数矩阵，I 2为二阶单位阵，u k-1为已知的确定性输入，Z k是量测向量，H k为量测矩阵，W k-1是系统噪声向量，V k是量测噪声向量；
步骤5、选取水平姿态角作为状态量X＝[θ φ] T(横滚角θ和俯仰角φ)，根据步骤3中得到的初始水平姿态θ 0和φ 0作为状态量X 0＝[θ 0 φ 0] T的初始值，初始均方误差为P 0，初始化卡尔曼滤波器；
步骤6：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
Figure PCTCN2022087805-appb-000040
和角速度
Figure PCTCN2022087805-appb-000041
步骤7、利用步骤6中第k时刻
Figure PCTCN2022087805-appb-000042
采样间隔内的增量
Figure PCTCN2022087805-appb-000043
(T为解算周期)作为已知的确定性输入u k-1，进行卡尔曼滤波一步预测：
Figure PCTCN2022087805-appb-000044
式中，
Figure PCTCN2022087805-appb-000045
为k-1时刻状态最优估计，P k-1为k-1时刻状态估计的均方误差阵，
Figure PCTCN2022087805-appb-000046
为k时刻状态一步预测，Φ k/k-1＝I 2为k时刻状态一步转移矩阵，
Figure PCTCN2022087805-appb-000047
为k时刻输入系数矩阵，φ k-1、θ k-1为k-1时刻水平姿态最优估计，u k-1为第k时刻角速度增量
Figure PCTCN2022087805-appb-000048
P k/k-1为k时刻状态一部预测均方误差阵，Q k为系统噪声方差阵；
步骤8、根据步骤6中获得k-N+1时刻到k时刻的比力f b和角速度ω b对运载体的机动状态进行计算，求取机动向量T k，从而实现对卡尔曼滤波量测噪声协方差矩阵R k的自适应调节：
Figure PCTCN2022087805-appb-000049
式中，
Figure PCTCN2022087805-appb-000050
和
Figure PCTCN2022087805-appb-000051
分别为对对步骤6中第k-N+1时刻到k时刻的比力f b和角速度ω b进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σ f为比力的权重系数(对于舰船常取0.5-5，对于车辆常取1-3),σ ω为角速度的权重系数(对于舰船常取1-5，对于车辆常取0.5-1)；
卡尔曼滤波量测噪声协方差矩阵R k的自适应调节规则如下：
Figure PCTCN2022087805-appb-000052
式中，T k,i为T k的第i个元素，
Figure PCTCN2022087805-appb-000053
为加速度计噪声对应方差；
步骤9、利用步骤6中第k时刻的比力
Figure PCTCN2022087805-appb-000054
作为量测向量Z k＝[f x,k f y,k] T进行量测更新，更新方程如下：
Figure PCTCN2022087805-appb-000055
式中，
Figure PCTCN2022087805-appb-000056
为k时刻量测矩阵，g为当地重力加速度，φ k、θ k为k时刻水平姿态一步预测值，K k为k时刻滤波增益，
Figure PCTCN2022087805-appb-000057
为k时刻状态估计，P k为k时刻状态估计均方误差阵；
步骤10、将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤6-9。
至此就完成了基于运动状态监测的自适应水平姿态测量方法。
为了说明算法的有效性，对算法进行仿真。仿真条件设置如下：设各轴陀螺仪零偏均为1°/h，加速度计零偏均为1×10 -4g。设置运动状态如下：运载体处于摇摆运动状态，各轴摇摆均服从正弦函数规律，其中横摇、纵摇与航向的摇摆幅值均为10°，摇摆周期均为10s，初始姿态角和相位角均为0°，仿真结果如附图3所示，水平姿态测量误差较小，精度较高。
尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和附图所公开的内容。

Claims (5)

  1. 一种基于运动状态监测的自适应水平姿态测量方法，其特征在于，包括以下步骤：
    步骤1：对捷联惯性导航系统进行初始对准，完成器件随机常值零偏的计算和初始水平姿态角的计算，包括横滚角
    Figure PCTCN2022087805-appb-100001
    和俯仰角φ 0，然后进入导航工作模式；
    步骤2：初始化卡尔曼滤波器，将步骤1中得到的初始水平姿态角
    Figure PCTCN2022087805-appb-100002
    和φ 0作为卡尔曼滤波状态量
    Figure PCTCN2022087805-appb-100003
    的初始值，初始均方误差为P 0；
    步骤3：对第k时刻MEMS-IMU数据进行采样，并补偿其随机常值零偏，获得补偿后的比力
    Figure PCTCN2022087805-appb-100004
    和角速度
    Figure PCTCN2022087805-appb-100005
    步骤4：利用第k时刻的角速度增量
    Figure PCTCN2022087805-appb-100006
    作为已知的确定性输入u k-1，进行卡尔曼滤波一步预测，其中，
    Figure PCTCN2022087805-appb-100007
    T为解算周期；
    步骤5：利用步骤3中获得k-N+1时刻到k时刻的比力f b和角速度ω b对运载体的机动状态进行判别，对卡尔曼滤波量测噪声协方差矩阵R k自适应调节，其中N为数据窗的大小；
    步骤6：第k时刻比力为
    Figure PCTCN2022087805-appb-100008
    选取量测向量Z k＝[f x,k f y,k] T进行量测更新，实现状态量的校正，其中f x,k、f y,k和f z,k分别为比力
    Figure PCTCN2022087805-appb-100009
    在载体系x轴、y轴和z轴方向的分量；
    步骤7：将第k时刻状态量的估计值作为下一时刻状态量的初始值，重复进行步骤3-6至导航工作状态结束。
  2. 根据权利要求1所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤4所述卡尔曼滤波一步预测具体为：
    Figure PCTCN2022087805-appb-100010
    式中，
    Figure PCTCN2022087805-appb-100011
    为k-1时刻状态最优估计，P k-1为k-1时刻状态估计的均方误差阵，
    Figure PCTCN2022087805-appb-100012
    为k时刻状态一步预测，Φ k/k-1＝I 2为k时刻状态一步转移矩阵，I 2为二阶单位阵，
    Figure PCTCN2022087805-appb-100013
    为k时刻输入系数矩阵，φ k-1、
    Figure PCTCN2022087805-appb-100014
    为k-1时刻水平姿态最优估计，u k-1为第k时刻角速度增量
    Figure PCTCN2022087805-appb-100015
    P k/k-1为k时刻状态一步预测均方误差阵，Q k为系统噪声方差阵。
  3. 根据权利要求1或2所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤5所述利用步骤3中获得的比力f b和角速度ω b对运载体的机动状态进行判别具体为：利用步骤3中获得的k-N+1时刻到k时刻的比力f b和角速度ω b对运载体的机动状态进行计算，求取机动向量T k，实现机动判断，T k满足：
    Figure PCTCN2022087805-appb-100016
    式中，
    Figure PCTCN2022087805-appb-100017
    和
    Figure PCTCN2022087805-appb-100018
    分别为对步骤3中第k-N+1时刻到k时刻的比力f b和角速度ω b进行滑动平均获得的均值，数据窗的大小为N，g为当地重力加速度，σ f和σ ω分别为权重系数。
  4. 根据权利要求3所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤5所述对卡尔曼滤波量测噪声协方差矩阵R k自适应调节具体为：
    Figure PCTCN2022087805-appb-100019
    式中，T k,i为T k的第i个元素，
    Figure PCTCN2022087805-appb-100020
    为加速度计噪声对应方差。
  5. 根据权利要求1或2所述的一种基于运动状态监测的自适应水平姿态测量方法，其特征在于：步骤6所述量测更新的更新方程为：
    Figure PCTCN2022087805-appb-100021
    式中，
    Figure PCTCN2022087805-appb-100022
    为k时刻量测矩阵，g为当地重力加速度，φ k、
    Figure PCTCN2022087805-appb-100023
    为k时刻水平姿态一步预测值，K k为k时刻滤波增益，
    Figure PCTCN2022087805-appb-100024
    为k时刻状态估计，P k为k时刻状态估计均方误差阵。
PCT/CN2022/087805 2021-04-21 2022-04-20 一种基于运动状态监测的自适应水平姿态测量方法 WO2022222938A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
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 (2)

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

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US17/844,224 Continuation US20220326017A1 (en) 2021-04-21 2022-06-20 Self-Adaptive Horizontal Attitude Measurement Method based on Motion State Monitoring

Publications (1)

Publication Number Publication Date
WO2022222938A1 true WO2022222938A1 (zh) 2022-10-27

Family

ID=76923958

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/087805 WO2022222938A1 (zh) 2021-04-21 2022-04-20 一种基于运动状态监测的自适应水平姿态测量方法

Country Status (2)

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

Families Citing this family (2)

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

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050065728A1 (en) * 2003-09-08 2005-03-24 Samsung Electronics Co., Ltd. Method and apparatus for compensating attitude of inertial navigation system and method and apparatus for calculating position of inertial navigation system using the same
CN102096086A (zh) * 2010-11-22 2011-06-15 北京航空航天大学 一种基于gps/ins组合导航系统不同测量特性的自适应滤波方法
CN108844536A (zh) * 2018-04-04 2018-11-20 中国科学院国家空间科学中心 一种基于量测噪声协方差矩阵估计的地磁导航方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN110132271A (zh) * 2019-01-02 2019-08-16 中国船舶重工集团公司第七0七研究所 一种自适应卡尔曼滤波姿态估计算法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN113175926A (zh) * 2021-04-21 2021-07-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法

Family Cites Families (19)

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

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050065728A1 (en) * 2003-09-08 2005-03-24 Samsung Electronics Co., Ltd. Method and apparatus for compensating attitude of inertial navigation system and method and apparatus for calculating position of inertial navigation system using the same
CN102096086A (zh) * 2010-11-22 2011-06-15 北京航空航天大学 一种基于gps/ins组合导航系统不同测量特性的自适应滤波方法
CN108844536A (zh) * 2018-04-04 2018-11-20 中国科学院国家空间科学中心 一种基于量测噪声协方差矩阵估计的地磁导航方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN110132271A (zh) * 2019-01-02 2019-08-16 中国船舶重工集团公司第七0七研究所 一种自适应卡尔曼滤波姿态估计算法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN113175926A (zh) * 2021-04-21 2021-07-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法

Also Published As

Publication number Publication date
CN113175926A (zh) 2021-07-27
CN113175926B (zh) 2022-06-21

Similar Documents

Publication Publication Date Title
WO2022222938A1 (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN108225308B (zh) 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN101033973B (zh) 微小型飞行器微惯性组合导航系统的姿态确定方法
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN110017850B (zh) 一种陀螺仪漂移估计方法、装置及定位系统
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN110702113B (zh) 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN111141313B (zh) 一种提高机载局部相对姿态匹配传递对准精度的方法
CN113670314B (zh) 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法
CN110954102A (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN112857398A (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN114111843B (zh) 一种捷联惯导系统最优动基座初始对准方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110471293B (zh) 一种估计时变角速度的z轴陀螺仪滑模控制方法
CN109674480B (zh) 一种基于改进互补滤波的人体运动姿态解算方法
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN112591153B (zh) 基于抗干扰多目标h2/h∞滤波的空间机械臂末端定位方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22791045

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22791045

Country of ref document: EP

Kind code of ref document: A1