CN116337115A - Sonar-based method and system for calibrating inertial sensor - Google Patents

Sonar-based method and system for calibrating inertial sensor Download PDF

Info

Publication number
CN116337115A
CN116337115A CN202310628706.1A CN202310628706A CN116337115A CN 116337115 A CN116337115 A CN 116337115A CN 202310628706 A CN202310628706 A CN 202310628706A CN 116337115 A CN116337115 A CN 116337115A
Authority
CN
China
Prior art keywords
error
sonar
inertial navigation
inertial
navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310628706.1A
Other languages
Chinese (zh)
Other versions
CN116337115B (en
Inventor
聂文锋
吴湃湃
刘杨范
徐天河
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Laoshan National Laboratory
Shandong University
Original Assignee
Laoshan National Laboratory
Shandong University
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 Laoshan National Laboratory, Shandong University filed Critical Laoshan National Laboratory
Priority to CN202310628706.1A priority Critical patent/CN116337115B/en
Publication of CN116337115A publication Critical patent/CN116337115A/en
Application granted granted Critical
Publication of CN116337115B publication Critical patent/CN116337115B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • 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
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/52—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S15/00
    • G01S7/52004—Means for monitoring or calibrating
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

The invention provides a method and a system for calibrating an inertial sensor based on sonar, and relates to the field of navigation. The method comprises the steps of establishing an inertial sensor measurement error model; establishing a state equation of a navigation system; taking the difference between sonar ranging information and ranging information reversely deduced by using the position independently deduced by inertial navigation and the position of the submarine reference station as an observation vector to construct a measurement equation; updating the acoustic observance quantity and the inertial navigation calculation observance quantity; updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; and when the acoustic observed quantity is updated, taking the difference between sonar ranging information and ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector, and carrying out measurement updating until the inertial navigation error is converged. According to the invention, the inertial navigation error is calibrated on line by using the observed sonar signals, so that the underwater vehicle can estimate and drive to the next submarine reference station network by means of the pure inertial navigation position when no sonar signals exist.

Description

Sonar-based method and system for calibrating inertial sensor
Technical Field
The invention belongs to the technical field of underwater integrated navigation, and particularly relates to a method and a system for calibrating an inertial sensor based on sonar.
Background
The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art.
With the proposal of ocean national strategies, the importance of underwater navigation is becoming more and more obvious. The global satellite navigation system GNSS based on radio propagation cannot be applied under water, and acoustic navigation overcomes the defect that electromagnetic waves attenuate fast in sea water, so that sonar signals become important signals for underwater navigation. The current underwater navigation technology comprises acoustic navigation, inertial navigation, geophysical field matching navigation and the like. Along with the wide application of underwater acoustic positioning, an integrated navigation system mainly comprising inertial navigation SINS/acoustic LBL becomes a development trend.
At present, research on inertial navigation by sonar calibration is not available at home, but a certain research result is obtained in the work of inertial navigation by GPS calibration. Li Haijiang et al in 2006 proposed calibrating strapdown inertial sensors in whole bullets, realizing calibration of main device errors; liu Zhidong et al designed a 17-dimensional kalman filter comprising a gyro and a random constant error with a meter and a scale factor error, and used the information provided by the GPS as an external observational quantity to excite a related error term by planning a proper flight path so as to realize the calibration of the basic error of an inertial device; zhang Xiaoyue et al of North navigation propose to decompose the output error of the IMU into several parts and build a 33-dimensional system error model to perform on-line calibration and compensation for error items including random constant error, scale factor error, installation error, random white noise, etc.; the building of North aviation and the like propose to build a 39-dimensional high-order error model containing error items such as calibration residual errors of installation errors, scale factor errors and the like, and experiments prove that the error precision is greatly improved compared with the traditional error model after the model is used on a POS system.
In the underwater integrated navigation, sonar is a main navigation information source, and the underwater vehicle can receive a sonar signal on the premise that a corresponding reference station is arranged on the seabed. Under the condition that the sonar signal is well observed, the single acoustic navigation precision can reach the meter level, but when the incidence angle of the sonar signal is too small, the navigation positioning precision can be rapidly deteriorated. The inertial navigation SINS can obtain the attitude, speed and position information by integrating the data acquired by the adding meter and the gyroscope, and is used as an autonomous navigation system, and has the advantages of quick measurement update frequency, complete measurement information, no external interference and the like, but the inertial navigation SINS needs to provide initialization information externally before working, and the navigation error is increased along with time. Therefore, SINS is often combined with sonar to be applied to underwater navigation, and the precision and stability of combined navigation can be improved through an elastic combined algorithm.
The inventor finds that the SINS/LBL integrated navigation system has the characteristics of high precision and high stability, and has better reliability and concealment. However, the integrated navigation system of SINS/LBL requires a reliable sea reference, i.e. a large number of reference stations are required to be deployed at the sea bottom, and a large amount of manpower and financial resources are required for deployment and maintenance of the sea bottom reference stations.
The inventors have also found that while existing methods can provide some navigational positioning capability in an underwater environment, a variety of problems remain. The distance from the underwater vehicle to the seabed datum point can be measured by the sonar signals, navigation and positioning are conducted on the underwater vehicle, the underwater vehicle is limited by the small number of seabed datum stations, and when the underwater vehicle is far away from the seabed datum point, the number of received signals is reduced due to the fact that the incidence angle of the sonar signals is too low and the signal transmission distance is far, or the sonar signals cannot be received at all due to environmental shielding, and therefore single acoustic navigation and positioning results are poor or positioning services cannot be provided at all. Although inertial navigation has autonomous calculation capability, positioning results can be provided without external observables, because navigation errors are accumulated continuously, general-precision inertial navigation calculation results deviate from a true value rapidly, and the requirement of providing navigation positioning for an underwater vehicle for a long time cannot be met. The inertial navigation and sonar integrated navigation can improve the accuracy of positioning calculation, but is limited by the fact that the number of the arranged seabed datum points is small, long-time integrated navigation cannot be performed, and long-term stable navigation positioning service is provided for the underwater vehicle.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a method and a system for calibrating an inertial sensor based on sonar, which utilize observed sonar signals to calibrate inertial navigation errors on line in an underwater environment, inhibit inertial navigation position error divergence speed, calibrate inertial navigation by using received sonar signals when a submarine reference station exists, reduce inertial navigation self errors, enable an underwater vehicle to estimate and drive to the next submarine reference station network by relying on pure inertial navigation positions when no sonar signals exist, solve the problem that the submarine reference station can not provide sonar signals for a long time, avoid the problem that the result error of the estimate is overlarge due to long-time independent work of inertial navigation, greatly improve the position accuracy of inertial navigation and improve the capability of adapting to the fewer current situations of the submarine reference station.
To achieve the above object, one or more embodiments of the present invention provide the following technical solutions:
the invention provides a method for calibrating an inertial sensor based on sonar calibration.
A method for calibrating an inertial sensor based on sonar comprises the following steps:
step one: establishing an inertial sensor measurement error model;
step two: based on an inertial sensor measurement error model, constructing a state vector under a local navigation coordinate system, and establishing a navigation system state equation based on the state vector;
step three: acquiring sonar ranging information, and constructing a measurement equation by taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector;
step four: updating the acoustic observables based on the sonar signal propagation speed and propagation time observed by the current epoch; updating the inertial navigation calculation observables based on the current epoch inertial navigation measurement updating position and the seabed reference station position;
step five: updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; when the acoustic observed quantity is updated and new sonar ranging information is acquired, taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector, and carrying out measurement updating by using the measurement equation constructed in the third step;
step six: the fourth step to the fifth step are circulated until the inertial navigation error converges, and the inertial navigation error calibration is completed;
step seven: and establishing an error feedback model, substituting a state vector of the inertial navigation error convergence moment into the error feedback model, and obtaining theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation.
The second aspect of the invention provides a system for calibrating an inertial sensor based on sonar.
A system for calibrating inertial sensors based on sonar, comprising:
a measurement error model building module configured to: establishing an inertial sensor measurement error model;
a state equation setup module configured to: based on an inertial sensor measurement error model, constructing a state vector under a local navigation coordinate system, and establishing a navigation system state equation based on the state vector;
a measurement equation setup module configured to: acquiring sonar ranging information, and constructing a measurement equation by taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector;
an update module configured to: updating the acoustic observables based on the sonar signal propagation speed and propagation time observed by the current epoch; updating the inertial navigation calculation observables based on the current epoch inertial navigation measurement updating position and the seabed reference station position;
a kalman filter module configured to: updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; when the acoustic observed quantity is updated and new sonar ranging information is acquired, taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the seabed reference station as an observation vector, and measuring and updating by using a measurement equation constructed in a measurement equation establishment module;
a loop module configured to: the fourth step to the fifth step are circulated until the inertial navigation error converges, and the inertial navigation error calibration is completed;
a feedback module configured to: and establishing an error feedback model, substituting a state vector of the inertial navigation error convergence moment into the error feedback model, and obtaining theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation.
A third aspect of the invention provides a computer readable storage medium having stored thereon a program which when executed by a processor performs the steps in a method of sonar-based calibration of inertial sensors as described in the first aspect of the invention.
A fourth aspect of the invention provides an electronic device comprising a memory, a processor and a program stored on the memory and executable on the processor, the processor implementing the steps in the method for calibrating inertial sensors based on sonar according to the first aspect of the invention when the program is executed.
The one or more of the above technical solutions have the following beneficial effects:
in order to maintain the accuracy of underwater navigation, the invention provides a method and a system for calibrating an inertial sensor based on sonar, which utilize a sonar ranging signal to calibrate an inertial navigation system when a sonar signal of a submarine reference station can be received, so that the accumulated inertial navigation error is reduced, an underwater carrier can still maintain certain accuracy by means of pure inertial navigation when the sonar signal cannot be received, and navigate to the next group of submarine reference station networks, and the like, and can provide higher-accuracy navigation position service for the underwater carrier on the premise of less arrangement quantity of the submarine reference stations.
When a submarine reference station exists, the difference between the received sonar ranging information and the ranging information reversely deduced from the inertial navigation resolving result is used as an observation vector to construct a measuring equation, the inertial navigation error is used as a part of a state vector, the inertial navigation system error is calibrated on line by using a Kalman filtering technology, the inertial navigation system error is fed back to the inertial navigation system, the inertial navigation position error divergence speed is restrained, and experimental test results show that the method can effectively restrain the inertial navigation error divergence speed for the micromechanical level inertial navigation, so that the inertial navigation position error divergence one kilometer maintenance time is improved by 121%, the inertial navigation position accuracy is greatly improved, and the capability of adapting to fewer current situations of the submarine reference station is improved.
Additional aspects of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention.
Fig. 1 is a flow chart of a method of a first embodiment.
Fig. 2 is a system configuration diagram of a second embodiment.
Detailed Description
It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the invention. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present invention.
Embodiments of the invention and features of the embodiments may be combined with each other without conflict.
Term interpretation:
DVL: a Doppler log (Doppler Velocity Log);
SINS: strapdown inertial navigation system (Strap-down Inertial Navigation System)
And (3) GNSS: global satellite navigation system (Global Navigation Satellite System)
LBL: long baseline positioning system Long BaseLine positioning system
GPS global positioning system Global Positioning System
IMU: inertia measuring unit (Inertial Measurement Unit)
POS: position and orientation system (Position and Orientation System)
Example 1
The embodiment discloses a sonar-based inertial sensor calibration method.
As shown in fig. 1, a sonar-based inertial sensor calibration method includes the following steps:
step one: establishing an inertial sensor measurement error model;
step two: based on an inertial sensor measurement error model, constructing a state vector under a local navigation coordinate system, and establishing a navigation system state equation based on the state vector;
step three: acquiring sonar ranging information, and constructing a measurement equation by taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector;
step four: updating the acoustic observables based on the sonar signal propagation speed and propagation time observed by the current epoch; updating the inertial navigation calculation observables based on the current epoch inertial navigation measurement updating position and the seabed reference station position;
step five: updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; when the acoustic observed quantity is updated and new sonar ranging information is acquired, taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector, and measuring and updating by using a measurement equation constructed in the third step;
step six: the fourth step to the fifth step are circulated until the inertial navigation error converges, and the inertial navigation error calibration is completed;
step seven: and establishing an error feedback model, substituting a state vector of the inertial navigation error convergence moment into the error feedback model, and obtaining theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation.
The invention provides a sonar calibration inertial navigation algorithm based on Kalman filtering for solving the problems of fast error divergence and low accuracy of inertial navigation independent calculation results, which comprises the following detailed steps:
and (one) modeling an inertial navigation system error.
During operation of the underwater vehicle, the measurement error of the inertial sensor will deviate significantly from the off-line calibration value in its laboratory environment. In order to effectively calibrate and compensate the inertial sensor errors in the running environment of the underwater carrier, the following inertial sensor measurement error model is established.
Figure SMS_1
(1)
in the formula ,
Figure SMS_4
and />
Figure SMS_8
Measuring errors of the gyroscope and the accelerometer in a carrier coordinate system respectively; />
Figure SMS_11
and />
Figure SMS_5
Installation error matrix for gyroscope and accelerometer, respectively, wherein +.>
Figure SMS_7
For the installation error coefficients of three gyroscopes, < +.>
Figure SMS_9
The installation error coefficients of the three accelerometers represent the influence of the j axis on the i axis; />
Figure SMS_13
And
Figure SMS_2
scale factor error matrices of the gyroscope and the accelerometer respectively;
Figure SMS_6
and />
Figure SMS_10
Random constant error matrixes of the gyroscope and the accelerometer respectively; />
Figure SMS_12
and />
Figure SMS_3
Ideal outputs of the gyroscope and accelerometer in the carrier coordinate system, respectively.
And (II) constructing a system state equation.
The system state equation is established in relation to the type of inertial sensor error, the combination and the selection of the coordinate system. For multi-source sensors, it is also contemplated to fuse common portions of different state vectors. The 27-dimensional state vector constructed under the local navigation coordinate system (local navigation frame) is:
Figure SMS_14
(2)
wherein ,
Figure SMS_17
the vector represents an attitude angle error in the northeast-north direction under the local navigation coordinate system; />
Figure SMS_20
The vector represents a velocity error in the northeast-north-sky direction under the local navigation coordinate system; />
Figure SMS_22
Expressed as a three-dimensional position error in the geodetic coordinate system; />
Figure SMS_16
and />
Figure SMS_19
Respectively represent the lower edges of the carrier coordinate systemX、Y、ZRandom constant error matrix of gyroscope and acceleration in direction; />
Figure SMS_21
and />
Figure SMS_23
Scale factor error matrices of the gyroscope and the accelerometer in the direction of X, Y, Z of the lower edge of the carrier coordinate system are respectively represented;
Figure SMS_15
and />
Figure SMS_18
Respectively the lower edges of the carrier coordinate systemsX、Y、ZInstallation error matrix of gyroscopes and accelerometers in direction.
The state equation of the navigation system is
Figure SMS_24
(3)
in the formula ,
Figure SMS_25
is a state transition matrix; />
Figure SMS_26
In order to process the noise vector of the process,
Figure SMS_27
random white noise for gyroscopes and accelerometers, < >>
Figure SMS_28
Random walk process noise for gyroscopes and accelerometers.
And (III) constructing a system measurement equation.
The sonar LBL-SINS filter can correct the system state vector through a measurement equation.
The observation vector of LBL is the distance between the carrier and the seabed datum point, and when the measurement is updated, the observation vector is differed from the observation value obtained through SINS estimation to obtain the observation information.
The measurement equation of the LBL-SINS tightly integrated navigation system is as follows:
Figure SMS_29
(4)
in the formula ,
Figure SMS_31
is->
Figure SMS_34
Time SINS estimated +.>
Figure SMS_37
Distance between the individual subsea reference station and the inertial sensor,/->
Figure SMS_32
Is->
Figure SMS_35
Time observed->
Figure SMS_38
The distance between the individual subsea reference stations to the submersible vehicle transducers; />
Figure SMS_40
For measuring noise vector, ">
Figure SMS_30
Is->
Figure SMS_33
Time observed->
Figure SMS_36
Distance error between the individual subsea reference stations to the submersible vehicle transducers;
Figure SMS_39
for the measurement relation matrix, the expression is:
Figure SMS_41
(5)
in the formula
Figure SMS_42
，/>
Figure SMS_43
，/>
Figure SMS_44
，
Figure SMS_45
，/>
Figure SMS_46
Figure SMS_49
、/>
Figure SMS_54
and />
Figure SMS_61
Respectively indicate->
Figure SMS_50
The radius of curvature of the circle of the mortise where the moment coordinate point is located, the radius of curvature of the circle of the noon and the elevation; />
Figure SMS_59
Representation->
Figure SMS_63
Latitude values corresponding to the moment coordinate points; />
Figure SMS_68
Representation->
Figure SMS_53
Longitude values corresponding to the moment coordinate points; />
Figure SMS_57
Representation->
Figure SMS_65
Coordinate rotation matrix from the moment carrier coordinate system to the local navigation coordinate system; />
Figure SMS_69
A lever arm representing a position between the inertial sensor and the submersible transducer; />
Figure SMS_48
Representing the first eccentricity of the earth; />
Figure SMS_55
Is a carrier coordinate system, ">
Figure SMS_62
Is a local navigation coordinate system, +.>
Figure SMS_67
Is a geodetic coordinate system, ">
Figure SMS_52
The system is a geocentric and geodetic coordinate system; />
Figure SMS_58
To->
Figure SMS_64
The line is transformed into->
Figure SMS_70
A coordinate transformation matrix of the system; />
Figure SMS_47
To->
Figure SMS_60
Is transformed into->
Figure SMS_66
A coordinate transformation matrix of the system; />
Figure SMS_71
Is->
Figure SMS_51
The line is transformed into->
Figure SMS_56
And (3) a coordinate transformation matrix of the system.
And (IV) updating the acoustic observance quantity.
The acoustic observations can be obtained from the sonar signal propagation velocity and propagation time observed for the current epoch. The underwater sound velocity can be obtained through on-site actual measurement or the existing sound velocity field; the sonar signal propagation time can be obtained by recording the time from the transmission of the sonar signal by the underwater vehicle transducer to the reception of the underwater vehicle transducer by the seabed reference; and integrating the underwater sound velocity and the sonar signal propagation time to obtain the acoustic observance.
And fifthly, updating SINS estimation observables.
And (3) according to the current epoch SINS measurement updating position and the seabed reference station position, after lever arm correction, reversely pushing to obtain the distance from the seabed reference point to the underwater vehicle transducer.
And (sixth) updating inertial navigation errors by Kalman filtering.
Discretizing the state equation and the measurement equation given in the steps (2) and (3), wherein the filter state updating process is as follows:
Figure SMS_72
(6)
the filter measurement and update process comprises the following steps:
Figure SMS_73
(7)
in the formula ,
Figure SMS_75
、/>
Figure SMS_79
、/>
Figure SMS_81
separate tableShowing a previous time state estimation, a state one-step prediction and a state estimation at the time after the discretization of the system; />
Figure SMS_77
、/>
Figure SMS_80
、/>
Figure SMS_83
Respectively representing a state mean square error matrix at the previous moment after discretization of the system, a state one-step prediction mean square error matrix and the state mean square error matrix at the moment; />
Figure SMS_85
Representing a system noise matrix at a last moment after system discretization; />
Figure SMS_74
Representing a state one-step transition matrix after discretization of the system; />
Figure SMS_78
、/>
Figure SMS_82
、/>
Figure SMS_84
、/>
Figure SMS_76
The Kalman filtering gain matrix, the measurement update matrix, the observation noise matrix and the observation quantity at the moment after the discretization of the system are respectively represented.
In this step, the Kalman filtering update is divided into two parts, the first part is a state update, corresponding to equation (6); the second part is a measurement update, corresponding to equation (7). Since the update frequency of inertial navigation is 100HZ, i.e., 100 updates in 1 second, the update frequency of acoustic observables is 0.5HZ, i.e., 1 update in two seconds. Therefore, inertial data in the gap updated by the acoustic observables are independently calculated according to a formula (6) to realize state updating; and when the acoustic observed quantity in the step four is updated, step five is performed, the distance from the submarine reference to the submarine transducer is calculated by using the position calculated by inertial navigation and the submarine reference, the updating results of the step four and the step five are brought into a formula (7) to realize measurement updating, and the updating of the inertial navigation error is realized by using Kalman filtering in the same way, and the inertial navigation error calibration can be completed when the inertial navigation error is converged.
And (seventh) establishing an error feedback model and feeding back inertial navigation errors.
The error feedback model is:
Figure SMS_86
(8)
Figure SMS_87
and />
Figure SMS_88
The measurement output of the gyroscope and the accelerometer;
in step (six), the Kalman filtering technique is utilized to estimate the state vector of the next moment
Figure SMS_89
Sum-state vector mean square error matrix>
Figure SMS_90
State vector mean square error matrix +.>
Figure SMS_91
Gradually going to zero, which also means that the estimated state vector +.>
Figure SMS_92
Tending to the true value, and introducing the value into the formula (8) to obtain theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation>
Figure SMS_93
and />
Figure SMS_94
。
Example two
The embodiment discloses a system based on sonar calibration inertial sensor.
As shown in fig. 2, a system for calibrating inertial sensors based on sonar, comprising:
a measurement error model building module configured to: establishing an inertial sensor measurement error model;
a state equation setup module configured to: based on an inertial sensor measurement error model, constructing a state vector under a local navigation coordinate system, and establishing a navigation system state equation based on the state vector;
a measurement equation setup module configured to: acquiring sonar ranging information, and constructing a measurement equation by taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector;
an update module configured to: updating the acoustic observables based on the sonar signal propagation speed and propagation time observed by the current epoch; updating the inertial navigation calculation observables based on the current epoch inertial navigation measurement updating position and the seabed reference station position;
a kalman filter module configured to: updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; when the acoustic observed quantity is updated and new sonar ranging information is acquired, taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the seabed reference station as an observation vector, and carrying out measurement updating by using a measurement equation constructed by a measurement equation construction module;
a loop module configured to: the fourth step to the fifth step are circulated until the inertial navigation error converges, and the inertial navigation error calibration is completed;
a feedback module configured to: and establishing an error feedback model, substituting a state vector of the inertial navigation error convergence moment into the error feedback model, and obtaining theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation.
Example III
An object of the present embodiment is to provide a computer-readable storage medium.
A computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps in a sonar-based calibration inertial sensor method as described in embodiment 1 of the present disclosure.
Example IV
An object of the present embodiment is to provide an electronic apparatus.
An electronic device comprising a memory, a processor and a program stored on the memory and executable on the processor, the processor implementing the steps in a method for calibrating inertial sensors based on sonar calibration as described in embodiment 1 of the present disclosure when the program is executed.
The steps involved in the devices of the second, third and fourth embodiments correspond to those of the first embodiment of the method, and the detailed description of the embodiments can be found in the related description section of the first embodiment. The term "computer-readable storage medium" should be taken to include a single medium or multiple media including one or more sets of instructions; it should also be understood to include any medium capable of storing, encoding or carrying a set of instructions for execution by a processor and that cause the processor to perform any one of the methods of the present invention.
It will be appreciated by those skilled in the art that the modules or steps of the invention described above may be implemented by general-purpose computer means, alternatively they may be implemented by program code executable by computing means, whereby they may be stored in storage means for execution by computing means, or they may be made into individual integrated circuit modules separately, or a plurality of modules or steps in them may be made into a single integrated circuit module. The present invention is not limited to any specific combination of hardware and software.
While the foregoing description of the embodiments of the present invention has been presented in conjunction with the drawings, it should be understood that it is not intended to limit the scope of the invention, but rather, it is intended to cover all modifications or variations within the scope of the invention as defined by the claims of the present invention.

Claims (10)

1. The method for calibrating the inertial sensor based on the sonar is characterized by comprising the following steps of:
step one: establishing an inertial sensor measurement error model;
step two: based on an inertial sensor measurement error model, constructing a state vector under a local navigation coordinate system, and establishing a navigation system state equation based on the state vector;
step three: acquiring sonar ranging information, and constructing a measurement equation by taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector;
step four: updating the acoustic observables based on the sonar signal propagation speed and propagation time observed by the current epoch; updating the inertial navigation calculation observables based on the current epoch inertial navigation measurement updating position and the seabed reference station position;
step five: updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; when the acoustic observed quantity is updated and new sonar ranging information is acquired, taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector, and measuring and updating by using a measurement equation constructed in the third step;
step six: the fourth step to the fifth step are circulated until the inertial navigation error converges, and the inertial navigation error calibration is completed;
step seven: and establishing an error feedback model, substituting a state vector of the inertial navigation error convergence moment into the error feedback model, and obtaining theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation.
2. A sonar-based calibration inertial sensor method as defined in claim 1, wherein said inertial sensor measurement error model is:
Figure QLYQS_1
wherein ,
Figure QLYQS_4
and />
Figure QLYQS_5
Measuring errors of the gyroscope and the accelerometer in a carrier coordinate system respectively; />
Figure QLYQS_8
and />
Figure QLYQS_3
Installation error matrix for gyroscope and accelerometer respectively,>
Figure QLYQS_7
and />
Figure QLYQS_10
Scale factor error matrices of the gyroscope and the accelerometer respectively; />
Figure QLYQS_11
and />
Figure QLYQS_2
Random constant error matrixes of the gyroscope and the accelerometer respectively; />
Figure QLYQS_6
and />
Figure QLYQS_9
Ideal outputs of the gyroscope and accelerometer in the carrier coordinate system, respectively.
3. The method for calibrating inertial sensors based on sonar of claim 1, wherein the state equation of the navigation system is:
Figure QLYQS_12
Figure QLYQS_13
wherein ,
Figure QLYQS_15
the vector represents an attitude angle error in the northeast-north direction under the local navigation coordinate system; />
Figure QLYQS_18
The vector represents a velocity error in the northeast-north-sky direction under the local navigation coordinate system; />
Figure QLYQS_21
Expressed as a three-dimensional position error in the geodetic coordinate system; />
Figure QLYQS_16
And
Figure QLYQS_17
respectively represent the lower edges of the carrier coordinate systemX、Y、ZRandom constant error matrix of gyroscope and acceleration in direction; />
Figure QLYQS_20
And
Figure QLYQS_22
scale factor error matrices of the gyroscope and the accelerometer in the direction of X, Y, Z of the lower edge of the carrier coordinate system are respectively represented; />
Figure QLYQS_14
and />
Figure QLYQS_19
Respectively the lower edges of the carrier coordinate systemsX、Y、ZInstallation error matrix of gyroscopes and accelerometers in direction.
4. A method for calibrating an inertial sensor based on sonar as defined in claim 1, wherein said measurement equation is:
Figure QLYQS_23
wherein ,
Figure QLYQS_25
is->
Figure QLYQS_29
Time of day inertial navigation estimation +.>
Figure QLYQS_32
Distance between the individual subsea reference station and the inertial sensor,/->
Figure QLYQS_26
Is->
Figure QLYQS_28
Time observed->
Figure QLYQS_31
The distance between the individual subsea reference stations to the submersible vehicle transducers; />
Figure QLYQS_34
For measuring noise vector, ">
Figure QLYQS_24
Is->
Figure QLYQS_27
Time observed->
Figure QLYQS_30
Distance error between the individual subsea reference stations to the submersible vehicle transducers; />
Figure QLYQS_33
Is a measurement relation matrix.
5. The method for calibrating an inertial sensor based on sonar according to claim 1, wherein the inertial sensor is updated by kalman filtering, specifically:
the filter state update process is:
Figure QLYQS_35
the filter measurement and update process comprises the following steps:
Figure QLYQS_36
wherein ,
Figure QLYQS_38
、/>
Figure QLYQS_43
、/>
Figure QLYQS_46
respectively representing the state estimation at the previous moment, the state one-step prediction and the state estimation at the moment after the discretization of the system; />
Figure QLYQS_39
、/>
Figure QLYQS_41
、/>
Figure QLYQS_44
Respectively represent the mean square error matrix and the state of the state at the previous moment after the discretization of the systemA mean square error matrix is predicted in one step and a mean square error matrix in the state at the moment; />
Figure QLYQS_47
Representing a system noise matrix at a last moment after system discretization; />
Figure QLYQS_37
Representing a state one-step transition matrix after discretization of the system; />
Figure QLYQS_42
、/>
Figure QLYQS_45
、/>
Figure QLYQS_48
、/>
Figure QLYQS_40
The Kalman filtering gain matrix, the measurement update matrix, the observation noise matrix and the observation quantity at the moment after the discretization of the system are respectively represented.
6. A method for calibrating an inertial sensor based on sonar as defined in claim 1, wherein the error feedback model is:
Figure QLYQS_49
wherein ,
Figure QLYQS_50
and />
Figure QLYQS_51
The measurement outputs of the gyroscope and the accelerometer are respectively; />
Figure QLYQS_52
and />
Figure QLYQS_53
Theoretical outputs of the gyroscope and the accelerometer after inertial navigation error compensation are respectively obtained.
7. A method for calibrating inertial sensors based on sonar as defined in claim 1, wherein after obtaining theoretical outputs of gyroscope and accelerometer compensated by inertial navigation error, the method relies on pure inertial navigation position to estimate the next sea reference station net without sonar signals.
8. A system based on sonar calibration inertial sensor is characterized in that: comprising the following steps:
a measurement error model building module configured to: establishing an inertial sensor measurement error model;
a state equation setup module configured to: based on an inertial sensor measurement error model, constructing a state vector under a local navigation coordinate system, and establishing a navigation system state equation based on the state vector;
a measurement equation setup module configured to: acquiring sonar ranging information, and constructing a measurement equation by taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the submarine reference station as an observation vector;
an update module configured to: updating the acoustic observables based on the sonar signal propagation speed and propagation time observed by the current epoch; updating the inertial navigation calculation observables based on the current epoch inertial navigation measurement updating position and the seabed reference station position;
a kalman filter module configured to: updating inertial navigation errors by adopting Kalman filtering, and independently calculating the position by inertial navigation when no acoustic observance exists; when the acoustic observed quantity is updated and new sonar ranging information is acquired, taking the difference between the sonar ranging information and the ranging information reversely deduced by using the independently deduced position of inertial navigation and the position of the seabed reference station as an observation vector, and measuring and updating by using a measuring equation constructed by a measuring equation building module;
a loop module configured to: the fourth step to the fifth step are circulated until the inertial navigation error converges, and the inertial navigation error calibration is completed;
a feedback module configured to: and establishing an error feedback model, substituting a state vector of the inertial navigation error convergence moment into the error feedback model, and obtaining theoretical output of the gyroscope and the accelerometer after inertial navigation error compensation.
9. Computer readable storage medium, having stored thereon a program, which when executed by a processor, implements the steps of a method for calibrating inertial sensors based on sonar as defined in any of claims 1-7.
10. Electronic device comprising a memory, a processor and a program stored on the memory and executable on the processor, characterized in that the processor implements the steps of the sonar-based inertial sensor method of any of claims 1-7 when said program is executed.
CN202310628706.1A 2023-05-31 2023-05-31 Sonar-based method and system for calibrating inertial sensor Active CN116337115B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310628706.1A CN116337115B (en) 2023-05-31 2023-05-31 Sonar-based method and system for calibrating inertial sensor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310628706.1A CN116337115B (en) 2023-05-31 2023-05-31 Sonar-based method and system for calibrating inertial sensor

Publications (2)

Publication Number Publication Date
CN116337115A true CN116337115A (en) 2023-06-27
CN116337115B CN116337115B (en) 2023-08-29

Family

ID=86880878

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310628706.1A Active CN116337115B (en) 2023-05-31 2023-05-31 Sonar-based method and system for calibrating inertial sensor

Country Status (1)

Country Link
CN (1) CN116337115B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007210402A (en) * 2006-02-08 2007-08-23 Kawasaki Heavy Ind Ltd Autonomous unmanned submersible and its underwater navigation method
US20100246322A1 (en) * 2009-03-27 2010-09-30 Welker Kenneth E Determining a position of a survey receiver in a body of water
CN109324330A (en) * 2018-09-18 2019-02-12 东南大学 Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter
CN109828296A (en) * 2019-03-08 2019-05-31 哈尔滨工程大学 A kind of non-linear tight integration synthesis correction method of INS/USBL
CN110207694A (en) * 2019-05-27 2019-09-06 哈尔滨工程大学 A kind of polar region grid inertial navigation/ultra-short baseline Combinated navigation method based on relative position information
CN110207698A (en) * 2019-05-27 2019-09-06 哈尔滨工程大学 A kind of polar region grid inertial navigation/ultra-short baseline tight integration air navigation aid
US20200311514A1 (en) * 2019-04-01 2020-10-01 Honeywell International Inc. Deep neural network-based inertial measurement unit (imu) sensor compensation method
CN112083425A (en) * 2020-09-14 2020-12-15 湖南航天机电设备与特种材料研究所 SINS/LBL tight combination navigation method introducing radial velocity
CN113670302A (en) * 2021-09-01 2021-11-19 东南大学 Inertia/ultra-short baseline combined navigation method under influence of motion effect
CN115307643A (en) * 2022-08-24 2022-11-08 东南大学 Double-responder assisted SINS/USBL combined navigation method

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007210402A (en) * 2006-02-08 2007-08-23 Kawasaki Heavy Ind Ltd Autonomous unmanned submersible and its underwater navigation method
US20100246322A1 (en) * 2009-03-27 2010-09-30 Welker Kenneth E Determining a position of a survey receiver in a body of water
CN109324330A (en) * 2018-09-18 2019-02-12 东南大学 Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter
CN109828296A (en) * 2019-03-08 2019-05-31 哈尔滨工程大学 A kind of non-linear tight integration synthesis correction method of INS/USBL
US20200311514A1 (en) * 2019-04-01 2020-10-01 Honeywell International Inc. Deep neural network-based inertial measurement unit (imu) sensor compensation method
CN110207694A (en) * 2019-05-27 2019-09-06 哈尔滨工程大学 A kind of polar region grid inertial navigation/ultra-short baseline Combinated navigation method based on relative position information
CN110207698A (en) * 2019-05-27 2019-09-06 哈尔滨工程大学 A kind of polar region grid inertial navigation/ultra-short baseline tight integration air navigation aid
CN112083425A (en) * 2020-09-14 2020-12-15 湖南航天机电设备与特种材料研究所 SINS/LBL tight combination navigation method introducing radial velocity
CN113670302A (en) * 2021-09-01 2021-11-19 东南大学 Inertia/ultra-short baseline combined navigation method under influence of motion effect
CN115307643A (en) * 2022-08-24 2022-11-08 东南大学 Double-responder assisted SINS/USBL combined navigation method

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
XINGMEI WANG; GUOQIANG WANG; YANXIA WU: "An Adaptive Particle Swarm Optimization for Underwater Target Tracking in Forward Looking Sonar Image Sequences", IEEE ACCESS *
王彬;翁海娜;梁瑾;王彦国;王桂如;: "一种惯性/水声单应答器距离组合导航方法", 中国惯性技术学报, vol. 25, no. 01 *
王银涛，贾晓宝，崔荣鑫，严卫生: "移动USBL测距辅助的UUV协同导航定位方法", 控制理论与应用, vol. 39, no. 11 *
陈建华;朱海;郭正东;栾禄雨;: "基于水声传播时延补偿的惯导误差修正方法", 舰船科学技术, vol. 38, no. 3 *

Also Published As

Publication number Publication date
CN116337115B (en) 2023-08-29

Similar Documents

Publication Publication Date Title
CN109324330B (en) USBL/SINS tight combination navigation positioning method based on mixed derivative-free extended Kalman filtering
CN110095800B (en) Multi-source fusion self-adaptive fault-tolerant federal filtering integrated navigation method
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
US8860609B2 (en) Loosely-coupled integration of global navigation satellite system and inertial navigation system
Morgado et al. Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles: An experimental validation
Hasan et al. A review of navigation systems (integration and algorithms)
CN108594272B (en) Robust Kalman filtering-based anti-deception jamming integrated navigation method
CN109443379A (en) A kind of underwater anti-shake dynamic alignment methods of the SINS/DVL of deep-sea submariner device
CN111323050B (en) Strapdown inertial navigation and Doppler combined system calibration method
CN103697910B (en) The correction method of autonomous underwater aircraft Doppler log installation error
De Agostino et al. Performances comparison of different MEMS-based IMUs
CN109471146B (en) Self-adaptive fault-tolerant GPS/INS integrated navigation method based on LS-SVM
Mandt et al. Integrating DGPS-USBL position measurements with inertial navigation in the HUGIN 3000 AUV
CN112146655B (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN102829777A (en) Integrated navigation system for autonomous underwater robot and method
CN102252677A (en) Time series analysis-based variable proportion self-adaptive federal filtering method
CN113834483B (en) Inertial/polarization/geomagnetic fault-tolerant navigation method based on observability degree
CN111965685B (en) Low-orbit satellite/inertia combined navigation positioning method based on Doppler information
CN104344836A (en) Posture observation-based redundant inertial navigation system fiber-optic gyroscope system level calibration method
CN113252041B (en) Combined navigation method suitable for small underwater robot
Troni et al. Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles
CN115096302A (en) Strapdown inertial base navigation system information filtering robust alignment method, system and terminal
CN114136310B (en) Autonomous error suppression system and method for inertial navigation system
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model
US8566055B1 (en) Gyro indexing compensation method and system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant