CN112729281A - Method for restraining course drift of inertial/satellite combined navigation in stationary state - Google Patents

Method for restraining course drift of inertial/satellite combined navigation in stationary state Download PDF

Info

Publication number
CN112729281A
CN112729281A CN202011502457.4A CN202011502457A CN112729281A CN 112729281 A CN112729281 A CN 112729281A CN 202011502457 A CN202011502457 A CN 202011502457A CN 112729281 A CN112729281 A CN 112729281A
Authority
CN
China
Prior art keywords
course
carrier
static
navigation
state
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.)
Pending
Application number
CN202011502457.4A
Other languages
Chinese (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.)
Wuxi Kalman Navigation Technology Co ltd
Original Assignee
Wuxi Kalman Navigation Technology Co ltd
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 Wuxi Kalman Navigation Technology Co ltd filed Critical Wuxi Kalman Navigation Technology Co ltd
Priority to CN202011502457.4A priority Critical patent/CN112729281A/en
Publication of CN112729281A publication Critical patent/CN112729281A/en
Pending legal-status Critical Current

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/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/20Instruments for performing navigational calculations
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42Determining position
    • G01S19/45Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

The invention discloses a method for restraining course drift of inertia/satellite combined navigation in a stationary state, and belongs to the field of multi-information fusion. The method comprises the steps of constraining according to the motion state of a carrier, carrying out normal integrated navigation calculation when the carrier moves, firstly locking the course when the carrier is static, then constraining the course by Kalman filtering estimation after the integrated navigation calculation, and unlocking the course locking to carry out conventional integrated navigation calculation when the carrier moves again.

Description

Method for restraining course drift of inertial/satellite combined navigation in stationary state
Technical Field
The invention belongs to the field of multi-information fusion, is applied to the fields of precision agriculture, automatic driving and the like, and relates to a method for restraining course drift when an inertia/satellite combined navigation is static.
Background
With the development of agricultural informatization and automation, the popularization of national agricultural benefits policies and the proportion of the aging of farmers rise, and intelligent agriculture and precision agriculture gradually become international research hotspots. The popularization of precision agriculture can liberate both hands, improve work efficiency, improve land utilization rate, sow in time and reap, practice thrift input such as seed, reach the target that reduces the labour, reduces input, increase output.
In the field of automatic driving of precision agricultural machinery, an inertia/satellite combined navigation mode is often adopted to provide position, speed and attitude information of full information for a carrier. In the normal operation process of the agricultural machinery, parking operations such as sorting agricultural implements, adding seeds and fertilizers and the like are often required, and if the parking time is too long, for example, more than 30 minutes, the course calculated by the combined navigation can slowly drift, so that the course of the carrier is wrong, and troubles are brought to agricultural production operation. The traditional method for restraining the course drift is to adopt the course direction of a double-antenna GNSS to carry out position/speed/course restraint, or adopt ZUPT (zero velocity correction) to carry out restraint aiming at the error characteristic of a gyroscope, or adopt a magnetometer to assist. And for low-cost single-antenna GNSS and 6-axis IMU, no course combination and magnetometer assistance exist, and only position/speed combination calculation can be carried out. Although the ZUPT can achieve certain effect, the zero speed correction method cannot well inhibit the course and still has small-amplitude drift along with the lapse of the rest time, such as rest for 1 hour, and the change of the environment where the gyroscope is located, such as temperature rise.
Disclosure of Invention
[ problem ] to
The technical problems to be solved by the invention are as follows: problem of course drift when inertial/satellite group navigation is at rest. The inertia/position combined navigation technology can provide position, speed and attitude information of full information, and is widely applied to various industries. However, when the carrier is stationary, the course observability is very weak or even unobservable, and the course information of the combined navigation solution can drift along with the time.
[ solution ]
The invention provides a method for restraining course drifting of an inertial/satellite combined navigation when the carrier is static, wherein the static course is locked when the carrier is static, the locked course and the course calculated by the combined navigation are utilized for carrying out filtering estimation, the course calculated by the combined navigation is restrained, the locked course is unlocked when the carrier moves, and only the conventional combined navigation calculation is carried out.
The method specifically comprises the following steps:
(1) determining the state of motion of the carrier
Judging whether the carrier is in a static state or a moving state by judging the position difference and the speed of the carrier at adjacent moments; the carrier comprises an agricultural machine;
(2) locking static course
If the judgment result shows that the carrier is in a static state, locking the course of the carrier when the carrier is static;
(3) course constraint
And carrying out Kalman filtering estimation on the combined navigation real-time solved course by using the locked course, and constraining the combined navigation solved course in real time to prevent long-time static drift.
The step (1) adopts an equation (1) to judge the motion state of the carrier:
Figure BDA0002844003040000021
wherein:
Figure BDA0002844003040000022
combining the carrier course resolved by navigation at the moment k-1;
Vehstate: the motion state of the carrier, 1 represents dynamic state, and-1 represents static state;
Vk-1: the carrier speed calculated by the k-1 moment integrated navigation;
Vk: the carrier speed calculated by the k-time integrated navigation;
δx:δx=xk-xk-1position difference in the x-axis direction;
δy:δy=yk-yk-1position difference in the y-axis direction;
δxy:
Figure BDA0002844003040000023
arithmetic square root of position difference.
The step (3) comprises the following steps: establishing a Kalman filtering estimation equation, initializing Kalman filtering estimation, and performing course constraint and feedback correction by the Kalman filtering estimation.
And (3) when Kalman filtering estimation is carried out, selecting a course error angle as a state quantity of filtering estimation, and selecting a locked course and a course difference value calculated by combined navigation as an observed quantity.
And (3) after filtering estimation, performing feedback correction on the current combined navigation resolved course by using the estimated state quantity.
[ advantageous effects ]
The invention provides a method for restraining course drifting when an inertia/satellite combined navigation is static, which comprises the steps of firstly locking the current course when a carrier is static; then directly constraining the course result of the integrated navigation solution in a static state; and when the carrier is in a motion state, performing conventional integrated navigation calculation and unlocking the locked heading. By using the method of the invention, when the carrier is in a static state, the course can be stable, does not drift and does not shake.
Drawings
FIG. 1 embodiment 1 shows a flow of a method for constraining heading drift when an inertial/satellite combined navigation is stationary
FIG. 2 model of the motion state of a carrier
FIG. 3 shows the carrier static combined navigation heading drift without any assistance
FIG. 4 illustrates small amplitude drift of the carrier static combined navigation course when zero speed correction is added
FIG. 5 shows that the carrier static combination navigation course is stable and not drifted when the course locking is added
Detailed Description
Example 1
(one) carrier motion state: the carrier moves from the k-1 moment to the k moment:
a) and (3) judging whether the carrier is static:
Figure BDA0002844003040000031
wherein:
Figure BDA0002844003040000032
combining the carrier course resolved by navigation at the moment k-1;
Vehstate: the motion state of the carrier, 1 represents dynamic state, and-1 represents static state;
Vk-1: the carrier speed calculated by the k-1 moment integrated navigation;
Vk: the carrier speed calculated by the k-time integrated navigation;
δx:δx=xk-xk-1position difference in the x-axis direction;
δy:δy=yk-yk-1position difference in the y-axis direction;
δxy:
Figure BDA0002844003040000041
arithmetic square root of position difference.
b) And (3) course locking when the carrier is static:
Figure BDA0002844003040000042
wherein:
Figure BDA0002844003040000043
locked carrier heading.
And (II) carrying out course constraint by Kalman filtering estimation: heading lock is first performed when the carrier is stationary. And then, carrying out Kalman filtering estimation on the combined navigation real-time solved course by using the locked course, and constraining the solved course in real time to prevent long-time static drift.
a) Establishing a Kalman filtering estimation equation:
selecting a course error angle X as a state quantity of filtering estimation, namely:
Figure BDA0002844003040000044
selecting a locked course and a course difference value calculated by the integrated navigation as an observed quantity Z, namely:
Figure BDA0002844003040000045
written in matrix form as:
Figure BDA0002844003040000046
the discrete form is:
Figure BDA0002844003040000047
wherein:
a: the A matrix is a 0 matrix, namely A is 0;
φk/k-1: the state transition matrix is discretization of the matrix A and takes a first-order form of Taylor expansion;
h: an observation matrix, which is known from the relationship between the observed quantity and the state quantity, wherein H is a unit matrix, i.e., H is 1;
g: the system noise matrix, and is correlated to the actual system. In the embodiment, the course precision is 0.4 degrees, wherein G is 0.4;
v: observing a noise matrix, wherein the course locking of the carrier is unchanged when the carrier is static, namely the accuracy of the course is very high, and the observation noise matrix V is 0.05;
b) initialization of Kalman filtering estimation:
and when the carrier is static, starting Kalman filtering estimation to carry out course constraint. Initial parameters are given as follows:
initial state quantity: x1=0
Initial state covariance matrix: p1=1,P1Is corresponding to the state quantity X1Estimate error covariance of
A system noise matrix: q ═ G ═ 0.16 formula (7)
Observing a noise matrix: r is V2.5V 1e-3 formula (8)
c) And (3) course constraint is carried out by Kalman filtering estimation:
and (3) state one-step prediction:
one-step prediction of state quantity from n-1 time to n time
Figure BDA0002844003040000051
And a state covariance matrix Pn|n-1Here, the n time is not different from the k time, and represents different discrete times.
Figure BDA0002844003040000052
Pn|n-1=φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T) Delta t/2 formula (10)
Wherein:
Figure BDA0002844003040000053
an estimated value of the state quantity X at the time n-1;
Pn-1: a state covariance matrix at time n-1;
φn|n-1: a state transition matrix from n-1 time to n time;
q: a system noise matrix;
δ t: the filter estimates the update period, here 50Hz, i.e. 0.02 seconds.
Updating the filter gain:
Kn=Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1formula (11)
Wherein:
Kn: n time filtering gain;
Hn: observation matrix of n time points, from observed quantity and formThe relationship between the state quantities is obtained.
Rn: and observing a noise covariance matrix at the n moment, and obtaining the noise covariance matrix through GNSS information statistics.
Updating the filtering estimation:
the filtered estimate updates the state quantity X and the state covariance matrix P.
And (3) updating the state:
Figure BDA0002844003040000061
wherein:
Figure BDA0002844003040000062
an estimated quantity of the state quantity X at the time n;
Zn: the observed quantity at the time n, namely the difference value between the locked course and the course calculated by the integrated navigation at the time n, namely:
Figure BDA0002844003040000063
updating the state covariance:
Pn=(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn Tformula (14)
Wherein:
Pn: a state covariance matrix at time n;
i: an identity matrix;
updating PnFor the next iteration, for example, when n is 5, P is needed for the next cycle, i.e., the 6 th cycle5What is, what is
With PnIs calculated for the next period Pn+1The filter estimate of (2) is periodically iterative.
d) And (3) feedback correction:
after filtering estimation, feedback correction is carried out on the current combined navigation resolved course by using the estimated state quantity, namely:
Figure BDA0002844003040000064
example 2
As shown in fig. 3:
when the carrier is in a static state at 320 th to 440 th seconds (epochs 16000 to 22000, 0.02 second per epoch), no heading auxiliary information is applied, and the heading drifts from 194 degrees to 218 degrees in the static state. Such large heading drift errors have not worked properly for agricultural autopilot.
As shown in fig. 4:
when the carrier is in a static state at 800 seconds to 940 seconds (epochs 40000 to 47000, 0.02 second per epoch), the course is subjected to the conventional ZUPT (zero speed correction), and the course is jittered within the range of 2 degrees to 5 degrees in the static state. For automatic driving of agricultural machinery, course shaking errors of a few degrees can cause turning when the agricultural machinery starts, and inconvenience is brought to agricultural production. If the static time continues to be added, no heading jitter or drift is excluded.
As shown in fig. 5:
when the carrier is in a static state in 40 seconds to 240 seconds (epoch 2000 to 12000, 0.02 second per epoch), the course is constrained by the method of the embodiment 1 of the invention, so that the course is stable at 191.5 degrees, does not drift and shake, and can well inhibit the course drift.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (8)

1. A method for restraining course drifting when an inertia/satellite combined navigation is static is characterized in that when a carrier is static, the static course is locked, filtering estimation is carried out by utilizing the locked course and the course resolved by the combined navigation, the course resolved by the combined navigation is restrained, the locked course is unlocked when the carrier moves, and only the combined navigation is resolved.
2. The method of claim 1, wherein the carrier comprises an agricultural machine.
3. The method of claim 1, comprising the steps of:
(1) judging whether the motion state of the carrier is static or moving;
(2) locking a static course;
(3) and course constraint, namely performing Kalman filtering estimation on the course solved by the integrated navigation in real time by using the locked course, and constraining the course solved by the integrated navigation in real time to prevent long-time static drift.
4. The method of claim 3, wherein the method further comprises the step of constraining heading drift at rest for the combined inertial/satellite navigation,
(1) determining the state of motion of the carrier
Judging whether the carrier is in a static state or a moving state by judging the position difference and the speed of the carrier at the adjacent moment;
(2) locking static course
If the judgment result shows that the carrier is in a static state, locking the course of the carrier when the carrier is static;
(3) course constraint
And carrying out Kalman filtering estimation on the combined navigation real-time resolved heading by using the locked heading, and constraining the combined navigation resolved heading in real time to prevent long-time static drift.
5. The method of claim 4, wherein the method further comprises the step of constraining the heading drift at rest for the combined inertial/satellite navigation,
the step (1) adopts an equation (1) to judge the motion state of the carrier:
Figure FDA0002844003030000011
wherein:
Figure FDA0002844003030000021
combining the carrier course resolved by navigation at the moment k-1;
Vehstate: the motion state of the carrier, 1 represents dynamic state, and-1 represents static state;
Vk-1: the carrier speed calculated by the k-1 moment integrated navigation;
Vk: the carrier speed calculated by the k-time integrated navigation;
δx:δx=xk-xk-1position difference in the x-axis direction;
δy:δy=yk-yk-1position difference in the y-axis direction;
δxy:
Figure FDA0002844003030000022
arithmetic square root of position difference.
6. The method of claim 4, wherein the step (3) comprises the steps of: establishing a Kalman filtering estimation equation, initializing Kalman filtering estimation, and performing course constraint and feedback correction by the Kalman filtering estimation.
7. The method for constraining the heading drift of the combined inertial/satellite navigation at rest according to claim 4 or 6, wherein in the step (3) of Kalman filtering estimation, a heading error angle is selected as a state quantity of the filtering estimation, and a heading difference value of the locked heading and the combined navigation solution is selected as an observed quantity.
8. The method for constraining the heading drift of the inertial/satellite combined navigation at rest according to claim 6, wherein the estimated state quantity is used for performing feedback correction on the heading of the current combined navigation solution after filtering estimation.
CN202011502457.4A 2020-12-18 2020-12-18 Method for restraining course drift of inertial/satellite combined navigation in stationary state Pending CN112729281A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011502457.4A CN112729281A (en) 2020-12-18 2020-12-18 Method for restraining course drift of inertial/satellite combined navigation in stationary state

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011502457.4A CN112729281A (en) 2020-12-18 2020-12-18 Method for restraining course drift of inertial/satellite combined navigation in stationary state

Publications (1)

Publication Number Publication Date
CN112729281A true CN112729281A (en) 2021-04-30

Family

ID=75602980

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011502457.4A Pending CN112729281A (en) 2020-12-18 2020-12-18 Method for restraining course drift of inertial/satellite combined navigation in stationary state

Country Status (1)

Country Link
CN (1) CN112729281A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459469A (en) * 2022-01-14 2022-05-10 北京信息科技大学 Multi-motion-state navigation method and device and intelligent wearable equipment

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102589552A (en) * 2012-01-19 2012-07-18 北京华力创通科技股份有限公司 Data fusion method and device for low-cost integrated navigation system
CN106950586A (en) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working
CN109059904A (en) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 Combinated navigation method for mobile carrier
WO2020220729A1 (en) * 2019-04-29 2020-11-05 南京航空航天大学 Inertial navigation solution method based on angular accelerometer/gyroscope/accelerometer

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102589552A (en) * 2012-01-19 2012-07-18 北京华力创通科技股份有限公司 Data fusion method and device for low-cost integrated navigation system
CN106950586A (en) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working
CN109059904A (en) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 Combinated navigation method for mobile carrier
WO2020220729A1 (en) * 2019-04-29 2020-11-05 南京航空航天大学 Inertial navigation solution method based on angular accelerometer/gyroscope/accelerometer

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
伍宗伟;姚敏立;马红光;贾维敏;田方浩;: "移动卫星通信低成本多传感器融合姿态估计方法", 西安交通大学学报, no. 12, 22 October 2012 (2012-10-22) *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459469A (en) * 2022-01-14 2022-05-10 北京信息科技大学 Multi-motion-state navigation method and device and intelligent wearable equipment
CN114459469B (en) * 2022-01-14 2023-05-23 北京信息科技大学 Multi-motion state navigation method and device and intelligent wearable equipment

Similar Documents

Publication Publication Date Title
US11566901B2 (en) Integrated navigation method for mobile vehicle
CN106950586A (en) GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working
CN112013836B (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
Sukkarieh et al. A high integrity IMU/GPS navigation loop for autonomous land vehicle applications
WO2018014602A1 (en) Volume kalman filtering method suitable for high-dimensional gnss/ins deep coupling
Jiancheng et al. Study on innovation adaptive EKF for in-flight alignment of airborne POS
CN107943060A (en) A kind of automatic pilot, method and computer-readable medium along tracking straight line guiding vehicle
CN112729281A (en) Method for restraining course drift of inertial/satellite combined navigation in stationary state
CN111190207B (en) PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method
Miao et al. Steering angle adaptive estimation system based on GNSS and MEMS gyro
Anbu et al. Integration of inertial navigation system with global positioning system using extended kalman filter
CN110221331B (en) Inertia/satellite combined navigation dynamic filtering method based on state transformation
Liu et al. An improved GNSS/INS navigation method based on cubature Kalman filter for occluded environment
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN110873577B (en) Underwater rapid-acting base alignment method and device
CN112484731A (en) High-precision real-time flight navigation calculation method
CN110045408A (en) One kind being based on the approximate satellite/inertia depth coupling process of code phase
CN116793365A (en) Agricultural machinery positioning navigation method based on time sequence neural network assistance
Luo et al. An imu/visual odometry integrated navigation method based on measurement model optimization
CN114459473B (en) Inertial/satellite/polarization integrated navigation method under error data injection attack
CN112945275B (en) Method for judging carrier forward and backward and integrated navigation course initial alignment
Li et al. Event-Triggered Moving Horizon Pose Estimation for Spacecraft Systems
Huang et al. A novel algorithm of improved cubature unscented kalman filter based on the model of magnetometer for underwater glider navigation system
Steffes et al. Investigation of the attitude error vector reference frame in the INS EKF
CN117516524A (en) GPS/INS Kalman filtering positioning method based on CNN-GRU prediction

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