Underwater binocular positioning method and system based on optical target prior constraint

Abstract

The invention belongs to the technical field of computer vision, and discloses an underwater binocular positioning method and system based on optical target prior constraint. The method comprises the following steps: establishing a binocular stereo imaging model considering camera distortion; the method comprises the following steps: constructing an image data set of an underwater prior optical target, extracting potential feature points through feature transformation identification, performing feature point matching on a denoised image in the image data set of the underwater prior optical target, and performing verification by using prior position information of an optical target point as a depth constraint condition. Eliminating wrong matching with overlarge position deviation in the matching feature points; and combining the projection position of the target to be measured with the prior projection position of the target to establish a re-projection error function, and iteratively optimizing the parameters of the binocular stereoscopic vision model by taking the collinear relationship generated by the optical target as a multi-target geometric constraint condition until the re-projection error meets the precision requirement. According to the method, the feature point identification and positioning precision of the underwater robot in a complex water body environment is improved.

Description

Technical Field

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

[0002] Binocular vision positioning is an optical recognition 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 the human eye. It uses two horizontally arranged cameras to simultaneously shoot the target scene and obtain image information from different perspectives. Due to the difference in camera position, the same target will have parallax in the two images. By calculating and processing the parallax information and combining the internal and external parameters of the camera, the position of the feature point in the three-dimensional space can be accurately determined.

[0003] At present, computer vision technology for feature point positioning has a certain degree of accuracy, and most of the time, image processing is used to improve the accuracy of feature point positioning, but the operations are basically carried out on the water. However, positioning through optical systems underwater will inevitably lead to poor image clarity and severe degradation, which may cause optical positioning to fail.

[0004] Through the above analysis, the problems and defects of the existing technology are: in unfavorable water environment, it is difficult to use traditional underwater robots equipped with binocular cameras for automatic feature extraction, and the positioning accuracy of matching feature points is not high. Summary of the invention

[0005] In order to overcome the problems existing in the related art, the embodiments disclosed in the present invention provide an underwater binocular positioning method and system with optical target prior constraints, which are used for high-precision optical recognition and positioning of feature points by underwater robots.

[0006] The technical solution is as follows: An underwater binocular positioning method with optical target prior constraints comprises the following steps: S1, calibrate the initial values ​​of the binocular camera parameters and establish a binocular stereo imaging 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, the projection position of the target to be measured is combined with the target prior projection position to establish a reprojection error function, 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 requirements.

[0007] In step S1, the initial values ​​of the binocular camera parameters are calibrated to establish a binocular stereo imaging model that takes into account camera distortion, including: S101, build an optical binocular stereo vision coordinate model, and any feature point on the optical target in the image pixel coordinate system , Image physical coordinate system , Monocular camera coordinate system , World Coordinate System The conversion relationship is: ; ; ; In the formula, are the horizontal and vertical coordinates of the feature point image plane, is the transpose operation, To constitute the internal parameters of the camera, To constitute the external parameters of the camera, They are the feature points in the world coordinate system. The horizontal and vertical coordinates of the plane, The camera is The normalized focal length in the direction, is the pixel coordinate of the image principal point projected onto the imaging plane; They are the parameter values ​​in the camera rotation matrix R and translation vector T matrix, where: ; 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 of the feature points in the left and right cameras after the distortion correction are obtained. ; S103, real-time calibration is performed on the left and right cameras to obtain the initial values ​​of the 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 , the right camera coordinate system is , the baseline length is , 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 Coordinates on the image The following relations are satisfied: ; Parallax of the horizontal coordinates of the same-name points on the image planes of the left and right cameras , substituting into the projection formula: , according to the parallax relationship, the depth of a point in the world coordinate system is obtained , based on which the depth information of the feature points is determined; In underwater positioning, the distortion-corrected image coordinates are used , the formula is as follows: ; In the formula, is the focal length of the binocular camera.

[0008] In step S102, the radial distortion model and the tangential distortion model are expressed as: ; ; In the formula, is the radial distortion, is the tangential distortion, Respectively in Direction and The radial distortion in the direction Respectively in Direction and The tangential distortion in the direction for Order distortion coefficient, are the horizontal and vertical coordinates of the feature point image plane, are the tangential distortion coefficients respectively; It is 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; is the distance between the center point of the camera's main optical axis and the feature point on the image, ;in, is the distortion order, is the pixel coordinate of the image principal point projected onto the imaging plane.

[0009] In step S102, the expression of distortion correction is: ; In the formula, The camera is The normalized focal length in the direction, 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 in the left and right cameras after distortion correction for: ; In the formula, are the horizontal and vertical coordinates of the feature points in the left and right camera images respectively.

[0010] 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 Convolution, by changing the scale parameter The value of generates a series of images of different scales; ; in, 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 a scale-variable Gaussian function, 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 is used to The Taylor expansion is as follows: ; In the formula, is the zero-order Taylor expansion, is the transpose 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, and it is necessary to remove the ones with low contrast ( ), take the derivative of the above formula and set it to 0 to get the exact position , bring in Take the first two terms of the Taylor expansion of: ; In the formula, is the exact position of a feature point obtained by the fitting function. for Inverse of function, superscript is the matrix inverse operation, is the second-order partial derivative, is the first-order partial derivative, for Function at the extreme point The function value at ; Calculate the feature points Matrix, partial derivative matrix is ​​as follows: ; In the formula, It is calculated by sampling feature points, and the edge response points are removed according to the eigenvalue calculation of the Hessian matrix. Refers to the Hessian matrix.

[0011] In step S201, by changing the scale parameter The value of generates a series of images of different scales, and the expression is: ; In the formula, is the original image space, is a scale-variable Gaussian function, is a set of images after Gaussian processing.

[0012] 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: ; 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; 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; 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: ; In the formula, is the depth error; When the depth error exceeds the set threshold , then the 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 , until the prior feature points are correctly matched.

[0013] 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: ; In the formula, Represent three optical targets respectively. They are The world coordinates of the three optical targets; The coordinates mapped in the image are: ; In the formula, Represent the mapping of three optical targets in the camera, , , 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 , The direction cosines are ,in: ; Compute directional consistency measure The match is considered correct, and the matching feature points in the image A priori have the same collinear relationship; if If it is much smaller than 1, it is an incorrect match, and the incorrect feature points in the area are re-matched; S403, the precision of the re-matched accurate feature points is verified and fixed. Optical targets, the two-dimensional coordinates of the feature points in the image coordinate system and the three-dimensional coordinates in the world coordinate system , based on the left eye coordinate system in the binocular, the projection transformation relationship is: ; ; 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; 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 coordinates The two-dimensional coordinates obtained by projection Establish the reprojection error function and define the vector as: ; In the formula, is the error vector, are the image coordinates of the known optical target; Using Euclidean distance to measure the error, the total reprojection error function is Constructed as the sum of squared reprojection errors of all feature points: ; In the formula, is the error function, The two-dimensional coordinates after projection transformation of known target feature points; S405, based on the pose estimation solved in real time by the underwater robot and , as a constraint to limit the external parameter differences to the centimeter level, combined with the reprojection error function.

[0014] In step S405, the expression associated with the reprojection error function is: ; 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.

[0015] Another object of the present invention is to provide an underwater binocular positioning system with optical target prior constraints, the system implements the underwater binocular positioning method with optical target prior constraints, and the system comprises: The binocular stereo imaging model building module is used to calibrate the initial values ​​of the binocular camera parameters and build a binocular stereo imaging model that takes into account camera distortion; Potential feature point extraction module, used to 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; The error matching elimination module is used to match the feature points of the denoised images 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 for verification, and eliminate the error matching with too large position deviation in the matching feature points; The reprojection error function establishment module is used to jointly establish the reprojection error function by combining the projection position of the target to be measured with the target prior projection position, 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 requirements.

[0016] In combination with all the above-mentioned technical solutions, the beneficial effects of the present invention are as follows: the present invention proposes a method for correcting binocular stereo vision feature matching errors using target prior information, innovatively using target feature point prior information to participate in the feature recognition and matching process, replacing the previous method that can only use depth estimation to distinguish, and solving the problem of low accuracy in binocular stereo vision recognition and positioning of underwater targets.

[0017] In deep-water operation environments, simple binocular stereo vision positioning methods often cannot meet accuracy requirements, and in adverse water environments, automated feature extraction is difficult and the effect is poor. The present invention proposes an underwater binocular positioning method with optical target prior constraints. First, a scale-invariant feature transformation is performed on a group of binocular photos taken underwater in different directions and angles, potential feature points are identified and extracted, and the image is denoised by fitting functions to improve the quality of feature points; secondly, an innovative optical target with 2D and 3D prior information is used, and the position information of the optical target point is used as a depth constraint condition to improve the accuracy of feature point matching, and a binocular vision underwater cooperative target multi-view recognition technology is developed. The collinear relationship generated by a group of optical targets is used as a multi-target geometric constraint condition to enable the binocular camera stereo positioning model to be corrected in real time, thereby improving the accuracy of feature point recognition and positioning of underwater robots in complex water environments.

[0018] The present invention can promote the automation and precision level of deep-sea underwater engineering construction. Through this method, mature products and software that can be promoted will be formed, which has good promotion and application prospects 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 on optical targets, studies the method of automatic feature extraction and positioning in adverse water environments, and proposes the use of spatial position constraint information of known optical targets on the basis of absolute positioning of targets such as optical targets using the binocular camera of an underwater robot, and develops the optimization positioning theory of underwater binocular vision depth constraint verification closed-loop detection, thus achieving high-precision positioning of underwater targets. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings herein are incorporated in and constitute a part of the specification, illustrate embodiments consistent with the present disclosure, and together with the description, serve to explain the principles of the present disclosure; Figure 1 It is a flow chart of an underwater binocular positioning method with optical target prior constraints provided by an embodiment of the present invention; Figure 2 Schematic diagram of an underwater binocular positioning system with optical target prior constraints provided by an embodiment of the present invention; Figure 3 It is a schematic diagram of an underwater binocular positioning method with optical target prior constraints provided by an embodiment of the present invention; Figure 4 It is a prior art binocular stereo vision positioning feature point matching result diagram provided by an embodiment of the present invention; Figure 5 It is a feature point matching and positioning result diagram after the optical target prior constraint in the present invention provided by an embodiment of the present invention; In the figure: 1. Binocular stereo imaging model establishment module; 2. Potential feature point extraction module; 3. Error matching elimination module; 4. Reprojection error function establishment module. DETAILED DESCRIPTION

[0020] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of 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 violating the connotation of the present invention, so the present invention is not limited by the specific implementation disclosed below.

[0021] The innovation of the present invention lies in: the present invention creates an underwater binocular positioning method with optical target prior constraints. On the basis of using the binocular camera of an underwater robot to absolutely locate targets such as optical targets, it proposes to use the spatial position constraint information of known optical targets and the multi-target position relationship composed of multiple groups of optical targets, and finally constructs an underwater binocular vision depth constraint verification closed-loop detection to achieve high-precision positioning of underwater targets. It has changed the previous technical means of only processing images to improve positioning accuracy, and has great innovation. Example 1, as shown in Figure 1 As shown, an embodiment of the present invention provides an underwater binocular positioning method with optical target prior constraints, comprising: S1, calibrate the initial values ​​of the binocular camera parameters and establish a binocular stereo imaging 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, the projection position of the target to be measured is combined with the target prior projection position to establish a reprojection error function, 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 requirements.

[0022] Exemplarily, step S1 specifically includes: S101, build an optical binocular stereo vision coordinate model, and any feature point on the optical target in the image pixel coordinate system , Image physical coordinate system , Monocular camera coordinate system , World Coordinate System The conversion relationship is: ; ; ; In the formula, are the horizontal and vertical coordinates of the feature point image plane, is the transpose operation, To constitute the internal parameters of the camera, To constitute the external parameters of the camera, They are the feature points in the world coordinate system. The horizontal and vertical coordinates of the plane, The camera is The normalized focal length in the direction, is the pixel coordinate of the image principal point projected onto the imaging plane; They are the parameter values ​​in the camera rotation matrix R and translation vector T matrix, where: ; It can be understood that the above formula combines the previous internal and external parameters to create a new situation with a Z coordinate of 0 for camera model construction.

[0023] S102, taking into account the influence of lens distortion on the three-dimensional coordinates of the target during underwater shooting, the present invention innovatively proposes to divide the distortion types into radial distortion and tangential distortion: The distortion model and coordinate transformation model are integrated to perform distortion correction; the image coordinates of the feature points in the left and right cameras after distortion correction are obtained. ; The expressions of radial distortion model and tangential distortion model are respectively: ; ; In the formula, is the radial distortion, is the tangential distortion, Respectively in Direction and The radial distortion in the direction Respectively in Direction and The tangential distortion in the direction for Order distortion coefficient, are the horizontal and vertical coordinates of the feature point image plane, are the tangential distortion coefficients respectively; It is 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; is the distance between the center point of the camera's main optical axis and the feature point on the image, ;in, is the distortion order, is the pixel coordinate of the image principal point projected onto the imaging plane.

[0024] The above formula incorporates the possibility of high-order lens distortion for lens distortion processing, further improving the accuracy of captured images during underwater shooting operations.

[0025] Combining the distortion model and the coordinate transformation model, the present invention innovatively proposes a distortion correction expression for solving the high-order distortion problem of the camera lens, as follows: ; In the formula, The camera is The normalized focal length in the direction, 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 in the left and right cameras after distortion correction for: ; In the formula, are the horizontal and vertical coordinates of the feature points in the left and right camera images respectively.

[0026] S103, real-time calibration is performed on the left and right cameras to obtain the initial values ​​of the 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 , the right camera coordinate system is , the baseline length is , 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 Coordinates on the image The following relations are satisfied: ; Parallax of the horizontal coordinates of the same-name points on the image planes of the left and right cameras , substituting into the projection formula: , according to the parallax relationship, the depth of a point in the world coordinate system is obtained , based on which the depth information of the feature points is determined; It can be understood that the present invention further adjusts the existing formula to describe the principle of binocular vision positioning, which can further accurately obtain depth information. , the formula is as follows: ; In the formula, is the focal length of the binocular camera.

[0027] It can be understood that the present invention further adjusts the existing formula to calculate the binocular vision positioning result, and can further obtain an accurate image.

[0028] Exemplarily, step S2 specifically includes: S201, using underwater binocular photos to detect feature points provided by a priori optical targets, converting the Gaussian function With the original image Convolution, by changing the scale parameter The value of generates a series of images of different scales; ; in, 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 a scale-variable Gaussian function, is a set of images after Gaussian processing.

[0029] It can be seen that the above innovative formula deforms the Gaussian function to change the image features, so as to obtain accurate a priori optical target feature points.

[0030] 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 is used to The Taylor expansion is as follows: ; In the formula, is the zero-order Taylor expansion, is the transpose operation, is the first-order Taylor expansion, is the second-order Taylor expansion, is the transpose of the function itself; It can be seen that the present invention utilizes a three-dimensional quadratic function model to further refine the feature points.

[0031] Among the potential feature points that have been detected, there are errors caused by noise extraction, and the contrast needs to be removed Point, take the derivative of the above formula and set it to 0 to get the exact position , bring in Take the first two terms of the Taylor expansion of: ; In the formula, is the exact position of a feature point obtained by the fitting function. for Inverse of function, superscript is the matrix inverse operation, is the second-order partial derivative, is the first-order partial derivative, for Function at the extreme point The function value at ; It will be appreciated that the above formula can be used to eliminate points with lower contrast.

[0032] Calculate the feature points Matrix, partial derivative matrix is ​​as follows: ; In the formula, It is calculated by sampling feature points, and the edge response points are removed according to the eigenvalue calculation of the Hessian matrix. is the Hessian matrix.

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

[0034] Exemplarily, step S3 includes: 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: ; 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; 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.

[0035] 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; 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: ; In the formula, is the depth error; When the depth error exceeds the set threshold , then the 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 , until the prior feature points are correctly matched.

[0036] It can be seen that the present invention can be further used for feature point matching accuracy verification through the innovative formula.

[0037] Exemplarily, step S4 specifically includes: 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: ; In the formula, Represent three optical targets respectively. , They are The world coordinates of the three optical targets; The coordinates mapped in the image are: ; In the formula, Represent the mapping of three optical targets in the camera, , , 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 , The direction cosines are ,in: ; Compute directional consistency measure The match is considered correct, and the matching feature points in the image A priori have the same collinear relationship; if If it is much smaller than 1, it is an incorrect match, and the incorrect feature points in the area are re-matched; S403, the accurate feature points after re-matching are verified for accuracy, and fixed Optical targets, the two-dimensional coordinates of the feature points in the image coordinate system and the three-dimensional coordinates in the world coordinate system , based on the left eye coordinate system in the binocular, the projection transformation relationship is: ; ; 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; The present invention innovatively proposes to combine the above two steps into the following formula to express the projection transformation relationship of the feature points in the camera, and write it as: ; 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: ; In the formula, is the error vector, are the image coordinates of the known optical target; The present invention innovatively proposes to use Euclidean distance to measure the error, then the total reprojection error function is Constructed as the sum of squared reprojection errors of all feature points: ; In the formula, is the error function, The two-dimensional coordinates after projection transformation of known target feature points; 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: ; 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.

[0038] 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.

[0039] 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.

[0040] 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: 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; 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; 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; The reprojection error function establishment module 4 is used to jointly establish the reprojection error function by combining the projection position of the target to be measured with the target prior projection position, 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 requirements.

[0041] Experimental example: An underwater binocular positioning method with optical target prior constraints, such as Figure 3 , Figure 4 , Figure 5 As shown, including: Step 1: Calibrate the initial values ​​of the binocular camera parameters and establish a binocular stereo imaging model that takes into account camera distortion.

[0042] When performing underwater binocular stereo vision positioning, the problem of coordinate system conversion will arise. First, the conversion relationship between the world coordinate system, camera coordinate system, image physical coordinate system and image pixel coordinate system is established to realize the construction of the internal and external parameter model of the monocular camera; secondly, the distortion of the underwater image is removed, and the left and right cameras of the camera are calibrated above water to obtain the camera parameters; finally, the binocular stereo imaging model is established according to the calibration results.

[0043] Step 2: 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.

[0044] Before accurately positioning the target point by binocular, the typical feature points on the target must be extracted and identified first. A group of target points are photographed at multiple angles and depths using an underwater robot equipped with a binocular camera to obtain a sufficient number of binocular photos. The photo features are transformed to determine whether they are potential feature points. For further precision, a three-dimensional quadratic function is fitted and the Hessian matrix is ​​calculated. Based on its characteristics, the Gaussian noise at the edge of the feature point is removed to obtain the precise sub-pixel position of the feature point.

[0045] Step 3: Perform feature point matching on the denoised image in the image dataset of the underwater prior optical target, use the prior position information of the optical target point as the depth constraint for verification, and eliminate erroneous matches with excessive position deviations in the matching feature points.

[0046] After the feature points are accurately extracted, the feature points in the left and right images can be matched. The Euclidean distance between each pair of feature point descriptors is calculated first to obtain the initial matching result. The accuracy of feature point matching is then checked using the prior target position information as a depth constraint condition, and the erroneous matching points with large depth deviations are eliminated. Based on this pair of feature points, an area of ​​interest with more errors is drawn, and the feature points in this area are re-matched. This process is repeated until the calculated depth and prior depth errors of all feature points on the target are less than the required range.

[0047] Step 4: Combine the projection position of the target to be measured with the target prior projection position to establish a reprojection error function, use the collinear relationship generated by the optical target as the multi-target geometric constraint, and iteratively optimize the binocular stereo vision model parameters until the reprojection error meets the accuracy requirements.

[0048] Multiple optical targets with prior position information are collinearly arranged and deployed underwater. The targets are represented as vectors and their direction cosines are calculated to determine the directional consistency. Feature points that are much smaller than the directional consistency metric are considered to be mismatched and re-matched. Finally, an objective function is established to verify the positioning accuracy. The prior position of the optical target point and the actual estimated position are used as the reprojection error. The prior feature points provided by the target point and the actual matching position are continuously iterated until the reprojection error is maintained within the accuracy index, and then the optical target point with unknown information is located.

[0049] The experimental analysis was conducted using a wire-controlled underwater robot equipped with a multi-source fusion navigation system. The multi-source fusion navigation system consists of four parts: a global satellite navigation system, an inertial navigation system, an ultra-short baseline acoustic positioning system, and an optical binocular camera visual positioning system. The global navigation satellite system performs absolute position measurement, the inertial navigation system performs attitude measurement, the hydroacoustic system performs dynamic positioning measurement, and the optical positioning system performs high-precision positioning of underwater short-range feature points. The optical target prior binocular visual positioning process is as follows: Figure 1 , using the target prior constraint information to intervene in the automatic extraction and matching of feature points, and achieving the target accuracy requirements in the process of continuous iteration; the binocular stereo feature point matching results before constraint are as follows Figure 4 As shown in Figure 2, the extracted feature points are relatively few and highly discrete, and there is no obvious aggregation of the feature points at the target. The matching effect of the new target feature points after constraint verification is shown in Figure 2. Figure 5 As shown, 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. The underwater binocular positioning method with optical target prior constraints of the present invention is adopted, and the reprojection error objective function is established using the prior information provided by the optical target. After verifying the feature point matching constraints, the matching and positioning of the target feature points in the image achieves rapid real-time correction, thereby improving the positioning of the underwater robot in a complex water environment and the autonomous recognition accuracy of feature points. Therefore, the underwater binocular positioning method with optical target prior constraints proposed by the present invention solves the problem that the combined navigation system cannot meet the underwater short-range high-precision positioning, and the problem that automated feature extraction is difficult in an unfavorable water environment.

[0050] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principles 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 imaging 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, the projection position of the target to be measured is combined with the target prior projection position to establish a reprojection error function, 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 requirements.

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 imaging model that takes into account camera distortion, including: S101, build an optical binocular stereo vision coordinate model, and any feature point on the optical target in the image pixel coordinate system , Image physical coordinate system , Monocular camera coordinate system , World Coordinate System The conversion relationship is: ； ； ； In the formula, are the horizontal and vertical coordinates of the feature point image plane, is the transpose operation, To constitute the internal parameters of the camera, To constitute the external parameters of the camera, They are the feature points in the world coordinate system. The horizontal and vertical coordinates of the plane, The camera is The normalized focal length in the direction, is the pixel coordinate of the image principal point projected onto the imaging plane; They are the parameter values ​​in the camera rotation matrix R and translation vector T matrix, where: ; 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 of the feature points in the left and right cameras after the distortion correction are obtained. ; S103, real-time calibration is performed on the left and right cameras to obtain the initial values ​​of the 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 , the right camera coordinate system is , the baseline length is , 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 Coordinates on the image The following relations are satisfied: ； Parallax of the horizontal coordinates of the same-name points on the image planes of the left and right cameras , substituting into the projection formula: , according to the parallax relationship, the depth of a point in the world coordinate system is obtained , based on which the depth information of the feature points is determined; In underwater positioning, the distortion-corrected image coordinates are used , the formula is as follows: ； In the formula, 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, is the radial distortion, is the tangential distortion, Respectively in Direction and The radial distortion in the direction Respectively in Direction and The tangential distortion in the direction for Order distortion coefficient, are the horizontal and vertical coordinates of the feature point image plane, are the tangential distortion coefficients respectively; It is 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; is the distance between the center point of the camera's main optical axis and the feature point on the image, ;in, is the distortion order, 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, The camera is The normalized focal length in the direction, 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 in the left and right cameras after distortion correction for: ； In the formula, 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 Convolution, by changing the scale parameter The value of generates a series of images of different scales; ； in, 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 a scale-variable Gaussian function, 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 is used to The Taylor expansion is as follows: ； In the formula, is the zero-order Taylor expansion, is the transpose 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 Take the first two terms of the Taylor expansion of: ； In the formula, is the exact position of a feature point obtained by the fitting function. for Inverse of function, superscript is the matrix inverse operation, is the second-order partial derivative, is the first-order partial derivative, for Function at the extreme point The function value at ; Calculate the feature points Matrix, partial derivative matrix is ​​as follows: ； In the formula, It is calculated by sampling feature points, and the edge response points are removed according to the eigenvalue calculation of the Hessian matrix. 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: ； In the formula, is the original image space, is a scale-variable Gaussian function, is a set of images after Gaussian processing.

7. The underwater binocular positioning method with optical target prior constraints according to claim 1 is characterized in that: 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: ； 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; 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; 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: ； In the formula, is the depth error; When the depth error exceeds the set threshold , then the 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 , until the prior feature points are correctly matched.

8. 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: ； In the formula, Represent three optical targets respectively. , They are The world coordinates of the three optical targets; The coordinates mapped in the image are: ； In the formula, Represent the mapping of three optical targets in the camera, , , 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 , The direction cosines are ,in: ; Compute directional consistency measure The match is considered correct, and the matching feature points in the image A priori have the same collinear relationship; if If it is much smaller than 1, it is an incorrect match, and the incorrect feature points in the area are re-matched; S403, the precision of the re-matched accurate feature points is verified and fixed. Optical targets, the two-dimensional coordinates of the feature points in the image coordinate system and the three-dimensional coordinates in the world coordinate system , based on the left eye coordinate system in the binocular, the projection transformation relationship is: ； ； 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; 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 coordinates The two-dimensional coordinates obtained by projection Establish the reprojection error function and define the vector as: ； In the formula, is the error vector, are the image coordinates of the known optical target; Using Euclidean distance to measure the error, the total reprojection error function is Constructed as the sum of squared reprojection errors of all feature points: ； In the formula, is the error function, The two-dimensional coordinates after projection transformation of known target feature points; S405, based on the pose estimation solved in real time by the underwater robot and , as a constraint to limit the external parameter differences to the centimeter level, combined with the reprojection error function.

9. The underwater binocular positioning method with optical target prior constraints according to claim 8, characterized in that: In step S405, the expression associated with the reprojection error function is: ； 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.

10. 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 9, and the system comprises: A binocular stereo imaging model building module (1) is used to calibrate the initial values ​​of various parameters of the binocular camera and to build a binocular stereo imaging 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