CN110440756B - Attitude estimation method of inertial navigation system - Google Patents

Attitude estimation method of inertial navigation system Download PDF

Info

Publication number
CN110440756B
CN110440756B CN201910571246.7A CN201910571246A CN110440756B CN 110440756 B CN110440756 B CN 110440756B CN 201910571246 A CN201910571246 A CN 201910571246A CN 110440756 B CN110440756 B CN 110440756B
Authority
CN
China
Prior art keywords
measurement
equation
attitude
accelerometer
quaternion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910571246.7A
Other languages
Chinese (zh)
Other versions
CN110440756A (en
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.)
707th Research Institute of CSIC
Original Assignee
707th Research Institute of CSIC
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 707th Research Institute of CSIC filed Critical 707th Research Institute of CSIC
Priority to CN201910571246.7A priority Critical patent/CN110440756B/en
Publication of CN110440756A publication Critical patent/CN110440756A/en
Application granted granted Critical
Publication of CN110440756B publication Critical patent/CN110440756B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/18Stabilised platforms, e.g. by gyroscope
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00Measuring inclination, e.g. by clinometers, by levels

Landscapes

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

Abstract

The invention relates to an attitude estimation method of an inertial navigation system, which is technically characterized by comprising the following steps: the method comprises the following steps: step 1, establishing a Kalman filter state equation on the basis of an attitude quaternion serving as a state quantity and a quaternion attitude updating differential equation; step 2, establishing a quaternion pseudo Kalman wave measurement equation by taking the measurement value of the accelerometer as input; and 3, performing Kalman filtering measurement updating according to the Kalman filtering state equation established in the step 1 and the quaternion pseudo Kalman wave measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on the measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of the measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under the dynamic condition. The invention effectively improves the estimation precision of the horizontal attitude under the dynamic condition.

Description

Attitude estimation method of inertial navigation system
Technical Field
The invention belongs to the technical field of IMU attitude measurement, and particularly relates to an attitude estimation method of an inertial navigation system.
Background
An IMU-based attitude measurement system uses accelerometer and gyroscope outputs to optimally estimate horizontal attitude. When the carrier has no motion acceleration, the accelerometer can accurately sense the local gravity vector and is fused with the output of the gyroscope to obtain a high-precision horizontal posture. However, when the carrier has a motion acceleration, the accelerometer output includes a local gravity acceleration and a motion acceleration, and a large error is caused when the horizontal attitude is calculated by taking the gravity vector as a reference. That is, the acceleration of the carrier movement interferes with the measurement of the accelerometer on the gravity vector, so that the horizontal attitude estimation is inaccurate. Therefore, how to effectively weaken the interference of the motion acceleration of the carrier enables the system to still have higher attitude measurement accuracy under the dynamic condition, and has certain research significance.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides an inertial navigation system attitude estimation method which can effectively weaken the interference of motion acceleration, realize the optimal estimation of attitude information and improve the horizontal attitude estimation precision under the dynamic condition of a system.
The invention solves the practical problem by adopting the following technical scheme:
an inertial navigation system attitude estimation method comprises the following steps:
step 1, establishing a Kalman filter state equation on the basis of an attitude quaternion serving as a state quantity and a quaternion attitude updating differential equation;
step 2, establishing a quaternion pseudo Kalman filtering measurement equation by taking the measurement value of the accelerometer as input;
and 3, performing Kalman filtering measurement updating according to the Kalman filtering state equation established in the step 1 and the quaternion pseudo Kalman wave measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on the measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of the measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under the dynamic condition.
Further, the specific steps of step 1 include:
(1) the output is y from the known accelerometera=[0 ax ay az]TThe gyroscope output is yg=[0 ωx ωyωz]TThe gravity vector is G ═ 000-G]TEstablishing a Kalman filtering state equation, wherein the basic form of the known state equation is as follows:
Xk+1=ΦkXk+Wk
wherein the state quantity X is [ q ]0 q1 q2 q3 εx εy εz]TQ is a carrier attitude quaternion, and epsilon is gyro constant drift; state transition matrix
Figure BDA0002110950150000021
Wherein T is a filter period;
Figure BDA0002110950150000022
wherein the content of the first and second substances,
Figure BDA0002110950150000023
Figure BDA0002110950150000024
Fεq=03×4,Fεε=03×3
(2) system noise matrix
Figure BDA0002110950150000025
Wherein, deltag,kThe noise variance is output for the gyroscope.
Further, the specific steps of step 2 include:
(1) the basic form of the known measurement equation is:
Zk=HkXk+Vk
when the carrier has no motion acceleration, the output of the accelerometer is yaAnd the gravity vector G has the following relationship:
Figure BDA0002110950150000031
both sides are multiplied by QkTo obtain
Figure BDA0002110950150000032
Further transformation can obtain
04×1=HkQk+Vk
Wherein the content of the first and second substances,
Figure BDA0002110950150000033
measuring Z as 04×1
(2) Measure noise matrix
Figure BDA0002110950150000034
δa,kOutput noise variance for accelerometer, ap,kIs the variance of the acceleration of the carrier motion.
Further, the specific steps of step 3 include:
(1) kalman filtering one-step prediction and measurement updating:
kalman filter parameter initial value X0=[1 0 0 0 0 0 0]T,P0=10I7×7The system state one-step prediction equation is as follows:
Figure BDA0002110950150000035
Figure BDA0002110950150000036
using accelerometer output ya=[0 ax ay az]TAnd (3) carrying out measurement updating, wherein an updating equation is as follows:
Figure BDA0002110950150000037
Figure BDA0002110950150000038
Figure BDA0002110950150000039
(2) and (3) self-adaptive adjustment of measurement noise variance:
accelerometer outputs a measurement update residual of rk=-HkXk+1,kAnd dynamically adjusting the measurement noise variance matrix by using the mean value of the residual values in a certain time period:
firstly, median filtering is carried out on residual error sequences, and a residual error queue r with the most recent number of N (taking N as 100) is recordedk-N+1,rk-N+2,……,rk
The queues are sequenced to obtain new queues r' v which are arranged from small to largek-N+1,r′vk-N+2,……,rk'; wherein, r' vk-N+1≤r′vk-N+2≤…≤r′vk
After the maximum number and the minimum number are removed, the residual data are averaged, and then the measurement and update residual value is as follows:
Figure BDA0002110950150000041
then measure the noise variance matrix RkThe unbiased estimate of (d) may be expressed as:
Figure BDA0002110950150000042
i.e. the filter updates the residual error based on accelerometer measurements
Figure BDA0002110950150000043
Dynamically adjust the measurement noise variance matrix RkThe adaptive adjustment of the filter parameters under different dynamic conditions is realized.
The invention has the advantages and beneficial effects that:
1. according to the method, the estimation algorithm based on the accelerometer measurement updating residual error is designed, the measurement variance matrix is adjusted in real time, the problem of horizontal attitude estimation of the inertial navigation system under the dynamic condition is well solved under the condition that the motion acceleration is unknown, the horizontal attitude estimation precision under the dynamic condition is effectively improved, and the method has a high engineering application value.
2. The invention can be used for an attitude Measurement system taking an Inertial Measurement Unit (IMU) as a core device. When the carrier has the motion acceleration, the filter can dynamically adjust the value of the measurement variance matrix according to the measurement update residual error of the accelerometer under the condition that the motion acceleration is unknown, so that the interference of the motion acceleration is effectively weakened, and the optimal estimation of the attitude information is realized.
3. Aiming at the problem that the motion acceleration is unpredictable, a pseudo measurement model based on the accelerometer output is designed, threshold judgment on a combined acceleration value output by the accelerometer is not needed, and the measurement noise variance is estimated in real time according to a residual value updated by the accelerometer, namely the measurement noise variance matrix is dynamically adjusted according to the motion acceleration, so that the attitude measurement precision of the system under a dynamic condition is ensured.
Drawings
FIG. 1 is a process flow diagram of the present invention.
Detailed Description
The embodiments of the invention will be described in further detail below with reference to the accompanying drawings:
an inertial navigation system attitude estimation method, as shown in fig. 1, includes the following steps:
step 1, establishing a Kalman filter state equation on the basis of an attitude quaternion serving as a state quantity and a quaternion attitude updating differential equation;
the specific steps of the step 1 comprise:
(1) the output is y from the known accelerometera=[0 ax ay az]TThe gyroscope output is yg=[0 ωx ωyωz]TThe gravity vector is G ═ 000-G]TEstablishing a Kalman filtering state equation, wherein the basic form of the known state equation is as follows:
Xk+1=ΦkXk+Wk
wherein the state quantity X is [ q ]0 q1 q2 q3 εx εy εz]TQ is a carrier attitude quaternion, and epsilon is gyro constant drift; state transition matrix
Figure BDA0002110950150000051
Where T is the filter period, F is the system continuous equation state matrix, WkIs the system noise;
Figure BDA0002110950150000052
wherein the content of the first and second substances,
Figure BDA0002110950150000053
Figure BDA0002110950150000061
Fεq=03×4,Fεε=03×3
(2) system noise matrix
Figure BDA0002110950150000062
Wherein, I3×3Is a unit matrix, δg,kThe variance of the noise output for the gyroscope depends on the characteristics of the gyroscope itself and can be obtained through measurement.
Step 2, establishing a quaternion pseudo Kalman wave measurement equation by taking the measurement value of the accelerometer as input;
the specific steps of the step 2 comprise:
(1) the basic form of the measurement equation is known as:
Zk=HkXk+Vk
when the carrier has no motion acceleration, the output of the accelerometer is yaAnd the gravity vector G has the following relationship:
Figure BDA0002110950150000063
both sides are multiplied by QkTo obtain
Figure BDA0002110950150000064
Further transformation can obtain
04×1=HkQk+Vk
Wherein the measuring matrix
Figure BDA0002110950150000065
QkIs an attitude quaternion, VkG is a local gravity acceleration value for measuring noise;
measuring Z as 04×1
(2) Measure noise matrix
Figure BDA0002110950150000066
δa,kOutput noise variance for accelerometer, ap,kIs the variance of the motion acceleration of the carrier;
and 3, performing Kalman filtering measurement updating according to the Kalman filtering state equation established in the step 1 and the quaternion pseudo Kalman wave measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on the measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of the measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under the dynamic condition.
The specific steps of the step 3 comprise:
(1) kalman filtering one-step prediction and measurement updating:
kalman filter parameter initial value X0=[1 0 0 0 0 0 0]T,P0=10I7×7The system state one-step prediction equation is as follows:
Figure BDA0002110950150000071
Figure BDA0002110950150000072
using accelerometer output ya=[0 ax ay az]TAnd (3) carrying out measurement updating, wherein an updating equation is as follows:
Figure BDA0002110950150000073
Figure BDA0002110950150000074
Figure BDA0002110950150000075
(2) and (3) self-adaptive adjustment of measurement noise variance:
accelerometer outputs a measurement update residual of rk=-HkXk+1,kAnd dynamically adjusting the measurement noise variance matrix by using the mean value of the residual values in a certain time period:
firstly, median filtering is carried out on residual error sequences, and a residual error queue r with the most recent number of N (taking N as 100) is recordedk-N+1,rk-N+2,……,rk(ii) a The queues are sequenced to obtain new queues r' v which are arranged from small to largek-N+1,r′vk-N+2,……,rk'; wherein, r' vk-N+1≤r′vk-N+2≤…≤r′vk. The remaining data is averaged after the largest and smallest numbers are removed.
Figure BDA0002110950150000081
Then measure the noise variance matrix RkCan be expressed as
Figure BDA0002110950150000082
I.e. the filter updates the residual error based on accelerometer measurements
Figure BDA0002110950150000083
Dynamically adjust the measurement noise variance matrix RkThe adaptive adjustment of the filter parameters under different dynamic conditions is realized.
It should be emphasized that the examples described herein are illustrative and not restrictive, and thus the present invention includes, but is not limited to, those examples described in this detailed description, as well as other embodiments that can be derived from the teachings of the present invention by those skilled in the art and that are within the scope of the present invention.

Claims (1)

1. An inertial navigation system attitude estimation method is characterized by comprising the following steps: the method comprises the following steps:
step 1, establishing a Kalman filter state equation on the basis of an attitude quaternion serving as a state quantity and a quaternion attitude updating differential equation;
step 2, establishing a quaternion pseudo Kalman filtering measurement equation by taking the measurement value of the accelerometer as input;
step 3, performing Kalman filtering measurement updating according to the Kalman filter state equation established in the step 1 and the quaternion pseudo Kalman filtering measurement equation established in the step 2, calculating a measurement updating residual value, performing real-time estimation on a measurement noise variance according to the residual value measured and updated by the accelerometer, and dynamically adjusting the size of a measurement noise variance matrix according to the size of the motion acceleration so as to ensure the attitude measurement precision of the system under a dynamic condition;
the specific steps of the step 1 comprise:
(1) the output is y from the known accelerometera=[0 ax ay az]TThe gyroscope output is yg=[0 ωx ωy ωz]TThe gravity vector is G ═ 000-G]TEstablishing a Kalman filter state equation, wherein the known state equation has the basic form:
Xk+1=ΦkXk+Wk
wherein the state quantity X is [ q ]0 q1 q2 q3 εx εy εz]TQ is a carrier attitude quaternion, and epsilon is gyro constant drift; state transition matrix
Figure FDA0003342651170000011
Where T is the filter period, F is the system continuous equation state matrix, WkIs the system noise;
Figure FDA0003342651170000012
wherein the content of the first and second substances,
Figure FDA0003342651170000021
Figure FDA0003342651170000022
Fεq=03×4,Fεε=03×3
(2) system noise matrix
Figure FDA0003342651170000023
Wherein, I3×3Is a 3 × 3 unit array, δg,kOutputting a noise variance for the gyroscope;
the specific steps of the step 2 comprise:
(1) the basic form of the known measurement equation is:
Zk=HkXk+Vk
when the carrier has no motion acceleration, the output of the accelerometer is yaAnd the gravity vector G has the following relationship:
Figure FDA0003342651170000024
both sides are multiplied by QkTo obtain
Figure FDA0003342651170000025
Further transformation may result in:
04×1=HkQk+Vk
wherein the measuring matrix
Figure FDA0003342651170000026
QkIs an attitude quaternion, VkG is a local gravity acceleration value for measuring noise;
measuring Z as 04×1
(2) Measure noise matrix
Figure FDA0003342651170000027
δa,kOutput noise variance for accelerometer, ap,kIs the variance of the motion acceleration of the carrier;
the specific steps of the step 3 comprise:
(1) kalman filtering one-step prediction and measurement updating:
kalman filter parameter initial value X0=[1 0 0 0 0 0 0]T,P0=10I7×7The system state one-step prediction equation is as follows:
Figure FDA0003342651170000031
Figure FDA0003342651170000032
using accelerometer output ya=[0 ax ay az]TAnd (3) carrying out measurement updating, wherein an updating equation is as follows:
Figure FDA0003342651170000033
Figure FDA0003342651170000034
Figure FDA0003342651170000035
(2) and (3) self-adaptive adjustment of measurement noise variance:
accelerometer outputs a measurement update residual of rk=-HkXk+1,kAnd dynamically adjusting the measurement noise variance matrix by using the mean value of the residual values in a certain time period:
firstly, median filtering is carried out on residual error sequences, and residual error queues r with the latest number of N are recordedk-N+1,rk-N+2,……,rk
Sequencing the queues to obtain a new queue r 'arranged from small to large'k-N+1,r′k-N+2,……,r′k(ii) a Wherein, N is 100, r'k-N+1≤r′k-N+2≤…≤r′k
After the maximum number and the minimum number are removed, the residual data are averaged, and then the measurement and update residual value is as follows:
Figure FDA0003342651170000036
then measure the noise variance matrix RkThe unbiased estimate of (d) may be expressed as:
Figure FDA0003342651170000037
i.e. the filter updates the residual error based on accelerometer measurements
Figure FDA0003342651170000041
Dynamically adjust the measurement noise variance matrix RkThe adaptive adjustment of the filter parameters under different dynamic conditions is realized.
CN201910571246.7A 2019-06-28 2019-06-28 Attitude estimation method of inertial navigation system Active CN110440756B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910571246.7A CN110440756B (en) 2019-06-28 2019-06-28 Attitude estimation method of inertial navigation system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910571246.7A CN110440756B (en) 2019-06-28 2019-06-28 Attitude estimation method of inertial navigation system

Publications (2)

Publication Number Publication Date
CN110440756A CN110440756A (en) 2019-11-12
CN110440756B true CN110440756B (en) 2021-12-31

Family

ID=68428386

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910571246.7A Active CN110440756B (en) 2019-06-28 2019-06-28 Attitude estimation method of inertial navigation system

Country Status (1)

Country Link
CN (1) CN110440756B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112729348B (en) * 2021-01-10 2023-11-28 河南理工大学 Gesture self-adaptive correction method for IMU system
CN113175926B (en) * 2021-04-21 2022-06-21 哈尔滨工程大学 Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN114264304B (en) * 2021-12-23 2023-07-14 湖南航天机电设备与特种材料研究所 High-precision horizontal attitude measurement method and system for complex dynamic environment

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (en) * 2009-06-03 2009-10-28 哈尔滨工程大学 Quick measurement method of initial attitude of ship local strap-down inertial navigation system
CN102486377A (en) * 2009-11-17 2012-06-06 哈尔滨工程大学 Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system
CN104154914A (en) * 2014-07-25 2014-11-19 辽宁工程技术大学 Initial attitude measurement method of space stabilization type strapdown inertial navigation system
CN105698792A (en) * 2016-01-26 2016-06-22 上海实汇机电科技有限公司 Dynamic MEMS (micro-electromechanical systems) inertial attitude measuring system based on self-adaptive robust integration algorithm

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8626441B2 (en) * 2008-06-17 2014-01-07 Agco Corporation Methods and apparatus for using position/attitude information to enhance a vehicle guidance system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (en) * 2009-06-03 2009-10-28 哈尔滨工程大学 Quick measurement method of initial attitude of ship local strap-down inertial navigation system
CN102486377A (en) * 2009-11-17 2012-06-06 哈尔滨工程大学 Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system
CN104154914A (en) * 2014-07-25 2014-11-19 辽宁工程技术大学 Initial attitude measurement method of space stabilization type strapdown inertial navigation system
CN105698792A (en) * 2016-01-26 2016-06-22 上海实汇机电科技有限公司 Dynamic MEMS (micro-electromechanical systems) inertial attitude measuring system based on self-adaptive robust integration algorithm

Also Published As

Publication number Publication date
CN110440756A (en) 2019-11-12

Similar Documents

Publication Publication Date Title
CN110440756B (en) Attitude estimation method of inertial navigation system
CN109459019B (en) Vehicle navigation calculation method based on cascade adaptive robust federal filtering
CN112013836B (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN107014376B (en) A kind of posture inclination angle estimation method suitable for the accurate operation of agricultural machinery
JP6083279B2 (en) Movement status information calculation method and movement status information calculation device
CN110887481A (en) Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN111551174A (en) High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN109945859B (en) Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering
CN110887480A (en) Flight attitude estimation method and system based on MEMS sensor
CN111307114B (en) Water surface ship horizontal attitude measurement method based on motion reference unit
CN110595434B (en) Quaternion fusion attitude estimation method based on MEMS sensor
CN112070170A (en) Dynamic residual threshold self-adaptive quaternion particle filtering attitude calculation data fusion method
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN112874529B (en) Vehicle mass center slip angle estimation method and system based on event trigger state estimation
CN109506674B (en) Acceleration correction method and device
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN110132271B (en) Adaptive Kalman filtering attitude estimation algorithm
CN110849364B (en) Adaptive Kalman attitude estimation method based on communication-in-motion
CN110471293B (en) Z-axis gyroscope sliding mode control method for estimating time-varying angular velocity
CN110736459B (en) Angular deformation measurement error evaluation method for inertial quantity matching alignment
CN117091592A (en) Gesture resolving method, gesture resolving device, and computer storage medium
CN114111843B (en) Initial alignment method for optimal movable base of strapdown inertial navigation system

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