CN112378401A - Motion acceleration estimation method of inertial navigation system - Google Patents

Motion acceleration estimation method of inertial navigation system Download PDF

Info

Publication number
CN112378401A
CN112378401A CN202010883340.9A CN202010883340A CN112378401A CN 112378401 A CN112378401 A CN 112378401A CN 202010883340 A CN202010883340 A CN 202010883340A CN 112378401 A CN112378401 A CN 112378401A
Authority
CN
China
Prior art keywords
equation
motion acceleration
state
inertial navigation
navigation system
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
CN202010883340.9A
Other languages
Chinese (zh)
Other versions
CN112378401B (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 CN202010883340.9A priority Critical patent/CN112378401B/en
Publication of CN112378401A publication Critical patent/CN112378401A/en
Application granted granted Critical
Publication of CN112378401B publication Critical patent/CN112378401B/en
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
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/14—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of gyroscopes

Abstract

The invention relates to a motion acceleration estimation method of an inertial navigation system, which comprises the following steps: step one, establishing a Kalman filtering state equation on the basis of a directional cosine matrix differential equation by taking elements related to gravity acceleration in a system directional cosine matrix and gyro constant drift as state quantities; step two, establishing a Kalman filter measurement equation based on a motion acceleration model based on a first-order Markov process; step three, performing one-step prediction and measurement updating on the Kalman filtering state equation established in the step one and the Kalman filter measurement equation established in the step two; and step four, extracting the motion acceleration according to the update equation obtained in the step three. By adopting the method, when no external reference information exists and the inertial navigation system cannot be self-aligned, the decoupling of the gravity acceleration and the motion acceleration is realized, so that the effective estimation of the motion acceleration is realized.

Description

Motion acceleration estimation method of inertial navigation system
Technical Field
The invention belongs to the technical field of inertial navigation systems, and particularly relates to a motion acceleration estimation method of an inertial navigation system.
Background
When the carrier moves, the accelerometer of the inertial navigation system senses the gravity acceleration and the movement acceleration at the same time. The carrier motion acceleration measurement based on the inertial navigation system usually utilizes the self-alignment capability of the inertial navigation system or the assistance of external reference information to calculate and obtain the projection of the gravity acceleration on the carrier system, and further separates the motion acceleration from the output of the accelerometer. When external reference information does not exist and the inertial navigation system cannot be self-aligned, the method has certain research significance on how to effectively extract the motion acceleration from the output of the accelerometer.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a motion acceleration estimation method of an inertial navigation system, which can effectively estimate the motion acceleration when no external reference information exists and the inertial navigation system cannot be self-aligned.
The above object of the present invention is achieved by the following technical solutions:
a motion acceleration estimation method of an inertial navigation system is characterized by comprising the following steps: the method comprises the following steps:
step one, establishing a Kalman filtering state equation on the basis of a directional cosine matrix differential equation by taking elements related to gravity acceleration in a system directional cosine matrix and gyro constant drift as state quantities;
step two, establishing a Kalman filter measurement equation based on a motion acceleration model based on a first-order Markov process;
step three, performing one-step prediction and measurement updating on the Kalman filtering state equation established in the step one and the Kalman filter measurement equation established in the step two;
and step four, extracting the motion acceleration according to the update equation obtained in the step three.
Further: the method comprises the following setting parameters:
defining a carrier coordinate system as a system b, and defining a local geographic coordinate system as a system n; at time t, the output of the accelerometer under b is
Figure RE-GDA0002895974390000011
b is the acceleration of the lower motion
Figure RE-GDA0002895974390000012
b is the output of the gyroscope
Figure RE-GDA0002895974390000013
Gamma and theta are respectively roll angle and pitch angle,
Figure RE-GDA0002895974390000014
and T is a resolving period for gyro constant drift.
Further: the specific steps of the first step comprise:
(1) design Kalman filter equation of state
Selecting a quantity of state
Figure RE-GDA0002895974390000015
The system state equation is:
Figure RE-GDA0002895974390000021
wherein the state matrix
Figure RE-GDA0002895974390000022
Noise driving array
Figure RE-GDA0002895974390000023
System noise matrix
Figure RE-GDA0002895974390000024
(2) Discretizing the system state equation can obtain
Xk+1＝ΦkXk+GWk
Wherein phik＝I6+ F.T is a state matrix discrete form;
Figure RE-GDA0002895974390000025
in the form of a discrete array of system noise.
Further: the second step comprises the following specific steps:
(1) building motion acceleration model
ak+1＝m·ak+ηa
Wherein m ∈ (0,1) is a first-order Markov coefficient; etaaIs the model error;
(2) design Kalman filter measurement equation
Zk＝HXk+Vk
Wherein the quantity is measured
Figure RE-GDA0002895974390000026
Measuring matrix
Figure RE-GDA0002895974390000027
Noise measurement array
Figure RE-GDA0002895974390000031
Further: the third step comprises the following specific steps:
kalman filter parameter initial value X0＝06×1，P0＝10I6×6(ii) a The one-step prediction equation of the system state is as follows:
Figure RE-GDA0002895974390000032
Figure RE-GDA0002895974390000033
the measurement update equation is as follows:
Figure RE-GDA0002895974390000034
Figure RE-GDA0002895974390000035
Figure RE-GDA0002895974390000036
wherein the content of the first and second substances,
system noise matrix
Figure RE-GDA0002895974390000037
Selecting a proper value according to the performance of the gyroscope;
measure noise matrix
Figure RE-GDA0002895974390000038
The appropriate values are chosen according to the carrier dynamic size and the accelerometer performance.
Further: the fourth step comprises the following specific steps:
the estimated value of the state of the filter at the k moment obtained in the fourth step is
Figure RE-GDA0002895974390000039
Then the estimated value of the motion acceleration of the carrier at this time is:
Figure RE-GDA00028959743900000310
wherein
Figure RE-GDA00028959743900000311
Is an estimate of the system state
Figure RE-GDA00028959743900000312
The first three elements of (a).
The invention has the advantages and positive effects that:
the invention can be used for a motion acceleration measuring system taking an Inertial Measurement Unit (IMU) as a core device. When the carrier moves, the accelerometer of the inertial navigation system senses the gravity acceleration and the movement acceleration at the same time. When no external reference information exists and the inertial navigation system cannot be self-aligned, the motion acceleration of the carrier is estimated by designing a Kalman filter based on a motion acceleration model, so that the decoupling of the gravity acceleration and the motion acceleration is realized, and the effective estimation of the motion acceleration is realized.
Drawings
FIG. 1 is a process flow diagram of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the following embodiments, which are illustrative, not restrictive, and the scope of the invention is not limited thereto.
Referring to fig. 1, a method for estimating a motion acceleration of an inertial navigation system includes: the parameters are set as follows:
defining a carrier coordinate system as a system b, and defining a local geographic coordinate system as a system n; at time t, the output of the accelerometer under b is
Figure RE-GDA0002895974390000041
b is the acceleration of the lower motion
Figure RE-GDA0002895974390000042
b is the output of the gyroscope
Figure RE-GDA0002895974390000043
Gamma and theta are respectively roll angle and pitch angle,
Figure RE-GDA0002895974390000044
and T is a resolving period for gyro constant drift. The method comprises the following steps:
step one, elements related to gravity acceleration in a system direction cosine matrix and gyro constant drift are used as state quantities, and a Kalman filtering state equation is established on the basis of a direction cosine matrix differential equation. The method comprises the following specific steps:
(1) design Kalman filter equation of state
Selecting a quantity of state
Figure RE-GDA0002895974390000045
The system state equation is:
Figure RE-GDA0002895974390000046
wherein the state matrix
Figure RE-GDA0002895974390000047
Noise driving array
Figure RE-GDA0002895974390000048
System noise matrix
Figure RE-GDA0002895974390000049
(2) Discretizing the system state equation can obtain
Xk+1＝ΦkXk+GWk
Wherein phik＝I6+ F.T is a state matrix discrete form;
Figure RE-GDA0002895974390000051
in the form of a discrete array of system noise.
And step two, establishing a Kalman filter measurement equation based on a motion acceleration model based on a first-order Markov process. The method comprises the following specific steps:
(1) building motion acceleration model
ak+1＝m·ak+ηa
Wherein m ∈ (0,1) is a first-order Markov coefficient; etaaIs the model error;
(2) design Kalman filter measurement equation
Zk＝HXk+Vk
Wherein the quantity is measured
Figure RE-GDA0002895974390000052
Measuring matrix
Figure RE-GDA0002895974390000053
Noise measurement array
Figure RE-GDA0002895974390000054
And step three, performing one-step prediction and measurement updating on the Kalman filtering state equation established in the step one and the Kalman filter measurement equation established in the step two. The method comprises the following specific steps:
kalman filter parameter initial value X0＝06×1，P0＝10I6×6(ii) a The one-step prediction equation of the system state is as follows:
Figure RE-GDA0002895974390000055
Figure RE-GDA0002895974390000056
the measurement update equation is as follows:
Figure RE-GDA0002895974390000057
Figure RE-GDA0002895974390000058
Figure RE-GDA0002895974390000059
wherein the content of the first and second substances,
system noise matrix
Figure RE-GDA0002895974390000061
Selecting a proper value according to the performance of the gyroscope;
measure noise matrix
Figure RE-GDA0002895974390000062
The appropriate values are chosen according to the carrier dynamic size and the accelerometer performance.
And step four, extracting the motion acceleration according to the update equation obtained in the step three. The method comprises the following specific steps:
the estimated value of the state of the filter at the k moment obtained in the fourth step is
Figure RE-GDA0002895974390000063
Then the estimated value of the acceleration of the carrier motion is
Figure RE-GDA0002895974390000064
Wherein
Figure RE-GDA0002895974390000065
Is an estimate of the system state
Figure RE-GDA0002895974390000066
The first three elements of (a).
Although the embodiments and figures of the present invention have been disclosed for illustrative purposes, those skilled in the art will appreciate that: various substitutions, changes and modifications are possible without departing from the spirit and scope of the invention and the appended claims, and therefore the scope of the invention is not limited to the disclosure of the embodiments and figures.

Claims (6)

1. A motion acceleration estimation method of an inertial navigation system is characterized by comprising the following steps: the method comprises the following steps:
step one, establishing a Kalman filtering state equation on the basis of a directional cosine matrix differential equation by taking elements related to gravity acceleration in a system directional cosine matrix and gyro constant drift as state quantities;
step two, establishing a Kalman filter measurement equation based on a motion acceleration model based on a first-order Markov process;
step three, performing one-step prediction and measurement updating on the Kalman filtering state equation established in the step one and the Kalman filter measurement equation established in the step two;
and step four, extracting the motion acceleration according to the update equation obtained in the step three.
2. The inertial navigation system motion acceleration estimation method of claim 1, characterized in that: the method comprises the following setting parameters:
defining a carrier coordinate system as a system b, and defining a local geographic coordinate system as a system n; at time t, the output of the accelerometer under b is
Figure RE-FDA0002895974380000011
b is the acceleration of the lower motion
Figure RE-FDA0002895974380000012
b is the output of the gyroscope
Figure RE-FDA0002895974380000013
Gamma and theta are respectively roll angle and pitch angle,
Figure RE-FDA0002895974380000014
and T is a resolving period for gyro constant drift.
3. The inertial navigation system motion acceleration estimation method according to claim 2, characterized in that: the specific steps of the first step comprise:
(1) design Kalman filter equation of state
Selecting a quantity of state
Figure RE-FDA0002895974380000015
The system state equation is:
Figure RE-FDA0002895974380000016
wherein the state matrix
Figure RE-FDA0002895974380000017
Noise driving array
Figure RE-FDA0002895974380000021
System noise matrix
Figure RE-FDA0002895974380000022
(2) Discretizing the system state equation can obtain
Xk+1＝ΦkXk+GWk
Wherein phik＝I6+ F.T is a state matrix discrete form;
Figure RE-FDA0002895974380000023
in the form of a discrete array of system noise.
4. The inertial navigation system motion acceleration estimation method of claim 3, characterized in that: the second step comprises the following specific steps:
(1) building motion acceleration model
ak+1＝m·ak+ηa
Wherein m ∈ (0,1) is a first-order Markov coefficient; etaaIs the model error;
(2) design Kalman filter measurement equation
Zk＝HXk+Vk
Wherein the quantity is measured
Figure RE-FDA0002895974380000024
Measurement ofMatrix array
Figure RE-FDA0002895974380000025
Noise measurement array
Figure RE-FDA0002895974380000026
5. The inertial navigation system motion acceleration estimation method of claim 4, characterized in that: the third step comprises the following specific steps:
kalman filter parameter initial value X0＝06×1，P0＝10I6×6(ii) a The one-step prediction equation of the system state is as follows:
Figure RE-FDA0002895974380000027
Figure RE-FDA0002895974380000028
the measurement update equation is as follows:
Figure RE-FDA0002895974380000031
Figure RE-FDA0002895974380000032
Figure RE-FDA0002895974380000033
wherein the content of the first and second substances,
system noise matrix
Figure RE-FDA0002895974380000034
Selecting a proper value according to the performance of the gyroscope;
measure noise matrix
Figure RE-FDA0002895974380000035
The appropriate values are chosen according to the carrier dynamic size and the accelerometer performance.
6. The inertial navigation system motion acceleration estimation method of claim 5, characterized in that: the fourth step comprises the following specific steps:
the estimated value of the state of the filter at the k moment obtained in the fourth step is
Figure RE-FDA0002895974380000036
Then the estimated value of the motion acceleration of the carrier at this time is:
Figure RE-FDA0002895974380000037
wherein
Figure RE-FDA0002895974380000038
Is an estimate of the system state
Figure RE-FDA0002895974380000039
The first three elements of (a).
CN202010883340.9A 2020-08-28 2020-08-28 Motion acceleration estimation method for inertial navigation system Active CN112378401B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010883340.9A CN112378401B (en) 2020-08-28 2020-08-28 Motion acceleration estimation method for inertial navigation system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010883340.9A CN112378401B (en) 2020-08-28 2020-08-28 Motion acceleration estimation method for inertial navigation system

Publications (2)

Publication Number Publication Date
CN112378401A true CN112378401A (en) 2021-02-19
CN112378401B CN112378401B (en) 2022-10-28

Family

ID=74586661

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010883340.9A Active CN112378401B (en) 2020-08-28 2020-08-28 Motion acceleration estimation method for inertial navigation system

Country Status (1)

Country Link
CN (1) CN112378401B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926A (en) * 2021-04-21 2021-07-27 哈尔滨工程大学 Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN115014391A (en) * 2022-05-31 2022-09-06 中南大学 Method for correcting MEMS inertial navigation error based on FOG

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
CN101173858A (en) * 2007-07-03 2008-05-07 北京控制工程研究所 Three-dimensional posture fixing and local locating method for lunar surface inspection prober
CN101290229A (en) * 2008-06-13 2008-10-22 哈尔滨工程大学 Silicon micro-navigation attitude system inertia/geomagnetism assembled method
CN102692225A (en) * 2011-03-24 2012-09-26 北京理工大学 Attitude heading reference system for low-cost small unmanned aerial vehicle
CN102809377A (en) * 2012-08-15 2012-12-05 南京航空航天大学 Aircraft inertia/pneumatic model integrated navigation method
CN105300379A (en) * 2015-10-13 2016-02-03 上海新纪元机器人有限公司 Kalman filtering attitude estimation method and system based on acceleration
CN106197376A (en) * 2016-09-23 2016-12-07 华南农业大学 Car body obliqueness measuring method based on single shaft MEMS inertial sensor
CN108121890A (en) * 2016-11-28 2018-06-05 北京雷动云合智能技术有限公司 A kind of navigation attitude information fusion method based on linear Kalman filter
CN108592917A (en) * 2018-04-25 2018-09-28 珠海全志科技股份有限公司 A kind of Kalman filtering Attitude estimation method based on misalignment

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
CN101173858A (en) * 2007-07-03 2008-05-07 北京控制工程研究所 Three-dimensional posture fixing and local locating method for lunar surface inspection prober
CN101290229A (en) * 2008-06-13 2008-10-22 哈尔滨工程大学 Silicon micro-navigation attitude system inertia/geomagnetism assembled method
CN102692225A (en) * 2011-03-24 2012-09-26 北京理工大学 Attitude heading reference system for low-cost small unmanned aerial vehicle
CN102809377A (en) * 2012-08-15 2012-12-05 南京航空航天大学 Aircraft inertia/pneumatic model integrated navigation method
CN105300379A (en) * 2015-10-13 2016-02-03 上海新纪元机器人有限公司 Kalman filtering attitude estimation method and system based on acceleration
CN106197376A (en) * 2016-09-23 2016-12-07 华南农业大学 Car body obliqueness measuring method based on single shaft MEMS inertial sensor
CN108121890A (en) * 2016-11-28 2018-06-05 北京雷动云合智能技术有限公司 A kind of navigation attitude information fusion method based on linear Kalman filter
CN108592917A (en) * 2018-04-25 2018-09-28 珠海全志科技股份有限公司 A kind of Kalman filtering Attitude estimation method based on misalignment

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SHI-YUAN HAN: "Output-Based Centralized Longitudinal CACC Systems with Wireless Communication Delay and Actuator Delay", 《2019 IEEE INTERNATIONAL CONFERENCE ON SYSTEMS, MAN AND CYBERNETICS (SMC)》 *
何昊晨: "一种实时移除 ＭＥＭＳ陀螺仪噪声的精准航姿技术", 《重庆大学学报》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926A (en) * 2021-04-21 2021-07-27 哈尔滨工程大学 Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN113175926B (en) * 2021-04-21 2022-06-21 哈尔滨工程大学 Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN115014391A (en) * 2022-05-31 2022-09-06 中南大学 Method for correcting MEMS inertial navigation error based on FOG

Also Published As

Publication number Publication date
CN112378401B (en) 2022-10-28

Similar Documents

Publication Publication Date Title
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN110398245B (en) Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit
CN112097763B (en) Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN110058288B (en) Course error correction method and system for unmanned aerial vehicle INS/GNSS combined navigation system
CN112378401B (en) Motion acceleration estimation method for inertial navigation system
CN111024064A (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN104635251A (en) Novel INS (inertial navigation system)/ GPS (global position system) combined position and orientation method
CN109596144B (en) GNSS position-assisted SINS inter-travel initial alignment method
CN111562603B (en) Navigation positioning method, equipment and storage medium based on dead reckoning
CN103822633A (en) Low-cost attitude estimation method based on second-order measurement update
CN112835085B (en) Method and device for determining vehicle position
CN109764870B (en) Carrier initial course estimation method based on transformation estimation modeling scheme
CN108827339B (en) High-efficient vision odometer based on inertia is supplementary
CN112033438B (en) Shaking base self-alignment method based on speed fitting
CN111696159B (en) Feature storage method of laser odometer, electronic device and storage medium
CN111207734B (en) EKF-based unmanned aerial vehicle integrated navigation method
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN104101345A (en) Multisensor attitude fusion method based on complementary reconstruction technology
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN114413898B (en) Multi-sensor data fusion method and device, computer equipment and storage medium
CN116380038A (en) Multisource navigation information fusion method based on online incremental scale factor graph
CN107808360B (en) Data processing method and device of end-to-end automatic driving system
CN113916226B (en) Minimum variance-based interference rejection filtering method for integrated navigation system
CN114001730B (en) Fusion positioning method, fusion positioning device, computer equipment and storage medium
CN115727871A (en) Track quality detection method and device, electronic equipment and storage medium

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