Navigation and positioning method and system for underwater vehicle based on combination of MEMS IMU and DVL

Abstract

The application provides a kind of underwater vehicle navigation positioning method and system based on MEMS IMU and DVL combination, comprising: taking the mean value of the attitude angle of MEMS IMU as the initial heading attitude information of underwater vehicle;Time update of Kalman filtering is carried out, the position and speed of underwater vehicle are calculated;The speed measured by DVL is multiplied by a proportion factor to eliminate the proportion error;Eliminate the arm error of the speed measured by DVL, further correct the DVL measurement value;The difference between the speed of underwater vehicle and the corrected DVL speed is used as the measurement difference of Kalman filter, the measurement update of Kalman filtering is carried out, and the optimal estimation of underwater vehicle position and speed is obtained;The position of underwater vehicle is used to calculate the attitude angle, and the navigation position, speed and attitude angle are output.The application eliminates the proportion error and arm error of DVL speed respectively, and further improves the navigation positioning accuracy of underwater vehicle.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and more specifically, to a navigation and positioning method and system for underwater vehicles based on a combination of MEMS IMU and DVL. Background Technology

[0002] The scarcity of terrestrial resources severely restricts the development of human society, leading more and more countries to focus their attention on the development, utilization, and protection of marine resources. Due to the unique limitations of the marine environment, human development activities cannot be carried out freely underwater, and underwater vehicles that do not require human intervention are widely used in the development and utilization of marine resources.

[0003] The navigation and positioning capability of underwater vehicles is one of the important indicators for evaluating their performance and a key technology restricting their development. Meanwhile, to protect their legitimate marine resources, countries are placing greater emphasis on naval investment, leading to the informatization, diversification, and complexity of modern naval warfare. This complex modern naval warfare places increasingly higher demands on the offensive and defensive capabilities of various underwater weapons. The precise navigation and positioning capability of underwater weapons has become one of the key factors determining the outcome of modern naval battles. To fully utilize and protect national marine resources, underwater navigation and positioning technology has received widespread attention and in-depth research. Due to the complexity of the underwater environment, integrated navigation methods are often used for underwater vehicle navigation and positioning; the design of the integrated navigation system is crucial to ensuring navigation accuracy.

[0004] Patent document CN115019412A (application number: CN202210618561.2) discloses an underwater AUV cable inspection system and method based on multiple sensors. The system comprises a power module, an AUV control module, an AUV power module, a cable detection module, a combined navigation and positioning module, and a wireless transmission module. By using multiple sensors in conjunction for cable detection, the system improves the accuracy of cable identification in complex underwater environments. A magnetometer detects the cable's direction and position, while side-scan sonar and underwater cameras identify the cable based on image information and record cable damage conditions, overcoming the limitations of single-sensor detection and improving accuracy. A combined navigation and positioning algorithm calculates the system's attitude and position, solving the problem of inaccurate underwater positioning of the detection equipment. A filtering gain compensation algorithm overcomes the accumulation of positioning errors. Real-time uploading of the cable inspection system's precise underwater location information improves the safety of the cable detection and inspection process.

[0005] Inertial measurement units (MEMS IMUs) can continuously provide information such as the attitude and velocity of a carrier. The data update frequency is fast, and it has high accuracy in a short period of time. However, as the system operates for a longer period of time, the navigation error of the MEMS IMU will accumulate and increase. At this time, the observation information of external sensors can be used to correct the navigation information, thereby suppressing the growth of the accumulated navigation error.

[0006] Doppler velocity logs (DVLs) are widely used velocity measurement devices in combined underwater and surface vehicle systems, capable of outputting the three-dimensional velocity of the vehicle in real time. DVLs obtain the frequency shift of the sound waves through four transducers mounted on the bottom of the AUV, and then calculate the three-dimensional velocity information of the vehicle in the vehicle coordinate system. They offer advantages such as high accuracy and strong reliability. Summary of the Invention

[0007] To address the shortcomings of existing technologies, the purpose of this invention is to provide a navigation and positioning method and system for underwater vehicles based on a combination of MEMS IMU and DVL.

[0008] The underwater vehicle navigation and positioning method based on a combination of MEMS IMU and DVL provided by the present invention includes:

[0009] Step S1: When the underwater vehicle is stationary, take the average attitude angle of the MEMS IMU over a period of time as the initial heading and attitude information of the underwater vehicle.

[0010] Step S2: When the underwater vehicle is navigating, the attitude angle and acceleration information output by the MEMS IMU are used to perform time updates by Kalman filtering to calculate the position and velocity of the underwater vehicle;

[0011] Step S3: Multiply the speed measured by DVL by a scaling factor to eliminate scaling error;

[0012] Step S4: Eliminate lever arm error from the DVL speed obtained in step S3, and further correct the DVL measurement value;

[0013] Step S5: The difference between the underwater vehicle speed obtained in step S2 and the corrected DVL speed obtained in step S4 is used as the measurement difference of the Kalman filter. The measurement is updated by Kalman filtering to obtain the optimal estimate of the underwater vehicle's position and speed.

[0014] Step S6: Calculate the attitude angles using the position of the underwater vehicle obtained in step S5, and output the navigation position, velocity, and attitude angles.

[0015] Preferably, step S1 includes:

[0016] The MEMS IMU is installed at the center of gravity of the underwater vehicle. Using the initial position of the MEMS IMU as the origin, a navigation coordinate system is established with east as the x-axis, north as the y-axis, and the sky as the z-axis. A carrier coordinate system is established with the carrier axis pointing forward as the x-axis, leftward as the y-axis, and upward as the z-axis. Therefore, the initial position coordinates of the unmanned underwater vehicle are X0(0,0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0017] Step S2 includes: selecting the state variables of the Kalman filter as:

[0018] X = [xyv] x v y v z ] T

[0019] Where x, y are the position coordinates of the underwater vehicle along the x-axis and y-axis; v x ,v y ,v z The velocities of the underwater vehicle in the x, y, and z axes;

[0020] The state transition equation of the Kalman filter can be obtained from the equation of motion of the underwater vehicle:

[0021]

[0022] in, F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0023]

[0024] Where Δt is the time interval between the moments before and after the underwater vehicle;

[0025] Using velocity as the measurement, the measurement equation for the Kalman filter is:

[0026]

[0027] Where Z represents the measured value of the Kalman filter; H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z The z-axis velocity represents the speed of the underwater vehicle in the coordinate system of the vehicle carrier; W is the process noise, which follows a normal distribution W-(0,Q); U is the measurement noise, which follows a normal distribution U~(0,R); Q represents the covariance matrix of the process noise; R represents the covariance matrix of the measurement noise.

[0028] Discretizing the above Kalman filter yields:

[0029]

[0030] Among them, X k Z represents the state variables of the Kalman filter at time k; k H represents the measurement vector of the Kalman filter at time k; k The measurement equation for the Kalman filter at time k is Φ. k-11 W represents the state transition matrix after discretization of the Kalman filter at time k-1; k-1 U represents the process noise of the Kalman filter at time k-1; k-1 The measurement noise of the Kalman filter at time k-1 is represented; k represents the k-th time after the Kalman filter is discretized.

[0031] in:

[0032]

[0033] Where I represents the identity matrix; F k-1 This represents the state transition matrix of the Kalman filter before discretization at time k;

[0034] The expression for calculating the state variables backward is:

[0035] X k =Φ k-1 X k-1 +W k-1

[0036] The forward calculation of the error covariance matrix is ​​expressed as follows:

[0037]

[0038] Preferably, step S3 includes: if the velocity measured at the DVL center is Then multiply it by the scaling factor δk for correction, and obtain the velocity measured by DVL with itself as the center.

[0039] Step S4 includes: if the MEMS IMU and DVL are installed and fixed in place, let the position lever arm between the center of the MEMS IMU and the center of the DVL be l, and the angular acceleration output by the MEMS IMU be ω. k According to rigid body dynamics, the lever arm error of the DVL velocity measurement is ω×l. Subtracting the lever arm error from the velocity measured by the DVL with itself as the center, we obtain the velocity measurement value at the center of the IMU:

[0040] Preferably, step S5 includes:

[0041] The measured values ​​of the Kalman filter are:

[0042]

[0043] in, This indicates the speed of the aircraft as measured by the IMU;

[0044] The Kalman filter gain is calculated using the following expression:

[0045] K k =P k|k-1 H T HP k|k-1 H T +R))

[0046] The optimal estimate for updating the Kalman filter is expressed as follows:

[0047]

[0048] The updated error covariance matrix is ​​expressed as follows:

[0049] P k|k =(IK k H)P k|k-1 .

[0050] Preferably, step S6 includes:

[0051] Using the pitch angle θ and roll angle φ output by the MEMS IMU as true values, the yaw angle ψ is calculated using the underwater vehicle's positions at both ends. The expression is:

[0052]

[0053] Where, x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k.

[0054] The underwater vehicle navigation and positioning system based on a combination of MEMS IMU and DVL provided by the present invention includes:

[0055] Module M1: When the underwater vehicle is stationary, take the average attitude angle of the MEMS IMU over a period of time as the initial heading and attitude information of the underwater vehicle.

[0056] Module M2: When the underwater vehicle is navigating, it uses the attitude angle and acceleration information output by the MEMS IMU to perform time updates by Kalman filtering, and calculates the position and velocity of the underwater vehicle.

[0057] Module M3: Multiplies the speed measured by DVL by a scaling factor to eliminate scaling error;

[0058] Module M4: Eliminates lever arm error from the DVL speed obtained from module M3, and further corrects the DVL measurement value;

[0059] Module M5: The difference between the underwater vehicle velocity obtained from Module M2 and the corrected DVL velocity obtained from Module M4 is used as the measurement difference for the Kalman filter. The measurement is then updated using the Kalman filter to obtain the optimal estimate of the underwater vehicle's position and velocity.

[0060] Module M6: Calculates the attitude angles of the underwater vehicle using the position obtained from Module M5, and outputs the navigation position, velocity, and attitude angles.

[0061] Preferably, module M1 includes:

[0062] The MEMS IMU is installed at the center of gravity of the underwater vehicle. Using the initial position of the MEMS IMU as the origin, a navigation coordinate system is established with east as the x-axis, north as the y-axis, and the sky as the z-axis. A carrier coordinate system is established with the carrier axis pointing forward as the x-axis, leftward as the y-axis, and upward as the z-axis. Therefore, the initial position coordinates of the unmanned underwater vehicle are X0(0,0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0063] The module M2 includes: selecting the state variables of the Kalman filter as:

[0064] X = [xyv] x v y v z ] T

[0065] Where x, y are the position coordinates of the underwater vehicle along the x-axis and y-axis; v x ,v y ,v z The velocities of the underwater vehicle in the x, y, and z axes;

[0066] The state transition equation of the Kalman filter can be obtained from the equation of motion of the underwater vehicle:

[0067]

[0068] in, F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0069]

[0070] Where Δt is the time interval between the moments before and after the underwater vehicle;

[0071] Using velocity as the measurement, the measurement equation for the Kalman filter is:

[0072]

[0073] Where Z represents the measured value of the Kalman filter; H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z The z-axis velocity represents the speed of the underwater vehicle in the coordinate system of the vehicle carrier; W is the process noise, which follows a normal distribution W-(0,Q); U is the measurement noise, which follows a normal distribution U~(0,R); Q represents the covariance matrix of the process noise; R represents the covariance matrix of the measurement noise.

[0074] Discretizing the above Kalman filter yields:

[0075]

[0076] Among them, X k Z represents the state variables of the Kalman filter at time k; k H represents the measurement vector of the Kalman filter at time k; k The measurement equation for the Kalman filter at time k is Φ. k-1 W represents the state transition matrix after discretization of the Kalman filter at time k-1; k-1 U represents the process noise of the Kalman filter at time k-1; k-1 The measurement noise of the Kalman filter at time k-1 is represented; k represents the k-th time after the Kalman filter is discretized.

[0077] in:

[0078]

[0079] Where I represents the identity matrix; F k-1 This represents the state transition matrix of the Kalman filter before discretization at time k;

[0080] The expression for calculating the state variables backward is:

[0081] X k =Φ k-1 X k-1 +W k-1

[0082] The forward calculation of the error covariance matrix is ​​expressed as follows:

[0083]

[0084] Preferably, the module M3 includes: if the speed measured at the DVL center is Then multiply it by the scaling factor δk for correction, and obtain the velocity measured by DVL with itself as the center.

[0085] The module M4 includes: if the MEMS IMU and DVL are installed and fixed in place, let the position arm between the center of the MEMS IMU and the center of the DVL be l, and the angular acceleration output by the MEMS IMU be ω. k According to rigid body dynamics, the lever arm error of the DVL velocity measurement is ω×l. Subtracting the lever arm error from the velocity measured by the DVL with itself as the center, we obtain the velocity measurement value at the center of the IMU:

[0086] Preferably, the module M5 includes:

[0087] The measured values ​​of the Kalman filter are:

[0088]

[0089] in, This indicates the speed of the aircraft as measured by the IMU;

[0090] The Kalman filter gain is calculated using the following expression:

[0091] K k =P k|k-1 H T HP k|k-1 H T +R))

[0092] The optimal estimate for updating the Kalman filter is expressed as follows:

[0093]

[0094] The updated error covariance matrix is ​​expressed as follows:

[0095] P k|k =(IK k H)P k|k-1 .

[0096] Preferably, the module M6 includes:

[0097] Using the pitch angle θ and roll angle φ output by the MEMS IMU as true values, the yaw angle ψ is calculated using the underwater vehicle's positions at both ends. The expression is:

[0098]

[0099] Where, x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k.

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

[0101] (1) This invention uses attitude angle and acceleration information directly output from MEMS IMU to construct a 5D Kalman filter, which effectively overcomes the shortcomings of conventional underwater navigation and positioning algorithms, such as large computational load and low computational efficiency.

[0102] (2) The present invention eliminates the proportional error and lever arm error of DVL speed measurement, corrects the speed measurement results of DVL, and further improves the navigation and positioning accuracy of underwater vehicles. Attached Figure Description

[0103] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0104] Figure 1 This is a flowchart of the method of the present invention;

[0105] Figure 2 This is a comparison chart of GPS track and navigation track. Detailed Implementation

[0106] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0107] Example 1:

[0108] This invention proposes a navigation and positioning method for underwater vehicles based on a combination of MEMS IMU and DVL. The method includes MEMS IMU components and DVL components. It utilizes the attitude angle and acceleration information output by the MEMS IMU, and constructs a 5D Kalman filter by combining it with the motion model of the underwater vehicle. A scaling factor is used to correct the velocity measurement scaling error of the DVL. Based on the rigid body kinematics principle, the lever arm error between the DVL and MEMS IMU is corrected. The corrected velocity is then used as the observation value of the Kalman filter for filtering, resulting in accurate underwater vehicle position, velocity, and attitude information, thus achieving precise navigation and positioning of the underwater vehicle.

[0109] like Figure 1The main steps of the technical solution of the present invention include:

[0110] Step S1: The underwater vehicle remains stationary for a period of time, and the average attitude angle of the MEMS IMU during this period is taken. As the initial heading and attitude information of the underwater vehicle;

[0111] The MEMS IMU is installed at the center of gravity of the underwater vehicle. Using the initial position of the MEMS IMU as the origin, a navigation coordinate system is established with east as the x-axis, north as the y-axis, and the sky as the z-axis. A carrier coordinate system is established with the carrier axis pointing forward as the x-axis, leftward as the y-axis, and upward as the z-axis. Therefore, the initial position coordinates of the unmanned underwater vehicle are X0(0,0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0112] Step S2: The underwater vehicle navigates, and the attitude angle and acceleration information directly output from the MEMS IMU are used to perform time updates by Kalman filtering to calculate the position and velocity of the underwater vehicle.

[0113] Since the depth information of underwater vehicles is generally obtained directly from depth gauges, the position coordinates of the underwater vehicle along the z-axis are not considered, and the state variables of the Kalman filter are selected as follows:

[0114] X = [xyv] x v y v z ] T

[0115] Where x, y are the position coordinates of the underwater vehicle along the x-axis and y-axis; v x ,v y ,v z Let be the velocities of the underwater vehicle along the x, y, and z axes. Based on the motion equations of the underwater vehicle, the state transition equations of the Kalman filter can be obtained:

[0116]

[0117] F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0118]

[0119] Where Δt is the time interval between the moments before and after the underwater vehicle.

[0120] The measurement equation for the Kalman filter, obtained by measuring velocity, is as follows:

[0121]

[0122] Z represents the measurement value of the Kalman filter; H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z This represents the velocity along the z-axis in the coordinate system of the underwater vehicle carrier.

[0123] in:

[0124]

[0125] W is the process noise, which follows a normal distribution W-(0,Q), and U is the measurement noise, which follows a normal distribution U~(0,R).

[0126] Q represents the covariance matrix of process noise; R represents the covariance matrix of measurement noise;

[0127] Discretizing the above Kalman filter yields:

[0128]

[0129] X k Z represents the state variables of the Kalman filter at time k; k H represents the measurement vector of the Kalman filter at time k; k The measurement equation for the Kalman filter at time k is Φ. k-1 W represents the state transition matrix after discretization of the Kalman filter at time k-1; k-1 U represents the process noise of the Kalman filter at time k-1; k-1 The measurement noise of the Kalman filter at time k-1 is represented; k represents the k-th time after the Kalman filter is discretized.

[0130] in:

[0131]

[0132] I represents the identity matrix; F k-1 This represents the state transition matrix of the Kalman filter before discretization at time k;

[0133] Forward calculation of state variables:

[0134] X k =Φ k-1 X k-1 +W k-1

[0135] Forward calculation of the error covariance matrix:

[0136]

[0137] Step S3: Multiply the speed measured by DVL by a scaling factor to eliminate scaling error;

[0138] Considering that the speed measured by low-precision DVL usually has a large proportional error, if the speed measured by the center of the DVL... Multiplying it by the scaling factor δk and correcting it, we can obtain the velocity measured by DVL with itself as the center.

[0139] Step S4: Eliminate lever arm error from the DVL speed obtained in step S3, and further correct the DVL measurement value;

[0140] If the MEMS IMU and DVL are installed and fixed in place, let the position lever arm between the center of the MEMS IMU and the center of the DVL be l, and the angular acceleration output by the MEMS IMU be ω. k According to rigid body dynamics, the lever arm error in the DVL velocities measurement is ω×l. Subtracting the lever arm error from the velocity measured by the DVL around itself yields the velocity measurement value at the IMU center.

[0141]

[0142] Step S5: The difference between the underwater vehicle velocity obtained in step S2 and the corrected DVL velocity obtained in step S4 is used as the measurement difference for the Kalman filter. This is used to update the Kalman filter measurements, obtaining the optimal estimates of the underwater vehicle's position and velocity. The measured values ​​of the Kalman filter are:

[0143]

[0144] This indicates the speed of the aircraft as measured by the IMU;

[0145] Calculate the Kalman filter gain:

[0146] K k =P k|k-1 H T HP k|k-1 H T +R))

[0147] Update the optimal Kalman filter estimate:

[0148]

[0149] Update the error covariance matrix:

[0150] P k|k =(IK k H)P k|k-1

[0151] Step S6: Calculate the attitude angles using the position interpolation of the underwater vehicle obtained in step S5, and output the navigation position, velocity, and attitude angles.

[0152] Considering that the pitch angle θ and roll angle φ change relatively little during the underwater vehicle's navigation, and the error in the MEMS IMU output is also small, the pitch angle θ and roll angle φ output by the MEMS IMU are directly used as the true values. The yaw angle ψ, which has a larger variation error, can be calculated using the underwater vehicle's positions at the beginning and end of its journey.

[0153]

[0154] x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k.

[0155] Thus, the underwater vehicle's position (x, y), attitude (θ, φ, ψ), and velocity (v) are obtained. x ,v y ,v z The optimal estimate.

[0156] The performance of the above-mentioned underwater vehicle navigation and positioning method based on the combination of MEMS IMU and DVL is illustrated through the following experimental example. AUV navigation data was tested on the surface of a reservoir in Zhejiang Province to verify the combined navigation algorithm. The total distance was 3 kilometers, taking 28 minutes, and data from MEMS IMU, DVL, and differential GPS were collected simultaneously. GPS has high positioning accuracy when there are many satellites, but its update frequency is low, only one percent of that of the inertial navigation system. Furthermore, GPS is significantly affected by environmental factors. Considering the open and interference-free location, GPS was used as the benchmark. The combined navigation algorithm results of IMU and DVL were compared with the trajectory of differential GPS, as shown below. Figure 2 As shown in the figure. According to the comparison results of the implementation examples, the maximum positioning error of the MEMS IMU+DVL method is 30 meters, which is less than one percent of the total path length, indicating that the proposed method can achieve accurate navigation and positioning of underwater vehicles.

[0157] Example 2:

[0158] The present invention also provides an underwater vehicle navigation and positioning system based on a combination of MEMS IMU and DVL. The underwater vehicle navigation and positioning system based on a combination of MEMS IMU and DVL can be implemented by executing the process steps of the underwater vehicle navigation and positioning method based on a combination of MEMS IMU and DVL. That is, those skilled in the art can understand the underwater vehicle navigation and positioning method based on a combination of MEMS IMU and DVL as a preferred embodiment of the underwater vehicle navigation and positioning system based on a combination of MEMS IMU and DVL.

[0159] The underwater vehicle navigation and positioning system based on a combination of MEMS IMU and DVL provided by the present invention includes: Module M1: When the underwater vehicle is stationary, the average attitude angle of the MEMS IMU over a period of time is taken as the initial heading and attitude information of the underwater vehicle; Module M2: When the underwater vehicle is navigating, the attitude angle and acceleration information output by the MEMS IMU are used for time-updating Kalman filtering to calculate the position and velocity of the underwater vehicle; Module M3: The velocity measured by DVL is multiplied by a scaling factor to eliminate scaling error; Module M4: The DVL velocity obtained by Module M3 is used to eliminate lever error and further correct the DVL measurement value; Module M5: The difference between the underwater vehicle velocity obtained by Module M2 and the corrected DVL velocity obtained by Module M4 is used as the measurement difference of the Kalman filter to perform measurement update of Kalman filtering and obtain the optimal estimate of the position and velocity of the underwater vehicle; Module M6: The position of the underwater vehicle obtained by Module M5 is used to calculate the attitude angle and output the navigation position, velocity and attitude angle.

[0160] Module M1 includes: a MEMS IMU installed at the center of gravity of the underwater vehicle; a navigation coordinate system established with the initial position of the MEMS IMU as the origin, east as the x-axis, north as the y-axis, and the sky as the z-axis; and a carrier coordinate system established with the carrier axis forward as the x-axis, the carrier to the left as the y-axis, and the carrier upward as the z-axis. Therefore, the initial position coordinates of the unmanned underwater vehicle are X0(0,0), the initial velocity is V0(0,0,0), and the initial attitude angle is...

[0161] The module M2 includes: selecting the state variables of the Kalman filter as:

[0162] X = [xyv] x v y v z ] T

[0163] Where x, y are the position coordinates of the underwater vehicle along the x-axis and y-axis; v x ,v y ,v zThe velocities of the underwater vehicle in the x, y, and z axes;

[0164] The state transition equation of the Kalman filter can be obtained from the equation of motion of the underwater vehicle:

[0165]

[0166] in, F represents the state variables of the Kalman filter; F represents the state transition matrix of the Kalman filter.

[0167]

[0168] Where Δt is the time interval between the moments before and after the underwater vehicle;

[0169] Using velocity as the measurement, the measurement equation for the Kalman filter is:

[0170]

[0171] Where Z represents the measured value of the Kalman filter; H represents the measurement matrix of the Kalman filter; v x The velocity along the x-axis in the coordinate system of the underwater vehicle carrier; v y This represents the velocity along the y-axis in the coordinate system of the underwater vehicle; v z The z-axis velocity represents the speed of the underwater vehicle in the coordinate system of the vehicle carrier; W is the process noise, which follows a normal distribution W ~ (0, Q); U is the measurement noise, which follows a normal distribution U ~ (0, R); Q represents the covariance matrix of the process noise; R represents the covariance matrix of the measurement noise.

[0172] Discretizing the above Kalman filter yields:

[0173]

[0174] Among them, X k Z represents the state variables of the Kalman filter at time k; k H represents the measurement vector of the Kalman filter at time k; k The measurement equation for the Kalman filter at time k is Φ. k-11 W represents the state transition matrix after discretization of the Kalman filter at time k-1; k-1 U represents the process noise of the Kalman filter at time k-1; k-1 The measurement noise of the Kalman filter at time k-1 is represented; k represents the k-th time after the Kalman filter is discretized.

[0175] in:

[0176]

[0177] Where I represents the identity matrix; F k-1 This represents the state transition matrix of the Kalman filter before discretization at time k;

[0178] The expression for calculating the state variables backward is:

[0179] X k =Φ k-1 X k-1 +W k-1

[0180] The forward calculation of the error covariance matrix is ​​expressed as follows:

[0181]

[0182] The module M3 includes: if the speed measured at the DVL center is Then multiply it by the scaling factor δk for correction, and obtain the velocity measured by DVL with itself as the center.

[0183] The module M4 includes: if the MEMS IMU and DVL are installed and fixed in place, let the position arm between the center of the MEMS IMU and the center of the DVL be l, and the angular acceleration output by the MEMS IMU be ω. k According to rigid body dynamics, the lever arm error of the DVL velocity measurement is ω×l. Subtracting the lever arm error from the velocity measured by the DVL with itself as the center, we obtain the velocity measurement value at the center of the IMU:

[0184] The module M5 includes:

[0185] The measured values ​​of the Kalman filter are:

[0186]

[0187] in, This indicates the speed of the aircraft as measured by the IMU;

[0188] The Kalman filter gain is calculated using the following expression:

[0189] K k =P k|k-1 H T HP k|k-1 H T +R))

[0190] The optimal estimate for updating the Kalman filter is expressed as follows:

[0191]

[0192] The updated error covariance matrix is ​​expressed as follows:

[0193] P k|k =(IK k H)P k|k-1 .

[0194] The module M6 includes:

[0195] Using the pitch angle θ and roll angle φ output by the MEMS IMU as true values, the yaw angle ψ is calculated using the underwater vehicle's positions at both ends. The expression is:

[0196]

[0197] Where, x k The x-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k; y k This represents the y-axis position coordinate of the underwater vehicle in the navigation coordinate system at time k.

[0198] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0199] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for navigation and positioning of an underwater vehicle based on a combination of MEMS IMU and DVL, characterized in that, Comprising: Step S1: taking the average of the attitude angle of MEMSIMU in a period of time as the initial heading attitude information of the underwater vehicle when the underwater vehicle is static; Step S2: using the attitude angle and acceleration information output by MEMSIMU to perform time update of Kalman filtering, and calculating the position and speed of the underwater vehicle when the underwater vehicle is navigating; Step S3: multiplying the speed measured by DVL by a proportional factor to eliminate proportional error; Step S4: eliminating the lever arm error of the DVL speed obtained in step S3, and further correcting the DVL measurement value; Step S5: using the underwater vehicle speed obtained in step S2 and the corrected DVL speed obtained in step S4 to obtain the measurement difference of the Kalman filter, and performing measurement update of Kalman filtering to obtain the optimal estimation of the position and speed of the underwater vehicle; Step S6: calculating the attitude angle using the position of the underwater vehicle obtained in step S5, and outputting the navigation position, speed and attitude angle; Step S1 comprises: The MEMSIMU is installed at the center of gravity of the underwater vehicle, and a navigation coordinate system is established with the initial position of the MEMSIMU as a coordinate origin, east as an x-axis, north as a y-axis, and sky as a z-axis; a carrier coordinate system is established with the carrier axis forward as an x-axis, the carrier left as a y-axis, and the carrier upward as a z-axis; and the initial position coordinates of the unmanned underwater vehicle are X0(0, 0), the initial velocity is V0(0, 0, 0), and the initial attitude angle is The step S2 comprises: selecting the state variable of the Kalman filter as: X = [x y v x v y v z ] T Wherein, x, y are the position coordinates of the underwater vehicle in the x-axis and y-axis directions; v x y z v is the velocity of the underwater vehicle in the x-axis, y-axis and z-axis directions;​​ According to the motion equation of the underwater vehicle, the state transition equation of the Kalman filter can be obtained: wherein, x represents the state variable of the Kalman filter; F represents the state transition matrix of the Kalman filter; Wherein, Δt is the time interval between the front and back time of the underwater vehicle; Taking the speed as the measurement, the measurement equation of the Kalman filter is: wherein Z represents the measurement of the Kalman filter; H represents the measurement matrix of the Kalman filter; v x represents the x-axis direction velocity in the underwater vehicle carrier coordinate system; v y represents the y-axis direction velocity in the underwater vehicle carrier coordinate system; v z represents the z-axis direction velocity in the underwater vehicle carrier coordinate system; W is the process noise, subject to a normal distribution W~(0, Q); U is the measurement noise, subject to a normal distribution U~(0, R); Q represents the covariance matrix of the process noise; R represents the covariance matrix of the measurement noise; Discretize the above Kalman filter to obtain: wherein X k represents the state variable of the Kalman filter at the kth moment; Z k represents the measurement vector of the Kalman filter at the kth moment; H k represents the measurement equation of the Kalman filter at the kth moment; Φ k-1 represents the state transition matrix of the Kalman filter after discretization at the k-1th moment; W k-1 represents the process noise of the Kalman filter at the k-1th moment; U k-1 represents the measurement noise of the Kalman filter at the k-1th moment; k represents the kth moment after discretization of the Kalman filter; Wherein: where I denotes an identity matrix; F k-1 denotes the state transition matrix of the Kalman filter before discretization at the kth moment; Forward calculation of state variable, expression is: X k = Φ k-1 X k-1 + W k-1 Forward calculation of error covariance matrix, expression is:

2. The MEMS IMU and DVL combined based underwater vehicle navigation positioning method according to claim 1, wherein, The step S3 includes: if the speed measured by the DVL center is then it is corrected by multiplying it by a proportional factor δk, and the speed measured by the DVL with itself as the center is The step S4 includes: if the MEMSIMU and the DVL are installed and fixed, record the position between the center of the MEMSIMU and the center of the DVL as a lever arm l, and the angular acceleration output by the MEMSIMU as ω k According to the rigid body dynamics, the lever arm error of the DVL speed measurement is ω×l, and the speed measurement value of the center of the IMU is obtained by subtracting the lever arm error from the speed measured by the DVL with itself as the center:

3. The MEMS IMU and DVL combined based underwater vehicle navigation positioning method according to claim 2, wherein, The step S5 comprises: The measurement value of the Kalman filter is: wherein, V represents the vehicle speed measured by the IMU; Calculate the Kalman filtering gain, the expression is: K k = P k|k-1 H T (HP k|k-1 H T + R)) Update the optimal estimation of Kalman filtering, the expression is: Update the error covariance matrix, the expression is: P k|k = (I - K k H)P k|k-1 .

4. The MEMS IMU and DVL combined based underwater vehicle navigation positioning method according to claim 3, characterized in that, The step S6 comprises: Directly taking the pitch angle θ and roll angle φ output by MEMSIMU as the true value, and calculating the yaw angle ψ using the position of the underwater vehicle at the front and back times, the expression is: wherein x k represents the x-axis position coordinate of the underwater vehicle navigation coordinate system at the kth moment; y k represents the y-axis position coordinate of the underwater vehicle navigation coordinate system at the kth moment.

5. An underwater vehicle navigation and positioning system based on a MEMS IMU and DVL combination, characterized in that, Comprising: Module M1: taking the average of the attitude angle of MEMSIMU in a period of time as the initial heading attitude information of the underwater vehicle when the underwater vehicle is static; Module M2: using the attitude angle and acceleration information output by MEMSIMU to perform time update of Kalman filtering, and calculating the position and speed of the underwater vehicle when the underwater vehicle is navigating; Module M3: multiplying the speed measured by DVL by a proportional factor to eliminate proportional error; Module M4: eliminating the lever arm error of the DVL speed obtained in module M3, and further correcting the DVL measurement value; Module M5: using the underwater vehicle speed obtained in module M2 and the corrected DVL speed obtained in module M4 to obtain the measurement difference of the Kalman filter, and performing measurement update of Kalman filtering to obtain the optimal estimation of the position and speed of the underwater vehicle; Module M6: calculating the attitude angle using the position of the underwater vehicle obtained in module M5, and outputting the navigation position, speed and attitude angle; Module M1 comprises: The MEMSIMU is installed at the center of gravity of the underwater vehicle, and a navigation coordinate system is established with the initial position of the MEMSIMU as a coordinate origin, east as an x-axis, north as a y-axis, and sky as a z-axis; a carrier coordinate system is established with the carrier axis forward as an x-axis, the carrier left as a y-axis, and the carrier upward as a z-axis; and the initial position coordinates of the unmanned underwater vehicle are X0(0, 0), the initial velocity is V0(0, 0, 0), and the initial attitude angle is The module M2 comprises: selecting the state variable of the Kalman filter as: X = [x y v x v y v z ] T Wherein, x, y are the position coordinates of the underwater vehicle in the x-axis and y-axis directions; v x y z are the velocities of the underwater vehicle in the x-axis, y-axis and z-axis directions;​​ According to the motion equation of the underwater vehicle, the state transition equation of the Kalman filter can be obtained: wherein, x represents the state variable of the Kalman filter; F represents the state transition matrix of the Kalman filter; Wherein, Δt is the time interval between the front and back time of the underwater vehicle; Taking the speed as the measurement, the measurement equation of the Kalman filter is: Discretize the above Kalman filter to obtain: Wherein: Forward calculation of state variable, expression is: Forward calculation of error covariance matrix, expression is: The step S5 comprises: The measurement value of the Kalman filter is: Calculate the Kalman filtering gain, the expression is: Update the optimal estimation of Kalman filtering, the expression is: Update the error covariance matrix, the expression is: The step S6 comprises: Directly taking the pitch angle θ and roll angle φ output by MEMSIMU as the true value, and calculating the yaw angle ψ using the position of the underwater vehicle at the front and back times, the expression is: The measurement equation of the Kalman filter is: wherein Z represents the measurement of the Kalman filter; H represents the measurement matrix of the Kalman filter; v x represents the x-axis direction velocity in the underwater vehicle carrier coordinate system; v y represents the y-axis direction velocity in the underwater vehicle carrier coordinate system; v z represents the z-axis direction velocity in the underwater vehicle carrier coordinate system; W is the process noise, subject to a normal distribution W~(0, Q); U is the measurement noise, subject to a normal distribution U~(0, R); Q represents the covariance matrix of the process noise; and R represents the covariance matrix of the measurement noise. The Kalman filter is discretized as: wherein X k represents the state variable of the Kalman filter at the kth moment; Z k represents the measurement vector of the Kalman filter at the kth moment; H k represents the measurement equation of the Kalman filter at the kth moment; Φ k-1 represents the state transition matrix of the Kalman filter after discretization at the k-1th moment; W k-1 represents the process noise of the Kalman filter at the k-1th moment; U k-1 represents the measurement noise of the Kalman filter at the k-1th moment; k represents the kth moment after discretization of the Kalman filter; Wherein: where I denotes an identity matrix; F k-1 denotes the state transition matrix of the Kalman filter before discretization at the kth moment; The state variable is calculated forwardly, and the expression is: X k = Φ k-1 X k-1 + W k-1 The error covariance matrix is calculated forwardly, and the expression is:

6. The MEMS IMU and DVL combined based underwater vehicle navigation positioning system according to claim 5, wherein, The module M3 includes: if the speed measured by the DVL center is then it is corrected by multiplying it by a proportional factor δk, obtaining the speed measured by the DVL with its own center as The module M4 includes: if MEMSIMU and DVL are installed and fixed, the position between the center of MEMSIMU and the center of DVL is a lever arm l, and the angular acceleration output by MEMSIMU is ω k According to rigid body dynamics, the lever arm error of DVL speed measurement is ωxl, and the speed measurement value of the center of IMU is obtained by subtracting the lever arm error from the speed measured by DVL with itself as the center:

7. The MEMS IMU and DVL combined based underwater vehicle navigation positioning system according to claim 6, wherein, The module M5 comprises: The measurement value of the Kalman filter is: wherein, V represents the vehicle speed measured by the IMU; The Kalman filter gain is calculated, and the expression is: K k = P k|k-1 H T (HP k|k-1 H T + R)) The optimal estimation of the Kalman filter is updated, and the expression is: The error covariance matrix is updated, and the expression is: P k|k = (I - K k H)P k|k-1 .

8. The MEMS IMU and DVL combined based underwater vehicle navigation positioning system according to claim 7, wherein, The module M6 comprises: The pitch angle θ and the roll angle φ output by the MEMSIMU are directly taken as the true values, the yaw angle ψ is calculated by using the positions of the underwater vehicle at two times, and the expression is: wherein x k represents the x-axis position coordinate of the underwater vehicle navigation coordinate system at the kth moment; y k represents the y-axis position coordinate of the underwater vehicle navigation coordinate system at the kth moment.

Images