CN103712625A - Method for estimating initial heading deviation filtering - Google Patents
Method for estimating initial heading deviation filtering Download PDFInfo
- Publication number
- CN103712625A CN103712625A CN201310713137.7A CN201310713137A CN103712625A CN 103712625 A CN103712625 A CN 103712625A CN 201310713137 A CN201310713137 A CN 201310713137A CN 103712625 A CN103712625 A CN 103712625A
- Authority
- CN
- China
- Prior art keywords
- ship
- boat
- slave
- initial heading
- heading deviation
- 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
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/20—Instruments for performing navigational calculations
Abstract
The invention discloses a method for estimating the initial heading deviation filtering. The method for estimating the initial heading deviation filtering comprises the following steps: preparing two main boats equipped with high-precision inertial navigation equipment to send subaqueous sound ranging signals with time stamps to a slave boat alternatively; carrying out dead reckoning by the slave boat by utilizing the speed measured by a Doppler velocimeter and the heading measured by an MEMS (Micro-electromechanical Systems) gyroscope, and calculating the distance between each main boat and the slave boat by multiplying the time difference of sending and receiving submarine sound signals and the sound velocity; updating and amending the position of the slave boat by using Kalman filter algorithm, estimating and compensating the initial heading deviation of the gyroscope. According to the method, through the radio broadcasting positional information of the main boat, the slave boat passively receives radio signals transmitted by the main boat, so that the expansibility is improved, and the condition of multiple slave boats can be simulated by using two main boats and one slave boat; the slave boat is provided with low-precision inertial navigation equipment for pushing, and the high-precision inertial navigation equipment of the main boat is utilized by measuring the distance between the slave boat and each main boat, thus the positioning precision is improved, and the equipment cost is greatly reduced.
Description
Technical field
The invention belongs to many unmanned boats collaborative navigation field of locating technology, relate in particular to a kind of method of estimating initial heading deviation filtering.
Background technology
The collaborative navigation of many unmanned boats is the high precision navigation informations that utilize other ships in system, by certain message exchange, realizes sharing of the resource of navigating between ship, and the ship of equipping low precision navigator can improve the navigation accuracy of self.When some ship is lost self-contained navigation ability due to sensor or environmental factor, collaborative navigation can recover the homing capability of these platforms to a certain extent.Therefore the collaborative navigation of studying unmanned water surface ship has important theory value and practical significance.Underwater sound communication can be realized and disposing respectively following location and the proceed in formation of ship, and its dependence is little.Subaqueous sound ranging method measurement range can reach 2km~6km, and distance accuracy is in 1m.Motion state for carrier does not all have harsh requirement with the precision aspect of uniting in time.Therefore, underwater sound devices broadcasting mode communicates, and the method for uniting while adopting underwater sound equipment to be aided with radio is found range.
Lower from the inertial navigation equipment precision of ship, by measuring and be equipped with the distance of the captain boat of High Accuracy Inertial equipment, adopt EKF method to revise the flight path pushing of self, estimate and compensate initial heading deviation, improve positioning precision.Because MEMS gyro cannot carry out initial alignment, initial heading deviation is random.In order to improve observability, consider that two captain boats are alternately to sending ranging information from ship.
Up to the present, also not using course deviation as state and carry out the precedent of Kalman filtering using principal and subordinate's ship distance as observed quantity.
Summary of the invention
The object of the embodiment of the present invention is to provide a kind of method of estimating initial heading deviation filtering, is also intended to solve at present not using course deviation as state and using principal and subordinate's ship distance as observed quantity, carries out the problem of Kalman filtering.
The embodiment of the present invention is achieved in that a kind of method of estimating initial heading deviation filtering, and the method for this estimation initial heading deviation filtering comprises the following steps:
Step 1, all relevant devices are arranged on from ship, read MEMS gyro output course as static data and preserve, after one hour, captain boat equipment installs, start experiment, three ships keep delta formation while sailing on the water, two captain boats alternately send to water sound communication signal and transmitting time stamp from ship, 10 seconds, interval, underwater sound communication module from ship receives the ranging information that captain boat is sent, by the mistiming of underwater sound signal sending and receiving, be multiplied by the distance of velocity of sound calculating principal and subordinate ship, utilize speed and the course measured to carry out reckoning, and the distance of utilizing principal and subordinate's ship is as observed quantity correction position information and estimate initial heading deviation,
Step 2, set up system equation
In formula, x
k, y
kexpression from ship at k position constantly, v
kfor from ship speed, t represents the pushing time interval,
represent the course that MEMS gyro records,
represent initial heading deviation, in equation estimated value correction course measured value
deviation, be expressed as:
X
k+1=f(X
k,u
k,t)+w
k
After linearization,
X
k+1=F
kX
k+B
ku
k+w
k
Step 3, set up measurement equation:
In formula, observed quantity Z
kthe distance r that represents principal and subordinate's ship, x
a, y
athe position that represents captain boat, x
b, y
bexpression is from ship position,
V
k-N (0, R
k) for measuring noise;
Step 4, with EKF correction from ship pushing error;
(1) time upgrades
(2) measure and upgrade
In formula,
for the state estimation of filtering output,
p
k/k-1for state and and variance one-step prediction, K
kfor filter gain.
Further, in step 4, because GPS is easily interfered, when can not receive gps signal or gps signal appearance mistake, a computing time is new portion more, i.e. reckoning, do not calculate and measure more new portion, after receiving correct gps signal, then the time of carrying out renewal and measure to upgrade, by the distance of principal and subordinate's ship to upgrading correction from the position of ship.
The method of estimation provided by the invention initial heading deviation filtering, by being housed, two captain boats of High Accuracy Inertial equipment replace to send the subaqueous sound ranging signal that is added with timestamp from ship, reckoning is carried out in the course that utilizes speed that Doppler anemometer records and MEMS gyro to record from ship, and be multiplied by the distance that the velocity of sound is calculated principal and subordinate's ship by the mistiming of underwater sound signal sending and receiving, use EKF algorithm to upgrading and revise from the position of ship, estimate and compensate the initial heading deviation of gyro;
The present invention has the following advantages:
1, due to captain boat radio broadcasting positional information, radio signal from the transmitting of ship passive receive captain boat, so can increase arbitrarily from the quantity of ship, can not affect the design of experimental procedure, so extendability of the present invention is fine, with two captain boats and, from ship, can simulate many situations from ship;
2, from ship, low precision inertial navigation equipment (as MEMS) is installed and is carried out pushing, and utilize the High Accuracy Inertial equipment of captain boat to improve positioning precision by the distance of measurement and captain boat, greatly reduce the cost of equipment.
Accompanying drawing explanation
Fig. 1 is the method flow diagram of the estimation initial heading deviation filtering that provides of the embodiment of the present invention.
Embodiment
In order to make object of the present invention, technical scheme and advantage clearer, below in conjunction with embodiment, the present invention is further elaborated.Should be appreciated that specific embodiment described herein, only in order to explain the present invention, is not intended to limit the present invention.
Below in conjunction with drawings and the specific embodiments, application principle of the present invention is further described.
As shown in Figure 1, the method for the estimation initial heading deviation filtering of the embodiment of the present invention comprises the following steps:
S101: two captain boats preparing High Accuracy Inertial equipment is housed are alternately to send the subaqueous sound ranging signal that is added with timestamp from ship;
S102: reckoning is carried out in the course that utilizes speed that Doppler anemometer records and MEMS gyro to record from ship, and be multiplied by the distance that the velocity of sound is calculated principal and subordinate's ship by the mistiming of underwater sound signal sending and receiving;
S103: use Kalman filtering algorithm to upgrading and revise from the position of ship, estimate and compensate the initial heading deviation of gyro.
Concrete steps of the present invention are:
Scheme is: two captain boat broadcast radio signal
Step 1, system are installed and configuration
High Accuracy Inertial equipment and underwater sound communication module are housed respectively on two captain boats, two captain boats alternately send to water sound communication signal and transmitting time stamp from ship, 10 seconds, interval, Doppler anemometer from ship provides velocity magnitude, MEMS gyro provides course, underwater sound communication module from ship receives the ranging information that captain boat is sent, and utilizes the computer carrying on ship to carry out reckoning and filtering, calculates longitude and latitude position, place and estimates initial heading deviation;
Step 2, specific implementation method
All relevant devices are arranged on from ship, read MEMS gyro output course as static data and preserve, after one hour, captain boat equipment installs, start experiment, three ships keep delta formation while sailing on the water, observability when raising is found range from ship, two captain boats alternately send to water sound communication signal and transmitting time stamp from ship, 10 seconds, interval, underwater sound communication module from ship receives the ranging information that captain boat is sent, by the mistiming of underwater sound signal sending and receiving, be multiplied by the distance of velocity of sound calculating principal and subordinate ship, utilize speed and the course measured to carry out reckoning, and the distance of utilizing principal and subordinate's ship is as observed quantity correction position information and estimate initial heading deviation,
Step 3, set up system equation
In formula, x
k, y
kexpression from ship at k position constantly, v
kfor from ship speed, t represents the pushing time interval,
represent the course that MEMS gyro records,
represent initial heading deviation, in equation with its estimated value correction course measured value
deviation, be expressed as general type and obtain:
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)
Step 4, set up measurement equation,
In formula, observed quantity Z
kthe distance r that represents principal and subordinate's ship, x
a, y
athe position that represents captain boat, x
b, y
bexpression is from ship position,
V
k-N (0, R
k) for measuring noise;
Step 5, with EKF correction from ship pushing error;
(1) time upgrades
(2) measure and upgrade
In formula,
for the state estimation of filtering output,
p
k/k-1for state and and variance one-step prediction, K
kfor filter gain, because GPS is easily interfered, when can not receive gps signal or gps signal appearance mistake, computing time is new portion (being reckoning) more, do not calculate and measure more new portion, after receiving correct gps signal, then the time of carrying out renewal and measure to upgrade, by the distance of principal and subordinate's ship to upgrading correction from the position of ship.
In order to further illustrate validity of the present invention, in urban district, carried out three ship experiments, the method that adopts two captain boats to communicate by letter from ship with, MEMS course deviation is 10 degree, image data track drafting are as follows:
By actual experiment analysis comparison: adopt and estimate that the EKF algorithm of initial heading deviation is more much higher than the positioning precision of simple pushing, what filtering obtained almost fits like a glove from ship track and real trace, and the track that pushing obtains and real trace have deviation; The effect of the initial heading deviation of employing EKF estimation MEMS is fine, has reached the object of correct correction course deviation, thereby has verified that this algorithm improves the validity of positioning precision.
The foregoing is only preferred embodiment of the present invention, not in order to limit the present invention, all any modifications of doing within the spirit and principles in the present invention, be equal to and replace and improvement etc., within all should being included in protection scope of the present invention.
Claims (2)
1. a method of estimating initial heading deviation filtering, is characterized in that, the method for this estimation initial heading deviation filtering comprises the following steps:
Step 1, all relevant devices are arranged on from ship, read MEMS gyro output course as static data and preserve, after one hour, captain boat equipment installs, three ships keep delta formation while sailing on the water, two captain boats alternately send to water sound communication signal and transmitting time stamp from ship, 10 seconds, interval, underwater sound communication module from ship receives the ranging information that captain boat is sent, by the mistiming of underwater sound signal sending and receiving, be multiplied by the distance of velocity of sound calculating principal and subordinate ship, utilize speed and the course measured to carry out reckoning, and the distance of utilizing principal and subordinate's ship is as observed quantity correction position information and estimate initial heading deviation,
Step 2, set up system equation:
In formula, x
k, y
kexpression from ship at k position constantly, v
kfor from ship speed, t represents the pushing time interval,
represent the course that MEMS gyro records,
represent initial heading deviation, in equation estimated value correction course measured value
deviation, be expressed as:
X
k+1=f(X
k,u
k,t)+w
k
After linearization,
X
k+1=F
kX
k+B
ku
k+w
k
Step 3, set up measurement equation:
In formula, observed quantity Z
kthe distance r that represents principal and subordinate's ship, x
a, y
athe position that represents captain boat, x
b, y
bexpression is from ship position,
V
k-N (0, R
k) for measuring noise;
Step 4, with EKF correction from ship pushing error;
(1) time upgrades:
(2) measure and upgrade:
2. the method for estimation as claimed in claim 1 initial heading deviation filtering, it is characterized in that, in step 4, because GPS is easily interfered, when can not receive gps signal or gps signal appearance mistake, computing time is new portion more, be reckoning, do not calculate and measure more new portion, after receiving correct gps signal, carry out again time renewal and measure to upgrade, by the distance of principal and subordinate's ship to upgrading correction from the position of ship.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310713137.7A CN103712625A (en) | 2013-12-23 | 2013-12-23 | Method for estimating initial heading deviation filtering |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310713137.7A CN103712625A (en) | 2013-12-23 | 2013-12-23 | Method for estimating initial heading deviation filtering |
Publications (1)
Publication Number | Publication Date |
---|---|
CN103712625A true CN103712625A (en) | 2014-04-09 |
Family
ID=50405787
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310713137.7A Pending CN103712625A (en) | 2013-12-23 | 2013-12-23 | Method for estimating initial heading deviation filtering |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103712625A (en) |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104034328A (en) * | 2014-05-21 | 2014-09-10 | 哈尔滨工程大学 | Cooperative navigation method based on combination of filtering method and curve fitting method |
CN104459707A (en) * | 2014-12-05 | 2015-03-25 | 北京航空航天大学 | Online obtaining method for initial position of underwater towed body dead reckoning system |
CN106568442A (en) * | 2016-10-18 | 2017-04-19 | 中冶华天南京电气工程技术有限公司 | Synergetic navigation wave filtering method having robust characteristic |
CN107525507A (en) * | 2016-10-18 | 2017-12-29 | 腾讯科技(深圳)有限公司 | The decision method and device of driftage |
CN110412546A (en) * | 2019-06-27 | 2019-11-05 | 厦门大学 | A kind of localization method and system for submarine target |
CN106643723B (en) * | 2016-11-07 | 2019-11-26 | 哈尔滨工程大学 | A kind of unmanned boat safe navigation dead reckoning method |
CN110501029A (en) * | 2019-09-26 | 2019-11-26 | 哈尔滨工程大学 | The online calibration method of MEMS gyroscope load is carried towards cluster aircraft |
CN111256709A (en) * | 2020-02-18 | 2020-06-09 | 北京九曜智能科技有限公司 | Vehicle dead reckoning positioning method and device based on encoder and gyroscope |
CN114440869A (en) * | 2021-12-27 | 2022-05-06 | 宜昌测试技术研究所 | Double-main AUV switching AUV cluster large-water-depth operation collaborative navigation positioning method |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4297700A (en) * | 1973-10-23 | 1981-10-27 | Societe D'etudes, Recherches Et Constructions Electroniques Sercel | Method and apparatus for measuring distances |
WO2008144139A1 (en) * | 2007-05-14 | 2008-11-27 | Zupt, Llc | System and process for the precise positioning of subsea units |
CN103292813A (en) * | 2013-05-24 | 2013-09-11 | 哈尔滨工程大学 | Information filtering method for improving formation and navigation accuracy of water surface boat |
CN103322999A (en) * | 2013-05-24 | 2013-09-25 | 哈尔滨工程大学 | History state reserved information filtering algorithm suitable for multi-boat navigation |
CN103398712A (en) * | 2013-08-02 | 2013-11-20 | 中国人民解放军63983部队 | Alternatively piloted collaborative navigation method |
-
2013
- 2013-12-23 CN CN201310713137.7A patent/CN103712625A/en active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4297700A (en) * | 1973-10-23 | 1981-10-27 | Societe D'etudes, Recherches Et Constructions Electroniques Sercel | Method and apparatus for measuring distances |
WO2008144139A1 (en) * | 2007-05-14 | 2008-11-27 | Zupt, Llc | System and process for the precise positioning of subsea units |
CN103292813A (en) * | 2013-05-24 | 2013-09-11 | 哈尔滨工程大学 | Information filtering method for improving formation and navigation accuracy of water surface boat |
CN103322999A (en) * | 2013-05-24 | 2013-09-25 | 哈尔滨工程大学 | History state reserved information filtering algorithm suitable for multi-boat navigation |
CN103398712A (en) * | 2013-08-02 | 2013-11-20 | 中国人民解放军63983部队 | Alternatively piloted collaborative navigation method |
Non-Patent Citations (4)
Title |
---|
YUKIKAZU S. HIDAKA等: "Optimal Formations for Cooperative Localization of mobile robots", 《PROCEEDINGS OF THE IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION(ICRA)》 * |
徐博等: "基于微惯性网络的多水面无人艇协同导航定位技术研究", 《惯性技术发展动态发展方向研讨会文集》 * |
杨峻巍: "水下航行器导航及数据融合技术研究", 《中国博士学位论文全文数据库工程科技II辑》 * |
高伟等: "基于双领航者的多AUV协同导航系统可观测性分析", 《基于双领航者的多AUV协同导航系统》 * |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104034328B (en) * | 2014-05-21 | 2017-03-29 | 哈尔滨工程大学 | A kind of collaborative navigation method combined based on filtering method and curve-fitting method |
CN104034328A (en) * | 2014-05-21 | 2014-09-10 | 哈尔滨工程大学 | Cooperative navigation method based on combination of filtering method and curve fitting method |
CN104459707A (en) * | 2014-12-05 | 2015-03-25 | 北京航空航天大学 | Online obtaining method for initial position of underwater towed body dead reckoning system |
CN106568442A (en) * | 2016-10-18 | 2017-04-19 | 中冶华天南京电气工程技术有限公司 | Synergetic navigation wave filtering method having robust characteristic |
CN107525507A (en) * | 2016-10-18 | 2017-12-29 | 腾讯科技(深圳)有限公司 | The decision method and device of driftage |
CN107525507B (en) * | 2016-10-18 | 2019-03-08 | 腾讯科技(深圳)有限公司 | The determination method and device of yaw |
CN106643723B (en) * | 2016-11-07 | 2019-11-26 | 哈尔滨工程大学 | A kind of unmanned boat safe navigation dead reckoning method |
CN110412546A (en) * | 2019-06-27 | 2019-11-05 | 厦门大学 | A kind of localization method and system for submarine target |
CN110412546B (en) * | 2019-06-27 | 2022-09-16 | 厦门大学 | Positioning method and system for underwater target |
CN110501029A (en) * | 2019-09-26 | 2019-11-26 | 哈尔滨工程大学 | The online calibration method of MEMS gyroscope load is carried towards cluster aircraft |
CN111256709A (en) * | 2020-02-18 | 2020-06-09 | 北京九曜智能科技有限公司 | Vehicle dead reckoning positioning method and device based on encoder and gyroscope |
CN114440869A (en) * | 2021-12-27 | 2022-05-06 | 宜昌测试技术研究所 | Double-main AUV switching AUV cluster large-water-depth operation collaborative navigation positioning method |
CN114440869B (en) * | 2021-12-27 | 2023-07-04 | 宜昌测试技术研究所 | Collaborative navigation positioning method for AUV cluster large water depth operation switched by double-master AUV |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103712625A (en) | Method for estimating initial heading deviation filtering | |
US20230041831A1 (en) | Methods of attitude and misalignment estimation for constraint free portable navigation | |
US10877059B2 (en) | Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method | |
CN201266089Y (en) | INS/GPS combined navigation system | |
CN103697910B (en) | The correction method of autonomous underwater aircraft Doppler log installation error | |
CN103299205B (en) | For the system and method that mobile base station RTK measures | |
CN102506857B (en) | Relative attitude measurement real-time dynamic filter method based on dual-inertial measurement unit/differential global positioning system (IMU/DGPS) combination | |
CN106595715B (en) | Based on inertial navigation and satellite combined guidance system mileage meter calibration method and device | |
CN104075715A (en) | Underwater navigation and positioning method capable of combining terrain and environment characteristics | |
CN102818567A (en) | AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering | |
CN103697892A (en) | Filtering method for gyroscopic drift under collaborative navigation condition of multiple unmanned surface vehicles | |
CN105319534A (en) | Multiple AUV cooperative positioning method based on underwater sound double pass range finding | |
CN102608642A (en) | Beidou/inertial combined navigation system | |
CN103389115A (en) | Integrated error calibrating method of SINS/DVL (strapdown inertial navigation system/doppler velocity sonar) combined navigation system | |
CN101183008A (en) | Inertia compensation method used for earth-based vehicle GPS navigation | |
CN102645222A (en) | Satellite inertial navigation method and equipment | |
CN103278163A (en) | Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method | |
CN102252677A (en) | Time series analysis-based variable proportion self-adaptive federal filtering method | |
CN102628691A (en) | Completely independent relative inertial navigation method | |
CN107015259A (en) | The tight integration method of pseudorange/pseudorange rates is calculated using Doppler anemometer | |
CN104061930A (en) | Navigation method based on strapdown inertial guidance and Doppler log | |
KR20100127343A (en) | Navigation system having enhanced reliability and method thereof | |
US20100090893A1 (en) | User based positioning aiding network by mobile GPS station/receiver | |
CN103968842A (en) | Method for improving collaborative navigation location precision of unmanned vehicle based on MEMS gyro | |
JP5994237B2 (en) | Positioning device and program |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20140409 |
|
WD01 | Invention patent application deemed withdrawn after publication |