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 PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 24
- 230000000452 restraining effect Effects 0.000 title claims abstract description 8
- 230000003068 static effect Effects 0.000 claims abstract description 42
- 238000001914 filtration Methods 0.000 claims abstract description 28
- 238000012937 correction Methods 0.000 claims description 10
- 102100029469 WD repeat and HMG-box DNA-binding protein 1 Human genes 0.000 claims description 3
- 101710097421 WD repeat and HMG-box DNA-binding protein 1 Proteins 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 6
- 230000004927 fusion Effects 0.000 abstract description 2
- 239000011159 matrix material Substances 0.000 description 23
- 238000012271 agricultural production Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000000875 corresponding effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000003337 fertilizer Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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/165—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 combined with non-inertial navigation instruments
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO 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/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining 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/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining 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
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:
wherein:
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;
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:
wherein:
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;
b) And (3) course locking when the carrier is static:
wherein:
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:
selecting a locked course and a course difference value calculated by the integrated navigation as an observed quantity Z, namely:
written in matrix form as:
the discrete form is:
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 timeAnd a state covariance matrix Pn|n-1Here, the n time is not different from the k time, and represents different discrete times.
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:
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:
wherein:
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:
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:
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:
wherein:
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;
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.
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)
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)
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 |
-
2020
- 2020-12-18 CN CN202011502457.4A patent/CN112729281A/en active Pending
Patent Citations (4)
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)
Title |
---|
伍宗伟;姚敏立;马红光;贾维敏;田方浩;: "移动卫星通信低成本多传感器融合姿态估计方法", 西安交通大学学报, no. 12, 22 October 2012 (2012-10-22) * |
Cited By (2)
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 |