Method and system for calibrating inertial sensor of Doppler odometer

Abstract

The present invention discloses a method and system for calibrating an inertial sensor using a Doppler odometer, relating to the technical field of underwater integrated navigation. The method comprises: obtaining an IMU pre-integration factor and a DVL velocity residual factor based on IMU observation data and DVL observation data; constructing a factor graph structure model using prior information of the initial states of a gyroscope and an accelerometer as prior factors, the prior factors, the IMU pre-integration factor, and the DVL velocity residual factor as factor nodes, the state variables of the gyroscope and accelerometer as variable nodes, and the relationships between the factor nodes and the variable nodes as edges; and obtaining theoretical outputs of the gyroscope and accelerometer after inertial navigation error compensation by correcting the state variables using the factor graph structure model. The IMU error is calibrated online using observed DVL data, and the underwater integrated navigation problem is modeled as a nonlinear least squares optimization method to achieve a global optimal estimation of the state quantity over the entire time period and suppress the divergence rate of the inertial navigation position error.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater integrated navigation, and in particular to a method and system for calibrating an inertial sensor of a Doppler odometer. Background Art

[0002] In applications such as underwater vehicles, the accuracy of the inertial measurement unit (IMU) plays a crucial role in navigation precision. IMUs rely on internal gyroscopes and accelerometers to measure motion parameters. However, errors such as gyroscope drift and accelerometer bias accumulate over time, causing a significant decrease in the accuracy of attitude, velocity, and position information. This makes it impossible to provide long-term navigation and positioning for underwater vehicles. Furthermore, the radio-based Global Navigation Satellite System (GNSS) cannot be directly used in underwater environments due to the rapid attenuation of electromagnetic waves in seawater.

[0003] Currently, sonar signals are widely used for underwater ranging. In acoustic positioning systems, such as long baseline (LBL) positioning systems, sonar signals can measure the distance between underwater submersibles and seafloor reference points, enabling navigation and positioning. However, absolute position calibration relies on seafloor reference stations, which not only increases system construction and maintenance costs but also limits seafloor topography and water quality. The high cost of building a seafloor reference station network makes it difficult to achieve full coverage. Furthermore, sound waves have a limited propagation distance. In complex marine environments (such as those with high turbidity and complex terrain), signals are susceptible to interference, causing them to weaken or distort, limiting positioning range and accuracy. This reliance on external infrastructure and the limitations of propagation characteristics directly impact the effectiveness of sonar applications in large-scale, complex environments. Consequently, sonar-calibrated inertial sensors suffer from high costs and limited range.

[0004] To address the shortcomings of sonar calibration methods, using a Doppler Velocity Logger (DVL) to calibrate the IMU can address the high cost and limited range. However, traditional filtering methods have two major bottlenecks: DVLs are prone to velocity measurement failure in turbid water, complex seabed topography, or when moving at high speeds, resulting in inaccurate filtering models; and Kalman filtering relies solely on current state estimates and cannot retroactively correct historical errors, leading to a sharp drop in navigation accuracy when the DVL signal is interrupted for an extended period. Summary of the Invention

[0005] In order to solve the above problems, the present invention proposes a method and system for calibrating inertial sensors with Doppler odometers, which uses observed DVL data to calibrate the IMU error online, models the underwater integrated navigation problem as a nonlinear least squares optimization, and constructs a factor graph structure model including IMU pre-integration factors, DVL velocity residual factors, sliding windows, and DVL velocity residual constraints to achieve global optimal estimation of state quantities over the entire time period and suppress the divergence rate of inertial navigation position errors.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] In a first aspect, the present invention provides a method for calibrating an inertial sensor of a Doppler odometer, comprising:

[0008] The pre-integrated values ​​of position, velocity, and attitude within a set time interval and the bias changes of the gyroscope and accelerometer are calculated based on the IMU observation data to obtain the IMU pre-integrated factor. The DVL velocity residual factor is obtained based on the IMU pre-integrated factor and the DVL observation data.

[0009] The prior information of the initial state of the gyroscope and accelerometer is used as the prior factor, the prior factor, the IMU pre-integration factor and the DVL velocity residual factor are used as factor nodes, the state variables of the gyroscope and accelerometer are used as variable nodes, and the relationship between the factor node and the variable node is used as the edge to construct a factor graph structure model;

[0010] According to the real-time observation data, the factor graph structure model is used to correct the state variables until the inertial guidance error converges to the set value. The theoretical outputs of the gyroscope and accelerometer after inertial guidance error compensation are obtained based on the state vector when the inertial guidance error converges.

[0011] In a second aspect, the present invention provides a system for calibrating an inertial sensor of a Doppler odometer, comprising:

[0012] a factor calculation module configured to calculate pre-integrated values ​​of position, velocity, and attitude within a set time interval and bias changes of the gyroscope and accelerometer based on the IMU observation data, thereby obtaining an IMU pre-integrated factor, and to obtain a DVL velocity residual factor based on the IMU pre-integrated factor and the DVL observation data;

[0013] A graph construction module is configured to use the prior information of the initial states of the gyroscope and the accelerometer as a prior factor, the prior factor, the IMU pre-integration factor, and the DVL velocity residual factor as factor nodes, the state variables of the gyroscope and the accelerometer as variable nodes, and the relationship between the factor node and the variable node as an edge to construct a factor graph structure model;

[0014] The calibration module is configured to correct the state variables using a factor graph structure model based on real-time observation data until the inertial guidance error converges to a set value, thereby obtaining the theoretical outputs of the gyroscope and accelerometer after inertial guidance error compensation based on the state vector when the inertial guidance error converges.

[0015] In a third aspect, the present invention provides an electronic device comprising a memory and a processor, and computer instructions stored in the memory and executed on the processor, wherein the computer instructions, when executed by the processor, perform the method described in the first aspect.

[0016] In a fourth aspect, the present invention provides a computer-readable storage medium for storing computer instructions, wherein when the computer instructions are executed by a processor, the method described in the first aspect is performed.

[0017] In a fifth aspect, the present invention provides a computer program product, comprising a computer program, which implements the method described in the first aspect when executed by a processor.

[0018] Compared with the prior art, the present invention has the following beneficial effects:

[0019] The present invention proposes a method and system for calibrating inertial sensors for Doppler odometers, models the navigation problem as a nonlinear least squares optimization, and realizes the global optimal estimation of state quantities over the entire time period by constructing a factor graph framework including an IMU pre-integration factor, a DVL velocity residual factor, and a sliding window marginalization and gross error detection and weight reduction optimization method. The graph optimization method has good scalability and flexibility. In practical applications, as the measurement data continues to increase and the system complexity increases, the graph optimization model can be easily expanded and adjusted to adapt to new measurement constraints and changes in system characteristics. In addition, graph optimization has strong robustness to the noise in the DVL velocity observation data, and can reduce the impact of noise on the estimation results through reasonable weight distribution, thereby achieving high-precision inertial sensor error calibration in a complex noise environment.

[0020] Based on a graph optimization method, the present invention calibrates inertial sensors using a Doppler odometer, aiming to overcome the shortcomings of traditional filtering methods and effectively improve the calibration accuracy of inertial sensors, thereby enhancing the navigation performance and reliability of systems such as underwater vehicles. This method can effectively suppress the speed of inertial sensor error divergence and improve accuracy, resulting in a 34.7% increase in the time that the inertial sensor position error divergence lasts for one kilometer, thereby improving the reliability of underwater vehicles.

[0021] Advantages of additional aspects of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0023] Figure 1 A flow chart of a method for calibrating an inertial sensor of a Doppler odometer provided in Example 1 of the present invention;

[0024] Figure 2 This is a schematic diagram of the method for calibrating an inertial sensor of a Doppler odometer provided in Example 1 of the present invention. DETAILED DESCRIPTION

[0025] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0026] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which the present invention belongs.

[0027] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that the terms "include" and "comprise" and any variations are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0028] In the absence of conflict, the embodiments of the present invention and the features thereof may be combined with each other.

[0029] Example 1

[0030] In underwater environments, the attitude and position estimation of the IMU is susceptible to accumulated errors, while the DVL can provide very accurate velocity, which helps calibrate inertial sensor errors. Therefore, this embodiment proposes a method for calibrating inertial sensors using a Doppler odometer. The IMU errors are calibrated online using observed DVL data. The underwater integrated navigation problem is modeled as a nonlinear least squares optimization. By constructing a factor graph structure model that includes an IMU pre-integration factor, a DVL velocity residual factor, a sliding window, and DVL velocity residual constraints, a global optimal estimation of the state quantity over the entire time period is achieved, thereby suppressing the divergence rate of the inertial navigation position error.

[0031] Combine Figure 1-Figure 2 The detailed steps of the method of this embodiment are described. Figure 2 The bipartite graph expression commonly used in factor graphs is used to describe the relationship between state variable nodes and IMU pre-integration factors and DVL velocity residual factor nodes, as well as the specific implementation methods of sliding window, marginalization, gross error detection and weight reduction optimization in the factor graph optimization process.

[0032] (1) Construct a factor graph structure model based on DVL calibration of IMU errors.

[0033] A factor graph decomposes a multivariable global function into a product of local functions. Each local function depends on a subset of variables, and this decomposition can be represented using a bipartite graph. Factor graphs intuitively represent the transitive relationships between system states.

[0034] A factor graph is a bipartite graph, using Represents, including variable nodes , Factor Node and the edge ; Among them, the variable node Refers to variables in a global multivariate function; factor node Refers to the local function in the factorization, expressed as: ; is the corresponding cost function, is the actual observation data; is only in and Connecting edges that exist when associated.

[0035] In a Bayesian network, the probability density of the joint of all variables is obtained by multiplying the conditional probability density associated with each node. Similarly, in a factor graph, the joint probability density is represented as the product of a series of factors. Therefore, the factor graph is described as: .

[0036] The corresponding variable node is the required state variable , so in The set of all state variables received at a moment is expressed as: .

[0037] The set of all observations received at time is expressed as: ; express The actual observation values ​​obtained by different sensors at each moment , is the IMU observation data; is the DVL observation data.

[0038] If the observation data of each time node are independent, the global function is factorized according to the Bayesian estimation, and all state variables in the kth time node are and observational data The joint posterior probability density for:

[0039] (1);

[0040] Where, Represents the prior information of the initial states of all state variables; Represents the state transition model using IMU recursion, are the i-th and i-1-th state variables, represents the DVL observation update model, is the jth set of the i-th variable node in the DVL observation update model, is the j-th DVL observation data.

[0041] When constructing factor nodes, the prior information of the carrier state is first modeled as a priori factor. This priori factor is only related to the variable node at each moment and is a unary factor.

[0042] In the joint probability density function In the factorization of , each factor represents an independent term, that is, ;in, is the state variable corresponding to the i-th moment.

[0043] According to the maximum a posteriori probability estimation, the problem of calculating the optimal value of the state variable from the observed data is converted into an equivalent least squares optimization problem, namely:

[0044] (2);

[0045] in, Represents the error function constructed from prior information, usually expressed as the difference between the observed value and the mean; represents the square of Mahalanobis distance, represents the covariance matrix; is the IMU error function, is the DVL error function; is the IMU observation function; is the DVL observation function; the observation function Through Taylor linear expansion, we can get ; It is to find the most likely The estimated value, i.e., the maximum a posteriori estimate; It is a priori error term, usually based on known prior signal information, indicating the impact of a priori model on the error; is the Jacobian matrix of the prior information, representing the linearization of the prior model; represents the initial state estimation vector; Indicates the initial state The sum of all variables is used to optimize the state; Represents the observation model function under the initial state, usually the mapping relationship between the state and the observation value; represents the weighted square error, which is used to express the relationship between the measurement error and the observation noise covariance; is the observation value of IMU; is the observed value of DVL.

[0046] According to nonlinear optimization theory, by adjusting the state variables To minimize it, The value can make the state reach the optimal estimated value.

[0047] (2) Constructing state variables .

[0048] If factor graph optimization is introduced into the fusion navigation of the long baseline underwater positioning system and the inertial navigation system, the corresponding variable nodes are the required state variables. The state variables at this moment are expressed as a 27-dimensional state vector constructed in the local navigation frame:

[0049] (3);

[0050] Where, 、 and Respectively The three-axis (e, n, u axis) position coordinates, velocity and attitude in the time navigation system, that is, the northeast celestial coordinate system of the base station; and They represent the accelerometer and gyroscope bias errors in the X, Y, and Z directions in the carrier coordinate system respectively; and are the scale factor error coefficients of the gyroscope and accelerometer along the X, Y, and Z directions in the carrier coordinate system respectively; and are the installation error coefficients of the gyroscope and accelerometer along the X, Y, and Z directions in the carrier coordinate system, respectively.

[0051] (3) Construct observation residual factors, including IMU pre-integration factors and DVL velocity residual factors.

[0052] (3-1) The output frequency of IMU information is typically much higher than that of DVL. Updating the factor graph every time the IMU outputs information would not only introduce a large amount of redundant information but also significantly increase the system's computational burden, thereby affecting its real-time performance. To alleviate this problem, this embodiment employs an IMU pre-integration method: This method integrates IMU observation data over a period of time into a pre-integration factor node before updating the inertial sensor system's state.

[0053] IMU observation data is received at each epoch for:

[0054] (4);

[0055] (5);

[0056] (6);

[0057] Where, represents the carrier coordinate system, and are the specific force and angular rate measured by IMU respectively; and are the ideal outputs of the gyroscope and accelerometer in the carrier coordinate system; and are the biases of the accelerometer and gyroscope, and are the noise terms of the gyroscope and accelerometer, respectively, which are generally considered to conform to the Gaussian noise distribution.

[0058] According to the INS kinematic model, the differential equations of position, velocity, and attitude with respect to time are obtained as follows:

[0059] (7);

[0060] (8);

[0061] (9);

[0062] Where, Represents the navigation coordinate system, which is defined as the "east-north-sky" geographic coordinate system. Indicates the velocity of the vehicle in the navigation coordinate system; Indicates the position of the vehicle in the navigation coordinate system; For location The time derivative of , i.e., velocity; For speed The time derivative of , i.e., acceleration; Represents the cross product operation; and are the Earth's rotation angular rate and Gravity vector under the frame; rad / s, is the angular rate of the earth's rotation at the current position; Describe the changes in the carrier's posture; Represents the current posture quaternion; represents the rotation matrix, for Relative to The rotation matrix of the system; is the gyroscope angular rate measurement value; Represents the multiplication operation of quaternion; L represents the current geographic latitude.

[0063] Through the differential equations of position, velocity, and attitude with respect to time, an IMU time interval is obtained. The kinematic equations within:

[0064] (10);

[0065] (11);

[0066] (12);

[0067] Where, and Respectively expressed in and The speed of time, and Respectively in and Position at the moment; and Respectively in and The posture of the moment; Indicates that within the time interval Changes in the system; It is the velocity change related to the specific force measurement, called the specific force integral term; is the change in velocity related to the Coriolis acceleration and gravity, also known as the gravity / Coriolis integral term; Indicates During this period, the attitude quaternion is corrected due to the rotation of the earth.

[0068] In classical INS calculations, the coordinate system The state at a moment is determined by The state of the moment is obtained recursively. When the iterative update is performed directly, multiple state transfers will result in a huge amount of calculation. The amount of calculation is reduced by IMU pre-integration. Integrate the IMU data within to get the pre-integrated quantity:

[0069] (13);

[0070] (14);

[0071] (15);

[0072] in:

[0073] (16);

[0074] (17);

[0075] Where, Indicates Moment and Consider the position corrections caused by gravity, Coriolis force, etc. between moments; For Moment and The time intervals represent the correction to the speed to take into account the rotation of the Earth; For Moment and The change in displacement between moments; For Moment and The amount of change in velocity between moments; For and The amount of posture change between moments; From the current IMU coordinate system To the initial moment The rotation matrix of the coordinate system; From the current IMU coordinate system The specific force measured by the lower IMU; and Respectively Moment and Time navigation coordinate system The corresponding position below; and express Moment and Time navigation coordinate system The corresponding speed is For Time navigation coordinate system Coordinate transformation matrix converted to carrier coordinate system b; Expressed as and The time between an IMU arrive The attitude change of the sampling epoch; and Respectively Moment and Time navigation coordinate system The corresponding posture below; is the gravity vector in the navigation coordinate system; In the navigation coordinate system and The amount of posture change between moments.

[0076] In order to simplify the calculation, it is usually assumed that the inertial zero bias remains unchanged within the integration interval. During the factor graph optimization process, if the bias estimate changes, a bias update must be added. The updated accelerometer bias and gyroscope bias Recalculate the corrected pre-integrated observations.

[0077] (18);

[0078] (19);

[0079] (20);

[0080] in, The Jacobian matrix representing the displacement to gyroscope bias; The Jacobian matrix representing the displacement to accelerometer bias; and The Jacobian matrix representing the velocity bias of the gyroscope and accelerometer; represents a Lie group operation that maps the Jacobian vector to the tangent space and generates a modified quaternion increment; The Jacobian matrix representing the attitude bias of the gyroscope; is the gyroscope zero bias that remains unchanged within the integration interval at the current moment; Indicates the accelerometer zero bias that remains unchanged within the current integration interval; Indicates the change in gyroscope bias; Indicates the change in accelerometer bias; Indicates that the integral interval remains unchanged and The original pre-integrated position change of Indicates that the integral interval remains unchanged and The original velocity pre-integration; Indicates that the integral interval remains unchanged and Posture pre-integration of ; 、 and They are respectively expressed as the position change, velocity change and attitude change after adding the zero bias update.

[0081] The IMU residual vector, that is, the IMU pre-integration factor is expressed as:

[0082] (twenty one);

[0083] (twenty two);

[0084] (twenty three);

[0085] Where, Represents the IMU pre-integration factor; and Respectively expressed in and The amount of change in the accelerometer and gyroscope bias between moments; and is the deviation of the accelerometer and gyroscope at time k; and is the deviation of the accelerometer and gyroscope at time k-1; Expressed as in the navigation coordinate system and The amount of posture change between moments.

[0086] (3-2) The DVL observation data vector is the velocity of the upper right front corresponding to the carrier coordinate system. When the measurement is updated, the observation residual is obtained by subtracting the velocity observation value obtained by IMU. The DVL velocity residual factor is expressed as:

[0087] (twenty four);

[0088] In the formula, since the state variables are in the navigation coordinate system, in order to maintain the coordinate system unity, Represents the coordinate rotation matrix from the carrier coordinate system to the local navigation coordinate system at time k; 、 and is the velocity along the X, Y, and Z directions in the carrier coordinate system calculated by the IMU pre-integration factor at time k, 、 and is the DVL velocity observation value along the X, Y, and Z directions in the carrier coordinate system at time k.

[0089] The corresponding Jacobian relationship matrix is ​​expressed as :

[0090] (25);

[0091] (26);

[0092] (27);

[0093] Where, , is the angular velocity caused by the Earth's rotation and the curvature of the Earth's surface; represents the angular velocity caused by the Earth's rotation; represents the angular velocity due to the curvature of the Earth's surface; is the angular velocity of the Earth's rotation; is the current location latitude; and are the components of velocity in the x and y directions; 、 Respectively The curvature radius of the meridian circle and the curvature radius of the meridian circle where the coordinate point at the time is located; is the tangent of latitude; Indicates The transformation matrix from the time navigation system to the carrier system; represents the lever arm between the inertial sensor and the DVL; represents the angular velocity of the gyroscope; Expressed as The identity matrix of size; Expressed as zero matrix of size; Expressed as A zero matrix of size .

[0094] (4) Sliding window and marginalization.

[0095] Set the IMU pre-integration interval to the same size as the DVL data sampling frequency. , the size of the sliding window The unit is seconds, and through repeated experiments, we can obtain a method that can maintain the local observability of IMU errors and reduce the computational complexity by fixing the window length, thereby achieving a balance between accuracy and real-time performance in dynamic scenes. .have:

[0096] (28);

[0097] In the sliding window framework of factor graph optimization, marginalization is introduced to limit the computational scale while retaining the constraints of historical information on the current state estimation, thereby balancing real-time performance and estimation accuracy.

[0098] (5) Detect gross errors in the DVL velocity residual factor and, if there are gross errors, downgrade the DVL velocity residual at the current moment to obtain a new DVL velocity residual factor.

[0099] Underwater vehicles face complex and demanding environmental challenges, such as deep-sea high pressure and acoustic signal multipath effects. These extreme conditions can easily lead to anomalies in DVL sensor data. If these outliers are not promptly removed, they can severely impact IMU calibration. Therefore, after constructing the DVL velocity residual factors, all residual factors within the sliding window are subjected to gross error detection and weight reduction.

[0100] Calculate the chi-square statistic for the DVL velocity residual factor:

[0101] (29);

[0102] Where, represents the chi-square statistic, Represents the covariance matrix of the DVL velocity residuals.

[0103] According to the significance level , set the critical threshold of the chi-square distribution ;

[0104] (30);

[0105] in, Represents the corresponding time weight established based on the original observation noise, which is used to quantify the reliability of observations at different times; Represents a factor adjusted according to the actual noise level to control the intensity of the noise's impact on the critical value.

[0106] if , it means that the chi-square test passes and it is believed that there is no gross error in the observation value of this epoch; if , it means that the test fails and the DVL speed residual factor at the current moment needs to be downgraded to , by re-weighting to reduce its impact on the optimization results, but if , it means that the current residual is too large, and the observation value at this time is eliminated.

[0107] (6) Establish an error feedback model, and correct the state variables of formula (3) during the subgraph optimization process. After a period of time, the inertial guidance error will converge to a certain value. Through the feedback model, the original IMU observation information and , corrected to and .

[0108] (31);

[0109] (32);

[0110] in, and are the theoretical outputs of the gyroscope and accelerometer in the carrier coordinate system respectively; and are the installation error matrices of the gyroscope and accelerometer, is the installation error coefficient of the three accelerometers, is the installation error coefficient of the three gyroscopes, indicating the influence of the j-axis on the i-axis; and are the scale factor error matrices of the accelerometer and gyroscope, respectively.

[0111] (7) The revised and , which is substituted into the IMU pre-integration calculation online, thereby improving the overall accuracy of the system.

[0112] Example 2

[0113] This embodiment provides a system for calibrating an inertial sensor of a Doppler odometer, including:

[0114] a factor calculation module configured to calculate pre-integrated values ​​of position, velocity, and attitude within a set time interval and bias changes of the gyroscope and accelerometer based on the IMU observation data, thereby obtaining an IMU pre-integrated factor, and to obtain a DVL velocity residual factor based on the IMU pre-integrated factor and the DVL observation data;

[0115] A graph construction module is configured to use the prior information of the initial states of the gyroscope and the accelerometer as a prior factor, the prior factor, the IMU pre-integration factor, and the DVL velocity residual factor as factor nodes, the state variables of the gyroscope and the accelerometer as variable nodes, and the relationship between the factor node and the variable node as an edge to construct a factor graph structure model;

[0116] The calibration module is configured to correct the state variables using a factor graph structure model based on real-time observation data until the inertial guidance error converges to a set value, thereby obtaining the theoretical outputs of the gyroscope and accelerometer after inertial guidance error compensation based on the state vector when the inertial guidance error converges.

[0117] It should be noted that the above modules correspond to the steps described in Example 1, and the examples and application scenarios implemented by the above modules and the corresponding steps are the same, but are not limited to the contents disclosed in the above Example 1. It should be noted that the above modules, as part of the system, can be executed in a computer system such as a set of computer-executable instructions.

[0118] In further embodiments, there is also provided:

[0119] An electronic device includes a memory and a processor, and computer instructions stored in the memory and executed by the processor, wherein when the computer instructions are executed by the processor, the method described in Example 1 is performed. For the sake of brevity, no further details are given here.

[0120] It should be understood that in this embodiment, the processor may be a central processing unit (CPU), or may be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), off-the-shelf field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc.

[0121] The memory may include a read-only memory and a random access memory, and provides instructions and data to the processor. A portion of the memory may also include a non-volatile random access memory. For example, the memory may also store information about the device type.

[0122] A computer-readable storage medium is used to store computer instructions, and when the computer instructions are executed by a processor, the method described in Example 1 is performed.

[0123] The method in Example 1 can be directly implemented as a hardware processor, or can be implemented using a combination of hardware and software modules in the processor. The software module can be located in a storage medium mature in the art, such as random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, etc. The storage medium is located in the memory, and the processor reads the information in the memory and, in conjunction with its hardware, completes the steps of the above method. To avoid repetition, it will not be described in detail here.

[0124] A computer program product includes a computer program, which implements the method described in embodiment 1 when executed by a processor.

[0125] The present invention also provides at least one computer program product tangibly stored on a non-transitory computer-readable storage medium. The computer program product includes computer-executable instructions, such as instructions contained in program modules, which are executed in a device on a real or virtual processor of a target to perform the process / method described above. Generally, program modules include routines, programs, libraries, objects, classes, components, data structures, etc. that perform specific tasks or implement specific abstract data types. In various embodiments, the functionality of program modules can be combined or divided between program modules as needed. The machine-executable instructions for the program modules can be executed in local or distributed devices. In distributed devices, program modules can be located in local and remote storage media.

[0126] The computer program code for implementing the method of the present invention can be written in one or more programming languages. These computer program codes can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device so that when the program code is executed by the computer or other programmable data processing device, the functions / operations specified in the flow chart and / or block diagram are implemented. The program code can be executed entirely on a computer, partially on a computer, as an independent software package, partially on a computer and partially on a remote computer, or entirely on a remote computer or server.

[0127] In the context of the present invention, computer program code or related data can be carried by any appropriate carrier to enable a device, apparatus, or processor to perform the various processes and operations described above. Examples of carriers include signals, computer-readable media, etc. Examples of signals include electrical, optical, radio, acoustic, or other forms of propagation signals, such as carrier waves, infrared signals, etc.

[0128] Those skilled in the art will appreciate that the units and algorithm steps of the various examples described in conjunction with this embodiment can be implemented using electronic hardware or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present invention.

[0129] Although the above describes the specific embodiments of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without any creative work are still within the scope of protection of the present invention.

Claims

1. A method for calibrating an inertial sensor of a Doppler odometer, characterized in that: include: The pre-integrated values ​​of position, velocity, and attitude within a set time interval and the bias changes of the gyroscope and accelerometer are calculated based on the IMU observation data to obtain the IMU pre-integrated factor. The DVL velocity residual factor is obtained based on the IMU pre-integrated factor and the DVL observation data. The prior information of the initial state of the gyroscope and accelerometer is used as the prior factor, the prior factor, the IMU pre-integration factor and the DVL velocity residual factor are used as factor nodes, the state variables of the gyroscope and accelerometer are used as variable nodes, and the relationship between the factor node and the variable node is used as the edge to construct a factor graph structure model; Based on real-time observation data, the factor graph structure model is used to correct the state variables until the inertial guidance error converges to the set value. The theoretical outputs of the gyroscope and accelerometer after inertial guidance error compensation are obtained based on the state vector when the inertial guidance error converges. Among them, if factor graph optimization is introduced into the fusion navigation of the long baseline underwater positioning system and the inertial navigation system, the corresponding variable nodes are the required state variables, and a 27-dimensional state vector is constructed in the local navigation coordinate system, including the three-axis (e, n, u axis) position coordinates, velocity and attitude in the northeast celestial coordinate system of the base station, the accelerometer and gyroscope zero bias errors in the X, Y, and Z directions in the carrier coordinate system, the gyroscope and accelerometer scale factor error coefficients in the X, Y, and Z directions in the carrier coordinate system, and the gyroscope and accelerometer installation error coefficients in the X, Y, and Z directions in the carrier coordinate system; Among them, if the bias estimate changes, a bias update must be added, and the updated accelerometer bias and gyroscope bias Recalculate the corrected pre-integration observation value, and the IMU pre-integration factor is expressed as: ； Where, Represents the IMU pre-integration factor; is a state variable; and Respectively expressed in Moment and The amount of change in the accelerometer and gyroscope bias between moments; For Moment and The change in displacement between moments; For Moment and The amount of change in velocity between moments; 、 and They are respectively expressed as the position change, velocity change and attitude change based on the zero bias update; and Respectively Moment and Time navigation coordinate system The corresponding posture below; In the navigation coordinate system and The amount of posture change between moments; After the DVL speed residual factor is obtained, gross error detection is performed on the DVL speed residual factor, and when a gross error exists, the DVL speed residual at the current moment is downgraded to obtain a new DVL speed residual factor; Specifically include: calculating the chi-square statistic for the DVL velocity residual factor , set the critical threshold of the chi-square distribution ;like , indicating that the chi-square test passes; if , then the DVL speed residual factor at the current moment is downgraded. If , then the current observation value will be eliminated; Indicates the factor adjusted according to the actual noise level.

2. The method for calibrating an inertial sensor of a Doppler odometer according to claim 1, wherein: The process of obtaining the IMU pre-integration factor includes: The IMU observation data within a set time interval is integrated to obtain the changes in displacement, velocity and attitude; the changes in displacement, velocity and attitude based on the bias update are calculated based on the gyroscope bias and accelerometer bias; the bias changes of the gyroscope and accelerometer are obtained according to the deviations of the gyroscope and accelerometer at adjacent moments; the IMU pre-integration factor is obtained according to the changes in displacement, velocity and attitude and the changes based on the bias update and the bias changes of the gyroscope and accelerometer.

3. The method for calibrating an inertial sensor of a Doppler odometer according to claim 1, wherein: The DVL velocity residual factor is expressed as: ； Where, represents the DVL velocity residual factor; is a state variable; Represents the coordinate rotation matrix from the carrier coordinate system to the local navigation coordinate system at time k; 、 and is the velocity on the xyz axis calculated by the IMU pre-integration factor at time k, 、 and is the DVL velocity observation value on the xyz axis at time k.

4. The method for calibrating an inertial sensor of a Doppler odometer according to claim 1, wherein: The theoretical outputs of the gyroscope and accelerometer are: ； ； in, and are the theoretical outputs of the gyroscope and accelerometer in the carrier coordinate system respectively; and are the installation error matrices of the gyroscope and accelerometer respectively; and are the scale factor error matrices of the accelerometer and gyroscope respectively; and are the specific force and angular rate measured by IMU respectively; and are the biases of the accelerometer and gyroscope, respectively.

5. A system for calibrating an inertial sensor of a Doppler odometer, characterized in that: include: a factor calculation module configured to calculate pre-integrated values ​​of position, velocity, and attitude within a set time interval and bias changes of the gyroscope and accelerometer based on the IMU observation data, thereby obtaining an IMU pre-integrated factor, and to obtain a DVL velocity residual factor based on the IMU pre-integrated factor and the DVL observation data; A graph construction module is configured to use the prior information of the initial states of the gyroscope and the accelerometer as a prior factor, the prior factor, the IMU pre-integration factor, and the DVL velocity residual factor as factor nodes, the state variables of the gyroscope and the accelerometer as variable nodes, and the relationship between the factor node and the variable node as an edge to construct a factor graph structure model; The calibration module is configured to correct the state variables using a factor graph structure model based on real-time observation data until the inertial guidance error converges to a set value, thereby obtaining the theoretical outputs of the gyroscope and accelerometer after inertial guidance error compensation based on the state vector when the inertial guidance error converges; Among them, if factor graph optimization is introduced into the fusion navigation of the long baseline underwater positioning system and the inertial navigation system, the corresponding variable nodes are the required state variables, and a 27-dimensional state vector is constructed in the local navigation coordinate system, including the three-axis (e, n, u axis) position coordinates, velocity and attitude in the northeast celestial coordinate system of the base station, the accelerometer and gyroscope zero bias errors in the X, Y, and Z directions in the carrier coordinate system, the gyroscope and accelerometer scale factor error coefficients in the X, Y, and Z directions in the carrier coordinate system, and the gyroscope and accelerometer installation error coefficients in the X, Y, and Z directions in the carrier coordinate system; Among them, if the bias estimate changes, a bias update must be added, and the updated accelerometer bias and gyroscope bias Recalculate the corrected pre-integration observation value, and the IMU pre-integration factor is expressed as: ； Where, Represents the IMU pre-integration factor; is a state variable; and Respectively expressed in Moment and The amount of change in the accelerometer and gyroscope bias between moments; For Moment and The change in displacement between moments; For Moment and The amount of change in velocity between moments; 、 and They are respectively expressed as the position change, velocity change and attitude change based on the zero bias update; and Respectively Moment and Time navigation coordinate system The corresponding posture below; In the navigation coordinate system and The amount of posture change between moments; After the DVL speed residual factor is obtained, gross error detection is performed on the DVL speed residual factor, and when a gross error exists, the DVL speed residual at the current moment is downgraded to obtain a new DVL speed residual factor; Specifically include: calculating the chi-square statistic for the DVL velocity residual factor , set the critical threshold of the chi-square distribution ;like , indicating that the chi-square test passes; if , then the DVL speed residual factor at the current moment is downgraded. If , then the current observation value will be eliminated; Indicates the factor adjusted according to the actual noise level.

6. An electronic device, characterized in that: The method comprises a memory and a processor, and computer instructions stored in the memory and executed on the processor, wherein when the computer instructions are executed by the processor, the method according to any one of claims 1 to 4 is completed.

7. A computer-readable storage medium, characterized in that Used to store computer instructions, which, when executed by a processor, complete the method according to any one of claims 1 to 4.

8. A computer program product, characterized in that The invention comprises a computer program, which is used to implement the method according to any one of claims 1 to 4 when executed by a processor.

Images