Error compensation method of DPC method for reduced synthetic aperture sonar fused with IMU

Abstract

The present invention discloses a method for reducing the error compensation of synthetic aperture sonar DPC method by fusing IMU, including: obtaining inertial sensor information of an underwater robot; using synthetic aperture sonar method to obtain motion information of the underwater robot; using error state Kalman filtering method to fuse the inertial sensor information and the motion information to obtain fused three-dimensional velocity information; comparing the fused three-dimensional velocity information with the preset error-free underwater robot velocity, and realizing motion compensation according to frequency domain phase compensation compensation of the original echo. The present invention fully considers the multi-sensor measurement error, and is more accurate than the traditional echo-based motion error compensation method and weakly coupled multi-sensor fusion method.

Description

Technical Field

[0001] The invention belongs to the technical field of sonar motion compensation and imaging, and in particular to an error compensation method of a DPC (Displaced Phase Center) method of reduced synthetic aperture sonar fused with an IMU (Inertial Measurement Unit). Background Art

[0002] at present (Synthetic Aperture Sonar ) is widely used in marine exploration, underwater landform mapping, submarine cable laying, etc. Because it solves the problem that the range of side-scan sonar depends on the resolution along the track and uses narrow waves and cannot solve the problem of path deviation. Synthetic aperture sonar provides both dense data for imaging and redundant data for motion compensation. Under ideal conditions, the carrier equipped with synthetic aperture sonar defaults to uniform linear motion. However, in the actual operation of sonar, due to the influence of sea breeze, ocean currents, etc., its motion trajectory often deviates greatly from the ideal trajectory. Related studies have shown that when the trajectory deviation reaches the wavelength When the temperature drops, the operation of synthetic aperture sonar will be seriously affected.

[0003] At present, there are two main methods for motion compensation of synthetic aperture sonar: one is the motion compensation method based on echo data, and the other is the motion compensation method based on multi-sensor fusion. Synthetic aperture sonar motion compensation methods based on echo data, such as equivalent phase center approximation. However, due to the long working time, the continuously accumulated process errors will lead to limited final accuracy. The other is to use sonar echo data and underwater navigation equipment for joint navigation, such as the motion compensation method of the joint Doppler velocimeter, but high-precision underwater navigation equipment cannot be widely used in engineering practice due to its high price. The motion method for joint IMU combined navigation is only a weak coupling method, and does not fully utilize IMU data for better error compensation. Summary of the invention

[0004] The purpose of the present invention is to propose an error compensation method for reducing the synthetic aperture sonar DPC method of fusion IMU, which fully considers the multi-sensor measurement error and is more accurate than the traditional echo-based motion error compensation method and weakly coupled multi-sensor fusion method.

[0005] To achieve the above object, the present invention provides a method for reducing the error compensation of synthetic aperture sonar DPC method by integrating IMU, comprising:

[0006] Obtain inertial sensor information of underwater robots;

[0007] Using synthetic aperture sonar method to obtain the motion information of the underwater robot;

[0008] The inertial sensing information and the motion information are fused by using an error state Kalman filter method to obtain fused three-dimensional velocity information;

[0009] The fused three-dimensional velocity information is compared with the preset error-free underwater robot velocity, and motion compensation can be achieved by compensating the original echo according to the frequency domain phase complement.

[0010] Optionally, the inertial sensing information includes: angular velocity information, acceleration information and attitude information, and the attitude information includes: yaw angle, pitch angle and roll angle information.

[0011] Optionally, the motion information includes: azimuth velocity information, distance velocity information or vertical velocity information.

[0012] Optionally, the motion information of the underwater robot is:

[0013]

[0014] in, is the speed of the underwater robot, To carry the range velocity information of synthetic aperture sonar, is the cosine of the yaw angle, is the cosine of the pitch angle.

[0015] Optionally, implementing motion compensation includes:

[0016] Based on the fused three-dimensional velocity information, a state model of the underwater robot is obtained;

[0017] Based on the state model, obtaining an error state model of the underwater robot;

[0018] Based on the error state model, obtaining an error state extrapolation model;

[0019] Based on the error state extrapolation model, obtaining a covariance extrapolation model;

[0020] Obtaining the Kalman gain based on the minimum covariance of the covariance extrapolation model;

[0021] Using the Kalman gain, the error state model is updated to obtain the optimized speed of the synthetic aperture sonar;

[0022] The speed of the optimized synthetic aperture sonar is decomposed and compared with the preset error-free underwater robot speed to solve the phase error, and then the original echo is compensated according to the frequency domain phase compensation to achieve the motion compensation.

[0023] Optionally, the state model of the underwater robot is:

[0024]

[0025] in, is the speed of the underwater robot, for All are zero bias, is gravity, is the state quantity of the underwater robot;

[0026] The error state model of the underwater robot is:

[0027]

[0028] in, They are arrive The speed, angular velocity, gyroscope bias, acceleration bias, and gravity at the moment, is the error state quantity of the underwater robot.

[0029] Optionally, the error state extrapolation model is:

[0030]

[0031] in, , They are The error state quantity and The error state quantity at time , For continuous time and The mapping relationship between is Gaussian noise with zero mean;

[0032] The covariance extrapolation model is:

[0033]

[0034] in, is the prediction covariance, is the covariance of the current estimate, F is the partial derivative of f with respect to the current estimate, is the covariance of the process noise.

[0035] Optionally, preprocessing the optimized synthetic aperture sonar velocity includes:

[0036] Converting the optimized synthetic aperture sonar velocity into celestial north-east three-dimensional velocity;

[0037] Integrate the three-direction speeds of the sky, north, east, and north respectively to obtain the actual routes of the three-direction speeds;

[0038] Performing least square fitting processing on the actual route to obtain an ideal route;

[0039] Based on the ideal route, using a preset geometric relationship, obtaining a sway error and a sinking error;

[0040] Based on the sway error and the depth error, the phase error is obtained, and then the motion compensation can be achieved by compensating the original echo according to the frequency domain phase compensation.

[0041] Optionally, the sway error and the depth error are:

[0042]

[0043] in, is the real path of the kth frame, is the ideal path for the kth frame, Indicates modulus, is the sway error, For deep error, The location of the target object.

[0044] Optionally, the phase error is:

[0045]

[0046] in, is the wavelength, is the sway error, For deep error, is the nanograzing angle, is the phase error.

[0047] The present invention has the following beneficial effects:

[0048] The present invention adopts a tightly coupled method to fuse the speed information estimated by the synthetic aperture sonar on the underwater autonomous robot, the acceleration information, angular velocity information and posture information collected by low-cost inertial sensors, estimate the motion of the underwater robot, and perform motion compensation on the synthetic aperture sonar DPC method. The present invention fully considers the multi-sensor measurement error and is more accurate than the traditional echo-based motion error compensation method and the weakly coupled multi-sensor fusion method. The present invention uses three-dimensional variables to express the rotation increment in rotation processing to avoid the use of higher dimensions. The present invention is more stable in terms of numerical value and will not have the problem of insufficient linearization. The present invention belongs to small-quantity kinematics and avoids the calculation of complex Jacobian matrices in most cases. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0050] Figure 1 It is a flow chart of an error compensation method for reducing synthetic aperture sonar DPC method by integrating IMU according to an embodiment of the present invention;

[0051] Figure 2 This is a schematic diagram of the equivalent phase center of a single-transmitter and single-receiver according to an embodiment of the present invention;

[0052] Figure 3 A schematic diagram of a single-transmit multiple-receive equivalent phase center of an embodiment of the present invention;

[0053] Figure 4 A schematic diagram of velocity vector decomposition estimation using the DPC method according to an embodiment of the present invention;

[0054] Figure 5 Schematic diagram of surge estimation of equivalent phase center array of adjacent frames according to an embodiment of the present invention;

[0055] Figure 6 A schematic diagram of a trajectory and error model according to an embodiment of the present invention;

[0056] Figure 7 Schematic diagram of the ESKF data fusion algorithm flow in an embodiment of the present invention. DETAILED DESCRIPTION

[0057] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0058] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0059] like Figure 1 As shown, the error compensation method of the synthetic aperture sonar DPC method of fusion IMU proposed in this embodiment includes the following steps:

[0060] Obtain inertial sensor information of underwater robots;

[0061] Use synthetic aperture sonar to obtain motion information of underwater robots;

[0062] The error state Kalman filter method is used to fuse the inertial sensor information and motion information to obtain the fused three-dimensional velocity information.

[0063] The fused three-dimensional velocity information is compared with the preset ideal motion state of the underwater robot to obtain motion compensation data, and the underwater robot is compensated based on the motion compensation data.

[0064] The inertial sensor collects the angular velocity information, acceleration information and attitude information of the underwater robot, and the synthetic aperture sonar estimates the motion information of the underwater robot; the attitude information collected by the inertial sensor includes the yaw angle information, the pitch angle information and the roll angle information; the motion information estimated by the synthetic aperture sonar includes one or more of the azimuth velocity information, the range velocity information or the vertical velocity information.

[0065] By fusing the two types of information through the error iterative Kalman filter method, the motion information of the underwater autonomous robot at any time can be estimated. By comparing it with the motion in the ideal state, motion compensation can be obtained to reduce the synthetic aperture sonar The motion error caused by the method.

[0066] The working principle of synthetic aperture sonar estimation of motion is as follows: Figure 2 As shown, the intersection of x and y is a minimum transmitting unit. As a minimum receiving unit, the equivalent phase center approximation method is used. and Equivalent to equivalent phase center For the transmitting unit and the measuring object The real distance between For receiving unit and measuring object The real distance between is the true equivalent sound path, is the equivalent phase center error.

[0067] Figure 4 The figure shows a single-transmit multiple-receive equivalent phase center array, which uses an equivalent phase center array to replace 12 receiving arrays and one transmitting array. It is a receiving unit used for imaging. It is the receiving unit used for pose estimation. Indicates the distance between two receiving units, using Two equivalent phase center units after the method The distance between them is half of the receiving unit. Figure 5 It is the principle of acoustic correlation velocity measurement based on the overlapping phase center of synthetic aperture sonar. Assume that an equivalent phase center array of the synthetic aperture sonar contains Equivalent phase center units, and are numbered sequentially. , and the distance between two adjacent equivalent phase centers is Assume that the underwater robot The velocity along the azimuth Movement and displacement However, due to the existence of motion error, the sonar also produces displacement in the slant range direction. , (x represents the slope direction). Assume Frame and The time interval between frames is (sampling frequency), Figure 5 The relationship between the displacement in the azimuth direction and the velocity in the azimuth direction during one pulse cycle is:

[0068]

[0069] In the formula, is the number of overlapping phase centers, is the distance between two equivalent phase center units.

[0070] In the continuous sonar movement Frame and In the frame, To indicate the Frame No. The overlapped phase center unit is obtained in the sonar signals, and represents the total number of equivalent phase centers of an array. The sampled signals are marked as ,and Indicates the total number of signals. In addition, To indicate the Frame No. The overlapped phase centers are obtained at sonar signals, and The windowed signal is expressed as:

[0071]

[0072] In the formula, Indicates Frame No. The phase center is obtained The signal is Windowing processing, and Represents the windowed signal.

[0073] We can get formula (4)

[0074]

[0075] In the formula, * represents the convolution operation, Indicates The time delay dimension, They are and simplification.

[0076] Take the time delay dimension of formula (4) middle The maximum value of can be obtained by formula (5):

[0077]

[0078] For all pairs of equivalent phase center units in adjacent frames, the cross-correlation coefficient matrix (6) can be obtained by performing operations (4) and (5)

[0079]

[0080] In the formula, The equivalent phase center can be obtained The mutual interference coefficient Order cross-correlation coefficient matrix.

[0081] Mutual Coherence Coefficient Matrix Taking the average values ​​in the direction parallel to the main diagonal, we can get formula (7):

[0082]

[0083] In the formula, Starting from the lower left corner The sub-diagonal elements, Represents the total number of phase center units in the equivalent phase center array. The equivalent phase center unit can be calculated by formula (7) to obtain Mean .

[0084] Obey the Gaussian interpolation kernel formula (8)

[0085]

[0086] In the formula, represents the peak correlation coefficient, represents the peak width, represents the peak deviation. According to formula (8), formula (9) can be obtained:

[0087]

[0088] In the formula, Indicates the maximum value of the independent variable operation, represents the maximum value of the mutual interference coefficient. According to formula (9), the number of phase center units that the k-th frame passes through relative to the k-1-th frame can be obtained as formula (10).

[0089]

[0090] According to formula (1) and We can get formula (11)

[0091]

[0092] From formula (11), we can get the azimuth velocity: for:

[0093]

[0094] The SAR and inertial sensors are rigidly connected, so the SAR and inertial sensors have the same motion information. Figure 3 It can be seen that the relationship between sonar speed and three-way speed is:

[0095]

[0096] In the formula, represents the distance velocity, represents the azimuth velocity, Indicates the vertical velocity; Respectively represent the yaw angle and pitch angle information, Indicates the actual speed of the sonar.

[0097] According to formula (12) and formula (13), formula (14) can be obtained:

[0098]

[0099] From formula (13), we can get: The speed of the underwater robot estimated by this method is:

[0100]

[0101] This embodiment integrates synthetic aperture sonar, inertial sensor The specific steps of the cumulative error compensation method are as follows:

[0102] (1) During the navigation of the underwater autonomous robot, the synthetic aperture sonar it carries The azimuth velocity, the range velocity or the vertical velocity is obtained by processing and estimating.

[0103] (2) During the navigation of the underwater autonomous robot, the inertial sensor rigidly connected to the synthetic aperture sonar measures acceleration information, heading angle, roll angle, pitch angle information, and angular velocity information.

[0104] In the present invention, if other sensors are introduced, for example, a Doppler velocimeter introduces speed information constraints The larger the error, the higher the accuracy.

[0105] The following example illustrates the data fusion method used in this embodiment at a fixed depth.

[0106] The error state Kalman filter method is used to fuse the motion information and attitude information obtained by the synthetic aperture sonar system and the inertial sensor, and jointly estimate more accurate motion information and attitude information to perform motion compensation for the synthetic aperture sonar. Example of compensation of errors.

[0107] The state quantity of the underwater robot is as shown in formula (16):

[0108]

[0109] in, For speed, For rotation, is zero bias, For gravity.

[0110] According to kinematics, the equation formula is:

[0111]

[0112] In the formula, is the acceleration measured by the inertial sensor; is the angular velocity measured by the inertial sensor; represents the Lie algebra to Lie group, for example in . And the above formula can be simplified to:

[0113]

[0114] The error state of the underwater robot is expressed as formula (22)

[0115]

[0116] In the formula, They are arrive The speed, angular velocity, gyroscope bias, acceleration bias, and gravity at each moment.

[0117] According to formula (22), we can get:

[0118]

[0119] In the formula, is an antisymmetric matrix; Measuring noise for speed; They are angular velocity noise, gyroscope bias noise, and acceleration bias noise. The noise here does not participate in the iteration, and the error state extrapolation equation can be obtained:

[0120]

[0121] The linearization of formula (28) yields the formula

[0122]

[0123] Formula (23)-(27) and formula (29) can be used to obtain F

[0124]

[0125] Applying the covariance to formula (29) yields the covariance extrapolation equation:

[0126]

[0127] In the formula, for The uncertainty of the state quantity at each moment, is the time t Estimation of the uncertainty at the moment. Where Q is the formula (32)

[0128]

[0129] In the formula, Represents covariance calculation.

[0130] The observation equation of the underwater robot is formula (33)

[0131]

[0132] In the formula, is the measurement noise, and obey The normal distribution of for:

[0133]

[0134] Figure 7 Given Static initialization: let the underwater robot stand still for a period of time, and calculate the average value of the gyroscope and the average value of the acceleration. ,at this time , , default when stationary Direction . Assume that the state quantity at time n is estimated, and the state quantity at time n-1 is used for extrapolation; the error state quantity is used to extrapolate the error state quantity at time n; the covariance at time n-1 is used for covariance extrapolation; and the Kalman gain is obtained based on the minimum covariance; the error state is updated using the Kalman gain, the state is updated, and the covariance is updated. Repeat the iteration once to obtain the state quantity at time n.

[0135] After prediction and updating, the speed at any time can be predicted as formula (35)

[0136]

[0137] Formula (35) and formula (13) can optimize the speed of synthetic aperture sonar at any time .

[0138] The following are the specific steps of motion compensation: The key to synthetic aperture sonar motion compensation is to calculate the translation error. Among the translation errors, the sway error and the depth error have the greatest impact on the echo phase. The phase error can be expressed as formula (36)

[0139]

[0140] In the formula, is the wavelength, is the sway error, For deep error, is the nanograzing angle, such as Figure 6 .

[0141] Convert the optimized sonar speed into the three-dimensional speed of the sky, north and east, and integrate the speed of the sky, north and east to get the actual route , use interpolation to get the ideal track , according to Figure 6 The geometric relationship shown can be used to obtain the sway error and depth error as formula:

[0142]

[0143] In the formula, is the real path of the kth frame; is the ideal path for the kth frame represents modulus, and T is the target object.

[0144] Substituting the results of (37) and (38) into (36) we can get the phase error. Motion compensation can be achieved by compensating the original echo in the frequency domain.

[0145] This embodiment adopts a tightly coupled method to combine the velocity information estimated by the synthetic aperture sonar on the underwater autonomous robot with the acceleration information, angular velocity information and attitude information collected by the low-cost inertial sensor to estimate the motion of the underwater robot. The present embodiment fully considers the multi-sensor measurement error and is more accurate than the traditional echo-based motion error compensation method and the weakly coupled multi-sensor fusion method. The present embodiment uses three-dimensional variables to express the rotation increment in the rotation processing to avoid the use of higher dimensions. The present embodiment is more stable in terms of numerical value and will not have the problem of insufficient linearization. The present embodiment belongs to small-volume kinematics and avoids the calculation of complex Jacobian matrices in most cases.

[0146] The above is only a preferred specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. Error compensation method of DPC method for reducing synthetic aperture sonar by integrating IMU, It is characterized in that include: Obtain inertial sensor information of underwater robots; Using synthetic aperture sonar method to obtain the motion information of the underwater robot; The inertial sensing information and the motion information are fused by using an error state Kalman filter method to obtain fused three-dimensional velocity information; The fused three-dimensional velocity information is compared with the preset error-free underwater robot velocity, and the original echo is compensated according to the frequency domain phase compensation to achieve motion compensation; Implementing motion compensation includes: Based on the fused three-dimensional velocity information, a state model of the underwater robot is obtained; Based on the state model, obtaining an error state model of the underwater robot; Based on the error state model, obtaining an error state extrapolation model; Based on the error state extrapolation model, obtaining a covariance extrapolation model; Obtaining the Kalman gain based on the minimum covariance of the covariance extrapolation model; Using the Kalman gain, the error state model is updated to obtain the optimized speed of the synthetic aperture sonar; The speed of the optimized synthetic aperture sonar is decomposed and compared with the preset error-free underwater robot speed to solve the phase error, and then the original echo is compensated according to the frequency domain phase compensation to achieve the motion compensation.

2. The error compensation method of DPC method for reducing synthetic aperture sonar by fusion IMU according to claim 1, It is characterized in that The inertial sensing information includes: angular velocity information, acceleration information and attitude information, and the attitude information includes: yaw angle, pitch angle and roll angle information.

3. The error compensation method of DPC method for reducing synthetic aperture sonar by fusion IMU according to claim 1, It is characterized in that The motion information includes: azimuth velocity information, distance velocity information or vertical velocity information.

4. The error compensation method of DPC method for reducing synthetic aperture sonar by fusion IMU according to claim 1, It is characterized in that The motion information of the underwater robot is: in, is the speed of the underwater robot, To carry the range velocity information of synthetic aperture sonar, is the cosine of the yaw angle, is the cosine of the pitch angle.

5. The error compensation method of DPC method for reducing synthetic aperture sonar by integrating IMU according to claim 1, It is characterized in that The state model of the underwater robot is: in, is the speed of the underwater robot, For rotation, All are zero bias, is gravity, is the state quantity of the underwater robot; The error state model of the underwater robot is: in, They are arrive The speed, angular velocity, gyroscope bias, acceleration bias, and gravity at the moment, is the error state quantity of the underwater robot.

6. The error compensation method of DPC method for reducing synthetic aperture sonar by integrating IMU according to claim 1, It is characterized in that The error state extrapolation model is: in, , They are The error state quantity and The error state quantity at time , For continuous time and The mapping relationship between is Gaussian noise with zero mean; The covariance extrapolation model is: in, is the prediction covariance, is the covariance of the current estimate, F is the partial derivative of f with respect to the current estimate, is the covariance of the process noise.

7. The error compensation method of DPC method for reducing synthetic aperture sonar by integrating IMU according to claim 1, It is characterized in that Decomposing the optimized synthetic aperture sonar velocity, comparing it with the preset error-free underwater robot velocity to solve the phase error, and then compensating the original echo according to the frequency domain phase compensation to achieve the motion compensation includes: Converting the optimized synthetic aperture sonar velocity into celestial north-east three-dimensional velocity; Integrate the three-direction speeds of the sky, north, east, and north respectively to obtain the actual routes of the three-direction speeds; Performing least square fitting processing on the actual route to obtain an ideal route; Based on the ideal route, using a preset geometric relationship, obtaining a sway error and a sinking error; Based on the sway error and the depth error, the phase error is obtained, and then the motion compensation can be achieved by compensating the original echo according to the frequency domain phase compensation.

8. The error compensation method of DPC method for reducing synthetic aperture sonar by integrating IMU according to claim 7, It is characterized in that The sway error and the depth error are: in, is the real path of the kth frame, is the ideal path for the kth frame, Indicates modulus, is the sway error, For deep error, is the location of the target object, represents the nanograzing angle.

9. The error compensation method of DPC method for reducing synthetic aperture sonar by integrating IMU according to claim 7, It is characterized in that The phase error is: in, is the wavelength, is the sway error, For deep error, is the nanograzing angle, is the phase error.

Images