CN112987054B - Method and device for calibrating SINS/DVL combined navigation system error - Google Patents

Method and device for calibrating SINS/DVL combined navigation system error Download PDF

Info

Publication number
CN112987054B
CN112987054B CN202110204090.6A CN202110204090A CN112987054B CN 112987054 B CN112987054 B CN 112987054B CN 202110204090 A CN202110204090 A CN 202110204090A CN 112987054 B CN112987054 B CN 112987054B
Authority
CN
China
Prior art keywords
dvl
sins
optimization function
nonlinear optimization
error
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.)
Active
Application number
CN202110204090.6A
Other languages
Chinese (zh)
Other versions
CN112987054A (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.)
Boya Gongdao Beijing Robot Technology Co Ltd
Original Assignee
Boya Gongdao Beijing Robot Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Boya Gongdao Beijing Robot Technology Co Ltd filed Critical Boya Gongdao Beijing Robot Technology Co Ltd
Priority to CN202110204090.6A priority Critical patent/CN112987054B/en
Publication of CN112987054A publication Critical patent/CN112987054A/en
Application granted granted Critical
Publication of CN112987054B publication Critical patent/CN112987054B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • 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
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

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)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention relates to a method and a device for calibrating an SINS/DVL combined navigation system error, wherein the method comprises the following steps: modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scale factor error in DVL velocity, and an azimuth mount error angle between the SINS and DVL; and solving the nonlinear optimization function to finish the calibration of two errors. The method for calibrating the SINS/DVL combined navigation system error accurately calibrates the two errors by modeling the nonlinear optimization function according to the two parameters and solving the nonlinear optimization function by using the Gauss-Newton algorithm, and tests that the calibration precision of the method can reach the optimal solution by comparing with an enumeration method. The method for calibrating the SINS/DVL integrated navigation system error can improve the SINS/DVL integrated navigation precision and has very high practical value.

Description

Method and device for calibrating SINS/DVL combined navigation system error
Technical Field
The invention relates to the technical field of inertial navigation, in particular to a method and a device for calibrating an SINS/DVL integrated navigation system error.
Background
In an underwater environment, GNSS signals are not available, and pure inertial navigation systems tend to have faster error divergence, so SINS/DVL combined navigation is a common way of underwater navigation. The influence of an installation error angle between the SINS and the DVL and the influence of a DVL scale coefficient error on the underwater SINS/DVL combined navigation precision is very large. If these two errors can be accurately calibrated, the precision of the SINS/DVL integrated navigation will be greatly improved.
The calibration for the SINS/DVL combined navigation system error mainly comprises 3 schemes, wherein the 3 schemes are based on the speed information or the position information of a GNSS (global navigation satellite system) and are used as true values, the scheme A uses a repeated trial and error method, the error value is continuously modified by observing the coincidence degree of an SINS/DVL navigation track and a GNSS track, and finally a calibration parameter with higher coincidence degree with the GNSS track is obtained. The scheme B uses a least square method, but the calibration precision is reduced because a motion model of the robot is not considered, and the scheme only calibrates the installation error between the SINS and the DVL, does not calibrate the DVL scale factor, and further reduces the navigation precision. The scheme C calibrates the installation error and the calibration factor at the same time, but a gradient descent method is used, and the method possibly falls into local minimum, so that the problem that an optimal solution cannot be found is caused.
Disclosure of Invention
In view of the above, the present invention provides a method and apparatus for calibrating an error of a SINS/DVL integrated navigation system to overcome the disadvantages of the prior art.
In order to achieve the purpose, the invention adopts the following technical scheme: a method of calibrating SINS/DVL combined navigation system errors, comprising:
modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scaling factor δ k for the DVL velocity, and an azimuthal misalignment angle β between the SINS and DVL;
and solving the nonlinear optimization function to finish the calibration of the scaling factor delta k of the velocity of the DVL and the azimuth installation error angle beta between the SINS and the DVL.
Optionally, the modeling a nonlinear optimization function includes:
the DVL self-carrier coordinate system is an m system, the inertial navigation coordinate system is a b system, the navigation coordinate system is an n system, and the DVL speed obtained in the DVL bottom-probing mode is
Figure BDA0002949744830000021
DVL data update time of
Figure BDA0002949744830000022
The dead reckoning formula is as follows:
Figure BDA0002949744830000023
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002949744830000024
θ q ＝θ y +β；
θ y representing the azimuthal misalignment angle, θ, of the SINS p Representing the pitch misalignment angle, θ r Representing the roll misalignment angle, θ q Indicating the azimuth deviation angle during the conversion of m to the navigation system,
Figure BDA0002949744830000025
representing the mounting error transition matrix between SINS and DVL,
Figure BDA0002949744830000026
representing a transformation matrix between inertial navigation and navigation systems;
Figure BDA0002949744830000027
Figure BDA0002949744830000028
Figure BDA0002949744830000029
acquiring GPS data corresponding to each DVL data;
the longitude and latitude in the GPS data are converted to a position under the northeast coordinate system, which is expressed as:
Figure BDA0002949744830000031
the position increment calculation formula of the GPS is as follows:
Figure BDA0002949744830000032
wherein the content of the first and second substances,
Figure BDA0002949744830000033
the first GPS data, the dead reckoning position data and the GPS data are incremental data, and the first DVL data and the GPS data are corresponding after linear interpolation;
the final optimization function is:
Figure BDA0002949744830000034
f (β, δ k) is a non-linear function with respect to β and δ k.
Optionally, the solving the nonlinear optimization function includes: and solving the nonlinear optimization function by adopting a Gauss-Newton method.
Optionally, the solving the nonlinear optimization function by using a gauss-newton method includes:
performing first-order Taylor expansion on the nonlinear optimization function;
determining an incremental equation according to a result after the first-order Taylor expansion;
and solving the incremental equation.
Optionally, the performing a first-order taylor expansion on the nonlinear optimization function includes:
let x = [ beta, delta k ], perform a first order Taylor expansion on the nonlinear optimization function f (x),
f(x+Δx)＝f(x)+J(x)Δx，
where J (x) is the derivative of f (x) with respect to x, and J (x) is referred to as the Jacobian matrix.
Optionally, determining an incremental equation according to a result after the first-order taylor expansion includes:
will find the descending vector Δ x such that | | f (x + Δ x) | luminance 2 The problem of reaching the minimum is converted to solving a linear least squares problem:
Figure BDA0002949744830000041
through the above conversion, the parameter to be optimized is changed from x to Δ x, and the square term of the objective function is expanded:
Figure BDA0002949744830000042
the derivative of the expanded equation with respect to Δ x is found and made 0, resulting in:
Figure BDA0002949744830000043
the incremental equation is obtained as:
Figure BDA0002949744830000044
defining the coefficients to the left of the incremental equation as H and the right as g, then the above equation becomes:
HΔx＝g。
optionally, the solving the nonlinear optimization function by using a gauss-newton method includes:
step 1: given an initial value x 0 ；
Step 2: for the kth iteration, the current Jacobian matrix J (x) is found k ) And error f (x) k )；
And step 3: solving the incremental equation H Δ x = g to obtain x k ；
And 4, step 4: if x k If the number of iterations is less than the preset value or reaches the set maximum value, x k Stopping iteration as an optimal solution; otherwise, let x k+1 ＝x k +Δx k And returning to the step 2 to continue the iterative operation.
The invention also provides a device for calibrating the SINS/DVL combined navigation system error, which comprises:
the modeling module is used for modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scaling factor δ k for the DVL velocity, and an azimuthal misalignment angle β between the SINS and DVL;
and the solving module is used for solving the nonlinear optimization function so as to complete the calibration of the scaling factor delta k of the velocity of the DVL and the azimuth installation error angle beta between the SINS and the DVL.
The present invention also provides a SINS/DVL integrated navigation system, comprising:
an apparatus for calibrating an error of a SINS/DVL combined navigation system as described above.
In addition, the invention also provides a controller for executing the method for calibrating the SINS/DVL combined navigation system error.
By adopting the technical scheme, the method for calibrating the error of the SINS/DVL integrated navigation system comprises the following steps: modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: scale factor error in DVL velocity, and azimuth mount error angle between SINS and DVL; and solving the nonlinear optimization function to finish the calibration of the scale factor error of the DVL speed and the azimuth installation error angle between the SINS and the DVL. The method for calibrating the SINS/DVL combined navigation system error accurately calibrates the two errors by modeling the nonlinear optimization function according to the two parameters and solving the nonlinear optimization function by using the Gauss-Newton algorithm, and tests that the calibration precision of the method can reach the optimal solution by comparing with an enumeration method. The method for calibrating the SINS/DVL integrated navigation system error can improve the precision of SINS/DVL integrated navigation and has very high practical value.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic flow chart of a method for calibrating an error of an SINS/DVL integrated navigation system according to the present invention;
FIG. 2 is a schematic view of a dead reckoning result without calibration error;
FIG. 3 is a schematic view of the dead reckoning results after error calibration by the method of the present invention;
FIG. 4 is a schematic of a trajectory without error calibration;
FIG. 5 is a schematic diagram of a trajectory calibrated for error;
fig. 6 is a schematic structural diagram provided by an apparatus for calibrating an error of an SINS/DVL integrated navigation system according to the present invention.
In the figure: 1. a modeling module; 2. and a solving module.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be described in detail below. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the invention. All other embodiments, which can be derived by a person skilled in the art from the examples given herein without making any creative effort, shall fall within the protection scope of the present invention.
FIG. 1 is a flow chart illustrating a method for calibrating an error of an SINS/DVL integrated navigation system according to the present invention.
As shown in fig. 1, a method for calibrating an error of a SINS/DVL integrated navigation system according to the present invention includes:
s11: modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scale factor error δ k of the DVL velocity, and an azimuthal mounting error angle β between the SINS and DVL;
s12: and solving the nonlinear optimization function to finish the calibration of the scale factor error delta k of the DVL speed and the azimuth installation error angle beta between the SINS and the DVL.
The SINS/DR combined navigation is a common combined navigation method, where DR is called dead reckoning, and is defined as calculating a relative position of the robot with respect to a starting point by using attitude, heading, and mileage information.
When the method is actually executed, the modeling nonlinear optimization function comprises the following steps:
the DVL self-carrier coordinate system is an m system, the inertial navigation coordinate system is a b system, the navigation coordinate system is an n system (northeast coordinate system), and the DVL speed obtained in the DVL bottom-probing mode is
Figure BDA0002949744830000071
In the invention, the pitch installation error angle and the roll installation error angle between the SINS and the DVL are not considered, and only the azimuth installation error angle between the SINS and the DVL is considered.
DVL data update time of
Figure BDA0002949744830000072
The dead reckoning formula is as follows:
Figure BDA0002949744830000073
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002949744830000074
θ q ＝θ y +β；
θ y representing the azimuthal misalignment angle, θ, of the SINS p Representing the pitch misalignment angle, θ r Representing the roll misalignment angle, θ q Indicating the azimuth deviation angle during the conversion of m to the navigation system,
Figure BDA0002949744830000075
representing the mounting error transition matrix between SINS and DVL,
Figure BDA0002949744830000076
representing a transformation matrix between inertial navigation and navigation systems;
Figure BDA0002949744830000077
Figure BDA0002949744830000078
Figure BDA0002949744830000079
acquiring GPS data corresponding to each DVL data;
specifically, the GPS data is subjected to time synchronization, the time synchronization comprises hardware time synchronization and software time synchronization, and in consideration of the fact that the speed of the autonomous underwater vehicle is low, the software time synchronization adopts a linear interpolation method to obtain the GPS data corresponding to each DVL data, and the GPS data is guaranteed to exist when the first DVL data is obtained.
Figure BDA0002949744830000086
And in the time T, converting the longitude and latitude in the GPS data into a position under a northeast coordinate system, and expressing as follows:
Figure BDA0002949744830000081
the position increment calculation formula of the GPS is as follows:
Figure BDA0002949744830000082
wherein the content of the first and second substances,
Figure BDA0002949744830000083
the first GPS data, the dead reckoning position data and the GPS data are incremental data, and the first DVL data and the GPS data are corresponding after linear interpolation;
the final optimization function is:
Figure BDA0002949744830000084
f (β, δ k) is a non-linear function with respect to β and δ k.
Solving the nonlinear optimization function f (beta, delta k) by adopting a Gauss-Newton method, wherein the process is as follows:
let x = [ β, δ k ], perform a first order taylor expansion on the nonlinear optimization function f (x),
f(x+Δx)＝f(x)+J(x)Δx，
where J (x) is the derivative of f (x) with respect to x, and J (x) is referred to as the Jacobian matrix.
Will find the descending vector Δ x such that | | f (x + Δ x) | luminance 2 The minimum problem is reached, and the conversion is to solve a linear least squares problem:
Figure BDA0002949744830000085
through the conversion, the parameter to be optimized is changed from x to deltax, and the square term of the objective function is expanded:
Figure BDA0002949744830000091
the derivative of the expanded equation with respect to Δ x is found and made 0, resulting in:
Figure BDA0002949744830000092
the incremental equation is obtained as:
Figure BDA0002949744830000093
defining the coefficients to the left of the incremental equation as H and the right as g, then the above equation becomes:
HΔx＝g。
gauss Newton method J (x) T J (x) is used as the approximation of a second-order Hessian matrix, and the solving of an incremental equation is the core of the optimization problem. The steps of the gauss-newton algorithm are as follows:
step 1: given an initial value x 0 ；
Step 2: for the k-th iteration, the current Jacobian matrix J (x) is solved k ) And error f (x) k )；
And step 3: solving the incremental equation H Δ x = g to obtain x k ；
And 4, step 4: if x k If the number of iterations is less than the preset value or reaches the set maximum value, x k Stopping iteration as an optimal solution; otherwise, let x k+1 ＝x k +Δx k And returning to the step 2 to continue the iterative operation. Wherein the preset value is set to be small enough according to actual requirements.
Fig. 2 is a schematic diagram of the result of dead reckoning without calibration error, and the calculated positioning accuracy is 2.31%, and the angle deviation between the two tracks is obvious from fig. 2.
FIG. 3 is a schematic diagram of the dead reckoning result after the error is calibrated by the method of the present invention, and it is obvious from FIG. 3 that the two tracks are almost coincident, and the calibrated error is as follows: the azimuth misalignment angle β is 1.44 ° and the DVL scale factor error δ k is 0.995, from which it can also be seen that the azimuth misalignment angle accounts for a greater proportion of the two errors.
Then, according to the calibrated two error parameters, a track is tested, wherein fig. 4 is a schematic diagram of the track without error calibration, and fig. 5 is a schematic diagram of the track with error calibration, which is shown in fig. 4 and 5, wherein the positioning accuracy of the track with error calibration is 0.27%.
By comparing the accuracy of the combined navigation before and after calibration, the error calibration method can improve the navigation accuracy by multiple times, and has very high practical value for improving the SINS/DVL combined navigation accuracy in practice.
DVL scale factor error and azimuth angle installation deviation between SINS and DVL are two very important factors influencing SINS/DVL combined navigation precision. The method for calibrating the SINS/DVL integrated navigation system error can improve the SINS/DVL integrated navigation precision and has very high practical value.
FIG. 6 is a schematic structural diagram of an apparatus for calibrating an error of the SINS/DVL integrated navigation system according to the present invention.
As shown in fig. 6, an apparatus for calibrating SINS/DVL combined navigation system error according to the present invention includes:
the modeling module 1 is used for modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scaling factor for the DVL velocity, and, an azimuthal misalignment angle between the SINS and DVL;
and the solving module 2 is used for solving the nonlinear optimization function so as to complete the calibration of the scaling factor of the DVL speed and the azimuth installation error angle between the SINS and the DVL.
The working principle of the apparatus for calibrating the error of the SINS/DVL integrated navigation system according to the present invention is the same as the working principle of the method for calibrating the error of the SINS/DVL integrated navigation system described above, and is not described herein again.
The present invention also provides a SINS/DVL integrated navigation system, comprising:
an apparatus for calibrating SINS/DVL combined navigation system error as described in fig. 6.
In addition, the present invention also provides a controller for executing the method for calibrating the SINS/DVL combined navigation system error described in FIG. 1.
It is understood that the same or similar parts in the above embodiments may be mutually referred to, and the same or similar parts in other embodiments may be referred to for the content which is not described in detail in some embodiments.
It should be noted that the terms "first," "second," and the like in the description of the present invention are used for descriptive purposes only and are not to be construed as indicating or implying relative importance. Further, in the description of the present invention, the meaning of "a plurality" means at least two unless otherwise specified.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more executable instructions for implementing specific logical functions or steps of the process, and alternate implementations are included within the scope of the preferred embodiment of the present invention in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of the present invention.
It should be understood that portions of the present invention may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, various steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or combination of the following techniques, which are known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a Programmable Gate Array (PGA), a Field Programmable Gate Array (FPGA), or the like.
It will be understood by those skilled in the art that all or part of the steps carried by the method for implementing the above embodiments may be implemented by hardware related to instructions of a program, which may be stored in a computer readable storage medium, and when the program is executed, the program includes one or a combination of the steps of the method embodiments.
In addition, functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may exist alone physically, or two or more units are integrated into one module. The integrated module can be realized in a hardware mode, and can also be realized in a software functional module mode. The integrated module, if implemented in the form of a software functional module and sold or used as a stand-alone product, may also be stored in a computer readable storage medium.
The storage medium mentioned above may be a read-only memory, a magnetic or optical disk, etc.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above do not necessarily refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and that variations, modifications, substitutions and alterations can be made to the above embodiments by those of ordinary skill in the art within the scope of the present invention.

Claims (9)

1. A method for calibrating SINS/DVL combined navigation system error, comprising:
modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scale factor error δ k of the DVL velocity, and an azimuthal mounting error angle β between the SINS and DVL;
solving the nonlinear optimization function to complete the calibration of the scale factor error delta k of the DVL speed and the azimuth installation error angle beta between the SINS and the DVL;
wherein the modeling nonlinear optimization function comprises:
the DVL self-carrier coordinate system is an m system, the inertial navigation coordinate system is a b system, the navigation coordinate system is an n system, and the DVL speed obtained in the DVL bottom-probing mode is
Figure FDA0003851466110000011
DVL data update time of
Figure FDA0003851466110000012
The dead reckoning formula is as follows:
Figure FDA0003851466110000013
wherein the content of the first and second substances,
Figure FDA0003851466110000014
θ q ＝θ y +β；
θ y representing the azimuthal misalignment angle, θ, of the SINS p Representing the pitch misalignment angle, θ r Representing the roll misalignment angle, θ q Indicating the azimuth deviation angle during the conversion of m to the navigation system,
Figure FDA0003851466110000015
representing the mounting error transition matrix between SINS and DVL,
Figure FDA0003851466110000016
representing a transformation matrix between the inertial navigation system and the navigation system;
Figure FDA0003851466110000017
Figure FDA0003851466110000018
Figure FDA0003851466110000019
acquiring GPS data corresponding to each DVL data;
converting the longitude and latitude in the GPS data into a position under a northeast coordinate system, and expressing as follows:
Figure FDA0003851466110000021
the position increment calculation formula of the GPS is as follows:
Figure FDA0003851466110000022
wherein the content of the first and second substances,
Figure FDA0003851466110000023
the first GPS data, the dead reckoning position data and the GPS data are incremental data, and the first DVL data and the GPS data are corresponding after linear interpolation;
the final optimization function is:
Figure FDA0003851466110000024
f (β, δ k) is a nonlinear function with respect to β and δ k.
2. The method of claim 1, wherein said solving said nonlinear optimization function comprises: and solving the nonlinear optimization function by adopting a Gauss-Newton method.
3. The method of claim 2, wherein solving the nonlinear optimization function using a gauss-newton method comprises:
performing first-order Taylor expansion on the nonlinear optimization function;
determining an incremental equation according to a result after the first-order Taylor expansion;
and solving the incremental equation.
4. The method of claim 3, wherein said subjecting the nonlinear optimization function to a first order Taylor expansion comprises:
let x = [ beta, delta k ], perform a first order Taylor expansion on the nonlinear optimization function f (x),
f(x+Δx)＝f(x)+J(x)Δx，
where J (x) is the derivative of f (x) with respect to x, and J (x) is referred to as the Jacobian matrix.
5. The method of claim 4, wherein determining an incremental equation from the first order Taylor expansion result comprises:
will find the descending vector Δ x such that | | f (x + Δ x) | luminance 2 The minimum problem is reached, and the conversion is to solve a linear least squares problem:
Figure FDA0003851466110000031
through the conversion, the parameter to be optimized is changed from x to deltax, and the square term of the objective function is expanded:
Figure FDA0003851466110000032
the derivative of the expanded equation with respect to Δ x is found and made 0, resulting in:
Figure FDA0003851466110000033
the incremental equation is obtained as:
Figure FDA0003851466110000034
defining the coefficients to the left of the incremental equation as H and the right as g, then the above equation becomes:
HΔx＝g。
6. the method of claim 5, wherein solving the nonlinear optimization function using the Gaussian Newton method comprises:
step 1: given an initial value x 0 ；
And 2, step: for the k-th iteration, the current Jacobian matrix J (x) is solved k ) And error f (x) k )；
And step 3: solving the incremental equation H Δ x = g to obtain x k ；
And 4, step 4: if x k If the number of iterations is less than the preset value or reaches the set maximum value, x k Stopping iteration as an optimal solution; otherwise, let x k+1 ＝x k +Δx k And returning to the step 2 to continue the iterative operation.
7. An apparatus for calibrating an SINS/DVL integrated navigation system error, comprising:
the modeling module is used for modeling a nonlinear optimization function; the parameters of the nonlinear optimization function include: a scaling factor δ k for the DVL velocity, and an azimuthal misalignment angle β between the SINS and DVL;
the solving module is used for solving the nonlinear optimization function so as to finish the calibration of a scale factor delta k of the DVL speed and an azimuth installation error angle beta between the SINS and the DVL;
wherein the modeling nonlinear optimization function comprises:
the DVL self-carrier coordinate system is m system, the inertial navigation coordinate system is b system, the navigation coordinate system is n system, the DVL speed obtained under the DVL exploring bottom mode is
Figure FDA0003851466110000041
DVL data update time of
Figure FDA0003851466110000042
The dead reckoning formula is as follows:
Figure FDA0003851466110000043
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003851466110000044
θ q ＝θ y +β；
θ y representing the azimuthal misalignment angle, θ, of the SINS p Representing the pitch misalignment angle, θ r Representing the roll misalignment angle, θ q Indicating the azimuth deviation angle during the conversion of m to the navigation system,
Figure FDA0003851466110000045
representing the mounting error transition matrix between SINS and DVL,
Figure FDA0003851466110000046
representing a transformation matrix between inertial navigation and navigation systems;
Figure FDA0003851466110000047
Figure FDA0003851466110000048
Figure FDA0003851466110000051
acquiring GPS data corresponding to each DVL data;
the longitude and latitude in the GPS data are converted to a position under the northeast coordinate system, which is expressed as:
Figure FDA0003851466110000052
the position increment calculation formula of the GPS is as follows:
Figure FDA0003851466110000053
wherein the content of the first and second substances,
Figure FDA0003851466110000054
the first GPS data, the dead reckoning position data and the GPS data are incremental data, and the first DVL data and the GPS data are corresponding after linear interpolation;
the final optimization function is:
Figure FDA0003851466110000055
f (β, δ k) is a nonlinear function with respect to β and δ k.
8. A SINS/DVL combined navigation system, comprising:
an apparatus for calibrating errors in a SINS/DVL combined navigation system as recited in claim 7.
9. A controller for performing the method of calibrating an error of a SINS/DVL combined navigation system of any one of claims 1 to 6.
CN202110204090.6A 2021-02-24 2021-02-24 Method and device for calibrating SINS/DVL combined navigation system error Active CN112987054B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110204090.6A CN112987054B (en) 2021-02-24 2021-02-24 Method and device for calibrating SINS/DVL combined navigation system error

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110204090.6A CN112987054B (en) 2021-02-24 2021-02-24 Method and device for calibrating SINS/DVL combined navigation system error

Publications (2)

Publication Number Publication Date
CN112987054A CN112987054A (en) 2021-06-18
CN112987054B true CN112987054B (en) 2023-03-03

Family

ID=76349905

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110204090.6A Active CN112987054B (en) 2021-02-24 2021-02-24 Method and device for calibrating SINS/DVL combined navigation system error

Country Status (1)

Country Link
CN (1) CN112987054B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114485730B (en) * 2022-01-25 2023-07-04 湖南大学 DVL error calibration method and system based on Newton iteration method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643806A (en) * 2016-12-30 2017-05-10 深圳友铂科技有限公司 Inertial navigation system alignment accuracy evaluation method
CN111323050A (en) * 2020-03-19 2020-06-23 哈尔滨工程大学 Strapdown inertial navigation and Doppler combined system calibration method
CN111337056A (en) * 2020-05-19 2020-06-26 北京数字绿土科技有限公司 Optimization-based LiDAR motion compensation position and attitude system alignment method
CN111947651A (en) * 2020-07-17 2020-11-17 中国人民解放军海军工程大学 Underwater combined navigation information fusion method and system and autonomous underwater vehicle
CN112082547A (en) * 2020-09-08 2020-12-15 北京邮电大学 Integrated navigation system optimization method and device, electronic equipment and storage medium

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11422253B2 (en) * 2018-11-19 2022-08-23 Tdk Corportation Method and system for positioning using tightly coupled radar, motion sensors and map information

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643806A (en) * 2016-12-30 2017-05-10 深圳友铂科技有限公司 Inertial navigation system alignment accuracy evaluation method
CN111323050A (en) * 2020-03-19 2020-06-23 哈尔滨工程大学 Strapdown inertial navigation and Doppler combined system calibration method
CN111337056A (en) * 2020-05-19 2020-06-26 北京数字绿土科技有限公司 Optimization-based LiDAR motion compensation position and attitude system alignment method
CN111947651A (en) * 2020-07-17 2020-11-17 中国人民解放军海军工程大学 Underwater combined navigation information fusion method and system and autonomous underwater vehicle
CN112082547A (en) * 2020-09-08 2020-12-15 北京邮电大学 Integrated navigation system optimization method and device, electronic equipment and storage medium

Also Published As

Publication number Publication date
CN112987054A (en) 2021-06-18

Similar Documents

Publication Publication Date Title
CN109211276B (en) SINS initial alignment method based on GPR and improved SRCKF
CN109507706B (en) GPS signal loss prediction positioning method
CN109471146B (en) Self-adaptive fault-tolerant GPS/INS integrated navigation method based on LS-SVM
CN104344837A (en) Speed observation-based redundant inertial navigation system accelerometer system level calibration method
CN110132308B (en) Attitude determination-based USBL installation error angle calibration method
CN112083457B (en) Neural network optimized IMM satellite positioning navigation method
CN112987054B (en) Method and device for calibrating SINS/DVL combined navigation system error
CN115979253A (en) Underwater robot multi-sensor tight combination navigation method based on robust filtering
CN110703205B (en) Ultra-short baseline positioning method based on self-adaptive unscented Kalman filtering
CN113203429A (en) Online estimation and compensation method for temperature drift error of gyroscope
CN108507587B (en) Three-dimensional vehicle-mounted positioning navigation method based on coordinate transformation
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN110912535B (en) Novel non-pilot Kalman filtering method
CN111982126A (en) Design method of full-source BeiDou/SINS elastic state observer model
CN116907503A (en) Remote sensing formation satellite positioning method and system based on robust positioning algorithm of outlier
CN114132531B (en) Low-orbit space target orbit correction method and device and electronic equipment
CN115451955A (en) INS/GPS tightly-coupled navigation method and system based on distributed robust filtering
CN115655285A (en) Weight and reference quaternion corrected unscented quaternion attitude estimation method
Hao et al. Research on data fusion for SINS/GPS/magnetometer integrated navigation based on modified CDKF
CN110470294B (en) Carrier attitude estimation method integrating virtual measurement and Kalman filtering
CN112683265A (en) MIMU/GPS integrated navigation method based on rapid ISS collective filtering
CN112556722B (en) System error compensation method based on automatic selection of preferred sources
CN110779552B (en) Self-adaptive alignment method under earth fixed connection coordinate system
CN112589806B (en) Robot pose information determination method, device, equipment and storage medium
CN115824224A (en) Underwater robot autonomous dead reckoning method based on AHRS and DVL

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