Method and system for calibrating inertial sensor by Doppler log

Abstract

The invention discloses a method and system for calibrating an inertial sensor by a Doppler log, and relates to the technical field of underwater integrated navigation, and the method comprises the following steps: obtaining an IMU pre-integration factor and a DVL speed residual factor according to IMU observation data and DVL observation data; the method comprises the following steps: constructing a factor graph structure model by taking prior information of initial states of a gyroscope and an accelerometer as prior factors, taking the prior factors, an IMU pre-integration factor and a DVL speed residual factor as factor nodes, taking state variables of the gyroscope and the accelerometer as variable nodes and taking a relationship between the factor nodes and the variable nodes as edges; and correcting the state variables by using the factor graph structure model to obtain theoretical outputs of the gyroscope and the accelerometer after inertial navigation error compensation. Observed DVL data are used for carrying out online calibration on IMU errors, an underwater integrated navigation problem is modeled as nonlinear least square optimization, global optimal estimation of all-time state quantities is achieved, and the divergence speed of inertial navigation position errors is restrained.

Description

Technical Field

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

[0002] In application scenarios such as underwater vehicles, the accuracy of an Inertial Measurement Unit (IMU) plays a crucial role in navigation accuracy. The IMU relies on internal gyroscopes and accelerometers to measure motion parameters. However, errors such as gyro drift and accelerometer zero bias will accumulate over time, resulting in a significant decline in the accuracy of attitude, velocity, and position information over time, and unable to meet the requirement of providing long-term navigation and positioning for underwater vehicles. And the Global Navigation Satellite System (GNSS) based on radio signals cannot be directly applied to the underwater environment due to the rapid attenuation of electromagnetic waves in seawater.

[0003] Currently, sonar signals are widely used in underwater ranging. In an acoustic positioning system (such as a Long Baseline (LBL) positioning system), sonar signals can measure the distance from an underwater vehicle to a seabed reference point for navigation and positioning of the vehicle; however, it relies on a seabed reference station for absolute position calibration, which not only increases the system construction and maintenance costs, but is also limited by seabed topography and water quality conditions. The construction cost of the seabed reference station network is high and it is difficult to achieve full coverage. In addition, the acoustic wave propagation distance is limited. In a complex marine environment (such as areas with high turbidity and complex topography), the signal is easily interfered and weakened or distorted, resulting in limited positioning range and accuracy. This dependence on external infrastructure and limitations of propagation characteristics directly affect the application effect of sonar in a large range and complex environment. Therefore, calibrating an inertial sensor with sonar has disadvantages such as high cost and limited range.

[0004] Aiming at the disadvantages of sonar calibration methods, calibrating an IMU with a Doppler Velocity Logger (DVL) can solve the problems of high cost and limited range. However, there are two major bottlenecks in traditional filtering methods: the DVL is prone to speed measurement failure when the water body is turbid, the seabed topography is complex, or the vehicle is moving at high speed, resulting in the inaccuracy of the filtering model; the Kalman filter is only based on the state estimation at the current moment and cannot backtrack and correct historical errors, and the navigation accuracy drops sharply when the DVL signal is interrupted for a long time. Summary of the Invention

[0005] To solve the above problems, the present invention proposes a method and system for calibrating an inertial sensor using a Doppler log. By using the observed DVL data to calibrate the IMU error online, the underwater integrated navigation problem is modeled as a non - linear least - squares optimization. Through constructing a factor graph structure model that includes IMU pre - integration factors, DVL velocity residual factors, a sliding window, and DVL velocity residual constraints, the global optimal estimation of state variables at all times is realized, and the divergence rate of the inertial navigation position error is suppressed.

[0006] To achieve the above object, the present invention adopts the following technical solutions: In a first aspect, the present invention provides a method for calibrating an inertial sensor using a Doppler log, including: Calculating the pre - integration values of position, velocity, and attitude, as well as the bias changes of the gyroscope and accelerometer within a set time interval according to the IMU observation data, thereby obtaining the IMU pre - integration factors, and obtaining the DVL velocity residual factors according to the IMU pre - integration factors and DVL observation data; Taking the prior information of the initial states of the gyroscope and accelerometer as prior factors, using the prior factors, IMU pre - integration factors, and DVL velocity residual factors as factor nodes, using the state variables of the gyroscope and accelerometer as variable nodes, and using the relationships between the factor nodes and variable nodes as edges to construct a factor graph structure model; According to the real - time observation data, using the factor graph structure model to correct the state variables until the inertial navigation error converges to a set value, and thereby obtaining the theoretical outputs of the gyroscope and accelerometer after inertial navigation error compensation according to the state vector when the inertial navigation error converges.

[0007] In a second aspect, the present invention provides a system for calibrating an inertial sensor using a Doppler log, including: A factor calculation module configured to calculate the pre - integration values of position, velocity, and attitude, as well as the bias changes of the gyroscope and accelerometer within a set time interval according to the IMU observation data, thereby obtaining the IMU pre - integration factors, and obtaining the DVL velocity residual factors according to the IMU pre - integration factors and DVL observation data; A graph construction module configured to take the prior information of the initial states of the gyroscope and accelerometer as prior factors, use the prior factors, IMU pre - integration factors, and DVL velocity residual factors as factor nodes, use the state variables of the gyroscope and accelerometer as variable nodes, and use the relationships between the factor nodes and variable nodes as edges to construct a factor graph structure model; A calibration module configured to correct the state variables according to the real - time observation data using the factor graph structure model until the inertial navigation error converges to a set value, and thereby obtaining the theoretical outputs of the gyroscope and accelerometer after inertial navigation error compensation according to the state vector when the inertial navigation error converges.

[0008] In a third aspect, the present invention provides an electronic device, including a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the method described in the first aspect is completed.

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

[0010] In a fifth aspect, the present invention provides a computer program product, including a computer program. When the computer program is executed by a processor, the method described in the first aspect is implemented.

[0011] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention proposes a method and system for calibrating an inertial sensor with a Doppler log, models the navigation problem as a non-linear least squares optimization, and realizes the global optimal estimation of state quantities at all times through a factor graph framework that includes an IMU pre-integration factor, a DVL velocity residual factor, and a sliding window marginalization and gross error detection and downweighting optimization method. The graph optimization method has good scalability and flexibility. In practical applications, as the measurement data continuously increases and the system complexity improves, the graph optimization model can be easily extended and adjusted to adapt to new measurement constraints and changes in system characteristics. In addition, the graph optimization is highly robust to the noise in the DVL velocity observation data, and can reduce the impact of the noise on the estimation result through reasonable weight allocation, so as to still achieve high-precision calibration of inertial sensor errors in a complex noise environment.

[0012] Based on the graph optimization method, the present invention calibrates the inertial sensor with a Doppler log, aims to overcome the deficiencies of traditional filtering methods, effectively improves the calibration accuracy of the inertial sensor, and further improves the navigation performance and reliability of systems such as underwater vehicles. It can effectively suppress the divergence speed of inertial sensor errors, improve the accuracy, increase the maintenance time of the inertial sensor position error divergence by 34.7% for one kilometer, and improve the reliability of the underwater vehicle.

[0013] The advantages of the additional aspects of the present invention will be partly given in the following description, partly become obvious from the following description, or be understood through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained according to the provided drawings without creative efforts.

[0015] Figure 1 This is the flowchart of the method for calibrating an inertial sensor using a Doppler log provided in Embodiment 1 of the present invention; Figure 2 This is the schematic diagram of the method for calibrating an inertial sensor using a Doppler log provided in Embodiment 1 of the present invention. Detailed implementation manners

[0016] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0017] It should be noted that the following detailed description is exemplary and is 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 of ordinary skill in the technical field to which the present invention belongs.

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

[0019] In the case of no conflict, the embodiments in the present invention and the features in the embodiments may be combined with each other.

[0020] Embodiment 1 In an underwater environment, the attitude and position estimation of an IMU are easily affected by cumulative errors, while a DVL can provide very accurate velocity, which helps to calibrate the inertial sensor errors. Therefore, this embodiment proposes a method for calibrating an inertial sensor using a Doppler log, online calibrating the IMU errors using the observed DVL data, modeling the underwater integrated navigation problem as a non-linear least squares optimization, and realizing the global optimal estimation of the state quantities at all times by constructing a factor graph structure model including IMU pre-integration factors, DVL velocity residual factors, a sliding window, and DVL velocity residual constraints, and suppressing the divergence speed of the inertial navigation position error.

[0021] Combined with Figure 1 - Figure 2 The detailed steps of the method in this embodiment will be described. Among them, Figure 2To describe the relationship between state variable nodes and IMU pre-integration factors and DVL velocity residual factor nodes using the common bipartite graph representation of factor graphs, as well as the specific implementation methods of processes such as sliding window, marginalization, and gross error detection and downweighting optimization in the factor graph optimization process.

[0022] (1) Construct a factor graph structure model for calibrating IMU errors based on DVL.

[0023] A factor graph decomposes a multi-variable global function into a product of multiple local functions. Each local function depends on a subset of variables, and this decomposition can be represented by a bipartite graph. The transfer relationship between system states is intuitively represented by the factor graph.

[0024] A factor graph is a bipartite graph, denoted by and includes variable nodes , factor nodes and edges ; among them, variable nodes refer to the variables in the global multivariate function; factor nodes refer to the local functions in the factorization, expressed as: ; is the corresponding cost function, is the actual observed data; edge exists only when is associated with .

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

[0026] The corresponding variable nodes are the required state variables , so the set of all state variables received at time is expressed as: .

[0027] The set of all observed values received at time is expressed as: ; represents the actual observed value obtained by different sensors at time, is the IMU observed data;

[0028] If the observed data at each time node is independent, the global function is factorized according to Bayesian estimation, and all state variables in the k-th time node and the observed data The joint posterior probability density is: (1); In the formula, represents the prior information of the initial state of all state variables; represents the state transition model deduced by the IMU, are the i-th and (i - 1)-th state variables, represents the DVL observation update model, is the j-th set of the i-th variable node in the DVL observation update model, is the j-th DVL observation data.

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

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

[0031] According to the maximum a posteriori probability estimation, the problem of calculating the optimal value of the state variable from the observed data is transformed into an equivalent least squares optimization problem, that is: (2); Among them, represents the error function constructed from the prior information, usually expressed as the difference between the observed value and the mean; represents the square of the 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 can be obtained by linear expansion through Taylor expansion ; is the most likely estimated value found through optimization, that is, the maximum a posteriori estimation; is the prior error term, usually based on the known prior signal information, representing the influence of a certain prior 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; represents the summation of all variables for the initial state to optimize the state; represents the observation model function in the initial state, usually the mapping relationship between the state and the observed value; represents the weighted squared error, used to represent the relationship between the measurement error and the covariance of the observation noise; is the observed value of the IMU; is the observed value of the DVL.

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

[0033] (2) Construct the state variables .

[0034] If the factor graph optimization is introduced into the integrated navigation of the long - baseline underwater positioning system and the inertial navigation system, the corresponding variable nodes are the required state variables. The state variable at time is represented as a 27 - dimensional state vector constructed in the local navigation frame: where, , and respectively represent the position coordinates, velocity, and attitude of the 3 - axis (e, n, u axes) in the navigation system at time and respectively represent the zero - bias errors of the accelerometers and gyroscopes along the X, Y, and Z directions in the vehicle coordinate system; and are respectively the scale - factor error coefficients of the gyroscopes and accelerometers along the X, Y, and Z directions in the vehicle coordinate system; and are respectively the installation error coefficients of the gyroscopes and accelerometers along the X, Y, and Z directions in the vehicle coordinate system.

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

[0036] The output frequency of IMU information is usually much higher than that of DVL. If the factor graph is updated every time the IMU outputs information, it will not only introduce a large amount of redundant information, but also significantly increase the computational burden of the system, thus affecting the real-time performance of the system. To alleviate this problem, this embodiment adopts the IMU pre-integration method, that is: integrating the IMU observation data within a period of time into a pre-integration factor node, and then updating the state of the inertial sensor system.

[0037] Receiving IMU observation data at each epoch is: (4); (5); (6); In the formula, represents the vehicle coordinate system, and are the specific force and angular rate measured by the IMU respectively; and are the ideal outputs of the gyroscope and accelerometer in the vehicle coordinate system respectively; and are the biases of the accelerometer and gyroscope respectively, and are the noise terms of the gyroscope and accelerometer respectively, and are generally considered to conform to the Gaussian noise distribution.

[0038] According to the INS kinematic model, the differential equations of position, velocity, and attitude with respect to time are: (7); (8); (9); In the formula, represents the navigation coordinate system, defined as the "east-north-up" geographic coordinate system, represents the velocity of the vehicle in the navigation coordinate system; represents the position of the vehicle in the navigation coordinate system; is the time derivative of the position , that is, the velocity; is the time derivative of the velocity , that is, the acceleration; represents the cross product operation; and represent the earth's angular velocity of rotation and the gravity vector in the rad / s, is the earth's angular velocity of rotation at the current position; Describe the change of the carrier attitude; Represent the current attitude quaternion; Represent the rotation matrix, which is the rotation matrix of the system with respect to the system; is the gyroscope angular rate measurement value; L represents the current geographical latitude.

[0039] Through the differential equations of position, velocity, and attitude with respect to time, a kinematic equation within an IMU time interval is obtained: (10); (11); (12); In the formula, and represent the velocities at and moments respectively, and are the positions at and moments respectively; and are the attitudes at and moments respectively; represents the change of the system within the time interval; is the velocity change amount related to the specific force measurement, called the specific force integration term; is the velocity change amount related to the Coriolis acceleration and gravity, also known as the gravity / Coriolis integration term; represents the correction of the attitude quaternion due to the Earth's rotation during ;

[0040] In the classical INS calculation, the state of the coordinate system at the moment is obtained by recursively deriving the state at the moment. When directly performing iterative updates, multiple state transfers will result in a huge amount of calculation. By using IMU pre-integration to reduce the calculation amount, the IMU data within a time interval is integrated to obtain the pre-integration quantity: (14); (15); where: (16); (17); Wherein, represents the correction of the position due to gravity, Coriolis force, etc. considered between the moment and the moment; is the correction of the velocity considering the rotation of the earth between the moment and the moment; is the displacement change between the moment and the moment; is the velocity change between the moment and the moment; is the attitude change between the and the moment; is the rotation matrix from the current IMU coordinate system to the coordinate system at the initial moment ; is the specific force measured by the IMU in the current IMU coordinate system ; and respectively represent the moment and the moment, and the corresponding positions in the navigation coordinate system ; and represent the moment and the moment, and the corresponding velocities in the navigation coordinate system ; is the coordinate transformation matrix from the navigation coordinate system at the moment to the vehicle coordinate system b; ; represents the attitude change of an IMU from to between the and sampling epochs; and respectively represent the moment and the moment, and the corresponding attitudes in the navigation coordinate system ; is the gravity vector in the navigation coordinate system; is the attitude change between the and moments in the navigation coordinate system.

[0041] To simplify the calculation, it is usually assumed that the inertial navigation zero bias remains unchanged within the integration interval. During the factor graph optimization process, if the bias estimation changes, a bias update must be added, and the updated accelerometer bias and the gyroscope zero bias are used to recalculate the corrected pre-integration observations.

[0042] (18); (19); (20); Among them, represents the Jacobian matrix of displacement with respect to the gyroscope zero bias; represents the Jacobian matrix of displacement with respect to the accelerometer zero bias; and represent the Jacobian matrices of velocity with respect to the gyroscope and accelerometer zero biases; represents a Lie group operation that maps the Jacobian vector to the tangent space to generate the corrected quaternion increment; represents the Jacobian matrix of attitude with respect to the gyroscope zero bias; is the gyroscope zero bias that remains unchanged within the integration interval at the current moment; represents the accelerometer zero bias that remains unchanged within the integration interval at the current moment; represents the change in the gyroscope zero bias; represents the change in the accelerometer zero bias; represents the position change of the original pre-integration based on the and that remain unchanged within the integration interval; represents the original velocity pre-integration based on the and that remain unchanged within the integration interval; represents the attitude pre-integration based on the and that remain unchanged within the integration interval; , and represent the position change, velocity change, and attitude change after adding the zero bias update, respectively.

[0043] The IMU residual vector, that is, the IMU pre-integration factor is expressed as: (21); (22); (23); In the formula, represents the IMU pre-integration factor; and respectively represent the bias change amounts of the accelerometer and the gyroscope between and moments; and are the biases of the accelerometer and the gyroscope at the k-th moment; and are the biases of the accelerometer and the gyroscope at the (k - 1)-th moment; represents the attitude change amount between and moments in the navigation coordinate system.

[0044] (3 - 2) The observation data vector of the DVL is the corresponding forward - right - up velocity in the body coordinate system. When performing measurement update, the observation residual is obtained by subtracting the velocity observation value calculated from the IMU. The DVL velocity residual factor is expressed as: (24); In the formula, since the state variables are in the navigation coordinate system, to maintain the unity of the coordinate system, represents the coordinate rotation matrix from the body coordinate system to the local navigation coordinate system at the k - th moment; , and are the velocities in the X, Y, and Z directions in the body coordinate system calculated from the IMU pre - integration factor at the k - th moment, , and are the DVL velocity observation values in the X, Y, and Z directions in the body coordinate system at the k - th moment.

[0045] The corresponding Jacobian relation matrix is expressed as : (25); (26); (27); In the formula, , is the angular velocity caused by the earth's rotation and the earth's surface curvature; represents the angular velocity caused by the earth's rotation; represents the angular velocity caused by the earth's surface curvature; is the earth's angular velocity of rotation; is the current position latitude; and are the components of the velocity in the x and y directions; , respectively represent the radius of curvature of the prime vertical and the radius of curvature of the meridian at the coordinate point at the is the tangent of the latitude; represents the transformation matrix from the navigation system to the vehicle system at moment; represents the lever arm between the inertial sensor and the DVL; represents the angular velocity of the gyroscope; is represented as the identity matrix of size is represented as the zero matrix of size is represented as the zero matrix of size

[0046] (4)Sliding window and marginalization.

[0047] Set the IMU pre-integration interval to be the same size as the DVL data sampling frequency , and the size of the sliding window is in seconds, and through repeated experiments, the value that can not only maintain the local observability of the IMU error but also reduce the computational complexity by fixing the window length is obtained, so as to achieve the balance between accuracy and real-time performance in dynamic scenarios . There is: (28); In the sliding window framework of factor graph optimization, marginalization is introduced to limit the computational scale while retaining the constraint of historical information on the current state estimation, so as to balance real-time performance and estimation accuracy.

[0048] (5)Detect the gross error of the DVL velocity residual factor, and after downweighting the DVL velocity residual at the current moment when there is a gross error, a new DVL velocity residual factor is obtained.

[0049] In the underwater environment, the vehicle faces complex and harsh environmental challenges, such as extreme conditions like deep-sea high pressure and acoustic signal multipath effects, which can easily lead to abnormal DVL sensor data. If these outliers are not removed in time, it will seriously affect the IMU calibration effect. Therefore, after constructing the DVL velocity residual factor, gross error detection and downweighting processing are performed on all residual factors within the sliding window.

[0050] Calculate the chi-square statistic for the DVL velocity residual factor: (29); In the formula, represents the chi-square statistic, represents the covariance matrix of the DVL velocity residual.

[0051] According to the significance level of , set the critical threshold of the chi-square distribution; (30); Among them, represents the weight at the corresponding moment established based on the original observation noise, and is used to quantify the reliability of the observed values at different moments; represents the factor adjusted according to the actual noise level, and is used to control the influence intensity of the noise on the critical value.

[0052] If , it means that the chi-square test passes, and it is considered that there is no gross error in the observed value of this epoch; if , it means that the test fails, and the weight of the DVL velocity residual factor at the current moment needs to be reduced to , and its influence on the optimization result is reduced by reweighting. However, if , it means that the current residual is too large, and the observed value at this time is removed. (6) Establish an error feedback model, correct the state variables in Equation (3) during the optimization process of the figure subgraph. After a period of time, the inertial navigation error will converge to a certain value. Through the feedback model, the original IMU observation information and are corrected to and .

[0053] (31); (32); Among them, and are the theoretical outputs of the gyroscope and accelerometer in the vehicle coordinate system respectively; and are the installation error matrices of the gyroscope and accelerometer respectively, 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. (7) Substitute the corrected and online into the IMU pre-integration calculation, so as to improve the overall accuracy of the system.

[0054] Embodiment 2 This embodiment provides a system for calibrating an inertial sensor with a Doppler log, including: A factor calculation module, configured to calculate pre-integrated values of position, velocity, and attitude, as well as bias variations of a gyroscope and an accelerometer within a set time interval based on IMU observation data, thereby obtaining an IMU pre-integration factor, and obtaining a DVL velocity residual factor based on the IMU pre-integration factor and DVL observation data; A graph construction module, configured to use prior information on the initial states of the gyroscope and the accelerometer as prior factors, construct a factor graph structure model with the prior factors, 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 relationships between the factor nodes and the variable nodes as edges; A calibration module, configured to correct the state variables according to real-time observation data by using the factor graph structure model until the inertial navigation error converges to a set value, and thereby obtain the theoretical outputs of the gyroscope and the accelerometer after inertial navigation error compensation based on the state vector when the inertial navigation error converges.

[0055] It should be noted here that the above modules correspond to the steps described in Embodiment 1. The examples and application scenarios implemented by the above modules and the corresponding steps are the same, but are not limited to the content disclosed in the above Embodiment 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.

[0056] In more embodiments, there is also provided: An electronic device, including a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the method described in Embodiment 1 is completed. For the sake of brevity, it will not be elaborated here.

[0057] It should be understood that in this embodiment, the processor may be a central processing unit CPU, and the processor may also be other general-purpose processors, digital signal processors DSP, application-specific integrated circuits ASIC, off-the-shelf 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 the processor may also be any conventional processor, etc.

[0058] The memory may include a read-only memory and a random access memory, and provide instructions and data to the processor. A part of the memory may also include a non-volatile random access memory. For example, the memory may also store information on the device type.

[0059] A computer-readable storage medium, used to store computer instructions. When the computer instructions are executed by the processor, the method described in Embodiment 1 is completed.

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

[0061] A computer program product includes a computer program that, when executed by a processor, implements the method described in Embodiment 1.

[0062] 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 included in program modules, which are executed in a device on a target real or virtual processor to perform the process / method as 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 functions of program modules can be combined or divided as needed. The machine-executable instructions for program modules can be executed within local or distributed devices. In a distributed device, program modules can be located in local and remote storage media.

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

[0064] In the context of the present invention, the computer program code or related data can be carried by any suitable carrier so that the device, apparatus, or processor can perform the various processes and operations described above. Examples of carriers include signals, computer-readable media, etc. Examples of signals can include electrical, optical, radio, sound, or other forms of propagated signals, such as carrier waves, infrared signals, etc.

[0065] Those of ordinary skill in the art can realize that the units and algorithm steps of the examples described in conjunction with this embodiment can be implemented by electronic hardware or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods for each specific application to implement the described functions, but such implementation should not be considered to exceed the scope of the present invention.

[0066] Although the specific implementation manners of the present invention have been described above in conjunction with the accompanying drawings, it is not a limitation to the protection scope of the present invention. Those skilled in the art should understand that, based on the technical solution of the present invention, various modifications or deformations that can be made by those skilled in the art without creative efforts are still within the protection scope of the present invention.

Claims

1. A method for calibrating an inertial sensor with a Doppler log, characterized in that Including: Calculating pre-integrated values of position, velocity, and attitude, as well as bias variations of the gyroscope and accelerometer within a set time interval based on IMU observation data to obtain IMU pre-integration factors, and obtaining DVL velocity residual factors based on the IMU pre-integration factors and DVL observation data; Taking the prior information of the initial states of the gyroscope and accelerometer as prior factors, using the prior factors, IMU pre-integration factors, and DVL velocity residual factors as factor nodes, the state variables of the gyroscope and accelerometer as variable nodes, and the relationships between the factor nodes and variable nodes as edges to construct a factor graph structure model; According to real-time observation data, using the factor graph structure model to correct the state variables until the inertial navigation error converges to a set value, and thus obtaining the theoretical outputs of the gyroscope and accelerometer after inertial navigation error compensation based on the state vector when the inertial navigation error converges.

2. The method for calibrating an inertial sensor of a Doppler log according to claim 1, wherein The process of obtaining the IMU pre-integration factors includes: Integrating the IMU observation data within a set time interval to obtain the variations of displacement, velocity, and attitude; calculating the variations of displacement, velocity, and attitude updated based on zero bias based on the gyroscope zero bias and accelerometer zero bias; respectively obtaining the bias variations of the gyroscope and accelerometer according to the biases of the gyroscope and accelerometer at adjacent times; obtaining the IMU pre-integration factors according to the variations of displacement, velocity, and attitude, the variations updated based on zero bias, and the bias variations of the gyroscope and accelerometer.

3. The method for calibrating an inertial sensor of a Doppler log according to claim 2, wherein The IMU pre-integration factor is expressed as: ； Wherein, represents the IMU pre-integration factor; is the state variable; and respectively represent the bias changes of the accelerometer and gyroscope between moment and moment; is the displacement change between moment and moment; is the velocity change between moment and moment; , and respectively represent the position change, velocity change and attitude change based on zero-bias update; and respectively represent moment and moment, the attitude corresponding to the navigation coordinate system ; is the attitude change between and moment in the navigation coordinate system.

4. The method for calibrating an inertial sensor of a Doppler log according to claim 1, wherein The DVL velocity residual factor is expressed as: ； In the formula, represents the DVL velocity residual factor; is the state variable; represents the coordinate rotation matrix from the vehicle coordinate system to the local navigation coordinate system at time k; , and are the velocities in the x, y, and z axes calculated from the IMU pre-integration factors at time k, , and are the DVL velocity observations in the x, y, and z axes at time k.

5. The method for calibrating an inertial sensor of a Doppler log according to claim 1, characterized in that, After obtaining the DVL velocity residual factor, performing gross error detection on the DVL velocity residual factor, and when there are gross errors, down-weighting the DVL velocity residual at the current moment to obtain a new DVL velocity residual factor; Specifically, it includes calculating the chi-square statistic for the DVL velocity residual factor , setting the critical threshold of the chi-square distribution ; if , it indicates that the chi-square test passes; if , then weight down the DVL velocity residual factor at the current moment. If , then reject the current observation value; represents the factor adjusted according to the actual noise level.

6. The method for calibrating an inertial sensor of a Doppler log according to claim 1, characterized in that, The theoretical outputs of the gyroscope and accelerometer are respectively: ； ； Wherein, and are the theoretical outputs of the gyroscope and accelerometer in the vehicle 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 the IMU, respectively; and are the biases of the accelerometer and gyroscope, respectively.

7. A system for calibrating an inertial sensor with a Doppler log, characterized in that, Including: A factor calculation module configured to calculate pre-integrated values of position, velocity, and attitude, as well as bias variations of the gyroscope and accelerometer within a set time interval based on IMU observation data to obtain IMU pre-integration factors, and obtain DVL velocity residual factors based on the IMU pre-integration factors and DVL observation data; A graph construction module configured to take the prior information of the initial states of the gyroscope and accelerometer as prior factors, use the prior factors, IMU pre-integration factors, and DVL velocity residual factors as factor nodes, the state variables of the gyroscope and accelerometer as variable nodes, and the relationships between the factor nodes and variable nodes as edges to construct a factor graph structure model; A calibration module configured to correct the state variables using the factor graph structure model according to real-time observation data until the inertial navigation error converges to a set value, and thus obtaining the theoretical outputs of the gyroscope and accelerometer after inertial navigation error compensation based on the state vector when the inertial navigation error converges.

8. An electronic device, characterized in that, Including a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the method according to any one of claims 1-6 is completed.

9. A computer-readable storage medium, characterized in that, For storing computer instructions, when the computer instructions are executed by the processor, the method according to any one of claims 1-6 is completed.

10. A computer program product, characterized in that, Comprising a computer program which, when executed by a processor, implements the method according to any one of claims 1-6.

Images