Inertial and ultra-short baseline multi-parameter joint calibration method based on graph optimization

Abstract

The application provides a multi-parameter joint calibration method based on graph optimization of inertial and ultra-short baseline, applies the graph optimization technology to the multi-parameter joint calibration of inertial / ultra-short baseline, and the whole process comprises the following steps: establishing a multi-error parameter state model, constructing a multi-error parameter calibration factor, calculating based on a nonlinear optimization problem of an incremental interval balance, and finally obtaining accurate multi-error parameters. Compared with the prior art, the application can simultaneously estimate the responder position error, the rod arm error and the installation angle error, sufficiently utilizes historical observation data, solves the nonlinear problem of the optimization equation through repeated iteration and relinearization, and has the advantages of high calibration precision and fast calibration speed.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater navigation and positioning, in particular to an inertial and ultra-short baseline multi-parameter joint calibration method based on graph optimization. BACKGROUND

[0002] The ocean contains rich biological resources, mineral resources, renewable energy and space resources, and is a huge treasure trove of resources. Long-time high-precision navigation and positioning technology in the water is the premise and key to solving the development of deep-sea resources. In order to ensure that the autonomous underwater vehicle (AUV) can successfully complete the related tasks underwater, it is required that the navigation system equipped with it has the ability of long-term autonomous navigation and positioning underwater.

[0003] Inertial navigation is a self-powered navigation method, which has the characteristics of not being disturbed by the environment and can work continuously all day long, and is suitable for long-time navigation and positioning of underwater vehicles. However, the inertial navigation error is easy to diverge, and needs to be supplemented by other sensors for periodic error correction. Acoustic signals have the advantage of small propagation attenuation underwater, and the integrated navigation system based on inertial / acoustic is becoming a new technology growth point for underwater navigation.

[0004] However, during the integration of the inertial / acoustic system, there will inevitably be artificial installation errors, such as position error of the underwater transponder, installation angle and space arm error between the inertial device and the acoustic transducer, etc. High-precision underwater navigation and positioning must rely on accurate transponder position and known installation error model. In the long-time navigation and positioning process, the existence of system error will seriously reduce the positioning accuracy of the integrated navigation, therefore, the multi-error parameter calibration of inertial / ultra-short baseline is the current research hotspot, and one of the key technologies is to estimate the multi-parameter error of the system and ensure the accuracy and rapidity of the estimation in the nonlinear, non-Gaussian noise environment. SUMMARY

[0005] The present application provides an inertial and ultra-short baseline multi-parameter joint calibration method based on graph optimization, which uses the advantages of factor graph to solve the nonlinear problem of optimization equation through repeated iteration and relinearization of batch historical observation data, and improves the calibration accuracy and speed.

[0006] The present application provides an inertial and ultra-short baseline multi-parameter joint calibration method based on graph optimization, which comprises:

[0007] Step 1, establishing a multi-error parameter state model;

[0008] Step 2, constructing a multi-error parameter calibration factor;

[0009] Step 3, according to the state model established in step 1, step 2 and the calibration factor, a multi-error parameter calibration factor graph optimization model is constructed, and the state is updated by using the interval incremental smoothing method.

[0010] Further, step 1, a multi-error parameter state model is established, including:

[0011] In inertial and ultra-short baseline integrated navigation, there are installation position deviations of transponders, rod arm errors between inertial devices and acoustic transducers, and installation angle errors. The jointly optimized state quantities in a sliding window are defined as follows:

[0012] X = [x0, x1, x2, … x M ] T

[0013]

[0014] Wherein, M represents the size of the sliding window; subscript i represents the i-th state in the sliding window; represents the transponder position state in the navigation system n; represents the rod arm state in the carrier coordinate system b; represents the installation error state matrix of the carrier coordinate system b relative to the coordinate system u of the acoustic transducer.

[0015] Further, step 2, a multi-error parameter calibration factor is constructed, including:

[0016] A binary constant constraint factor is established to connect the state relationship at different times. The residual error r1 of the binary constant constraint factor is represented as follows:

[0017]

[0018] Wherein, represents the i+1-th transponder position state in the sliding window, represents the i+1-th rod arm state in the sliding window, represents the i+1-th installation error state matrix in the sliding window;

[0019] A position measurement factor is established to calculate the position measurement factor residual error by using the position and attitude output by the inertial and satellite integrated navigation and the position obtained by the ultra-short baseline system. First, the position calculation model p g of the ultra-short baseline system is represented as:

[0020]

[0021] Wherein, P n represents the position of the transponder, represents the attitude transformation matrix from the carrier coordinate system to the navigation coordinate system, which is provided by the inertial / satellite integrated navigation system, represents the installation error matrix of the carrier coordinate system b relative to the coordinate system u of the acoustic transducer, L b represents the rod arm size in the carrier coordinate system b, r, alpha, beta represent the slant range and azimuth angle obtained by the ultra-short baseline measurement, represents the conversion matrix from the navigation coordinate system to the earth coordinate system, L represents the latitude, h represents the height, R M and R N The earth meridian curvature radius and the equinoctial curvature radius;

[0022] Secondly, considering that the inertial / satellite combined navigation provides a real reference position as Therefore, the residual model r2 is represented as:

[0023]

[0024] Further, in step 3, according to the state model established in steps 1 and 2 and the calibration factor, a multi-error parameter calibration factor graph optimization model is constructed, and an interval incremental smoothing method is used for state updating, including:

[0025] The inertial / ultra-short baseline multi-parameter joint calibration problem is converted into a maximum likelihood estimation problem of state, and according to the state model and the measurement residual established in steps 1 and 2, a least squares optimization equation is established as follows:

[0026]

[0027] Wherein, r p represents the prior residual information obtained by marginalization, Σ p represents the variance of the prior information; r 1,m and Σ1 represent the mth binary constant constraint residual and the corresponding variance in the sliding window respectively; r 2,m and Σ2 represent the mth position factor constraint residual and the corresponding variance in the sliding window respectively.

[0028] Based on the optimization equation, an interval incremental smoothing method based on isam2 is used to estimate the state quantity, and the nonlinearity problem of the optimization equation is solved through repeated iteration and relinearization, so as to improve the calibration precision and speed of all parameters.

[0029] The technical scheme of the present application has the following beneficial effects:

[0030] According to the inertial / ultra-short baseline multi-parameter joint calibration method established according to the above steps, the transponder position error, the rod arm error and the installation angle error can be estimated at the same time, the historical observation data is fully utilized, the nonlinearity problem of the optimization equation is solved through repeated iteration and relinearization, and the method has the advantages of high calibration precision and fast calibration speed. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 Factor graph based inertial / ultra-short baseline multi-parameter joint calibration method DETAILED DESCRIPTION

[0032] In order to make the purpose, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0033] Step 1: Establish a multi-error parameter state model, specifically:

[0034] In inertial / ultra-short baseline integrated navigation, there are installation position deviations of transponders, rod arm errors between inertial devices and acoustic transducers, and installation angle errors, and the state quantity jointly optimized in a sliding window is defined as follows:

[0035] X = [x0, x1, x2, … x M ] T

[0036]

[0037] Wherein, M represents the size of the sliding window. The subscript i represents the i-th state in the sliding window. represents the transponder position state in the navigation system (n). represents the rod arm state in the carrier coordinate system (b). represents the installation error state matrix of the carrier coordinate system (b) relative to the coordinate system (u) where the acoustic transducer is located.

[0038] Step 2: Build a multi-error parameter calibration factor, specifically:

[0039] Establish a binary constant constraint factor to connect the state relationship at different times, and the residual error (r1) of the binary constant constraint factor is represented as follows:

[0040]

[0041] Wherein, represents the i+1-th transponder position state in the sliding window, represents the i+1-th rod arm state in the sliding window, represents the i+1-th installation error state matrix in the sliding window.

[0042] Establish a position measurement factor to calculate the position measurement factor residual error from the position and attitude output by the inertial / satellite integrated navigation and the position obtained by the ultra-short baseline system. First, the position calculation model (p g ) of the ultra-short baseline system is represented as:

[0043]

[0044] where P n denotes the position of the transponder, denotes the pose transformation matrix from the carrier coordinate system to the navigation coordinate system, which is provided by the inertial / satellite integrated navigation system, denotes the installation error matrix of the carrier coordinate system (b) relative to the coordinate system (u) in which the acoustic transducer is located, L b denotes the rod arm size in the carrier coordinate system (b), and r, a, b denote the slant range and azimuth angle obtained by the ultra-short baseline measurement, denotes the conversion matrix from the navigation coordinate system to the earth coordinate system, L denotes the latitude, and h denotes the height, R M and R N the earth meridian radius of curvature and the equinoctial radius of curvature.

[0045] Secondly, considering that the inertial / satellite integrated navigation provides a real reference position as Therefore, the residual error model (r2) is expressed as:

[0046]

[0047] Step 3: According to the state model established in steps 1-2 and the calibration factor, a multi-error parameter calibration factor graph optimization model is constructed, and the state is updated by using the interval incremental smoothing technique method. Specifically,

[0048] The inertial / ultra-short baseline multi-parameter joint calibration problem is converted into a maximum likelihood estimation problem of the state. According to the state model and the measurement residual error established in steps (1) and (2), the least squares optimization equation is established as follows:

[0049]

[0050] where r p denotes the prior residual error information obtained by marginalization, Σ p denotes the variance of the prior information. r 1,m and Σ1 respectively denote the mth binary constant constraint residual error in the sliding window and the variance corresponding thereto. r 2,m and Σ2 respectively denote the mth position factor constraint residual error in the sliding window and the variance corresponding thereto.

[0051] Based on the above optimization equation, the interval incremental smoothing technique based on isam2 is adopted to estimate the state quantity. The nonlinear problem of the optimization equation is solved by repeated iteration and relinearization, so as to improve the calibration precision and speed of the full parameter.

[0052] The above-described embodiments of the present application do not constitute a limitation on the protection scope of the present application.

Claims

1. A method for inertial and ultra-short baseline multi-parameter joint calibration based on graph optimization, characterized in that, The method comprises: Step 1, establishing a multi-error parameter state model; Step 2, constructing a multi-error parameter calibration factor; Step 3, according to the state model and the calibration factor established in steps 1 and 2, constructing a multi-error parameter calibration factor graph optimization model, and updating the state by using an interval incremental smoothing method; Step 2, constructing a multi-error parameter calibration factor, comprising: A binary constant constraint factor is established, a state relationship at different times is connected, and a residual of the binary constant constraint factor is represented as follows: wherein, represents the state of the +1 transponder position within the sliding window, represents the state of the +1 lever arm within the sliding window, represents the state of the +1 installation error state matrix within the sliding window; The position measurement factor is established to calculate the position measurement factor residual error from the position and attitude output by the inertial and satellite combined navigation and the position obtained by the ultra-short baseline system. First, the position calculation model of the ultra-short baseline system is is represented as: wherein, represents the position of the transponder, represents the pose transformation matrix from the carrier coordinate system to the navigation coordinate system, provided by the inertial / satellite integrated navigation system, represents the installation error matrix of the carrier coordinate system b relative to the coordinate system u in which the acoustic transducer is located, represents the rod arm size in the carrier coordinate system b, represents the slant range and azimuth angle obtained by the ultra-short baseline measurement, represents the conversion matrix from the navigation coordinate system to the earth coordinate system, L represents the latitude, and h represents the height, and the earth meridian radius of curvature and the prime vertical radius of curvature; Second, considering that the inertial / satellite integrated navigation provides a real reference position as Therefore, the residual model is expressed as: ； In step 3, according to the state model and the calibration factor established in steps 1 and 2, constructing a multi-error parameter calibration factor graph optimization model, and updating the state by using an interval incremental smoothing method, comprising: The inertial / ultra-short baseline multi-parameter joint calibration problem is converted into a maximum likelihood estimation problem of a state, and a least square optimization equation is established according to the state model and the measurement residual error established in steps 1 and 2 as follows: wherein, represents the prior residual information obtained by the edge, represents the variance of the prior information; and respectively represent the mth binary constant constraint residual and the corresponding variance in the sliding window; and respectively represent the mth position factor constraint residual and the corresponding variance in the sliding window; M represents the size of the sliding window; Based on the optimization equation, the state quantity is estimated by using an interval incremental smoothing method based on isam2, and the nonlinearity problem of the optimization equation is solved by repeated iteration and relinearization.

2. The method of claim 1, wherein, Step 1, establishing a multi-error parameter state model, comprising: In inertial and ultra-short baseline integrated navigation, there are installation position deviations of transponders, rod arm errors and installation angle errors between inertial devices and acoustic transducers, and the state quantity optimized jointly in a sliding window is defined as follows: where M denotes the size of the sliding window; the subscript denotes the state of the th state within the sliding window; denotes the transponder position state in the navigation system n; denotes the lever arm state in the carrier coordinate system b; denotes the installation error state matrix of the carrier coordinate system b relative to the coordinate system u of the acoustic transducer.

Images