ROV positioning navigation system and method based on visual inertial navigation fusion and pose correction

Abstract

The invention discloses an ROV positioning navigation system based on vision inertial navigation fusion and pose correction, which comprises an inertial measurement value and image preprocessing module loaded on an ROV, the inertial measurement value and image preprocessing module is connected with a vision and inertial navigation data fusion module through a wire, and the vision and inertial navigation data fusion module is connected with a pose correction module. The invention also discloses a positioning navigation method of the ROV positioning navigation system based on visual inertial navigation fusion and pose correction. According to the method, the inertial data provided by the IMU and the environment characteristic information captured by the visual sensor are fused, high-precision estimation of the position and attitude of the ROV can be realized, the sensor data are optimally fused by using the MSCKF algorithm, the influence of the error of a single sensor on the positioning result is reduced, and the positioning precision and reliability of underwater operation are remarkably improved.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater navigation and positioning systems, relates to an ROV positioning and navigation system based on visual inertial navigation fusion and posture correction, and also relates to a positioning and navigation method of the system. Background Art

[0002] The navigation and positioning technology of remotely operated vehicles (ROVs) plays a vital role in marine exploration projects. It is the basic premise for ROVs to accurately complete tasks such as seabed topography surveys, deep-sea resource exploration, underwater archaeology, biological surveys, and underwater pipeline monitoring. Although ROVs have been widely used in practical projects, most of the current underwater navigation and positioning methods are costly or heavily dependent on external information, which restricts the development of marine exploration technology in terms of robustness, stability, and accuracy. Therefore, it is crucial to develop a technical solution that can provide a higher-performance underwater navigation and positioning algorithm for ROVs in marine exploration projects.

[0003] The accuracy of traditional ROV positioning and navigation systems is limited in the presence of magnetic field interference or complex underwater environments. The high-precision navigation system is part of the core technology of ROV and faces the dual challenges of the complex and changeable underwater environment and the demand for high reliability and high stability. Achieving precise ROV navigation has become an extremely challenging task. Summary of the invention

[0004] The purpose of the present invention is to provide an ROV positioning and navigation system based on visual inertial navigation fusion and posture correction, which solves the problem of limited accuracy of existing positioning and navigation systems in complex underwater environments.

[0005] Another object of the present invention is to provide a positioning and navigation method for an ROV positioning and navigation system based on visual inertial navigation fusion and posture correction.

[0006] The technical solution adopted by the present invention is an ROV positioning and navigation system based on visual inertial navigation fusion and posture correction, including an inertial measurement value and image preprocessing module mounted on the ROV, the inertial measurement value and image preprocessing module is connected to a visual and inertial navigation data fusion module through a wire, and the visual and inertial navigation data fusion module is connected to a posture correction module.

[0007] The technical solution of the present invention is also characterized in that:

[0008] The inertial measurement value and image preprocessing modules are respectively a strapdown inertial navigation system and a visual sensor, and both the strapdown inertial navigation system and the visual sensor are connected to the visual and inertial navigation data fusion module.

[0009] The vision and inertial navigation data fusion module includes a multi-state constrained Kalman filter, the multi-state constrained Kalman filter is connected with the strapdown inertial navigation system and the vision sensor, and the multi-state constrained Kalman filter is connected with the posture correction module.

[0010] Another technical solution adopted by the present invention is a positioning and navigation method of an ROV positioning and navigation system based on visual inertial navigation fusion and posture correction, which is specifically implemented according to the following steps:

[0011] Step 1: Install a strapdown inertial navigation system and a visual sensor at the front end of the ROV. The inertial navigation sensor in the inertial navigation system obtains the position and attitude information of the body, and the visual sensor detects the feature points in the image and captures the real-time acceleration angular velocity and the surrounding environment image;

[0012] Step 2: Use underwater image enhancement algorithm to optimize the image quality of the captured image, complete image feature extraction, tracking and mismatch elimination tasks, and perform SINS trajectory solution on the data obtained by the inertial navigation sensor to obtain the initial estimated information of the body posture;

[0013] Step 3: Based on the EKF, the pose information in step 2 is introduced into the state constraint by using the multi-state constrained Kalman filter, and the sliding window technology is used to optimize the calculation efficiency to achieve the tight coupling of the high-precision inertial measurement unit and the visual sensor data;

[0014] Step 4: The tightly coupled information in step 3 is converted into a posture correction module through an underwater visual dictionary based on the bag-of-words model, the Harris corner detection algorithm, and the TF-IDF algorithm. Two main correction strategies, namely, visual loop detection technology and outer loop posture correction, are introduced to improve the accuracy and reliability of navigation and positioning, and to achieve precise navigation and positioning of the ROV in an underwater environment.

[0015] Another technical solution of the present invention is characterized in that:

[0016] In step 1, the initial estimated information of the body posture is obtained by performing SINS trajectory solution on the acquired data, which is specifically:

[0017] Step 1.1: The system attitude solution uses attitude quaternions to represent the rotation of the three-dimensional coordinate system in space: Assuming that the angular velocity is constant per unit time, the attitude quaternion uses zero-order integration. When the inertial coordinate system {I} moves relative to the ground reference system {G}, the rate of change or the derivative of the corresponding quaternion describing the relationship is obtained by calculating the limit of the difference quotient, and the following is derived based on the properties of the quaternion:

[0018]

[0019] In the formula, ω m ——The measured value of angular velocity expressed in the {I} frame, ωm =[ω x ω y ω z ] T ;

[0020] ——ω m The antisymmetric matrix of

[0021] Using the Picard method, different mathematical formulas are used to update the quaternion depending on whether the angular velocity approaches 0:

[0022]

[0023] When using quaternions When performing rotation representation, the equation should be satisfied:

[0024]

[0025] Since there are certain errors in the calculation, Perform normalization:

[0026]

[0027] Based on the relative relationship of vector projection, select an arbitrary vector R, assume that the {I} system can reach the {G} system by rotating around the rotation axis q, and the R before and after the rotation I , R G The projections are R I =x I i+y I j+z I k, R G =x G i+y G j+z G k;

[0028] Based on the quaternion representation of rotation, we can get:

[0029]

[0030] Among them, the axis q is the rotation quaternion The corresponding vector part in ;

[0031] The vector R I and R G They are expressed in the form of quaternions:

[0032]

[0033] Combining the above formula, we can get:

[0034] xG i+y G j+z G k=(q0+q1i+q2j+q3k)(x I i+y I j+z I k)(q0-q1i-q2j-q3k) (7)

[0035] According to quaternion multiplication, the above formula is expanded and written in matrix form:

[0036]

[0037] The strapdown matrix represented by quaternion can be expressed as:

[0038]

[0039] Strapdown Matrix There is a corresponding relationship with the transformation matrix J1, so the body attitude angle can be solved by the following formula:

[0040]

[0041] The fourth-order Runge-Kutta method is used to recursively deduce the velocity and position information. The specific calculation process is as follows:

[0042]

[0043] The specific calculation process of the speed at the next moment is as follows:

[0044]

[0045] The specific calculation process for the next moment position is as follows:

[0046]

[0047] The Runge-Kutta coefficients k in the velocity and position prediction formulas are:

[0048]

[0049] In the measurement process of step 1.2 and step 1.1, there will be interference information from various factors, the most common of which is Gaussian white noise. Taking the corresponding noise into consideration, parameterized modeling of IM is performed, and the noise model of IMU can be expressed as follows:

[0050]

[0051] The established IMU measurement model will be used in the subsequent vision and inertial navigation data fusion algorithm.

[0052] In step 2, an image enhancement algorithm is used to preprocess underwater visual information, as follows:

[0053] Step 2.1, contrast adaptive histogram equalization is used to segment the image and locally apply histogram equalization. By implementing contrast limitation and combining bilinear interpolation technology, the image after quality optimization is subjected to feature extraction, tracking and mismatch elimination tasks. The system uses features based on accelerated segmentation test to detect feature points, matches the feature points extracted in the current frame with the feature points of adjacent frames, establishes the correspondence between these feature points in continuous image frames, and realizes the association of feature points between images. The system uses LK optical flow method to track the movement process of feature points to characterize the motion state of the camera, so as to achieve the purpose of ROV pose estimation.

[0054] Step 2.2: In step 2.1, the RANSAC algorithm is used to eliminate false matches from the multiple feature point pairs obtained by matching. Samples are randomly selected from the matching data set, and consistent data is found. The RANSAC algorithm combines the observed data with the parameterized model and continuously cycles the test data. The goal is to find a 3×3 optimal homography matrix H and retain the required results. The mathematical model can be expressed as follows:

[0055]

[0056] The MSCKF algorithm in step 3 is as follows:

[0057] Step 3.1: In order to fuse the IMU and visual sensor data to estimate the underwater robot's posture, the system state vector is selected as:

[0058]

[0059] To ensure the stability of the filter and avoid the singularity of the covariance matrix, the state quantity is described as the form of error: that is, the error of the estimate of a quantity x is x is the true value of the state quantity, is the estimated value of the state quantity. Different error definitions are used for the quaternion state quantity. If is a quaternion The estimated value of the direction error is given by the error quaternion Description, which consists of the relationship Definition, Symbol Represents quaternion multiplication, the error quaternion is:

[0060]

[0061] The error state quantity of IMU is expressed as:

[0062]

[0063] The error of the i-th camera pose is expressed as

[0064] The error state vector of the entire system is

[0065] Since the IMU measurement value is a discrete quantity, the IMU measures the signal ω with a period T. m and a m Sampling is performed, and these measurement results are used for state propagation. The discrete form of the state equation is as follows:

[0066]

[0067] Using Taylor's formula to expand and further discretize, we can get the noise covariance matrix Q at time k. k And the state transfer matrix Φ of the IMU from time k to time k+1 k as follows:

[0068]

[0069] Noise covariance matrix Q k and the state transfer matrix Φ k , the IMU state covariance matrix from time k to time k+1 can be calculated:

[0070]

[0071] When the state of k-1 is recursively deduced to time k according to the IMU, the covariance of the corresponding state must become larger, where represents the covariance of the IMU state at time k, Q k It represents the covariance of the influence of the IMU error on the state from k-1 to k. The actual covariance matrix is ​​as follows:

[0072]

[0073] Assume that in M j Camera poses In the same feature point f j is observed continuously, i∈S j , at each camera pose, this feature point is described by the model:

[0074]

[0075] Among them, n j,i is the 2×1 dimensional image noise vector when camera i observes the jth feature point at time, and the feature position represented in the camera frame for:

[0076]

[0077] Step 3.2: In step 3.1, the feature position in the ground coordinate system is unknown, and the least squares method is used to minimize the error to obtain the estimated values ​​of these feature positions The observation items are a series of points on the normalized plane, and the errors are the errors in the x and y directions on the normalized plane. After the feature position is estimated, the measurement residual is calculated:

[0078]

[0079] In the formula, z j,i and They are respectively the observed value and estimated value of the j-th feature point when observing the camera pose at the i-th moment;

[0080] The estimation of feature position and camera pose is linearized and can be expressed in a linear form:

[0081]

[0082] H j,i and They are the Jacobian matrices of the observations with respect to the feature position and state, and all M j The residuals of the measured values ​​are superimposed to obtain:

[0083]

[0084] Since EKF is used for state estimation, for constructing the measurement model, according to the general form Define a linear dependence on the state error The residual r of is sufficient. The noise term must be zero-mean white noise and uncorrelated with the state error to apply the EKF framework, where H is the measurement Jacobian matrix; error With error is related, so the residual r j This is inconsistent with the general form, which makes it unsuitable for direct application in the measurement update process of EKF. j Projection to matrix H j The left null space V T get The redefined new residual is consistent with the general form, that is,

[0085] V T H j =0 (31)

[0086]

[0087] Finally, the observation equations of all feature points are superimposed to obtain the final form of the observation equation:

[0088]

[0089] After the observation model is established, the filter will update the observation data only when specific preset conditions are met.

[0090] Step 4 is as follows: the system uses visual loop detection technology. When the ROV passes a previously visited location, it detects and corrects possible posture deviations by comparing the current image with the historical image. When the ROV has not passed the historical location, the system uses known landmarks to correct the outer ring posture and calibrates the current posture estimate through the precise position information of the landmarks.

[0091] The beneficial effects of the present invention are:

[0092] 1) The present invention can achieve high-precision estimation of the ROV position and posture by fusing the inertial data provided by the IMU and the environmental feature information captured by the visual sensor. The MSCKF algorithm is used to optimally fuse the sensor data, thereby reducing the impact of a single sensor error on the positioning result and significantly improving the positioning accuracy and reliability of underwater operations.

[0093] 2) The present invention combines the MSCKF algorithm with visual loop detection technology. The system exhibits stronger robustness when facing underwater environmental changes, sensor noise and dynamic disturbances, ensuring the continuity and reliability of navigation positioning.

[0094] 3) The present invention adopts an innovative posture correction method. When the ROV passes a known position, the underwater loop detection technology corrects the accumulated error in time through visual recognition and matching. The outer ring posture correction based on the marker point corrects the long-term positioning drift of the system by identifying the preset underwater marker points, and the positioning result is stable.

[0095] 4) The present invention takes into account both real-time performance and cost-effectiveness. By optimizing the algorithm and filter design, it can achieve fast data processing and status update to meet the needs of real-time navigation. The sensors and algorithms used have low costs, making high-precision underwater navigation technology more economical and easy to popularize.

[0096] 5) The present invention provides an underwater navigation and positioning solution that is independent of external signals or equipment, is cost-effective, and is easy to deploy and maintain. It has good autonomy and universality. Both small reconnaissance ROVs and large operational ROVs can be applied to the present invention, and is suitable for a variety of underwater operating environments such as deep-sea exploration, underwater archaeology, and seabed construction monitoring. BRIEF DESCRIPTION OF THE DRAWINGS

[0097] Figure 1 It is the overall block diagram of the system of the present invention;

[0098] Figure 2 is a system schematic diagram of the present invention;

[0099] Figure 3 It is a physical configuration diagram of the sensors carried by the system of the present invention;

[0100] Figure 4 is a schematic diagram of sensor data flow of the present invention;

[0101] Figure 5 It is a flowchart of the strapdown inertial navigation system posture settlement process of the present invention. DETAILED DESCRIPTION

[0102] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0103] The present invention is based on the ROV positioning and navigation system of visual inertial navigation fusion and posture correction, such as Figure 1-4 As shown, it includes an inertial measurement value and image preprocessing module mounted on the ROV, the inertial measurement value and image preprocessing module is connected to a visual and inertial navigation data fusion module through a wire, and the visual and inertial navigation data fusion module is connected to a posture correction module. By mounting a strapdown inertial navigation system and a visual sensor on the ROV to capture real-time environmental images, the initial estimation information of the body posture is obtained after image preprocessing, and the information is introduced into the visual and inertial navigation data fusion module using MSCKF state constraints to achieve tight coupling of sensor data, and further introduced into the posture correction module to correct the navigation strategy.

[0104] The inertial measurement value and image preprocessing modules are respectively a strapdown inertial navigation system (including an IMU sensor) and a visual sensor, and both the strapdown inertial navigation system and the visual sensor are connected to the visual and inertial navigation data fusion module.

[0105] The vision and inertial navigation data fusion module includes a multi-state constrained Kalman filter (MSCKF), which is connected to the strapdown inertial navigation system and the visual sensor, and is connected to the posture correction module; the posture correction module includes an underwater visual dictionary based on the bag-of-words model, Harris corner detection algorithm and TF-IDF.

[0106] The positioning and navigation method of the ROV positioning and navigation system based on visual inertial navigation fusion and posture correction is implemented in the following steps:

[0107] Step 1: Install the strapdown inertial navigation system and visual sensor at the front end of the ROV to form an inertial measurement value and image preprocessing module, such as Figure 5As shown, the inertial navigation sensor in the inertial navigation system obtains the position and attitude information of the body, and the visual sensor detects the feature points in the image and captures the real-time acceleration angular velocity and the surrounding environment image;

[0108] In step 1, the initial estimated information of the body posture is obtained by performing SINS trajectory solution on the acquired data, which is specifically:

[0109] Step 1.1: The system attitude solution uses attitude quaternions to represent the rotation of the three-dimensional coordinate system in space. Compared with other methods, the quaternion method has a smaller amount of calculation and is more efficient: Assuming that the angular velocity is constant per unit time, the attitude quaternion uses zero-order integration. When the inertial coordinate system {I} moves relative to the ground reference system {G}, the rate of change or the derivative of the corresponding quaternion describing its relationship is obtained by calculating the limit of the difference quotient, and the following is derived based on the properties of the quaternion:

[0110]

[0111] In the formula, ω m ——The measured value of angular velocity expressed in the {I} frame, ω m =[ω x ω y ω z ] T ;

[0112] ——ω m The antisymmetric matrix of

[0113] Using the Picard method, different mathematical formulas are used to update the quaternion depending on whether the angular velocity approaches 0:

[0114]

[0115] When using quaternions When performing rotation representation, the equation should be satisfied:

[0116]

[0117] Since there are certain errors in the calculation, Perform normalization:

[0118]

[0119] Based on the relative relationship of vector projection, select an arbitrary vector R, assume that the {I} system can reach the {G} system by rotating around the rotation axis q, and the R before and after the rotation I , R G The projections are R I =x I i+yI j+z I k, R G =x G i+y G j+z G k;

[0120] Based on the quaternion representation of rotation, we can get:

[0121]

[0122] Among them, the axis q is the rotation quaternion The corresponding vector part in ;

[0123] The vector R I and R G They are expressed in the form of quaternions:

[0124]

[0125] Combining the above formula, we can get:

[0126] x G i+y G j+z G k=(q0+q1i+q2j+q3k)(x I i+y I j+z I k)(q0-q1i-q2j-q3k) (7)

[0127] According to quaternion multiplication, the above formula is expanded and written in matrix form:

[0128]

[0129] The strapdown matrix represented by quaternion can be expressed as:

[0130]

[0131] Strapdown Matrix There is a corresponding relationship with the transformation matrix J1, so the body attitude angle can be solved by the following formula:

[0132]

[0133] The fourth-order Runge-Kutta method is used to recursively infer the velocity and position information. The specific calculation process is as follows:

[0134]

[0135] The specific calculation process of the speed at the next moment is as follows:

[0136]

[0137] The specific calculation process for the next moment position is as follows:

[0138]

[0139] The Runge-Kutta coefficients k in the velocity and position prediction formulas are:

[0140]

[0141] In the measurement process of step 1.2 and step 1.1, there will be interference information from various factors, the most common of which is Gaussian white noise. Taking the corresponding noise into consideration, parameterized modeling of IM is performed, and the noise model of IMU can be expressed as follows:

[0142]

[0143] The established IMU measurement model will be used in the subsequent vision and inertial navigation data fusion algorithm;

[0144] Step 2: The captured image is optimized using the underwater image enhancement algorithm CLAHE to complete the image feature extraction, tracking and mismatch elimination tasks, and the SINS trajectory solution is performed on the data obtained by the inertial navigation sensor to obtain the initial estimated information of the body posture;

[0145] In step 2, an image enhancement algorithm is used to preprocess underwater visual information, as follows:

[0146] Step 2.1, Contrast Adaptive Histogram Equalization (CLAHE) is used to segment the image and locally apply histogram equalization (HE). By implementing contrast limitation and combining bilinear interpolation technology, the image after quality optimization is subjected to feature extraction, tracking and mismatch elimination tasks. The system uses the feature based on accelerated segmentation test (FAST) for feature point detection, matches the feature points extracted in the current frame with the feature points of the adjacent frames, establishes the correspondence between these feature points in continuous image frames, realizes the association of feature points between images, and facilitates the estimation and posture optimization of the camera motion posture. The system uses the LK optical flow method to track the movement process of the feature points to characterize the motion state of the camera, so as to achieve the purpose of ROV posture estimation.

[0147] Step 2.2: In the multiple feature point pairs obtained by matching in step 2.1, the RANSAC algorithm is used to eliminate false matches. Samples are randomly selected from the matching data set, and consistent data are found. The RANSAC algorithm combines the observed data with the parameterized model and continuously cycles the test data. The goal is to find a 3×3 optimal homography matrix H and retain the required results to remove the false matching feature point pairs. The mathematical model can be expressed as follows:

[0148]

[0149] First, 4 sets of feature point pairs are randomly selected from the matching sample data set. Then, the homography matrix of the model is calculated and all data are tested using this model. After continuous calculation and iteration, H with the largest number of feature point matches is found, which is the optimal parameter model.

[0150] Step 3: Based on the EKF, the pose information in step 2 is introduced into the state constraint by using the multi-state constrained Kalman filter to form a visual and inertial navigation data fusion module, and the sliding window technology is used to optimize the calculation efficiency to achieve the tight coupling of high-precision inertial measurement unit (IMU) and visual sensor data;

[0151] The MSCKF algorithm is as follows:

[0152] Step 3.1: In order to fuse the IMU and visual sensor data to estimate the underwater robot's posture, the system state vector is selected as:

[0153]

[0154] To ensure the stability of the filter and avoid the singularity of the covariance matrix, the state quantity is described as the form of error: that is, the error of the estimate of a quantity x is x is the true value of the state quantity, is the estimated value of the state quantity. Different error definitions are used for the quaternion state quantity. If is a quaternion The estimated value of the direction error is given by the error quaternion Description, which consists of the relationship Definition, Symbol Represents quaternion multiplication, the error quaternion is:

[0155]

[0156] Tiny rotation It intuitively describes the deviation between the true posture and the estimated posture, which is almost the same. Since the posture has three degrees of freedom, using a small rotation such as δθ to represent the posture error is the most concise and direct way.

[0157] The error state quantity of IMU is expressed as:

[0158]

[0159] The error of the i-th camera pose is expressed as

[0160] The error state vector of the entire system is

[0161] Since the IMU measurement value is a discrete quantity, the IMU measures the signal ω with a period T. m and a m Sampling is performed, and these measurement results are used for state propagation. The discrete form of the state equation is as follows:

[0162]

[0163] Using Taylor's formula to expand and further discretize, we can get the noise covariance matrix Q at time k. k And the state transfer matrix Φ of the IMU from time k to time k+1 k as follows:

[0164]

[0165] With the noise covariance matrix Q k and the state transfer matrix Φ k , the IMU state covariance matrix from time k to time k+1 can be calculated:

[0166]

[0167] When describing the system state, covariance is needed to measure the stability of the state. When the state of k-1 is recursively pushed to the k moment based on the IMU, the covariance of the corresponding state must become larger, because the IMU has errors, and new errors are introduced, and the covariance naturally becomes larger. It is necessary to update the new covariance based on the covariance of the previous moment combined with the IMU error. represents the covariance of the IMU state at time k, Q k It represents the covariance of the influence of the IMU error on the state from time k-1 to k. This value is derived from the state variance based on the noise variance (calibrated) of the IMU readings. The actual covariance matrix is ​​as follows:

[0168]

[0169] Assume that in Mj Camera poses In the same feature point f j is observed continuously, i∈S j , at each camera pose, this feature point is described by the model:

[0170]

[0171] Among them, n j,i is the 2×1 dimensional image noise vector when camera i observes the jth feature point at time, and the feature position represented in the camera frame for:

[0172]

[0173] Step 3.2: In the first step of the algorithm in step 3.1, the feature position in the ground coordinate system is unknown, and the least squares method is used to minimize the error to obtain the estimated values ​​of these feature positions The observation items are a series of points on the normalized plane, and the errors are the errors in the x and y directions on the normalized plane. After the feature position is estimated, the measurement residual is calculated:

[0174]

[0175] In the formula, z j,i and They are respectively the observed value and estimated value of the j-th feature point when observing the camera pose at the i-th moment;

[0176] The estimation of feature position and camera pose is linearized and can be expressed in a linear form:

[0177]

[0178] H j,i and They are the Jacobian matrices of the observations with respect to the feature position and state, and all M j The residuals of the measured values ​​are superimposed to obtain:

[0179]

[0180] Since EKF is used for state estimation, for constructing the measurement model, according to the general form Define a linear dependence on the state error The residual r of is sufficient. The noise term must be zero-mean white noise and uncorrelated with the state error to apply the EKF framework, where H is the measurement Jacobian matrix; error With error is related, so the residual rj This is inconsistent with the general form, which makes it unsuitable for direct application in the measurement update process of EKF. j Projection to matrix H j The left null space V T get The redefined new residual is consistent with the general form, that is,

[0181] V T H j =0 (31)

[0182]

[0183] Finally, the observation equations of all feature points are superimposed to obtain the final form of the observation equation:

[0184]

[0185] After the observation model is established, the filter will update the observation data only when certain preset conditions are met (feature point tracking is lost, the continuous tracking length of the same feature point reaches the set maximum sliding window length), avoiding the continuous growth of the camera state caused by the movement of the system, thereby reducing the computing resources required by the algorithm;

[0186] Step 4: The tightly coupled information in step 3 is converted into a posture correction module by using an underwater visual dictionary based on a bag-of-words model, a Harris corner detection algorithm, and a TF-IDF algorithm. Two main correction strategies, namely, visual loop detection technology and outer loop posture correction, are introduced to improve the accuracy and reliability of navigation and positioning, thereby achieving accurate navigation and positioning of the ROV in an underwater environment.

[0187] In step 4, two main correction strategies are used to improve the accuracy and reliability of navigation positioning. First, the system uses visual loop detection technology. When the ROV passes a previously visited location, it detects and corrects possible posture deviations by comparing the current image with the historical image. When the ROV has not passed the historical location, the system uses known landmarks to perform outer ring posture corrections and calibrates the current posture estimate through the precise position information of the landmarks.

[0188] Example 1

[0189] The present invention combines the vision and inertial navigation fusion algorithm based on filtering, and is equipped with a strapdown inertial navigation system (SINS) and a camera sensor, avoiding complex electromechanical systems. It is compact, cost-effective, and easy to maintain. The IMU sensor is used to obtain the position and posture information of the body, and the camera sensor detects the feature points in the image. The acquired underwater image is subjected to limited contrast adaptive histogram equalization (CLAHE) to enhance the detail information, and the LK optical flow method is used to track the movement process of the feature points to characterize the motion state of the camera, so as to achieve the purpose of ROV posture estimation. Among them, CLAHE divides the image and locally applies histogram equalization (HE). By implementing contrast limitation and combining bilinear interpolation technology, while enhancing the image contrast, it also significantly improves the visibility and clarity of the details in the image, solving the defects of traditional HE:

[0190] The image to be processed is divided into multiple non-overlapping sub-blocks of equal size. These areas are called local histogram equalization units, and the number of pixels in each sub-block is N;

[0191] For each local histogram equalization unit, the traditional HE algorithm is applied to make the pixel distribution more uniform, so as to improve the contrast of the image. Specifically, it includes the following steps: calculate the grayscale histogram of the pixels in each area; calculate the corresponding cumulative distribution function (CDF) according to the histogram; reallocate the grayscale value of the pixel according to the CDF to achieve the equalization of the histogram; update the pixel value of the corresponding area in the image according to the allocated grayscale value. For grayscale images, the pixel value distribution is 0-255, n x is the number of points with pixel value x, then the pixel value of the point with pixel value x after enhancement is:

[0192]

[0193] In order to avoid over-enhancement, the grayscale value of each local histogram equalization unit image pixel is restricted, the histogram is truncated at the threshold T, and the part exceeding the truncation threshold is redistributed evenly over the entire grayscale range. The entire histogram rises by a height of L to ensure that the total area of ​​the histogram remains unchanged and the integral of the probability density is still 1. By adjusting the truncation threshold, CLAHE constrains the enhancement amplitude of the local contrast, can achieve differentiated enhancement effects, and retain the local details of the image as much as possible;

[0194] In order to alleviate the edge effect between segmented areas, bilinear interpolation is introduced to improve the local equalization units processed by CLAHE and obtain the final enhanced image.

[0195] Example 2

[0196] The present invention uses the LK optical flow method to track the movement process of feature points to characterize the motion state of the camera, so as to achieve the purpose of ROV pose estimation. The LK optical flow method performs differential calculations on the feature points of adjacent frame images to perform sparse optical flow estimation, and its mathematical model is as follows:

[0197] The grayscale value of a pixel in an image is defined as a function of time and coordinates. Specifically, at time t, the grayscale value of a pixel with coordinates (x, y) is represented as I(x, y, t); the next frame time is t+dt, the position of the pixel is (x+dx, y+dy), and the grayscale value is I(x+dx, y+dy, t+dt). Based on the grayscale constant assumption, the following equation can be listed:

[0198] I(x+dx,y+dy,t+dt)=I(x,y,t) (35)

[0199] According to the small motion assumption, the Taylor expansion ignores higher-order terms:

[0200]

[0201] Combining the above formula, we can get:

[0202] I x u+I y v=-I t (37)

[0203] I x ,I y and I t It can be obtained by the following formula:

[0204]

[0205] The above equation contains two unknown velocities u and v. According to the spatial consistency assumption, the movement of pixels is consistent in the local neighborhood of the image. Therefore, for n pixels in an r×r square, n equations can be constructed based on their movement consistency:

[0206]

[0207] Write the above formula as follows:

[0208]

[0209] Where:

[0210]

[0211] Using the least squares method to solve it, we get:

[0212]

[0213] When A T When A is reversible, the above equation has a unique solution in the least squares sense. In practical applications, in order to solve the optical flow, an iterative method is usually used for numerical calculation. Each iteration uses the above formula to obtain the motion speed between the previous and next two frames of images.

[0214] Example 3

[0215] After the observation model is established, the visual and inertial navigation data fusion module of the present invention adopts a strategy of managing the number of camera states by using a sliding window in order to better solve the problem of the continuous increase in the dimensions of the camera covariance matrix and the state vector caused by continuous observation. This strategy maintains a limited number of camera states by maintaining a sliding window for storing the camera states in time series, limits the length of the sliding window, and also records the feature point information under each camera state accordingly. Then, geometric constraints are established for the same feature point observed under different camera states, and a corresponding observation model is established on this basis. As the camera moves, the camera state in the sliding window is also continuously updated, which enables real-time estimation of the motion of the system. The introduction of the sliding window ensures that the number of camera states is always limited to a predetermined threshold range, avoiding the continuous growth of the camera state as the system moves, thereby reducing the computing resources required for the algorithm.

[0216] Every time the system receives the latest camera position and attitude information, the filter will automatically update the covariance matrix and the camera state. The first case that triggers the filter update is the loss of feature point tracking. When the number of camera states of the lost feature points observed in the sliding window reaches the maximum value, the lost feature points are triangulated, and more constraints are currently established. The feature points successfully solved by triangulation are used to update the filter observation. In this way, the updated three-dimensional feature point coordinates and the updated posture will be more accurate; the second case is that the continuous tracking length of the same feature point reaches the set maximum sliding window length, which will also trigger the observation update. The initial camera state in the sliding window at the next moment will be squeezed out. If this feature point is continued to be tracked, the filter observation update will continue. In addition, the system will decide whether to update based on the current moving state. If it is judged that the current camera is moving too fast, the system will retain the current camera state but no longer make observation updates.

[0217] With the observation model established by the feature points, the matrix H X The dimensionality reduction is performed through QR decomposition to reduce the computational complexity of EKF update:

[0218]

[0219]

[0220] Residual is just noise, discard it completely. To update the EKF, the following residual is used for calculation:

[0221]

[0222] in is the noise vector, and its covariance matrix is r is the number of columns in Q1. The EKF update is performed by calculating the Kalman gain:

[0223]

[0224] The correction to the state is given by the vector:

[0225] ΔX=Kr n (47)

[0226] Finally, the state covariance matrix is ​​updated as:

[0227] P k+1|k+1 =(I-KT H ) k+1|k (I-KT H ) T +KR n K T (48).

[0228] Example 4

[0229] In order to complete the efficient detection of underwater visual loops, the posture correction module of the present invention uses a bag of words (BOW)-based model to perform loop detection, and adopts the tightly coupled positioning technology based on MSCKF to combine the measurement data of the IMU with the perception data of the camera, so as to estimate the real-time posture of the ROV. The camera carried by the ROV has limited pixels, which affects the clarity of the captured images to a certain extent, and the computing power of the onboard processor is also limited, resulting in limited image processing capabilities. A marker point detection technology that matches the characteristics of the ROV, has high robustness and low latency is studied. The system receives the enhanced underwater image as input, and expects to obtain the pixel coordinates of the corner points of the marker area. The following are the specific detection steps:

[0230] 1) In order to clearly distinguish the logo area from the background, binarization processing was first performed;

[0231] 2) In the binary image, the outline of the landmark area is accurately extracted;

[0232] 3) Then the corner information is further extracted from the binary image;

[0233] 4) In order to filter out the true corner point coordinates, the non-maximum suppression technique was used to successfully extract the actual corner point coordinates from the marked area.

[0234] In order to optimize the computational efficiency and enhance the stability of the algorithm, a rectangular area is selected as the target landmark area, and the four corner points of the area are set as the key landmark points to be extracted. When the system captures the underwater visual information, the background in the grayscale image is first distinguished from the landmark area through a binarization operation. Next, the rectangular outline is accurately extracted from the processed image, and the minimum enclosing rectangle of the outline is calculated to obtain its corner point data.

[0235] By identifying the landmark points, their coordinate positions on the pixel plane can be accurately determined. Given that the exact coordinates of the four landmark points in the {G} system are known in advance, this information can be used to transform the problem into a classic PnP problem. Subsequently, combined with the transformation relationship between the landmark points in the pixel coordinate system and the {G} system, the specific position and posture of the camera coordinate system in the {G} system can be accurately determined. The position and posture of the camera coordinate system in the system can be determined, thereby determining the position and posture of the IMU in {G}, and making appropriate corrections to the state of the inner loop estimate. Assume that the current position and posture of the camera coordinate system obtained from the landmark points in the {G} system is When the pose of the camera coordinate system relative to the {I} system is known, the pose of the IMU relative to the {G} system at the current moment can be determined:

[0236]

[0237] The IMU state quantity pose residual observation based on the feature point observation is:

[0238]

[0239] Establish the observation equation related to the outer loop residual observation:

[0240]

[0241] Where the observation matrix H (m) =[I60 6×(6N+9) ].

[0242] The observation matrix can be directly applied to the following operations due to its sparsity and fast calculation speed. The corrected gain matrix, state quantity and covariance matrix are calculated by the following formula:

[0243] K (m) =PH (m)T (H (m) PH(m)T +R n ) -1 (54)

[0244] ΔX=K (m) r (m) (55)

[0245]

[0246] Since MSCKF is very sensitive in selecting the initial values ​​of related parameters, this may cause the problem that MSCKF cannot converge. In order to reduce the adverse effects of landmark correction on filter convergence, only the inner loop MSCKF is used for positioning in the initial operation stage. When it reaches a stable convergence state, the outer loop correction process is added to reduce the offset of the inner loop filter.

Claims

1. The ROV positioning and navigation system based on visual inertial navigation fusion and posture correction is characterized by: The invention comprises an inertial measurement value and image preprocessing module mounted on the ROV, wherein the inertial measurement value and image preprocessing module is connected to a vision and inertial navigation data fusion module through a wire, and the vision and inertial navigation data fusion module is connected to a posture correction module.

2. The ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to claim 1 is characterized in that: The inertial measurement value and image preprocessing module are respectively a strapdown inertial navigation system and a visual sensor, and both the strapdown inertial navigation system and the visual sensor are connected to the visual and inertial navigation data fusion module.

3. The ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to claim 2 is characterized in that: The vision and inertial navigation data fusion module comprises a multi-state constrained Kalman filter, the multi-state constrained Kalman filter is connected to the strapdown inertial navigation system and the visual sensor, and the multi-state constrained Kalman filter is connected to the posture correction module.

4. The positioning and navigation method of the ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to any of claims 1 to 3, characterized in that: Follow the steps below to implement it: Step 1: Install a strapdown inertial navigation system and a visual sensor at the front end of the ROV. The inertial navigation sensor in the inertial navigation system obtains the position and attitude information of the body, and the visual sensor detects the feature points in the image and captures the real-time acceleration angular velocity and the surrounding environment image; Step 2: Use underwater image enhancement algorithm to optimize the image quality of the captured image, complete image feature extraction, tracking and mismatch elimination tasks, and perform SINS trajectory solution on the data obtained by the inertial navigation sensor to obtain the initial estimated information of the body posture; Step 3: Based on the EKF, the posture information in step 2 is introduced into the state constraint by using the multi-state constrained Kalman filter, and the calculation efficiency is optimized by using the sliding window technology to achieve the tight coupling of the high-precision inertial measurement unit and the visual sensor data; Step 4: The tightly coupled information in step 3 is converted into a posture correction module through an underwater visual dictionary based on a bag-of-words model, a Harris corner detection algorithm, and a TF-IDF algorithm. Two main correction strategies, namely, visual loop detection technology and outer loop posture correction, are introduced to improve the accuracy and reliability of navigation and positioning, so as to achieve accurate navigation and positioning of the ROV in an underwater environment.

5. The positioning and navigation method of the ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to claim 4, characterized in that: In step 1, the acquired data is subjected to SINS trajectory solution to obtain the initial estimated information of the body posture, specifically: Step 1.1: The system attitude solution uses attitude quaternions to represent the rotation of the three-dimensional coordinate system in space: Assuming that the angular velocity is constant per unit time, the attitude quaternion uses zero-order integration. When the inertial coordinate system {I} moves relative to the ground reference system {G}, the rate of change or the derivative of the corresponding quaternion describing the relationship is obtained by calculating the limit of the difference quotient, and the following is derived based on the properties of the quaternion: In the formula, ω m ——The measured value of angular velocity expressed in the {I} frame, ω m =[ω x ω y ω z ] T ; ——ω m The antisymmetric matrix of Using the Picard method, different mathematical formulas are used to update the quaternion depending on whether the angular velocity approaches 0: When using quaternions When performing rotation representation, the equation should be satisfied: Since there are certain errors in the calculation, Perform normalization: Based on the relative relationship of vector projection, select an arbitrary vector R, assume that the {I} system can reach the {G} system by rotating around the rotation axis q, and the R before and after the rotation I , R G The projections are R I =x I i+y I j+z I k, R G =x G i+y G j+z G k; Based on the quaternion representation of rotation, we can get: Among them, the axis q is the rotation quaternion The corresponding vector part in ; The vector R I and R G They are expressed in the form of quaternions: Combining the above formula, we can get: x G i+y G j+z G k=(q0+q1i+q2j+q3k)(x I i+y I j+z I k)(q0-q1i-q2j-q3k) (7) According to quaternion multiplication, the above formula is expanded and written in matrix form: The strapdown matrix represented by quaternion can be expressed as: Strapdown Matrix There is a corresponding relationship with the transformation matrix J1, so the body attitude angle can be solved by the following formula: The fourth-order Runge-Kutta method is used to recursively deduce the velocity and position information. The specific calculation process is as follows: The specific calculation process of the speed at the next moment is as follows: The specific calculation process for the next moment position is as follows: The Runge-Kutta coefficients k in the velocity and position prediction formulas are: Step 1.2: In the measurement process of step 1.1, there will be interference information from various factors, the most common of which is Gaussian white noise. The corresponding noise is taken into consideration and parameterized modeling is performed on the IM. The noise model of the IMU can be expressed by the following formula: The established IMU measurement model will be used in the subsequent vision and inertial navigation data fusion algorithm.

6. The positioning and navigation method of the ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to claim 4, characterized in that: In step 2, an image enhancement algorithm is used to preprocess underwater visual information, as follows: Step 2.1, contrast adaptive histogram equalization is used to segment the image and locally apply histogram equalization. By implementing contrast limitation and combining bilinear interpolation technology, the image after quality optimization is subjected to feature extraction, tracking and mismatch elimination tasks. The system uses features based on accelerated segmentation test to detect feature points, matches the feature points extracted in the current frame with the feature points of adjacent frames, establishes the correspondence between these feature points in continuous image frames, and realizes the association of feature points between images. The system uses LK optical flow method to track the movement process of feature points to characterize the motion state of the camera, so as to achieve the purpose of ROV pose estimation. Step 2.2: In the multiple sets of feature point pairs obtained by matching in step 2.1, the RANSAC algorithm is used to eliminate false matches, randomly select samples from the matching data set, and find consistent data. The RANSAC algorithm combines the observed data with the parameterized model and continuously cycles the test data. The goal is to find a 3×3 optimal homography matrix H and retain the required results. The mathematical model can be expressed as follows:

7. The positioning and navigation method of the ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to claim 4, characterized in that: The MSCKF algorithm in step 3 is specifically: Step 3.1: In order to fuse the IMU and visual sensor data to estimate the underwater robot's posture, the system state vector is selected as: To ensure the stability of the filter and avoid the singularity of the covariance matrix, the state quantity is described as the form of error: that is, the error of the estimate of a quantity x is x is the true value of the state quantity, is the estimated value of the state quantity. Different error definitions are used for the quaternion state quantity. If is a quaternion The estimated value of the direction error is given by the error quaternion Description, which consists of the relationship Definition, Symbol Represents quaternion multiplication, the error quaternion is: The error state quantity of IMU is expressed as: The error of the i-th camera pose is expressed as The error state vector of the entire system is Since the IMU measurement value is a discrete quantity, the IMU measures the signal ω with a period T. m and a m Sampling is performed, and these measurement results are used for state propagation. The discrete form of the state equation is as follows: Using Taylor's formula to expand and further discretize, we can get the noise covariance matrix Q at time k. k And the state transfer matrix Φ of the IMU from time k to time k+1 k as follows: Noise covariance matrix Q k and the state transfer matrix Φ k , the IMU state covariance matrix from time k to time k+1 can be calculated: When the state of k-1 is recursively deduced to time k according to the IMU, the covariance of the corresponding state must become larger, where represents the covariance of the IMU state at time k, Q k It represents the covariance of the influence of the IMU error on the state from k-1 to k. The actual covariance matrix is ​​as follows: Assume that in M j Camera poses In the same feature point f j is observed continuously, i∈S j , at each camera pose, this feature point is described by the model: Among them, n j,i is the 2×1 dimensional image noise vector when camera i observes the jth feature point at time, and the feature position represented in the camera frame for: Step 3.2: In step 3.1, the feature position in the ground coordinate system is unknown, and the least squares method is used to minimize the error to obtain the estimated values ​​of these feature positions The observation items are a series of points on the normalized plane, and the errors are the errors in the x and y directions on the normalized plane. After the feature position is estimated, the measurement residual is calculated: In the formula, z j,i and They are respectively the observed value and estimated value of the j-th feature point when observing the camera pose at the i-th moment; The estimation of feature position and camera pose is linearized and can be expressed in a linear form: H j,i and They are the Jacobian matrices of the observations with respect to the feature position and state, and all M j The residuals of the measured values ​​are superimposed to obtain: Since EKF is used for state estimation, for constructing the measurement model, according to the general form Define a linear dependence on the state error The residual r of is sufficient. The noise term must be zero-mean white noise and uncorrelated with the state error to apply the EKF framework, where H is the measurement Jacobian matrix; error With error is related, so the residual r j This is inconsistent with the general form, which makes it unsuitable for direct application in the measurement update process of EKF. j Projection to matrix H j The left null space V T get The redefined new residual is consistent with the general form, that is, V T H j =0 (31) Finally, the observation equations of all feature points are superimposed to obtain the final form of the observation equation: After the observation model is established, the filter will update the observation data only when specific preset conditions are met.

8. The positioning and navigation method of the ROV positioning and navigation system based on visual inertial navigation fusion and posture correction according to claim 4, characterized in that: Specifically, step 4 is as follows: the system uses visual loop detection technology to detect and correct possible posture deviations by comparing the current image with the historical image when the ROV passes through a previously visited location; when the ROV has not passed through the historical location, the system uses known landmarks to correct the outer ring posture, and calibrates the current posture estimate through the precise position information of the landmarks.

Images