An underwater binocular positioning method and system with prior constraints of an optical target

Abstract

The present invention belongs to the field of computer vision technology, and discloses an underwater binocular positioning method and system with prior constraints of an optical target. This method establishes a binocular stereo imaging model considering camera distortion; constructs an image dataset of an underwater prior optical target, identifies and extracts potential feature points through feature transformation, performs feature point matching on the denoised images in the image dataset of the underwater prior optical target, uses the prior position information of the optical target points as a depth constraint condition for verification, and eliminates the incorrect matches with excessive position deviations among the matching feature points; jointly establishes a reprojection error function between the projected position of the target to be measured and the prior projected position of the target, uses the collinear relationship generated by the optical target as a multi-target geometric constraint condition, and iteratively optimizes the parameters of the binocular stereo vision model until the reprojection error meets the accuracy requirements. The present invention improves the accuracy of feature point recognition and positioning of an underwater robot in a complex water environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of computer vision, and particularly relates to an underwater binocular positioning method and system with prior constraints of an optical target. Background Art

[0002] Binocular vision positioning is an optical recognition and positioning measurement technology. Based on the principle of human vision, it obtains the three-dimensional information of the target object by simulating the binocular parallax of human eyes. It uses two horizontally arranged cameras to simultaneously capture the target scene and obtain image information from different perspectives. Due to the difference in camera positions, there will be parallax of the same target in the two images. By calculating and processing the parallax information and combining the internal and external parameters of the cameras, the position of the feature points in the three-dimensional space can be accurately determined.

[0003] At the present stage, the computer vision technology for feature point positioning has a certain degree of accuracy. Most of them improve the feature point positioning accuracy through image processing, but basically operate on the water surface; while positioning through an optical system underwater, the situation of poor image clarity and serious degradation is inevitable, which is likely to lead to the failure of optical positioning.

[0004] Through the above analysis, the problems and defects of the existing technology are: in an adverse water environment, it is difficult to perform automatic feature extraction using a traditional underwater robot equipped with binocular cameras, and the positioning accuracy of the matching feature points is not high. Summary of the Invention

[0005] To overcome the problems existing in the related technology, the disclosed embodiments of the present invention provide an underwater binocular positioning method and system with prior constraints of an optical target for high-precision optical recognition and positioning of feature points by an underwater robot.

[0006] The technical solution is as follows: The underwater binocular positioning method with prior constraints of an optical target includes the following steps:

[0007] S1, calibrate the initial values of the parameters of the binocular cameras and establish a binocular stereo imaging model considering camera distortion;

[0008] S2, construct an image data set of an underwater prior optical target, identify and extract potential feature points through feature transformation, and use a fitting function to denoise the image to improve the quality of the feature points;

[0009] S3, perform feature point matching on the denoised images in the image data set of the underwater prior optical target, use the prior position information of the optical target points as a depth constraint condition for verification, and eliminate the incorrect matches with too large position deviations among the matching feature points;

[0010] S4. Establish a reprojection error function by combining the projected position of the target to be measured with the prior projected position of the target. Using the collinearity relationship generated by the optical target as the geometric constraint condition for multiple targets, iteratively optimize the parameters of the binocular stereo vision model until the reprojection error meets the accuracy requirements.

[0011] In step S1, calibrate the initial values of the parameters of each binocular camera, and establish a binocular stereo imaging model considering camera distortion, including:

[0012] S101. Construct an optical binocular stereo vision coordinate model. The conversion relationships of any feature point on the optical target in the image pixel coordinate system , the image physical coordinate system , the monocular camera coordinate system , and the world coordinate system are as follows:

[0013] ;

[0014] ;

[0015] ;

[0016] In the formula, are respectively the abscissa and ordinate of the feature point image plane, is the transpose operation, are the internal parameters of the camera, are the external parameters of the camera, are respectively the abscissa and ordinate of the feature point in the world coordinate system under the plane, are respectively the normalized focal lengths of the camera in the directions, is the pixel coordinate of the principal point of the image projected onto the imaging plane; are respectively the parameter values in the camera rotation matrix R and translation vector T matrix, where ;

[0017] S102. When performing underwater shooting operations, consider the influence of lens distortion on the three-dimensional coordinates of the target. Divide the distortion type into a radial distortion model and a tangential distortion model; combine the distortion model and the coordinate conversion model to perform distortion correction; obtain the image coordinates of the distortion-corrected feature points in the left and right cameras ;

[0018] S103. Perform real-time calibration on the left and right cameras to obtain the initial values of the internal and external parameters. After calibration, the two cameras have no rotation relationship. Define the left camera coordinate system as , the right camera coordinate system as , and the baseline length as , assuming that the origin of the camera coordinate system coincides with the origin of the world coordinate system, then for any point in the world The coordinates on the image Satisfy the following relationship:

[0019] ;

[0020] The horizontal parallax of corresponding points on the left and right camera image planes , substituting into the projection formula gives: , according to the parallax relationship, the depth of a certain point in the world coordinate system , and the depth information of the feature points is determined accordingly;

[0021] In underwater positioning, the image coordinates after distortion correction are , and the formula is as follows:

[0022] ;

[0023] In the formula, Is the focal length of the binocular camera.

[0024] In step S102, the expressions of the radial distortion model and the tangential distortion model are respectively:

[0025] ;

[0026] ;

[0027] In the formula, Is the radial distortion, Is the tangential distortion, Are respectively in Direction and The radial distortion generated in the direction, Are respectively in Direction and The tangential distortion generated in the direction, Is Order distortion coefficient, Are respectively the horizontal and vertical coordinates of the feature point image plane, Are respectively the tangential distortion coefficients; Is the different order parameter of the distance between the center point of the camera's principal optical axis and the feature point on the image; Is the distance between the center point of the camera's principal optical axis and the feature point on the image, ; Among them, Is the distortion order, Is the pixel coordinate of the principal point of the image projected onto the imaging plane.

[0028] In step S102, the expression of distortion correction is:

[0029] ;

[0030] Wherein, are respectively the normalized focal lengths of the camera in the direction, are respectively the abscissa, ordinate and depth coordinate of the feature point in the world coordinate system;

[0031] The image coordinates of the feature point after distortion correction in the left and right cameras are:

[0032] ;

[0033] Wherein, are respectively the abscissa and ordinate of the feature point in the left and right camera images.

[0034] In step S2, an underwater prior optical target image dataset is constructed, potential feature points are identified and extracted through feature transformation, and a fitting function is used to denoise the image to improve the quality of the feature points, including:

[0035] S201, using the underwater binocular photos to detect the feature points provided by the prior optical target, convolving the Gaussian function with the original image to generate a series of images with different scales by changing the value of the scale parameter ;

[0036] ;

[0037] Wherein, is the Gaussian function and its three independent variables, is the scale parameter, are respectively the abscissa and ordinate of the scale space; is the original image space, is the scale-variable Gaussian function, is a set of images after Gaussian processing;

[0038] S202, combining the images after scale transformation to form a multi-layer Gaussian pyramid space, comparing each pixel point in the picture with its neighboring pixels to determine whether it is a feature point; fitting a three-dimensional quadratic function to refine the mis-matched feature points, and performing Taylor expansion on the spatial scale function as follows:

[0039] ;

[0040] Wherein, is the zero-order Taylor expansion formula, is the transpose operation, is the first-order Taylor expansion formula, is the second-order Taylor expansion, is the transpose of the function itself;

[0041] Among the detected potential feature points, there are misextractions caused by noise, and points with relatively low contrast ( ) need to be removed. Take the derivative of the above formula and set it to 0 to obtain the exact position , substitute it into and take the first two terms in the Taylor expansion of

[0042] ;

[0043] In the formula, is the exact position of a feature point obtained by the fitting function, is the inverse of the function, and the subscript is the matrix inverse operation, is the second-order partial derivative, is the first-order partial derivative, is the function value of the function at the extreme point;

[0044] Calculate the matrix at the feature point. The partial derivative matrix is as follows:

[0045] ;

[0046] In the formula, is calculated from the sampled feature points. According to the eigenvalues of the Hessian matrix, edge response points are removed, denotes the Hessian matrix.

[0047] In step S201, a series of images with different scales are generated by changing the value of the scale parameter . The expression is:

[0048] ;

[0049] In the formula, is the original image space, is the scale-variable Gaussian function, is a set of images after Gaussian processing.

[0050] In step S3, feature point matching is performed on the denoised images in the underwater prior optical target image dataset. Using the prior position information of the optical target points as the depth constraint condition for verification, incorrect matches with excessive position deviations among the matching feature points are removed, including:

[0051] S301. Assign rotational invariance to the denoised and rematched feature points to prevent feature point matching failures caused by the tiny movements of the underwater robot. Calculate the gradient magnitude and direction within the neighborhood of the feature point, with the expressions as follows:

[0052] ;

[0053] In the formula, is the image after Gaussian filtering in the scale space, is the distance parameter of the feature point, is the direction parameter of the feature point, is the arctangent function;

[0054] S302. Match a pair of feature points in the binocular images and calculate the Euclidean distance between a pair of feature descriptors. For the feature points and in two underwater images, the descriptors are and respectively, and their Euclidean distance ; is the descriptor of a pair of feature points in the left eye, is the descriptor of a pair of feature points in the right eye, is the direction element of the left feature point descriptor, is the direction element of the right feature point descriptor;

[0055] S303. Verify the recognition accuracy of the feature points according to the prior position depth. The binocular stereo vision model calculates two accurate image points of the optical target on the image, with pixel coordinates , and the depth of a matched pair of feature points is . According to the prior information, the world coordinates of the target feature point are , is the true value. The calculated depth of this target feature point and the prior depth define the depth error as:

[0056] ;

[0057] In the formula, is the depth error;

[0058] When the depth error value exceeds the set threshold , it is considered that this pair of feature points is mismatched; eliminate the error, and set a rematching range according to the prior depth and the target feature point area in the image. Recalculate the extreme values of its pixel set within this range, and continuously iterate the depth error until the prior feature points are correctly matched.

[0059] In step S4, a reprojection error function is established by combining the projected position of the target to be measured with the prior projected position of the target. Using the collinearity relationship generated by the optical target as the geometric constraint condition for multiple targets, the parameters of the binocular stereo vision model are iteratively optimized until the reprojection error meets the accuracy requirements, including:

[0060] S401, Arrange three or more optical targets with prior position information in a collinear manner and deploy them underwater to form a three-point collinearity condition for feature point matching; the prior coordinates of the feature points of the collinear targets are:

[0061] ;

[0062] In the formula, respectively represent three optical targets, are respectively the world coordinates of the three optical targets;

[0063] The coordinates mapped in the image are:

[0064] ;

[0065] In the formula, respectively represent the mappings of the three optical targets in the camera, , , are the image coordinates of the three optical targets;

[0066] Calculate the collinear vectors of the target and , as well as the vectors and on the image;

[0067] S402, Calculate the direction cosine of the vector to judge the collinear relationship. Let the direction cosine be , the direction cosine be , where: ; Calculate the direction consistency measure It is considered that the matching is correct, and the matching feature points in the image and the external prior have the same collinear relationship; if is much less than 1, it is a wrong match, and the wrong feature points in the area are rematched;

[0068] S403, Perform accuracy verification on the accurately rematched feature points. Fix optical targets, the two-dimensional coordinates of the feature points in the image coordinate system and the three-dimensional coordinates , based on the left eye coordinate system in the binocular, the projection transformation relationship is:

[0069] ;

[0070] ;

[0071] In the formula, are the known world coordinates of the optical target, For corner mark, is the camera coordinate of the known optical target, is the extrinsic rotation matrix, is the extrinsic translation vector, is the two-dimensional coordinate after projection transformation, is the camera intrinsic parameter matrix;

[0072] The above two steps are combined into the following formula to express the projection transformation relationship of the feature points in the camera, and written as:

[0073] ;

[0074] S404, based on the actual detection of the a priori image plane two-dimensional coordinates The two-dimensional coordinates obtained by projection Establish the reprojection error function and define the vector as:

[0075] ;

[0076] In the formula, is the error vector, are the image coordinates of the known optical target;

[0077] Using Euclidean distance to measure the error, the total reprojection error function is Constructed as the sum of squares of reprojection errors of all feature points:

[0078] ;

[0079] In the formula, is the error function, The two-dimensional coordinates after projection transformation of known target feature points;

[0080] S405, based on the pose estimation solved in real time by the underwater robot and , as a constraint to limit the external parameter difference to the centimeter level, combined with the reprojection error function.

[0081] In step S405, the expression associated with the reprojection error function is:

[0082] ;

[0083] In the formula, is the real-time pose of the camera, is the pose estimation of the camera, is the Frobenius norm, is the rotation matrix constraint condition, is the translation vector constraint condition, is a pre-set rotation matrix difference threshold. In the formula, is the error function, is the two-dimensional coordinate after projection transformation.

[0084] Another object of the present invention is to provide an underwater binocular positioning system with prior constraints of an optical target, which implements the underwater binocular positioning method with prior constraints of the optical target. The system includes:

[0085] A binocular stereo imaging model establishment module, which is used to calibrate the initial values of the parameters of the binocular camera and establish a binocular stereo imaging model considering camera distortion;

[0086] A potential feature point extraction module, which is used to construct an image data set of an underwater prior optical target, identify and extract potential feature points through feature transformation, and use a fitting function to denoise the image to improve the quality of the feature points;

[0087] An incorrect matching rejection module, which is used to perform feature point matching on the denoised images in the image data set of the underwater prior optical target, use the prior position information of the optical target points as a depth constraint condition for verification, and reject the incorrect matches with too large position deviations among the matching feature points;

[0088] A reprojection error function establishment module, which is used to jointly establish a reprojection error function between the projected position of the target to be measured and the prior projected position of the target, use the collinear relationship generated by the optical target as a multi-target geometric constraint condition, and iteratively optimize the parameters of the binocular stereo vision model until the reprojection error meets the accuracy requirements.

[0089] Combining all the above technical solutions, the beneficial effects of the present invention are as follows: The present invention proposes a method for correcting errors in binocular stereo vision feature matching using prior information of a target, innovatively uses the prior information of target feature points to participate in the feature recognition and matching process, replaces the method that could only use depth estimation for discrimination in the past, and solves the problem of low accuracy in the recognition and positioning of underwater target binocular stereo vision.

[0090] In a deep - water operation environment, simple binocular stereo vision positioning methods often fail to meet the accuracy requirements. Moreover, in adverse water environments, automated feature extraction is difficult and the effect is poor. The present invention proposes an underwater binocular positioning method with prior constraints of an optical target. First, scale - invariant feature transform is performed on a group of binocular photos taken underwater in different directions and angles to identify and extract potential feature points, and a fitting function is used to denoise the images, improving the quality of the feature points. Secondly, an optical target with 2D and 3D prior information is innovatively used. The position information of the optical target points is used as a depth constraint condition to improve the accuracy of feature point matching. The multi - perspective recognition technology of underwater cooperative targets for binocular vision is developed. The collinear relationship generated by a group of optical targets is used as a multi - target geometric constraint condition to correct the binocular camera stereo - positioning model in real - time, improving the accuracy of the underwater robot's recognition and positioning of feature points in complex water environments.

[0091] The present invention can promote the automation and precision levels of construction in the field of deep - sea underwater engineering. Through this method, mature products and software that can be promoted will be formed, and it has good prospects for popularization and application in marine engineering such as immersed tube tunnels, offshore oil, and offshore wind power. The present invention creates an underwater binocular positioning method with prior constraints of an optical target, studies the method of automated feature extraction and positioning in adverse water environments, and on the basis of using the binocular cameras of an underwater robot to perform absolute positioning on targets such as optical targets, proposes to use the spatial position constraint information of known optical targets, develops an optimized positioning theory for verifying the closed - loop detection of depth constraints in underwater binocular vision, and realizes high - precision positioning of underwater targets. Brief Description of the Drawings

[0092] The accompanying drawings here are incorporated into the specification and form a part of this specification, showing embodiments consistent with the present disclosure, and are used together with the specification to explain the principles of the present disclosure;

[0093] Figure 1 is a flowchart of the underwater binocular positioning method with prior constraints of an optical target provided by an embodiment of the present invention;

[0094] Figure 2 is a schematic diagram of the underwater binocular positioning system with prior constraints of an optical target provided by an embodiment of the present invention;

[0095] Figure 3 is a schematic diagram of the principle of the underwater binocular positioning method with prior constraints of an optical target provided by an embodiment of the present invention;

[0096] Figure 4 is a diagram of the matching result of feature points in binocular stereo vision positioning of the prior art provided by an embodiment of the present invention;

[0097] Figure 5 is a diagram of the matching and positioning result of feature points after prior constraints of the optical target in the present invention provided by an embodiment of the present invention;

[0098] In the figure: 1. Binocular stereo imaging model establishment module; 2. Potential feature point extraction module; 3. Incorrect matching elimination module; 4. Reprojection error function establishment module. Specific implementation manner

[0099] To make the above objects, features, and advantages of the present invention more obvious and understandable, the specific implementation manner of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description in order to fully understand the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without departing from the connotation of the present invention. Therefore, the present invention is not limited by the specific implementations disclosed below.

[0100] The innovation of the present invention lies in: The present invention creates an underwater binocular positioning method with prior constraints of an optical target. Based on the absolute positioning of an optical target and other targets using a binocular camera of an underwater robot, it proposes to utilize the spatial position constraint information of a known optical target and the multi-target position relationship formed by multiple groups of optical targets. Finally, an underwater binocular vision depth constraint verification closed-loop detection is constructed to achieve high-precision positioning of underwater targets. It changes the previous technical means of only processing images to improve positioning accuracy and has good innovation. Example 1, as Figure 1 shown, the embodiment of the present invention provides an underwater binocular positioning method with prior constraints of an optical target, including:

[0101] S1. Calibrate the initial values of the parameters of the binocular camera and establish a binocular stereo imaging model considering camera distortion;

[0102] S2. Construct an image data set of underwater prior optical targets, identify and extract potential feature points through feature transformation, and use a fitting function to denoise the image to improve the quality of feature points;

[0103] S3. Perform feature point matching on the denoised images in the image data set of underwater prior optical targets, use the prior position information of the optical target points as a depth constraint condition for verification, and eliminate incorrect matches with excessive position deviations among the matching feature points;

[0104] S4. Jointly establish a reprojection error function between the projected position of the target to be measured and the prior projected position of the target, use the collinear relationship generated by the optical target as a multi-target geometric constraint condition, and iteratively optimize the parameters of the binocular stereo vision model until the reprojection error meets the accuracy requirements.

[0105] Exemplarily, step S1 specifically includes:

[0106] S101. Construct an optical binocular stereo vision coordinate model, and any feature point on the optical target is in the image pixel coordinate system , the physical coordinate system of the image , the monocular camera coordinate system , the world coordinate system The conversion relationships are as follows:

[0107] ;

[0108] ;

[0109] ;

[0110] In the formula, are respectively the abscissa and ordinate of the feature point on the image plane, is the transpose operation, are the internal parameters of the camera, are the external parameters of the camera, are respectively the abscissa and ordinate of the feature point in the world coordinate system under the plane, are respectively the normalized focal lengths of the camera in the direction, is the pixel coordinate of the principal point of the image projected onto the imaging plane; are respectively the parameter values in the camera rotation matrix R and translation vector T matrix, where, ;

[0111] It can be understood that the above formula combines the previous internal and external parameters to create a new situation with Z coordinate being 0 for camera model construction.

[0112] S102. When performing underwater shooting operations, considering the influence of lens distortion on the target three-dimensional coordinates, the present invention innovatively proposes to divide the distortion types into radial distortion and tangential distortion:

[0113] Combining the comprehensive distortion model and the coordinate transformation model to perform distortion correction; obtaining the image coordinates of the feature points after distortion correction in the left and right cameras ; The expressions of the radial distortion model and the tangential distortion model are respectively:

[0114] ;

[0115] ;

[0116] In the formula, is the radial distortion, is the tangential distortion, are respectively in the direction and The radial distortion generated in the direction, respectively, is the tangential distortion generated in the direction and the direction, is the -order distortion coefficient, are respectively the abscissa and ordinate of the feature point image plane, are respectively the tangential distortion coefficients; is the distance between the center point of the camera's principal optical axis and the feature point on the image, ; where is the distortion order, are the pixel coordinates of the principal point of the image projected onto the imaging plane.

[0117] Adding the possibility of high-order lens distortion to the above formula for lens distortion processing further improves the accuracy of the captured images during underwater shooting operations.

[0118] Combining the comprehensive distortion model and the coordinate transformation model, the present invention innovatively proposes a distortion correction expression for solving the problem of high-order distortion of the camera lens, as follows:

[0119] ;

[0120] In the formula, are respectively the normalized focal lengths of the camera in the direction, are respectively the abscissa, ordinate, and depth coordinate of the feature point in the world coordinate system;

[0121] The image coordinates of the feature point after distortion correction in the left and right cameras are:

[0122] ;

[0123] In the formula, are respectively the abscissa and ordinate of the feature point in the left and right camera images.

[0124] S103. Perform real-time calibration on the left and right cameras to obtain the initial values of the internal and external parameters. After calibration, the two cameras have no rotational relationship. Define the left camera coordinate system as , and the right camera coordinate system as . The baseline length is . Assume that the origin of the camera coordinate system coincides with the origin of the world coordinate system. Then, for any point in the world, its coordinates on the image satisfy the following relationship:

[0125] ;

[0126] The horizontal parallax of corresponding points on the left and right camera image planes , substituting into the projection formula gives: , obtaining the depth of a point in the world coordinate system according to the parallax relationship , and determining the depth information of the feature points accordingly;

[0127] It can be understood that the present invention further adjusts the existing formula to describe the binocular vision positioning principle, and can further accurately obtain depth information. In underwater positioning, the image coordinates after distortion correction are used , and the formula is as follows:

[0128] ;

[0129] In the formula, is the focal length of the binocular camera.

[0130] It can be understood that the present invention further adjusts the existing formula to calculate the binocular vision positioning result. An accurate image can be further obtained.

[0131] Exemplarily, step S2 specifically includes:

[0132] S201, detecting the feature points provided by the prior optical target using underwater binocular photos, convolving the Gaussian function with the original image , and generating a series of images with different scales by changing the value of the scale parameter ;

[0133] ;

[0134] Among them, is the Gaussian function and its three independent variables, is the scale parameter, are the horizontal and vertical coordinates of the scale space respectively; is the original image space, is the scale-variable Gaussian function, is a set of images after Gaussian processing.

[0135] It can be seen that the above innovative formula deforms the Gaussian function to change the image features, and accurate prior optical target feature points can be obtained.

[0136] S202, combining the images after scale transformation to form a multi-layer Gaussian pyramid space, comparing each pixel point in the picture with its neighboring pixels to determine whether it is a feature point; fitting a three-dimensional quadratic function to refine the mis-matched feature points, and the spatial scale function The Taylor expansion is as follows:

[0137] ;

[0138] In the formula, is the zero-order Taylor expansion formula, is the transpose operation, is the first-order Taylor expansion formula, is the second-order Taylor expansion formula, is the transpose of the function itself;

[0139] It can be seen that the present invention utilizes a three-dimensional quadratic function model to further refine the feature points.

[0140] Among the already detected potential feature points, there are erroneously extracted points caused by noise, and it is necessary to remove the points with low contrast . Take the derivative of the above formula and set it to 0 to obtain the exact position . Substitute it into and take the first two terms in the Taylor expansion formula to get:

[0141] ;

[0142] In the formula, is the exact position of a feature point obtained by the fitting function, is the inverse of the function, and the subscript is the matrix inverse operation, is the second-order partial derivative, is the first-order partial derivative, is the function value of the function at the extreme point ;

[0143] It can be understood that the above formula can be used to eliminate the points with low contrast.

[0144] Calculate the matrix at the feature point, and the partial derivative matrix is as follows:

[0145] ;

[0146] In the formula, is calculated from the sampled feature points, and the edge response points are removed according to the eigenvalues of the Hessian matrix, is used to represent the Hessian matrix.

[0147] It can be understood that the above formula can be used to remove the edge response points.

[0148] Exemplarily, step S3 includes:

[0149] S301, assigning rotation invariance to the denoised and re-matched feature points to prevent the feature point matching failure caused by the small movement of the underwater robot; taking the feature point as the center, calculating the gradient amplitude and direction in its neighborhood, the expression is:

[0150] ;

[0151] In the formula, is the image after Gaussian filtering in scale space, is the distance parameter of the feature point, is the direction parameter of the feature point, is the inverse tangent function;

[0152] The gradient direction histogram is calculated in the neighborhood window centered on the feature point, and each feature point has a main direction. In the subsequent matching and positioning, even if the image is rotated or translated, it can be correctly matched according to the direction information. It can be seen that the present invention further adjusts the general formula for calculating the gradient amplitude and direction to obtain the above-mentioned positive effect.

[0153] S302, matching a pair of feature points in the binocular image, calculating the Euclidean distance between a pair of feature descriptors, and for the feature points in the two underwater images and , the descriptors are and , whose Euclidean distance ; is a pair of descriptors with feature points on the left. is a pair of descriptors with feature points on the right. is the direction element of the left feature point descriptor, It is the direction element of the right feature point descriptor;

[0154] S303, verify the accuracy of feature point recognition based on the prior position depth, and calculate the binocular stereo vision model to obtain the accurate image points of the optical target at two positions on the image. The pixel coordinates are , the depth of a pair of matching feature points is , according to the prior information, the world coordinates of the target feature point are , is the true value, the target feature point calculates the depth and prior depth The depth error is defined as:

[0155] ;

[0156] In the formula, is the depth error;

[0157] When the depth error exceeds the set threshold Then it is considered that this pair of feature points is mismatched; eliminate the error, and set up a re-matching range according to the prior depth and the area of the target feature points in the image. Recalculate the extreme values of its pixel set within this range, and continuously iterate the depth error until the prior feature points are correctly matched.

[0158] It can be seen that the innovative formula of the present invention can be further used to verify the matching accuracy of feature points.

[0159] Exemplarily, step S4 specifically includes:

[0160] S401, arrange three or more optical targets with prior position information collinearly and deploy them underwater to form a three-point collinear condition to constrain the feature point matching; the prior coordinates of the collinear target feature points are:

[0161] ;

[0162] In the formula, respectively represent three optical targets, , are respectively the world coordinates of the three optical targets;

[0163] The coordinates mapped in the image are:

[0164] ;

[0165] In the formula, respectively represent the mappings of the three optical targets in the camera, , , are the image coordinates of the three optical targets;

[0166] Calculate the collinear vectors and of the target according to the three directions formed by the three points, and the vectors and on the image;

[0167] S402, calculate the direction cosine of the vector to judge the collinear relationship, and let the direction cosine be , the direction cosine be , where: ; Calculate the direction consistency measure It is considered that the matching is correct, and the matched feature points in the image have the same collinear relationship as the external prior Much less than 1, it is a wrong match, and the wrong feature points in the area are rematched;

[0168] S403, the accuracy of the rematched precise feature points is verified and fixed an optical target, the two-dimensional coordinates of the feature points in the image coordinate system and the three-dimensional coordinates in the world coordinate system , taking the left-eye coordinate system in the binocular as the reference, the projection transformation relationship is:

[0169] ;

[0170] ;

[0171] In the formula, is the known world coordinate of the optical target, is the subscript, is the camera coordinate of the known optical target, is the external parameter rotation matrix, is the external parameter translation vector, is the two-dimensional coordinate after projection conversion, is the camera internal parameter matrix;

[0172] The present invention innovatively proposes to combine the above two steps into the following formula representing the projection conversion relationship of the feature points in the camera, and write it as:

[0173] ;

[0174] S404, according to the actual detected prior two-dimensional coordinates of the image plane and the two-dimensional coordinates obtained by projection to establish a reprojection error function, and define the vector as:

[0175] ;

[0176] In the formula, is the error vector, is the image coordinate of the known optical target;

[0177] The present invention innovatively proposes to use the Euclidean distance to measure the error, then the total reprojection error function is constructed as the sum of the squares of the reprojection errors of all feature points:

[0178] ;

[0179] In the formula, is the error function, is the two-dimensional coordinate after projection conversion of the known target feature points;

[0180] S405, based on the pose estimation solved in real time by the underwater robot and , as a constraint to limit the external parameter difference to the centimeter level, combined with the reprojection error function, the present invention innovatively proposes the following formula for the final matching accuracy test of unknown points:

[0181] ;

[0182] In the formula, is the real-time pose of the camera, is the estimated camera pose, is the Frobenius norm, is the rotation matrix constraint, is the translation vector constraint, is the preset rotation matrix difference threshold, where is the error function, is the two-dimensional coordinate after projection transformation.

[0183] Observe the target with prior information multiple times until the reprojection error is maintained within the experimental accuracy index, and then locate the optical target point with unknown information.

[0184] It can be seen from the above embodiments that the present invention can effectively improve the underwater target positioning accuracy by using target prior information to verify feature point matching positioning constraints and correct erroneous positioning in real time when the underwater image clarity is poor and the image is severely degraded.

[0185] Embodiment 2, as Figure 2 As shown, an embodiment of the present invention provides an underwater binocular positioning system with optical target prior constraints, comprising:

[0186] The binocular stereo imaging model building module 1 is used to calibrate the initial values ​​of various parameters of the binocular camera and build a binocular stereo imaging model that takes into account camera distortion;

[0187] Potential feature point extraction module 2 is used to construct an image data set of underwater prior optical targets, extract potential feature points through feature transformation recognition, and use fitting functions to denoise the image and improve the quality of feature points;

[0188] The error matching elimination module 3 is used to match the feature points of the denoised image in the image data set of the underwater prior optical target, use the prior position information of the optical target point as the depth constraint condition for verification, and eliminate the error matching with too large position deviation in the matching feature points;

[0189] The reprojection error function establishment module 4 is used to jointly establish a reprojection error function between the projected position of the target to be measured and the prior projected position of the target, using the collinearity relationship generated by the optical target as the geometric constraint condition for multiple targets, and iteratively optimizing the parameters of the binocular stereo vision model until the reprojection error meets the accuracy requirements.

[0190] Experimental example: An underwater binocular positioning method with prior constraints of an optical target, as Figure 3 、 Figure 4 、 Figure 5 shown, includes:

[0191] Step 1: Calibrate the initial values of the parameters of each binocular camera, and establish a binocular stereo imaging model considering camera distortion.

[0192] When performing underwater binocular stereo vision positioning, there will be a coordinate system conversion problem. First, establish the conversion relationship between the world coordinate system, camera coordinate system, image physical coordinate system, and image pixel coordinate system to realize the construction of the internal and external parameter models of the monocular camera; secondly, remove the distortion of the underwater captured images, calibrate the left and right eye cameras of the camera on the water to obtain the parameters of each camera; finally, establish a binocular stereo imaging model according to the calibration results.

[0193] Step 2: Construct an image dataset of the underwater prior optical target, identify and extract potential feature points through feature transformation, and use a fitting function to denoise the image to improve the quality of the feature points.

[0194] Before performing accurate binocular positioning of the target points, first extract and identify the typical feature points on the target. Use an underwater robot equipped with a binocular camera to take multi-angle and multi-depth photos of a group of target points to obtain a sufficient number of binocular photos; transform the features of the photos to determine whether they are potential feature points. For further precision, select to fit a three-dimensional quadratic function and calculate the Hessian matrix, and remove the Gaussian noise at the edges of the feature points according to its characteristics to obtain the accurate position of the feature points at the sub-pixel level.

[0195] Step 3: Perform feature point matching on the denoised images in the image dataset of the underwater prior optical target, use the prior position information of the optical target points as the depth constraint condition for verification, and eliminate the incorrect matches with too large position deviations among the matching feature points.

[0196] After accurately extracting the feature points, the feature points in the left and right eye images can be matched. First, calculate the Euclidean distance between the descriptors of each pair of feature points to obtain the initial matching result. Then, use the prior target position information as the depth constraint condition to verify the accuracy of the feature point matching, eliminate the mis-matched points with large depth deviations, and demarcate an area of interest with more errors based on this pair of feature points. Re-match the feature points in this area, and continuously repeat this process until the calculated depth and the prior depth error of all feature points on the target are less than the required range.

[0197] Step 4: Jointly establish a reprojection error function between the projected position of the target to be measured and the prior projected position of the target, and use the collinear relationship generated by the optical target as the multi-target geometric constraint condition. Iteratively optimize the parameters of the binocular stereo vision model until the reprojection error meets the accuracy requirements.

[0198] Multiple optical targets with prior position information are arranged collinearly and deployed underwater. The targets are represented in the form of vectors and their direction cosines are calculated to judge the direction consistency. The feature points with much smaller direction consistency metrics are identified as mis-matches and the feature points are re-matched. Finally, establish an objective function to verify the positioning accuracy, using the prior position and the actually calculated position of the optical target points as the reprojection error, and continuously iterate the prior feature points and the actual matching positions provided by the target points until the reprojection error is maintained within the accuracy index, and then position the optical target points with unknown information.

[0199] An experimental analysis was carried out using a wire-controlled underwater robot equipped with a multi-source fusion navigation system. The multi-source fusion navigation system consists of four parts: the global satellite navigation system, the inertial navigation system, the ultra-short baseline acoustic positioning system, and the optical binocular camera vision positioning system. Among them, the global navigation satellite system measures the absolute position, the inertial navigation system measures the attitude, the underwater acoustic system measures the dynamic position, and the optical positioning system performs high-precision positioning of underwater short-range feature points. The prior binocular vision positioning process of the optical target is as Figure 1 shown. Use the prior constraint information of the target to intervene in the automatic extraction and matching positioning of feature points, and achieve the target accuracy requirements in the continuous iteration process. The matching results of binocular stereo vision feature points before constraint are as Figure 4 shown. Generally, fewer feature points are extracted, the degree of dispersion is strong, and there is no obvious aggregation of feature points at the target. The matching effect of the new target feature points after constraint verification is as Figure 5As shown in the figure, after the five known feature points on the target are constrained, the overall matching effect is stronger, the feature points at the optical target are dense, and the matching and positioning accuracy is better. For the underwater binocular positioning method with the prior constraint of the optical target of the present invention, the prior information provided by the optical target is used to establish the reprojection error objective function. After verifying the matching constraint of the feature points, the matching and positioning of the target feature points in the image are quickly and real-time corrected, improving the positioning and autonomous recognition accuracy of the underwater robot in a complex water environment. Therefore, the underwater binocular positioning method with the prior constraint of the optical target proposed by the present invention solves the problems that the integrated navigation system cannot meet the short-range high-precision positioning underwater and the difficulty of automatic feature extraction in an adverse water environment.

[0200] As described above, the above is only a relatively preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, any modifications, equivalent replacements, and improvements made within the spirit and principle of the present invention should be covered within the protection scope of the present invention.

Claims

1. An underwater binocular positioning method with optical target prior constraints, characterized in that: The method comprises the following steps: S1, calibrate the initial values ​​of the binocular camera parameters and establish a binocular stereo vision model that takes into account camera distortion; S2, construct an image dataset of underwater prior optical targets, extract potential feature points through feature transformation recognition, and use fitting functions to denoise the image and improve the quality of feature points; S3, performing feature point matching on the denoised image in the image data set of the underwater prior optical target, using the prior position information of the optical target point as the depth constraint for verification, and eliminating erroneous matches with excessive position deviations in the matching feature points; S4, jointly establishing a reprojection error function by combining the projection position of the target to be measured with the prior projection position of the target, taking the collinear relationship generated by the optical target as the multi-target geometric constraint condition, and iteratively optimizing the binocular stereo vision model parameters until the reprojection error meets the accuracy requirement; In step S3, feature point matching is performed on the denoised image in the image data set of the underwater prior optical target, and the prior position information of the optical target point is used as the depth constraint for verification, and erroneous matches with excessive position deviations in the matching feature points are eliminated, including: S301, assigning rotation invariance to the denoised and re-matched feature points to prevent the feature point matching failure caused by the small movement of the underwater robot; taking the feature point as the center, calculating the gradient amplitude and direction in its neighborhood, the expression is: Where, L pic (x, y) is the image after Gaussian filtering in the scale space, ρ(x, y) is the distance parameter of the feature point, θ(x, y) is the direction parameter of the feature point, and atctan is the inverse tangent function; S302, matching a pair of feature points in the binocular images, calculating the Euclidean distance between a pair of feature descriptors, and for feature points P in the two underwater images left and P right , the descriptors are P left =(D l1 ,D l2 …D l128 ) and P right =(D r1 ,D r2 …D r128 ), whose Euclidean distance P left is a pair of descriptors with feature points on the left and right, P right is a pair of descriptors with feature points on the right, D li is the direction element of the left feature point descriptor, D ri It is the direction element of the right feature point descriptor; S303, verify the accuracy of feature point recognition based on the prior position depth, and calculate the binocular stereo vision model to obtain the two accurate image points of the optical target on the image, with pixel coordinates P left (x left ,y left ) right (x right ,y right ), the depth of a pair of matched feature points is According to the prior information, the world coordinates of the target feature point are truth is the true value, the target feature point calculates the depth Z depth and prior depth The depth error is defined as: Where ΔZ is the depth error; When the depth error exceeds the set threshold ε Z , then this pair of feature points is considered to be mismatched; eliminate the errors, set up a rematch range in the image according to the prior depth and the target feature point area, recalculate the extreme value of its pixel set within this range, and continuously iterate the depth error ΔZ until the prior feature points are correctly matched.

2. The underwater binocular positioning method with optical target prior constraints according to claim 1 is characterized in that: In step S1, the initial values ​​of the binocular camera parameters are calibrated to establish a binocular stereo vision model that takes into account camera distortion, including: S101, construct an optical binocular stereo vision coordinate model, and any feature point on the optical target is located in the image pixel coordinate system O PIC -U PIC V PIC , image physical coordinate system O PHY -X PHY Y PHY , monocular camera coordinate system O CAM -X CAM Y CAM Z CAM , world coordinate system O WOR -X WOR Y WOR Z WOR The conversion relationship is: In the formula, u PIC , v PIC are the horizontal and vertical coordinates of the feature point image plane, T is the transposition operation, M in To constitute the internal parameters of the camera, M out To constitute the external parameters of the camera, X WOR , Y WOR They are the feature points in the world coordinate system at Z WOR = horizontal and vertical coordinates in the 0 plane, f x , f y The camera is in u PIC , v PIC The normalized focal length in the direction, (u p , v p ) is the pixel coordinate of the image principal point projected onto the imaging plane; r ij , t i are the parameter values ​​in the camera rotation matrix R and translation vector T matrix, where ij = 1, 2, 3; S102, during underwater shooting, taking into account the influence of lens distortion on the three-dimensional coordinates of the target, the distortion types are divided into radial distortion model and tangential distortion model; the distortion model and the coordinate transformation model are integrated to perform distortion correction; and the image coordinates (u ) of the feature points in the left and right cameras after distortion correction are obtained. left , v left ), (u right , v right ); S103, real-time calibration is performed on the left and right cameras to obtain initial values ​​of internal and external parameters. The two cameras that have been calibrated do not have a rotation relationship. The left camera coordinate system is defined as l -X left Y left Z left , the right camera coordinate system is O l -X right Y right Z right , the baseline length is b, and the origin of the camera coordinate system coincides with the origin of the world coordinate system. Then for any point P(X WOR , Y WOR , Z WOR ) coordinates P on the image l (x left ,y left ), P r (x right ,y right ) satisfies the following relationship: The horizontal coordinate parallax d = x of the same-name point on the left and right camera image planes left -x right , substituting into the projection formula: Obtain the depth of a point in the world coordinate system based on the parallax relationship Determine the depth information of the feature points accordingly; In underwater positioning, the image coordinates after distortion correction (u left , v left ), the formula is as follows: Where f is the focal length of the binocular camera.

3. The underwater binocular positioning method with optical target prior constraints according to claim 2 is characterized in that: In step S102, the radial distortion model and the tangential distortion model are expressed as: In the formula, Radial is the radial distortion, Tangential is the tangential distortion, Δσ rd , Δμ rd are the radial distortions in the x and y directions, Δσ td , Δμ td are the tangential distortions in the x and y directions, respectively, and k n is the n-order distortion coefficient, u PIC , v PIC are the horizontal and vertical coordinates of the feature point image plane, p1 and p2 are the tangential distortion coefficients, r and r 2 …r 2n are the different order parameters of the distance between the center point of the camera's main optical axis and the feature point on the image; r is the distance between the center point of the camera's main optical axis and the feature point on the image, Where n is the distortion order, (u p ,v p ) is the pixel coordinate of the image principal point projected onto the imaging plane.

4. The underwater binocular positioning method with optical target prior constraints according to claim 3 is characterized in that: In step S102, the expression of distortion correction is: In the formula, f x , f y The camera is in u PIC , v PIC Normalized focal length in the X direction WOR , Y WOR , Z WOR They are the horizontal coordinate, vertical coordinate and depth coordinate of the feature point in the world coordinate system respectively; Image coordinates of feature points after distortion correction in left and right cameras (u left , v left ), (u right , v right )for: In the formula, (u left , v left ), (u right , v right ) are the horizontal and vertical coordinates of the feature points in the left and right camera images respectively.

5. The underwater binocular positioning method with optical target prior constraints according to claim 1, characterized in that: In step S2, an image dataset of an underwater priori optical target is constructed, potential feature points are extracted through feature transformation recognition, and the image is denoised using a fitting function to improve the quality of feature points, including: S201, using underwater binocular photos to detect feature points provided by a priori optical targets, converting the Gaussian function With the original image I pic (x, y) convolution, by changing the scale parameter The value of generates a series of images of different scales; L pic (x,y,σG)=G pic (x,y,σ G )×I pic (x,y) Among them, G pic (x, y, σ G ) is the Gaussian function and its three independent variables, σ G is the scale parameter, x and y are the horizontal and vertical coordinates of the scale space respectively; I pic (xy) is the original image space, G pic (x, y, σG) is a scale-variable Gaussian function, L pic (x, y, σ G ) is a set of images after Gaussian processing; S202, the scale-transformed images are combined to form a multi-layer Gaussian pyramid space, and each pixel in the image is compared with the neighboring pixels to determine whether it is a feature point; a three-dimensional quadratic function is fitted to refine the mismatched feature points, and the spatial scale function D is used to calculate the feature points. pic (x) The Taylor expansion is as follows: Where D pic is the zero-order Taylor expansion, T is the transposition operation, is the first-order Taylor expansion, is the second-order Taylor expansion, is the transpose of the function itself; Among the potential feature points that have been detected, there are errors caused by noise extraction, removing the contrast Point, take the derivative of the above formula and set it to 0 to get the exact position Bring in D pic Taking the first two terms of the Taylor expansion of (x) we get: In the formula, is the exact position of a feature point obtained by the fitting function. D pic The inverse of the function, the subscript -1 is the matrix inverse operation, is the second-order partial derivative, is the first-order partial derivative, D pic (x) function at the extreme point The function value at ; Calculate the 2×2 Hessian matrix at the feature point, and the partial derivative matrix is ​​as follows: Where D xx ,D xy ,D yx ,D yy It is calculated from the sampling feature points, and the edge response points are removed according to the eigenvalue calculation of the Hessian matrix. H refers to the Hessian matrix.

6. The underwater binocular positioning method with optical target prior constraints according to claim 5 is characterized in that: In step S201, by changing the scale parameter The value of generates a series of images of different scales, and the expression is: L pic (x, y, σ G )=G pic (x, y, σ G )×I pic (x, y) In the formula, I pic (x, y) is the original image space, G pic (x, y, σ G ) is a scale-variable Gaussian function, L pic (x, y, σ G ) is a set of images after Gaussian processing.

7. The underwater binocular positioning method with optical target prior constraints according to claim 1, characterized in that: In step S4, the projection position of the target to be measured is combined with the prior projection position of the target to establish a reprojection error function, and the collinear relationship generated by the optical target is used as the multi-target geometric constraint condition, and the binocular stereo vision model parameters are iteratively optimized until the reprojection error meets the accuracy requirement, including: S401, three or more optical targets with prior position information are collinearly arranged and placed underwater to form a three-point collinearity condition to constrain feature point matching; the prior coordinates of the collinear target feature points are: A=(X A ,Y A ,Z A ),B=(X B ,Y B ,Z B ),C=(X C ,Y C ,Z C ) In the formula, A, B, and C represent three optical targets respectively, (X A ,Y A ,Z A )、(X B , Y B , Z B )、(X C ,Y C ,Z C ) are the world coordinates of the three optical targets A, B, and C respectively; The coordinates mapped in the image are: a=(X a ,Y a ),b=(X b ,Y b ),c=(X c ,Y c ) In the formula, a, b, and c represent the mapping of three optical targets in the camera, respectively. a ,Y a )、(X b ,Y b )、(X c , Y c ) are the image coordinates of the three optical targets; Calculate the target collinear vector based on the three directions formed by the three points and and the vector on the image and S402, calculate the direction cosines of the vectors to determine the collinear relationship, let The direction cosines are (l1, m1, n1), The direction cosines are (l2, m2, n2), where: Calculate the directional consistency measure Direct=|l1l2+m1m2+n1n2|, Dircet≈1 is considered a correct match, and the matched feature points a, b, c in the image have the same collinear relationship with the external prior A, B, C; if Dircet is much less than 1, it is a wrong match, and the wrong feature points in the area are re-matched; S403, the precision of the re-matched accurate feature points is verified, N≥3 optical targets are fixed, and the two-dimensional coordinates x of the feature points in the image coordinate system are i =(x i ,y i ) T and the three-dimensional coordinate X in the world coordinate system i =(X i , Y i , Z i ) T , based on the left eye coordinate system in the binocular, the projection transformation relationship is: Where, X i , Y i , Z i is the known world coordinate of the optical target, c is the corner mark, is the camera coordinate of the known optical target, R is the extrinsic rotation matrix, t is the extrinsic translation vector, is the two-dimensional coordinate after projection transformation, M in is the camera intrinsic parameter matrix; The above two steps are combined into the following formula to express the projection transformation relationship of the feature points in the camera, and written as: S404, based on the actual detection of the a priori image plane two-dimensional coordinate x i The two-dimensional coordinates obtained by projection Establish the reprojection error function and define the vector as: In the formula, e i is the error vector, x i are the image coordinates of the known optical target; Using Euclidean distance to measure the error, the total reprojection error function E is constructed as the sum of squares of reprojection errors of all feature points: Where E(K, R, t) is the error function, M in [R|-Rt] is the two-dimensional coordinate of the known target feature point after projection transformation; S405, based on the pose estimation solved in real time by the underwater robot and It is used as a constraint to limit the difference of external parameters to the centimeter level and is combined with the reprojection error function.

8. The underwater binocular positioning method with optical target prior constraints according to claim 7 is characterized in that: In step S405, the expression associated with the reprojection error function is: Where R,t is the real-time pose of the camera, is the estimated camera pose, ‖x‖ f is the Frobenius norm, is the rotation matrix constraint, is the translation vector constraint, ε1, ε2 are the pre-set rotation matrix difference thresholds, where, is the error function, is the two-dimensional coordinate after projection transformation.

9. An underwater binocular positioning system with optical target prior constraints, characterized in that: The system implements the underwater binocular positioning method with optical target prior constraints as described in any one of claims 1 to 8, and the system comprises: A binocular stereo vision model building module (1) is used to calibrate the initial values ​​of various parameters of the binocular camera and build a binocular stereo vision model that takes into account camera distortion; A potential feature point extraction module (2) is used to construct an image data set of an underwater prior optical target, extract potential feature points through feature transformation recognition, and use a fitting function to denoise the image and improve the quality of feature points; An error matching elimination module (3) is used to match feature points of a denoised image in an image data set of an underwater prior optical target, use the prior position information of the optical target point as a depth constraint for verification, and eliminate error matching with excessive position deviation in the matching feature points; The reprojection error function establishment module (4) is used to establish a reprojection error function by combining the projection position of the target to be measured with the prior projection position of the target, taking the collinear relationship generated by the optical target as the multi-target geometric constraint condition, and iteratively optimizing the binocular stereo vision model parameters until the reprojection error meets the accuracy requirement.

Images