CN103968842A - Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro - Google Patents
Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro Download PDFInfo
- Publication number
- CN103968842A CN103968842A CN201410216171.8A CN201410216171A CN103968842A CN 103968842 A CN103968842 A CN 103968842A CN 201410216171 A CN201410216171 A CN 201410216171A CN 103968842 A CN103968842 A CN 103968842A
- Authority
- CN
- China
- Prior art keywords
- ship
- filtering
- represent
- mems gyro
- model
- 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
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/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
The invention relates to a method for improving collaborative navigation location precision of an unmanned vehicle based on a MEMS gyro, which is used for rapidly compensating the error of a micro-electromechanical system gyro. The method comprises the following steps: establishing a forward filter model; storing forward filter data; establishing a reverse filter system model; establishing a reverse filter measurement model; utilizing the established system model and measurement model; carrying out the forward filter. By adopting the method, the heading measurement error of the MEMS gyro is enlarged to a state vector, so that the error of the MEMS gyro can be more effectively compensated in the filter process. The reverse filter is simple to operate in the collaborative navigation model and easy to realize, only a symbol of a velocity v needs to be changed and the data stored in the forward filter needs to be reversely utilized, and the data includes the position reckoning of an auxiliary vehicle and the distance between a main vehicle and the auxiliary vehicle. A small section of stored data can be repeatedly utilized by the reverse filter, and more data is unnecessary to be collected, so that the filter estimation speed can be remarkably increased, and the calculation speed of a computer is high.
Description
Technical field
The present invention relates to the method for the unmanned boat collaborative navigation positioning precision of a kind of raising compensating fast for MEMS (micro electro mechanical system) (micro-electro-mechanical system, MEMS) gyro error based on MEMS gyro.
Background technology
Unmanned water surface ship is that one is travelled at the water surface, can be carried out by people the water surface ships and light boats of straighforward operation or autonomous operation.Along with the intensification to ocean development understanding, unmanned water surface ship becomes study hotspot with advantages such as its mobility strong, cost are low.The collaborative navigation of many unmanned boats is the high precision navigation informations that utilize captain boat in system, by certain message exchange, realizes sharing of the resource of navigating between ship, and what make to equip low precision navigator can improve the navigation accuracy of self from ship.Therefore the collaborative navigation of studying unmanned water surface ship has important theory value and practical significance.
Although GPS can provide accurate positional information, and energy correction of timing is from ship ins error, is easily subject to geographical conditions and artificial interference.By comparison, use the synchronous underwater sound data of transmitting time can calculate the relative distance of principal and subordinate's ship, this information can assist to carry out navigator fix from ship.Merge by EKF (EKF) distance and the positional information that record from ship utilization and estimate to reduce evaluated error.
In recent years, MEMS gyro, as the very important branch in one, inertia field, has obtained significant progress.Because its cost is low, size is little, lightweight, high reliability, in low cost inertia system, more and more applied, in collaborative navigation, can be used as from ship inertial navigation set and equip.Be subject to the lower restriction of the current precision of MEMS gyro, before use, must set up rational drifting error model, thereby gyro error is estimated and compensated.In order to solve the observability problem of gyro error, can first estimate gyro initial heading deviation, then estimate gyroscopic drift.Adopt backward filtering method to estimate that gyro error can shorten estimated time greatly, improve positioning precision.
Summary of the invention
The object of the present invention is to provide a kind of data of reverse utilization storage to carry out backward filtering, shorten the estimated time of MEME systematic error, and then the method for the unmanned boat collaborative navigation positioning precision of the raising of raising collaborative navigation positioning precision based on MEMS gyro.
The object of the present invention is achieved like this:
(1) set up forward filtering model:
Underwater sound communication module from ship receives the information that captain boat is sent, be multiplied by distance that the velocity of sound calculates principal and subordinate ship as observed quantity by the mistiming of underwater sound signal sending and receiving, utilize the speed of Doppler anemometer measurement and the course that MEMS gyro is measured, estimate from the position of ship and the error of MEMS gyro by EKF
State one-step prediction equation is:
In formula, x
k, y
krepresent from ship the position in the k moment, v
kfor from ship speed, t represents the pushing time interval,
represent the course that MEMS gyro records,
represent the error in MEMS gyro to measure course, in equation with its estimated value correction course measured value
deviation:
X
k+1＝f(X
k,u
k,t)+w
k
After linearization,
X
k+1＝F
kX
k+B
ku
k+w
k
In formula,
system noise w
k～N (0, Q
k), measurement equation is
In formula, observed quantity Z
krepresent the distance r of principal and subordinate's ship, x
a, y
arepresent the position of captain boat, x
b, y
brepresent from ship position,
v
k～N (0, R
k) be measurement noise;
(2) storage forward filtering data
In filtering, store the measured distance from ship speed, course and principal and subordinate's ship;
(3) backward filtering system model is set up
It is oppositely equivalent that backward filtering is compared forward filtering speed, even
set up state one-step prediction model:
X
k-1＝f(X
k,u
k,t)+w
k
After linearization,
X
k-1＝F
kX
k+B
ku
k+w
k
In formula,
System noise w
k～N (0, Q
k);
(4) set up backward filtering measurement model
In formula, observed quantity Z
krepresent the distance r of principal and subordinate's ship, x
a, y
arepresent the position of captain boat, x
b, y
brepresent from ship position,
v
k-N (0, R
k) for measuring noise;
(5) utilize system model and the measurement model set up, that while oppositely utilizing forward filtering, stores carries out time renewal from ship speed, course, utilize the distance of principal and subordinate's ship to measure renewal, estimate from the position of ship and the error of MEMS gyro by EKF
Time upgrades:
Measure and upgrade:
P
k＝(I-K
kH
k)P
k/k-1
In formula,
for the state estimation of filtering output,
p
k/k-1for state and and variance one-step prediction, K
kfor filter gain;
(6) forward filtering
Forward filtering is got back to breakpoint.
Beneficial effect of the present invention is: the present invention is by the error in MEMS gyro to measure course
also be expanded into for state vector, make can more effectively compensate in filtering the error of MEMS gyro.Backward filtering is simple to operate in collaborative navigation model, is easy to realize, and the data of storing when only need to changing the symbol of speed v and oppositely utilizing forward filtering comprise from ship boat and push away the position that obtains and the distance of principal and subordinate's ship.Other parts of the system equation of EKF and measurement equation are constant.Because backward filtering can be recycled a bit of data of having stored, do not need to gather more data, thereby significantly improve the speed that filtering is estimated, and computing machine calculates speed, so repeatedly inverse algorithm is almost being specified instantaneous completing of moment, do not take extra time, therefore do not affect the whole operating process of actual use.
Brief description of the drawings
Fig. 1 is fast filtering method process flow diagram.
Fig. 2 is normal filtering initial heading estimation of deviation.
Fig. 3 is backward filtering initial heading estimation of deviation.
Fig. 4 estimates initial heading deviation fast.
Fig. 5 is quick filter and normal filtering positioning error comparison.
Embodiment
Below in conjunction with accompanying drawing, the present invention is described further.
Step 1, set up forward filtering model
High Accuracy Inertial equipment and underwater sound communication module are housed respectively on two captain boats, water sound communication signal and transmitting time stamp are alternately sent to from ship to 10 seconds, interval.Underwater sound communication module from ship receives the information that captain boat is sent, and is multiplied by distance that the velocity of sound calculates principal and subordinate's ship as observed quantity by the mistiming of underwater sound signal sending and receiving.Observability while range finding from ship in order to improve, two captain boats and one keep delta formation in the time that ship sails on the water.Utilize the speed of Doppler anemometer measurement and the course that MEMS gyro is measured simultaneously, estimate from the position of ship and the error of MEMS gyro by EKF.
State one-step prediction equation is as follows,
In formula, x
k, y
krepresent from ship the position in the k moment, v
kfor from ship speed, t represents the pushing time interval.
represent the course that MEMS gyro records,
represent the error in MEMS gyro to measure course, in equation with its estimated value correction course measured value
deviation.Being expressed as general type is:
X
k+1＝f(X
k,u
k,t)+w
k(2)
After linearization,
X
k+1＝F
kX
k+B
ku
k+w
k(3)
In formula,
system noise w
k～N (0, Q
k).
Measurement equation is as follows,
In formula, observed quantity Z
krepresent the distance r of principal and subordinate's ship, x
a, y
arepresent the position of captain boat, x
b, y
brepresent from ship position,
v
k～N (0, R
k) be measurement noise.
Step 2, storage forward filtering data
In filtering, store the measured distance from ship speed, course and principal and subordinate's ship, in order to backward filtering estimated service life.Because backward filtering is equivalent to from ship backward going, so oppositely utilize the data of storage, the course recording from ship speed, MEMS gyro like this and the distance of principal and subordinate's ship with have one-to-one relationship from the track of ship, thereby ensure that backward filtering obtains good estimation effect.
Step 3, backward filtering system model are set up
It is oppositely equivalent that backward filtering is compared forward filtering speed, even
be equivalent to from ship backward going.In addition, in model its dependent variable to compare forward filtering constant.Thereby it is as follows to set up state one-step prediction model,
X
k-1＝f(X
k,u
k,t)+w
k(6)
After linearization,
X
k-1＝F
kX
k+B
ku
k+w
k(7)
In formula,
system noise w
k～N (0, Q
k).
The foundation of step 4, backward filtering measurement model
During due to backward filtering, oppositely utilized the captain boat data that gather, be equivalent to captain boat also backward going and captain boat position and have one-to-one relationship from ship, so identical during with forward filtering of measurement equation.
In formula, observed quantity Z
krepresent the distance r of principal and subordinate's ship, x
a, y
arepresent the position of captain boat, x
b, y
brepresent from ship position,
v
k-N (0, R
k) for measuring noise.
Said system model and measurement model that step 5, utilization are set up, that while oppositely utilizing forward filtering, stores carries out time renewal from ship speed, course, utilize the distance of principal and subordinate's ship to measure renewal, estimate from the position of ship and the error of MEMS gyro by EKF.
(1) time upgrades:
(9)
(2) measure and upgrade
P
k＝(I-K
kH
k)P
k/k-1
In formula,
for the state estimation of filtering output,
p
k/k-1for state and and variance one-step prediction, K
kfor filter gain.
Step 6, forward filtering
In order to obtain correct filtering result, also need forward filtering to get back to breakpoint.Then both can be from breakpoint succession forward filtering, also can repeat reverse and forward filtering repeatedly, further shorten estimated time, as shown in Figure 1.
Fig. 2 has compared the estimation effect of backward filtering for MEMS gyro error to Fig. 4.
Can find out from second subgraph of Fig. 3, backward filtering oppositely utilizes the data of storage, the corresponding transverse axis time is 45 seconds-0 second, make to estimate further to approach true value, but with forward filtering Fig. 2, to compare effect slightly poor, but it does not take extra time after all, so have very great help for the shortening of overall estimate time.
Fig. 4 shows, the data within 0-45 second of locating to utilize storage for 45 seconds are carried out forward, backward filtering repeatedly.Owing to not needing to carry out data acquisition, computer speed is fast, so filtering has been used the data in 45 seconds to complete estimation, has reached the object of convergence speedup time.The normal filtering estimating speed of Fig. 2 is slow many.Fig. 5 further illustrates, and because quick filter has improved the estimating speed of initial heading deviation, has improved positioning precision, and it is little that its error (solid line) is compared normal filtering (dotted line).
Claims (1)
1. a method for the unmanned boat collaborative navigation positioning precision of raising based on MEMS gyro, is characterized in that:
(1) set up forward filtering model
Underwater sound communication module from ship receives the information that captain boat is sent, be multiplied by distance that the velocity of sound calculates principal and subordinate ship as observed quantity by the mistiming of underwater sound signal sending and receiving, utilize the speed of Doppler anemometer measurement and the course that MEMS gyro is measured, estimate from the position of ship and the error of MEMS gyro by EKF
State one-step prediction equation is:
In formula, x
k, y
krepresent from ship the position in the k moment, v
kfor from ship speed, t represents the pushing time interval,
represent the course that MEMS gyro records,
represent the error in MEMS gyro to measure course, in equation with its estimated value correction course measured value
deviation:
X
k+1＝f(X
k,u
k,t)+w
k
After linearization,
X
k+1＝F
kX
k+B
ku
k+w
k
In formula,
system noise w
k～N (0, Q
k), measurement equation is
In formula, observed quantity Z
krepresent the distance r of principal and subordinate's ship, x
a, y
arepresent the position of captain boat, x
b, y
brepresent from ship position,
v
k～N (0, R
k) be measurement noise;
(2) storage forward filtering data
In filtering, store the measured distance from ship speed, course and principal and subordinate's ship;
(3) backward filtering system model is set up
It is oppositely equivalent that backward filtering is compared forward filtering speed, even
set up state one-step prediction model:
X
k-1＝f(X
k,u
k,t)+w
k
After linearization,
X
k-1＝F
kX
k+B
ku
k+w
k
In formula,
System noise w
k～N (0, Q
k);
(4) set up backward filtering measurement model
In formula, observed quantity Z
krepresent the distance r of principal and subordinate's ship, x
a, y
arepresent the position of captain boat, x
b, y
brepresent from ship position,
v
k-N (0, R
k) for measuring noise;
(5) utilize system model and the measurement model set up, that while oppositely utilizing forward filtering, stores carries out time renewal from ship speed, course, utilize the distance of principal and subordinate's ship to measure renewal, estimate from the position of ship and the error of MEMS gyro by EKF
Time upgrades:
Measure and upgrade:
P
k＝(I-K
kH
k)P
k/k-1
In formula,
for the state estimation of filtering output,
p
k/k-1for state and and variance one-step prediction, K
kfor filter gain;
(6) forward filtering
Forward filtering is got back to breakpoint.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201410216171.8A CN103968842A (en) | 2014-05-21 | 2014-05-21 | Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro |
| NL2026221A NL2026221B1 (en) | 2014-05-21 | 2020-08-05 | An improved multi-boat cooperative navigation method based on MEMS |
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201410216171.8A CN103968842A (en) | 2014-05-21 | 2014-05-21 | Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro |
| NL2026221A NL2026221B1 (en) | 2014-05-21 | 2020-08-05 | An improved multi-boat cooperative navigation method based on MEMS |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN103968842A true CN103968842A (en) | 2014-08-06 |
Family
ID=78595200
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201410216171.8A Pending CN103968842A (en) | 2014-05-21 | 2014-05-21 | Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN103968842A (en) |
| NL (1) | NL2026221B1 (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105584599A (en) * | 2016-01-25 | 2016-05-18 | 大连海事大学 | Marine environmental monitoring system based on unmanned ship formation motion |
| CN107003134A (en) * | 2014-11-26 | 2017-08-01 | 罗伯特·博世有限公司 | Server for running parking lot |
| NL2026221B1 (en) * | 2014-05-21 | 2021-04-22 | Univ Harbin Eng | An improved multi-boat cooperative navigation method based on MEMS |
| CN113031660A (en) * | 2021-04-02 | 2021-06-25 | 中北大学 | Aircraft directional antenna tracking and positioning device |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103968842A (en) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro |
-
2014
- 2014-05-21 CN CN201410216171.8A patent/CN103968842A/en active Pending
-
2020
- 2020-08-05 NL NL2026221A patent/NL2026221B1/en not_active IP Right Cessation
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| NL2026221B1 (en) * | 2014-05-21 | 2021-04-22 | Univ Harbin Eng | An improved multi-boat cooperative navigation method based on MEMS |
| CN107003134A (en) * | 2014-11-26 | 2017-08-01 | 罗伯特·博世有限公司 | Server for running parking lot |
| CN105584599A (en) * | 2016-01-25 | 2016-05-18 | 大连海事大学 | Marine environmental monitoring system based on unmanned ship formation motion |
| CN105584599B (en) * | 2016-01-25 | 2017-10-31 | 大连海事大学 | A kind of marine environmental monitoring system for motion of being formed into columns based on unmanned boat |
| CN113031660A (en) * | 2021-04-02 | 2021-06-25 | 中北大学 | Aircraft directional antenna tracking and positioning device |
Also Published As
| Publication number | Publication date |
|---|---|
| NL2026221B1 (en) | 2021-04-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | A novel backtracking navigation scheme for autonomous underwater vehicles | |
| CN112097763B (en) | Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination | |
| JP4819802B2 (en) | System and method for alignment of multiple navigation components | |
| CN111595348B (en) | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system | |
| CN104075715A (en) | Underwater navigation and positioning method capable of combining terrain and environment characteristics | |
| Li et al. | Optimization-based INS in-motion alignment approach for underwater vehicles | |
| CN102608642A (en) | Beidou/inertial combined navigation system | |
| CN105319534A (en) | Multiple AUV cooperative positioning method based on underwater sound double pass range finding | |
| CN102628691A (en) | Completely independent relative inertial navigation method | |
| CN106679662A (en) | Combined underwater robot navigation method based on TMA (target motion analysis) technology and single beacon | |
| CN103712625A (en) | Method for estimating initial heading deviation filtering | |
| CN104280025A (en) | Adaptive unscented Kalman filter-based deepwater robot short-baseline combined navigation method | |
| CN108303079B (en) | Data smoothing method for underwater USBL reverse application | |
| CN107966145B (en) | AUV underwater navigation method based on sparse long baseline tight combination | |
| CN103968842A (en) | Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro | |
| CN103968838A (en) | Co-location method of AUVs (Autonomous Underwater Vehicles) in curvilinear motion state based on polar coordinate system | |
| CN103389115A (en) | Integrated error calibrating method of SINS/DVL (strapdown inertial navigation system/doppler velocity sonar) combined navigation system | |
| CN104236586A (en) | Moving base transfer alignment method based on measurement of misalignment angle | |
| CN109945895A (en) | Inertial navigation Initial Alignment Method based on the smooth structure changes filtering that fades | |
| CN105547300A (en) | All-source navigation system and method used for AUV (Autonomous Underwater Vehicle) | |
| CN104931994A (en) | Software receiver-based distributed deep integrated navigation method and system | |
| CN103697892B (en) | Filtering method for gyroscopic drift under collaborative navigation condition of multiple unmanned surface vehicles | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| CN104949687A (en) | Whole parameter precision evaluation method for long-time navigation system | |
| CN104296741A (en) | WSN/AHRS (Wireless Sensor Network/Attitude Heading Reference System) tight combination method adopting distance square and distance square change rate |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20140806 |
|
| RJ01 | Rejection of invention patent application after publication |