A DVL calibration method, system, medium, device and terminal

Abstract

The present application belongs to the technical field of underwater integrated navigation, and discloses a DVL calibration method, system, medium, equipment and terminal. When an AUV is in a water surface navigation state and can receive GNSS signals, a Kalman filter optimal estimation method is used for SINS / GNSS integrated navigation to obtain high-precision attitude and speed information of the carrier as reference values. The speed values in the d system measured by the DVL are integrated, the scale factor error and installation error angle are estimated by a calibration algorithm, and the DVL output is compensated. The present application proposes a Davenport quaternion method based on position observation information, and applies it to a DVL error calibration system. The effectiveness of the method is verified through a shipborne test. Under simple and complex different maneuvering conditions, the method has advantages in accuracy and stability, provides accurate speed information for subsequent underwater carriers for integrated navigation, and has good engineering application value.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater integrated navigation, and particularly relates to a DVL calibration method, system, medium, device and terminal. BACKGROUND

[0002] At present, autonomous underwater vehicles (AUVs) are widely used in national sea defense construction, marine resource exploration and other military and civilian fields, and high-precision and high-reliability autonomous navigation capability is a prerequisite for AUVs to successfully complete tasks. The strapdown inertial navigation system (SINS) is widely concerned in the field of underwater positioning, navigation and timing (PNT) due to its simple structure, small size, strong concealment and convenience for integrated design with other devices, and becomes an important means for AUV autonomous navigation and positioning. However, the SINS is essentially an integral calculation system based on Newton's second law, and needs to integrate the output of the inertial measurement unit (IMU), which leads to the accumulation of navigation errors over time. The underwater environment has a shielding effect on global navigation satellite system (GNSS) signals, while the doppler velocity log (DVL) has stable velocity measurement accuracy and can provide real-time external velocity auxiliary fusion information for the SINS, so the SINS / DVL integrated navigation has become one of the mainstream solutions to the underwater navigation problem.

[0003] The DVL measures the velocity of the AUV relative to the water bottom or water flow through the acoustic Doppler effect. In engineering practice, due to the limitations of manufacturing process, external environment, installation deviation and other conditions, the velocity measurement error of the DVL is difficult to avoid. Therefore, on the basis of pre-calibration of the DVL error parameters, it is of great significance to improve the accuracy of the SINS / DVL integrated navigation system to ensure the accuracy of the velocity measurement information by real-time compensation of the original output of the DVL based on the calibration results.

[0004] The existing DVL calibration methods are roughly divided into three categories: (1) calibration method based on emerging machine learning theory, the intelligent algorithms such as genetic algorithm, PSO, SVR are used to predict the DVL error parameters, although the calibration accuracy is improved, but a large amount of offline data and powerful computing ability are needed, which is difficult to meet the actual needs of engineering application at present; (2) calibration method without external information assistance, the carrier velocity error, position error, acceleration error and other self-information are used as observation and the DVL error is estimated by least square method, although this method is helpful to improve the concealment, but it needs to set a complex maneuvering trajectory, and the efficiency and accuracy of calibration are affected to a certain extent; (3) calibration method with external information assistance, the external sensors such as GNSS are used to provide external auxiliary navigation information, and the DVL error is calibrated by using Kalman filtering, convex optimization and other theories, the research results of this method are relatively abundant, and the system is relatively mature, which is the commonly used calibration method at present, but the system stability and accuracy of the method still need to be further improved. Aiming at the above problems, a new DVL calibration method needs to be designed.

[0005] Through the above analysis, the problems and defects of the prior art are:

[0006] (1) The existing calibration method based on machine learning theory needs a large amount of samples and has a large amount of calculation, which is difficult to meet the practical application of engineering.

[0007] (2) The existing calibration method without external information assistance has high requirements for the carrier navigation trajectory, and the efficiency and accuracy of calibration are poor.

[0008] (3) The existing calibration method with external information assistance still needs to be further improved in the stability and accuracy of the system. SUMMARY

[0009] In view of the problems existing in the prior art, the present application provides a DVL calibration method, system, medium, equipment and terminal, especially a DVL calibration method, system, medium, equipment and terminal based on position observation information Davenport quaternion method.

[0010] The present application is realized in this way, a DVL calibration method, the DVL calibration method comprises:

[0011] When the AUV is in the water surface navigation state and can receive GNSS signals, the Kalman filter optimal estimation method is used for SINS / GNSS integrated navigation to obtain high-precision attitude and speed information of the carrier as reference values; the speed values in the d system measured by the DVL are integrated, the calibration method is used to estimate the scale factor error and installation error angle, and the estimated results are used to calibrate and compensate the original output of the DVL.

[0012] Further, the DVL calibration method comprises the following steps:

[0013] Step one, constructing a DVL error model, and explicitly identifying the main sources of DVL speed measurement error, focusing on the analysis of scale factor error and installation error angle;

[0014] Step two, taking the SINS / GNSS integrated navigation results as the b system reference speed to provide a speed benchmark for the subsequent calibration method;

[0015] Step three, based on the position observation operation of the Doppler speed measurement principle to calibrate the scale factor error and reduce the adverse effects of speed measurement noise;

[0016] Step four, based on the Davenport quaternion method to solve the position observation vector equation to calibrate the installation error angle and enhance the stability of the attitude rotation matrix solving process;

[0017] Step five, calibrating the measurement output of the DVL according to the calculation results.

[0018] Further, the construction of the DVL error model in step one comprises:

[0019] Define the coordinate system: select the "East-North-Sky: E-N-U" geographic coordinate system as the navigation coordinate system, denoted as n system; select the "right-front-up" coordinate system as the carrier coordinate system, denoted as b system; the DVL installation coordinate system is denoted as d system; the geocentric inertial coordinate system is denoted as i system; the earth coordinate system is denoted as e system; and the calculation navigation coordinate system is denoted as n' system.

[0020] Under ideal installation conditions, the coordinate axes of the DVL installation coordinate system d system and the carrier coordinate system b system are consistent with each other, and the installation matrix is the unit matrix I 3×3 , x b -y b -z b denotes b system, x d -y d -z d denotes d system.

[0021] According to the working principle of the DVL, the speed measurement error model is represented as:

[0022]

[0023] In the formula, This represents the measurement output value of DVL, where 's' represents the scale factor error. Let v represent the attitude transformation matrix from frame b to frame d. b Indicates the actual speed of the carrier. This represents the projection of the rotational angular velocity of the carrier system relative to the navigation system onto the carrier system. This represents the lever arm vector error between SINS and DVL.

[0024] Furthermore, the construction of the DVL error model in step one also includes:

[0025] In practical engineering applications, since SINS and DVL are usually closely installed and the lever arm vector is directly obtained and compensated by measurement, ignoring the influence of lever arm error, equation (1) is simplified to:

[0026]

[0027] Estimate the scale factor error s. When the installation error angle ε from the b-system to the d-system is small, the attitude transformation matrix... (·×) denotes antisymmetric matrix operations, and ε is defined as [ε x ,ε y ,ε z ] T ,but:

[0028]

[0029] When the actual speed of the carrier is v b When known, only DVL error exists. v is the term to be determined. b The DVL velocity measurement error model, derived from the results of SINS and GNSS combined navigation, is expressed as follows:

[0030]

[0031] The error calibration of DVL is transformed into solving the problem of scale factor error and installation error angle in equation (4). After the corresponding unknown parameters are obtained, the DVL measurement output is... The b-series velocity is obtained through compensation and used for SINS / DVL underwater integrated navigation calculations.

[0032] Furthermore, the calibration of the scale factor error in step three includes:

[0033] Since the magnitude of the attitude rotation matrix is ​​1, when the attitude rotation matrix is ​​multiplied by a vector, only the direction of the vector is changed, but the magnitude of the vector is not changed. Therefore, taking the modulus of both sides of equation (4) simultaneously yields:

[0034]

[0035] Based on the DVL measured carrier d system velocity, we get:

[0036]

[0037] At the same time, the position operation is carried out on both sides of formula (4), and the left side of formula (4) is expressed as:

[0038]

[0039] In the formula, k represents a discrete time, and Δt represents a sampling time interval.

[0040] Suppose that the SINS / GNSS integrated navigation system is updated n times in each sampling time interval Δt of the DVL, at this time, the right side of formula (4) is obtained by discretizing the position information:

[0041]

[0042] In the formula,

[0043] The expression of the scale factor error s based on position information is obtained as:

[0044]

[0045] Further, the calibration of the installation error angle in the fourth step includes:

[0046] Using formula (4) and formula (9), the installation error angle calculation expression based on position information is obtained as:

[0047]

[0048] Let:

[0049]

[0050] At this time, formula (10) is transformed as:

[0051]

[0052] Since and are known quantities, the DVL installation error angle calibration problem is converted into solving the Wahba problem of the rotation matrix between the two vector groups, and the rotation matrix R is solved to make the objective function f minimum:

[0053]

[0054] In the formula, ω kTo measure the weight of the corresponding vector, ω k = 1.

[0055] The Davenport quaternion method is used to solve the rotation matrix R.

[0056] In equation (13), since the constant has no effect on the minimization of the objective function, a linear transformation is performed to obtain:

[0057]

[0058] Since and the inner product with itself is 1 and R T R = I, so expand equation (14) to obtain:

[0059]

[0060] Neglecting the constant term, the objective function f is simplified to:

[0061]

[0062] Since the trace operation of the matrix has no effect on the objective function, according to the properties of the matrix trace, we get:

[0063]

[0064] In equation (17),

[0065] The rotation matrix R is expressed in quaternion form as:

[0066] R = (q0 2 -q T q)I + 2qq T -2q0[q×] (18)

[0067] In equation (18), q0 and q are the scalar part and vector part of the quaternion respectively, and the objective function is rewritten as:

[0068]

[0069] According to the properties of the matrix trace, further operations are performed on equation (19):

[0070]

[0071] Expand tr([q×]A T ) to obtain:

[0072]

[0073] Let:

[0074]

[0075] Then equation (21) is simplified as:

[0076] tr([q×]A T )=-a T q (23)

[0077] Substitute equation (23) into equation (20) to get:

[0078] min f(Q)=-(q0 2 -q T q)tr(A)-q T (A+A T )q-2q0a T q (24)

[0079] Let tr(A)=ρ and A+A T =B, equation (24) is transformed as:

[0080] min f(Q)=-(q T (B-ρI)q+q0a T q+q0q T a+q0 2 ρ) (25)

[0081] According to the matrix operation rules, equation (25) is rewritten as a linear form:

[0082]

[0083] In the equation, Q is the attitude matrix in the quaternion form with the vector part in front, and D is the Davenport matrix.

[0084] The objective function f is simplified as min f(Q)=-Q T DQ and Q T Q=1. The Lagrange multiplier method is used to solve the minimization problem of the objective function f, and then:

[0085] min f(Q,λ)=-Q T DQ+λ(Q T Q-1) (27)

[0086] The derivative of the equation is:

[0087] DQ=λQ (28)

[0088] Therefore, Q is the eigenvector corresponding to the eigenvalue of D, at this time, the attitude quaternion target function f minimization problem is converted into the problem of solving the eigenvector corresponding to the maximum eigenvalue of the matrix D, and the DVL installation error angle is obtained by converting the Euler angle based on Q.

[0089] Another object of the present application is to provide a DVL calibration system applying the DVL calibration method.

[0090] The model construction module is configured to construct a DVL error model.

[0091] The reference velocity determination module is configured to take the SINS / GNSS integrated navigation result as the b-system reference velocity.

[0092] The scale factor error calibration module is configured to calibrate the scale factor error based on the position observation operation according to the Doppler velocity measurement principle.

[0093] The installation error angle calibration module is configured to calibrate the installation error angle by solving the position observation vector equation based on the Davenport quaternion method.

[0094] The DVL calibration module is configured to calibrate the measurement output of the DVL according to the calculation result.

[0095] Another object of the present application is to provide a computer device including a memory and a processor, wherein the memory stores a computer program, and the computer program is executed by the processor to enable the processor to perform the steps of the DVL calibration method.

[0096] Another object of the present application is to provide a computer readable storage medium storing a computer program, wherein the computer program is executed by a processor to enable the processor to perform the steps of the DVL calibration method.

[0097] Another object of the present application is to provide an information data processing terminal for implementing the DVL calibration system.

[0098] In combination with the above technical solutions and the technical problems solved, the technical solutions to be protected by the present application have the following advantages and positive effects:

[0099] First, in view of the technical problems existing in the prior art and the difficulty in solving the problems, the technical solutions to be protected by the present application are closely combined with the results and data in the research and development process, and the technical problems solved by the technical solutions are analyzed in detail and deeply. Some creative technical effects brought about after solving the problems are described as follows:

[0100] The application proposes a Davenport quaternion method based on position observation information to realize the calibration of DVL scale factor error and installation error angle. In the scale factor calibration process, the adverse effect of measurement noise on the speed measurement accuracy is reduced through the position operation based on Doppler velocity; in the installation error angle calibration process, the stability of the rotation matrix solution is enhanced through the position observation vector equation based on the Davenport quaternion method. The original measurement output of the DVL is compensated by using the calibrated parameter results, and the accurate DVL velocity information can be obtained.

[0101] The application proposes a Davenport quaternion method based on position observation information, and applies it to the DVL error calibration system. The effectiveness of the method is verified through the shipborne lake test. Under the conditions of simple and complex different maneuvers, the calibration method has improvement in accuracy and stability, has good engineering application value, and can be further applied to the field of integrated navigation.

[0102] Secondly, from the perspective of the product or as a whole, the technical effects and advantages of the technical solution to be protected by the application are described as follows:

[0103] The calibration effect of the calibration method and other existing methods is compared and analyzed through the shipborne lake test, and the results show that under the conditions of simple and complex different maneuvers, the stability and accuracy of the speed calibration result of the method are higher; at the same time, the carrier position error obtained by the method is smaller when the SINS / DVL integrated navigation is carried out by using the DVL speed measurement output after calibration compensation.

[0104] Thirdly, the creativity of the claims of the application is also reflected in the following important aspects:

[0105] (1) The expected income and commercial value of the technical solution of the application after transformation are as follows:

[0106] The Davenport quaternion calibration method based on position observation information of the application realizes the calibration of the main speed error of the DVL by following the idea of "calibrating the scale factor error first, and then calibrating the installation error angle", compared with the conventional DVL calibration method at the present stage, effectively improves the stability of the calibration system and improves the final calibration accuracy

[0107] (2) The technical solution of the application solves the technical problems that people have been eager to solve but have failed to succeed:

[0108] For a long time, Doppler velocity log (DVL) is an important speed measuring method for underwater carrier, but its speed measuring accuracy depends largely on the effect of pre-calibration, and accurate and stable DVL calibration is a long-term problem in underwater navigation field. The DVL calibration problem which has not been solved for a long time can be solved by calibrating scale factor error of Doppler speed measurement through position operation and calibrating installation error angle through Davenport quaternion method.

[0109] (3) The technical scheme of the present application overcomes the technical bias:

[0110] The present application overcomes the inertial thinking of calibrating scale factor error and installation error angle at the same time under the condition of external information assistance for a long time, follows the idea of "calibrating scale factor error first, and then calibrating installation error angle", points out the adverse effect of DVL speed measurement noise on calibration accuracy in practical application, effectively suppresses the noise through position operation, enhances the stability of the system in the process of solving attitude rotation matrix through Davenport quaternion method, and finally improves the calibration accuracy of DVL. BRIEF DESCRIPTION OF DRAWINGS

[0111] In order to more clearly illustrate the technical scheme of the embodiments of the present application, the drawings needed in the embodiments of the present application will be briefly introduced as follows. Obviously, the drawings described below are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0112] Figure 1 is a DVL calibration method flowchart provided by the embodiments of the present application;

[0113] Figure 2 is a DVL installation error schematic diagram provided by the embodiments of the present application;

[0114] Figure 3 is a DVL error calibration system structure diagram provided by the embodiments of the present application;

[0115] Figure 4 is a test platform installation schematic diagram provided by the embodiments of the present application;

[0116] Figure 5 is a DVL original output and corresponding trajectory diagram under simple maneuvering provided by the embodiments of the present application;

[0117] Figure 6 is a DVL output and corresponding trajectory diagram under complex maneuvering provided by the embodiments of the present application;

[0118] Figure 7 is a right direction speed calibration error diagram under simple maneuvering provided by the embodiments of the present application;

[0119] Figure 8 is a forward velocity calibration error graph in simple maneuver provided by the embodiment of the present application;

[0120] Figure 9 is an upward velocity calibration error graph in simple maneuver provided by the embodiment of the present application;

[0121] Figure 10 is a combined navigation position error comparison graph in simple maneuver provided by the embodiment of the present application;

[0122] Figure 11 is a rightward velocity calibration error graph in complex maneuver provided by the embodiment of the present application;

[0123] Figure 12 is a forward velocity calibration error graph in complex maneuver provided by the embodiment of the present application;

[0124] Figure 13 is an upward velocity calibration error graph in complex maneuver provided by the embodiment of the present application;

[0125] Figure 14 is a combined navigation position error comparison graph in complex maneuver provided by the embodiment of the present application. DETAILED DESCRIPTION

[0126] In order to make the objects, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and not used to limit the present application.

[0127] In view of the problems in the prior art, the present application provides a DVL calibration method, system, medium, device and terminal, which will be described in detail below with reference to the accompanying drawings.

[0128] I. Explanation of embodiments. In order to enable those skilled in the art to fully understand how the present application is specifically implemented, this part is an explanation of the embodiments of the technical solutions of the claims.

[0129] As shown in Figure 1 , the DVL calibration method provided by the embodiment of the present application comprises the following steps:

[0130] S101, constructing a DVL error model;

[0131] S102, taking the SINS / GNSS combined navigation result as a b-system reference velocity;

[0132] S103, position observation operation based on Doppler velocity measurement principle to calibrate scale factor error;

[0133] S104, solve the position observation vector equation based on the Davenport quaternion method to calibrate the installation error angle;

[0134] S105, calibrate the measurement output of the DVL according to the calculation result.

[0135] In the underwater environment, the DVL can measure the speed of the carrier relative to the water bottom or water flow through the acoustic Doppler effect, but in engineering practice, due to various conditions such as manufacturing process, external environment, installation deviation, etc., the speed measurement error of the DVL is difficult to avoid. Based on the analysis and modeling of the scale factor error and the installation angle error, the SINS / GNSS combined navigation obtained carrier body speed is used as a reference value, then the scale factor error is calibrated through the position operation of the Doppler speed measurement, further, the installation angle error is calibrated by solving the position observation vector equation by using the Davenport quaternion method, finally, the original measurement output of the DVL is compensated in real time by using the error parameters obtained, so as to achieve the purpose of ensuring the high accuracy of the DVL speed measurement information.

[0136] As a preferred embodiment, the DVL calibration method provided by the embodiment of the present application specifically includes the following steps:

[0137] I. Analysis and modeling of DVL speed measurement error

[0138] Define the coordinate system: select the "East-North-Sky (E-N-U)" geographic coordinate system as the navigation coordinate system, denoted as n system; select the "right-front-up" coordinate system as the carrier coordinate system, denoted as b system; the DVL installation coordinate system is denoted as d system; the earth-centered inertial coordinate system is denoted as i system; the earth coordinate system is denoted as e system; the calculation navigation coordinate system is denoted as n' system.

[0139] In the ideal installation condition, the coordinate axes of the DVL installation coordinate system d system and the carrier coordinate system b system should be consistent with each other, that is, the installation matrix is a unit matrix I 3×3 , but in engineering practice, due to the limitation of technical process, the installation error angle is difficult to avoid, and the installation error diagram of the DVL on the carrier is as shown in Figure 2 , wherein x b -y b -z b represents b system, x d -y d -z d represents d system.

[0140] According to the working principle of the DVL, the speed measurement error model can be expressed as:

[0141]

[0142] In the formula, represents the measured output value of the DVL, s represents the scale factor error, represents the attitude transformation matrix from the b-frame to the d-frame, v b represents the true velocity of the carrier, represents the projection of the angular velocity of the carrier in the b-frame on the d-frame, represents the lever arm vector error between the SINS and the DVL.

[0143] In actual engineering applications, since the SINS and the DVL are usually closely mounted and the lever arm vector can be directly measured and compensated, the influence of the lever arm error is ignored in the embodiments of the present application, and formula (1) can be simplified as:

[0144]

[0145] It should be noted that the scale factor error s is not a fixed value, and its value changes with the change of the sound wave propagation speed in water, and there are small fluctuations in different temperature, salinity, depth and the like, therefore, in order to ensure the accuracy of subsequent integrated navigation, s needs to be estimated. When the installation error angle ε of the b-frame to the d-frame is a small angle, the attitude transformation matrix where (·x) represents the skew-symmetric matrix operation, and ε = [ε x , ε y , ε z ] T , and the following formula is obtained:

[0146]

[0147] As can be seen from formula (2), when the true velocity v b of the carrier is known, only the DVL error is to be solved, in the embodiments of the present application, v b is obtained from the SINS and GNSS integrated navigation result, and at this time, the DVL velocity error model can be further represented as:

[0148]

[0149] Based on the above analysis, the error calibration of the DVL is converted into the problem of solving the scale factor error and the installation error angle in formula (4), and after the corresponding unknown parameters are solved, the DVL measured output is compensated so as to obtain the accurate b-frame velocity for SINS / DVL underwater integrated navigation solution.

[0150] II. Basic principles of the DP-P calibration method

[0151] 2.1 Definition of coordinate system

[0152] 1. Inertial coordinate system (O-xi y i z i , i system)

[0153] The coordinate origin is the center of the earth, the x i axis points to the vernal equinox of the earth, the z i axis points to the north pole of the earth and coincides with the rotation axis of the earth, and the y i axis is in the equatorial plane of the earth and forms a right-handed coordinate system with the x i and z i axes. The directions of the coordinate axes of the inertial coordinate system relative to the inertial space do not change.

[0154] 2. Earth coordinate system (O-x e y e z e , e system)

[0155] The coordinate origin is the center of the earth, the x e axis points to the Greenwich meridian of the earth, the z e axis coincides with the z i axis of the i system, and the y e axis is in the equatorial plane of the earth and forms a right-handed coordinate system with the x e and z e axes. The earth coordinate system is always fixed to the earth and rotates with the earth.

[0156] 3. Navigation coordinate system (O n -x n y n z n , n system)

[0157] The coordinate origin is the center of mass of the moving carrier, and according to the different directions of the coordinate axes, it can be divided into an “east-north-sky” coordinate system and a “north-east-ground” coordinate system. In the embodiment of the application, the “east-north-sky” coordinate system is uniformly selected and marked as the n system.

[0158] 4. Carrier coordinate system (O b -x b y b z b , b system)

[0159] The coordinate origin is the center of mass of the moving carrier, the x b axis, the y b axis and the z b axis respectively point to the right direction, the front direction and the upward direction of the carrier, and form a “right-front-up” right-handed coordinate system.

[0160] 5. DVL coordinate system (O d -x d y d z d , d system)

[0161] d is theoretically completely coincident with b, but in engineering practice, due to the installation error of DVL, there is a certain deviation between d and b.

[0162] 2.2 DVL error calibration system structure

[0163] In the proposed DVL error calibration system, when the AUV is in the water surface navigation state and can receive GNSS signals, the Kalman filter optimal estimation method is used for SINS / GNSS integrated navigation to obtain high-precision attitude and velocity information of the carrier as reference values, and then the DVL measured velocity value in the d system is comprehensively used to estimate the scale factor error and installation error angle and compensate the DVL output. The DVL error calibration system structure is shown in Figure 3

[0164] 2.3 Calibration of scale factor error

[0165] Since the modulus of the attitude rotation matrix is 1, that is, when the attitude rotation matrix is multiplied by the vector, only the direction of the vector is changed but not the size of the vector, therefore, the modulus of both sides of equation (4) can be obtained:

[0166]

[0167] At this time, based on the DVL measured carrier d system velocity, the following can be obtained:

[0168]

[0169] However, it should be pointed out that when calculating the scale factor error s using equation (6), since the measurement value of the DVL output is directly used, measurement noise error is introduced, which further reduces the calibration accuracy of the scale factor error. Therefore, in order to further eliminate the influence of DVL measurement noise, the position operation is performed on both sides of equation (4), at this time, the left side of equation (4) can be expressed as:

[0170]

[0171] In the formula, k represents a certain discrete time, and Δt represents the sampling time interval.

[0172] Suppose that in each sampling time interval Δt of DVL, the SINS / GNSS integrated navigation system is updated n times, at this time, the discrete processing of the position information can be obtained, and the right side of equation (4) is:

[0173]

[0174] In the formula,

[0175] ​Therefore, the scale factor error s expression based on position information can be obtained as:

[0176]

[0177] 2.4 Calibration of installation error angle

[0178] Using equation (4) and equation (9), the installation error angle calculation expression based on position information can be obtained as:

[0179]

[0180] Let:

[0181]

[0182] At this time, equation (10) will be transformed into:

[0183]

[0184] From equation (12), it can be seen that and are known quantities, so the DVL installation error angle calibration problem is converted into solving the Wahba problem of the rotation matrix between the two vector groups, that is, solving the rotation matrix R to minimize the objective function f:

[0185]

[0186] In the equation, ω k is the weight corresponding to the measurement vector, and in the calibration problem of the present application, ω k = 1.

[0187] In order to quickly and accurately complete the DVL installation error angle calibration, the Davenport quaternion method is used to solve the rotation matrix R.

[0188] In equation (13), since the constant has no effect on the minimization of the objective function, after linear transformation, it can be obtained as:

[0189]

[0190] Since and have an inner product of 1 with themselves and R T R = I, therefore, equation (14) is expanded to obtain:

[0191]

[0192] Neglecting the constant term, the objective function f can be further simplified as:

[0193]

[0194] Since the trace operation of a matrix has no effect on the objective function, according to the property of the matrix trace, we have:

[0195]

[0196] where,

[0197] The rotation matrix R can be expressed in quaternion form as:

[0198] R = (q0 2 -q T q)I + 2qq T -2q0[q×] (18)

[0199] where q0and q are the scalar and vector parts of the quaternion, respectively, and the objective function can be rewritten as:

[0200]

[0201] According to the property of the matrix trace, we further operate equation (19) as:

[0202]

[0203] For tr([q×]A T ), it can be expanded as:

[0204]

[0205] Let:

[0206]

[0207] Then equation (21) can be simplified as:

[0208] tr([q×]A T ) = -a T q (23)

[0209] Substituting equation (23) into equation (20) gives:

[0210] min f(Q) = -(q0 2 -q T q)tr(A) - q T (A + A T )q - 2q0a T q (24)

[0211] Let tr(A) = p and A + A T = B, equation (24) can be transformed as:

[0212] min f(Q) = -(q T (B-ρI)q+q0a T q+q0q T a+q0 2 ρ) (25)

[0213] According to the matrix operation rule, formula (25) can be rewritten in linear form:

[0214]

[0215] In the formula, Q is a pose matrix in the form of a quaternion with the vector part in front, and D is a Davenport matrix.

[0216] Based on the above derivation, the objective function f is simplified as min f(Q) = -Q T DQ and Q T Q = 1, the objective function f is minimized by using the Lagrange multiplier method, then:

[0217] min f(Q, λ) = -Q T DQ + λ(Q T Q-1) (27)

[0218] Derivation is performed on the equation to obtain:

[0219] DQ = λQ (28)

[0220] Therefore, Q is the eigenvector of D corresponding to the eigenvalue λ. At this time, the pose quaternion objective function f minimization problem is converted into solving the eigenvector corresponding to the maximum eigenvalue of the matrix D, and the Euler angle converted on the basis of Q is the calibrated DVL installation error angle. The flow of DVL error calibration based on the Davenport quaternion of position observation information is as shown in Figure 1

[0221] The DVL calibration system provided by the embodiment of the application comprises:

[0222] A model construction module, configured to construct a DVL error model;

[0223] A reference speed determination module, configured to take the SINS / GNSS integrated navigation result as the b-system reference speed;

[0224] A scale factor error calibration module, configured to calibrate the scale factor error based on position observation operation of the Doppler velocity measurement principle;

[0225] An installation error angle calibration module, configured to calibrate the installation error angle based on solving the position observation vector equation by using the Davenport quaternion method; ​

[0226] a DVL calibration module for calibrating the measurement output of the DVL according to the calculation result.

[0227] II. Application Examples. In order to prove the creativity and technical value of the technical solutions of the present application, this part is an application example of the technical solutions of the claims on specific products or related technologies.

[0228] In order to realize accurate calibration of DVL error, on the basis of analyzing and establishing the source and model of DVL error, the present application proposes a Davenport quaternion calibration method based on position observation information. In the present application, the scale factor error is first calibrated, and the adverse effects of velocity measurement noise are reduced through position observation operation based on Doppler velocity measurement principle; then the installation error angle is calibrated, and the stability of the rotation matrix solution is enhanced through solving the position observation vector equation based on the Davenport quaternion method. The method proposed in the present application can realize accurate calibration of DVL velocity measurement error under different simple and complex vehicle maneuvers, and can be further applied to the field of underwater multi-sensor integrated navigation.

[0229] III. Evidence of the effects of the embodiments. The embodiments of the present application have achieved some positive effects in the process of research and development or use, and indeed have great advantages compared with the prior art. The following content is described in combination with the data and graphs of the test process.

[0230] Test verification: In order to verify the feasibility and effectiveness of the DVL calibration method provided by the embodiments of the present application, a shipborne lake test was carried out in Mulan Lake of a city in a province. The shipborne navigation equipment includes CY-JG90J type SINS, PA600 type DVL sensor and GNSS receiver, and the parameters of the equipment are shown in Tables 1-2. During the test, the results obtained after combining high-precision RTK-GNSS with SINS are taken as the reference benchmark. The main test equipment physical map and test platform installation overview are shown in Figs. 1-2. Figure 4

[0231] Table 1 Performance parameters of CY-JG90J type SINS

[0232]

[0233] Table 2 Performance parameters of PA600 type DVL

[0234]

[0235]

[0236] ​The shipborne test collects 12000 seconds of measured data, of which 0-1200 seconds is the initial alignment stage of SINS to meet the basic needs of subsequent high-precision integrated navigation. Two 1200-second test data after the initial alignment stage are selected to verify the DVL calibration effect under different maneuvering conditions. The original output of the DVL and the corresponding trajectory in the selected two time periods are shown in Figures 5-6 , wherein the white icon represents the starting point of the trajectory, and the gray icon represents the end point of the trajectory.

[0237] 1. Performance verification under simple maneuvering conditions

[0238] The trajectory shown in Figure 5 is used to verify the DVL calibration effect of the carrier under simple maneuvering conditions. In order to compare the performance of the methods, 1200-second calibration tests are performed using the SVD calibration method, the KF calibration method and the calibration method provided by the embodiment of the application, respectively. The DVL measurement output is compensated using the calibration results, and the compensated results are compared with the DVL reference speed obtained by GNSS / SINS integrated navigation to obtain the speed error results in three directions as shown in Figures 7-9 . For convenience of description, the above three calibration methods are denoted as SVD, KF and DP-P, respectively.

[0239] It can be seen from Figures 7-9 that for the right direction speed, the errors of the three calibration methods after compensation have been improved relative to the uncalibrated data, the errors of the SVD and KF methods are basically stable at 0.1 m·s -1 , the error of the DP-P method is basically stable at 0.08 m·s -1 and is smoother; for the forward direction speed, since the test carrier mainly moves forward during the movement and the speed measurement value is inevitably affected by external noise, the error result fluctuates greatly, but the error obtained by other calibration methods is smaller than that of the uncalibrated and KF method calibration error; for the upward direction speed, since the KF calibration method diverges in the sky direction channel, the final calibration result is not reliable, and the calibration error effects of the SVD and DP-P methods are basically equivalent. In general, since the test carrier under simple maneuvering conditions moves approximately at a constant speed in a straight line, the observability of part of the state quantity in the KF method is weak, so the calibration effect of the KF method is greatly reduced, and the speed errors of other calibration methods can be basically stable at 0.1 m·s -1 , which can meet the requirements of subsequent integrated navigation.

[0240] To quantify the effectiveness of the comparative methods, the maximum absolute error and mean absolute error (MAE) of the speed within 1200 seconds were selected as performance evaluation indicators. The definition of MAE is shown in equation (29), and the final calculation results are shown in Table 3. It can be seen that when the test vehicle performs simple maneuvers, the calibration results of the KF method are not ideal, with only the right-hand speed error being better than the uncalibrated value. In contrast, the speed calibration errors of the SVD and DP-P methods are better than the uncalibrated value in all directions, thus achieving the purpose of speed calibration.

[0241]

[0242] In the formula, n represents the test duration, x(t), These represent the reference value and the estimated value of the velocity at time t, respectively.

[0243] Table 3. Speed ​​error calibration results under different calibration methods during simple maneuvers.

[0244]

[0245] To further compare the effectiveness of the methods, the DVL measurement output after calibration and compensation using the SVD and DP-P methods was combined with SINS using a standard Kalman filter algorithm to obtain the position result. This result was then compared with the KF calibration method to indirectly reflect the quality of the calibration. The position error obtained from the experiment is shown below. Figure 10 As shown in Table 4. The formula for calculating the position error is as follows:

[0246]

[0247] In the formula, ΔP represents the position error, L represents latitude, and λ represents longitude.

[0248] Table 4. Maximum and average position errors (meters) of integrated navigation during simple maneuvers.

[0249]

[0250] Depend on Figure 10 It can be seen that the trajectory obtained by using uncalibrated speed for combined navigation is severely divergent. However, the trajectory obtained by compensating the DVL output through calibration technology before combined navigation can track the reference trajectory well. It should be noted that the position information obtained by using the KF method has high accuracy, indicating that when the test vehicle is approximately moving in uniform linear motion, the position state quantity is different from the installation error angular state quantity, and its observability is not affected.

[0251] Table 4 provides a more intuitive view of the position error results. It shows that, compared to other methods, the DP-P method significantly improves the calibration performance, with improvements exceeding 80%. The experimental results validate the effectiveness of the proposed DP-P calibration method under simple maneuvering conditions of the test vehicle.

[0252] 2. Performance verification under complex maneuvering conditions

[0253] use Figure 6 The trajectory shown verifies the DVL calibration effect of the vehicle under complex maneuvering conditions. The DVL measurement outputs obtained using different calibration methods are compared with the DVL reference velocity, and the velocity error results in three directions are obtained as follows: Figures 11-13 As shown in Table 5.

[0254] Table 5. Speed ​​error calibration results under different calibration methods during complex maneuvers.

[0255]

[0256] from Figures 11-13 It can be seen that when the test vehicle performs continuous "S"-shaped complex maneuvers, the calibration effect of the SVD method declines, and the velocity error curve fluctuates drastically. In the KF method, the observability of the installation error angular state quantity is enhanced, and the calibration error results for rightward and forward velocities are significantly better than those for simple maneuvers. However, the calibration result for upward velocity remains unreliable due to the divergence of the celestial channel. The DP-P method has better calibration accuracy than other methods. Table 5 shows that when the test vehicle performs complex maneuvers, the velocity errors after using the calibration methods are reduced to varying degrees compared to the uncalibrated velocity errors. However, overall, the DP-P method provided in this embodiment of the invention has the best calibration effect.

[0257] The trajectory position error obtained by combining velocities calibrated using different methods is as follows: Figure 14 As shown in Table 6.

[0258] Table 6. Maximum and average position errors (meters) of integrated navigation during complex maneuvers.

[0259]

[0260] Depend on Figure 14It can be seen that the trajectory obtained by using the uncalibrated velocity for integrated navigation has a large deviation from the reference trajectory; the position error gradually increases with time when the compensated velocity is calibrated by using the SVD method; the position error can be controlled within 20 meters within 1200 seconds when the compensated velocity is calibrated by using the KF and DP-P methods. Table 6 further illustrates the advantages of the KF and DP-P methods in terms of navigation accuracy, and the DP-P method has higher accuracy than the KF method, for example, the average position error is increased by 62.74% compared with the KF method. The test results verify the effectiveness of the proposed DP-P calibration method under complex maneuvers of the test carrier.

[0261] It should be noted that embodiments of the present application can be realized by hardware, software, or a combination of software and hardware. The hardware part can be realized by special logic; the software part can be stored in a memory and executed by a suitable instruction execution system, such as a microprocessor or a specially designed hardware. Those skilled in the art can understand that the above-mentioned devices and methods can be realized by computer executable instructions and / or included in processor control code, such as carrier media, such as magnetic disk, CD or DVD-ROM, programmable memory, such as read-only memory (firmware), or data carrier, such as optical or electronic signal carrier. The devices and modules of the present application can be realized by hardware circuit, such as ultra-large scale integrated circuit or gate array, semiconductor, such as logic chip, transistor, or programmable hardware device, such as field programmable gate array, programmable logic device, or software executed by various types of processors, or a combination of the above-mentioned hardware circuit and software, such as firmware.

[0262] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, any modification, equivalent replacement and improvement within the technical range disclosed by the present application, which is within the spirit and principle of the present application, should be covered within the protection scope of the present application.

Claims

1. A DVL calibration method, characterized in that, The DVL calibration method includes: when the AUV is in a surface navigation state and can receive GNSS signals, the Kalman filter optimal estimation method is used to perform SINS / GNSS integrated navigation to obtain high-precision attitude and velocity information of the vehicle as reference values; the velocity values ​​in the d-frame measured by DVL are combined, and the scale factor error and installation error angle are estimated by the calibration algorithm, and the DVL output is compensated. The calibration algorithm estimates the scale factor error and installation error angle, and compensates for the DVL output by including the following steps: Step 1: Construct the DVL error model; Step 2: Use the SINS / GNSS integrated navigation results as the b-series reference speed; Step 3: Calculate the position observation based on the Doppler velocity measurement principle to calibrate the scale factor error; Step 4: Solve the position observation vector equations using the Davenport quaternion method to calibrate the installation error angles; Step 5: Calibrate the DVL measurement output based on the calculation results; The construction of the DVL error model in step one includes: Define coordinate systems: Select the "East-North-Sky:ENU" geographic coordinate system as the navigation coordinate system, denoted as the n system; select the "Right-Front-Up" coordinate system as the carrier coordinate system, denoted as the b system; the DVL installation coordinate system is denoted as the d system; the geocentric inertial coordinate system is denoted as the i system; the Earth coordinate system is denoted as the e system; and the calculation navigation coordinate system is denoted as the n′ system. Under ideal installation conditions, the coordinate axes of the DVL installation coordinate system (d-frame) and the carrier coordinate system (b-frame) are consistent, and the installation matrix is ​​the identity matrix I. 3×3 x b -y b -z b Indicates the b series, x d -y d -z d Indicates the d-series; Based on the working principle of DVL, the speed measurement error model is expressed as: In the formula, This represents the measurement output value of DVL, where 's' represents the scale factor error. Let v represent the attitude transformation matrix from frame b to frame d. b Indicates the actual speed of the carrier. This represents the projection of the rotational angular velocity of the carrier system relative to the navigation system onto the carrier system. This represents the lever arm vector error between SINS and DVL.

2. The DVL calibration method as described in claim 1, characterized in that, The construction of the DVL error model in step one also includes: In practical engineering applications, since SINS and DVL are usually closely installed and the lever arm vector is directly obtained and compensated by measurement, ignoring the influence of lever arm error, equation (1) is simplified to: Estimate the scale factor error s. When the installation error angle ε from the b-system to the d-system is small, the attitude transformation matrix... (·×) denotes antisymmetric matrix operations, and ε is defined as [ε x ,ε y ,ε z ] T ,but: When the actual speed of the carrier is v b When known, only DVL error exists. v is the term to be determined. b The DVL velocity measurement error model, derived from the results of SINS and GNSS combined navigation, is expressed as follows: The error calibration of DVL is transformed into solving the problem of scale factor error and installation error angle in equation (4). After the corresponding unknown parameters are obtained, the DVL measurement output is... The b-series velocity is obtained through compensation and used for SINS / DVL underwater integrated navigation calculations.

3. The DVL calibration method as described in claim 1, characterized in that, The calibration of the scale factor error in step three includes: Since the magnitude of the attitude rotation matrix is ​​1, when the attitude rotation matrix is ​​multiplied by a vector, only the direction of the vector is changed, but the magnitude of the vector is not changed. Therefore, taking the modulus of both sides of equation (4) simultaneously yields: Based on the d-system velocities of the carrier obtained from DVL measurements, the following conclusions are drawn: Simultaneously performing positional operations on both sides of equation (4), the left side of equation (4) is represented as: In the formula, k represents a discrete time point, and Δt represents the sampling time interval; Suppose that within each sampling time interval Δt of DVL, the SINS / GNSS integrated navigation system updates n times. At this time, the right side of equation (4) is obtained by discretizing the position information: In the formula, The expression for the scale factor error s based on location information is as follows:

4. The DVL calibration method as described in claim 1, characterized in that, The calibration of the installation error angle in step four includes: Using equations (4) and (9), the expression for calculating the installation error angle based on location information is obtained as follows: make: At this point, equation (10) will be transformed into: because and Since all quantities are known, the DVL installation error angle calibration problem is transformed into a Wahba problem of solving the rotation matrix between two vector groups, and the rotation matrix R is solved to minimize the objective function f: In the formula, ω k ω represents the weights corresponding to the measurement vector. k =1; The rotation matrix R is solved using the Davenport quaternion method; In equation (13), since the constant has no effect on minimizing the objective function, after a linear transformation, we get: because and The inner product of itself and is 1 and R T R = I, therefore expanding equation (14) yields: Ignoring the constant term, the objective function f simplifies to: Since the trace operation on a matrix does not affect the objective function, based on the properties of the matrix trace, we obtain: In the formula, The rotation matrix R is represented in quaternion form as follows: R=(q0 2 -q T q)I+2qq T -2q0[q×](18) In the formula, q0 and q are the scalar and vector parts of the quaternion, respectively, and the objective function is rewritten as: Based on the properties of the matrix trace, further operations are performed on equation (19): For tr([q×]A) T Expanding on this, we get: make: Equation (21) simplifies to: tr([q×]A T )=-a T q(23) Substituting equation (23) into equation (20), we get: min f(Q)=-(q0 2 -q T q)tr(A)-q T (A+A T )q-2q0a T q(24) Let tr(A)=ρ and A+A T =B, equation (24) is transformed into: where f(Q)=-(q T (B-ρI)q+q0a T q+q0q T a+q0 2 ρ)(25) According to the rules of matrix operations, equation (25) can be rewritten in linear form: In the formula, Q is the attitude matrix in quaternion form with the vector part first, and D is the Davenport matrix; The objective function f simplifies to min f(Q) = -Q T DQ and Q T If Q = 1, and the Lagrange multiplier method is used to solve the problem of minimizing the objective function f, then: min f(Q,λ)=-Q T DQ+λ(Q T Q-1)(27) Differentiating the equation, we get: DQ=λQ(28) Therefore, Q is the eigenvector of D corresponding to the eigenvalue λ. At this time, the problem of minimizing the attitude quaternion objective function f is transformed into the problem of finding the eigenvector corresponding to the largest eigenvalue of matrix D. Based on the calculation of Q, it is converted into Euler angles to obtain the calibrated DVL installation error angle.

5. A DVL calibration system applying the DVL calibration method as described in any one of claims 1 to 4, characterized in that, The DVL calibration system includes: The model building module is used to build DVL error models; The reference speed determination module is used to use the SINS / GNSS integrated navigation results as the b-series reference speed; The scale factor error calibration module is used to calibrate the scale factor error in position observation calculations based on the Doppler velocity measurement principle. The installation error angle calibration module is used to calibrate the installation error angle by solving the position observation vector equation based on the Davenport quaternion method. The DVL calibration module is used to calibrate the measurement output of DVL based on the calculation results.

6. A computer device, characterized in that, The computer device includes a memory and a processor. The memory stores a computer program, which, when executed by the processor, causes the processor to perform the steps of the DVL calibration method as described in any one of claims 1 to 4.

7. A computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of the DVL calibration method as described in any one of claims 1 to 4.

8. An information data processing terminal, characterized in that, The information data processing terminal is used to implement the DVL calibration system as described in claim 5.

Images