A method for positioning and attitude determination of a UAV towed body based on binocular vision positioning

Abstract

The application discloses a UAV towed body positioning and pose measurement method based on binocular vision positioning, and relates to the technical field of computer processing.The method comprises the following steps: S01, a measurement system is constructed, the measurement system comprises a UAV and a towed body connected with the UAV through a cable; the UAV is provided with a GNSS module, an IMU module and a binocular camera; the upper surface of the towed body is provided with a coded pattern and is provided with a sonar sensor; S02, a towed body coordinate system is established with the acoustic center of the sonar sensor as the origin, three-dimensional coordinates of feature points on the towed body in the towed body coordinate system are obtained, and a three-dimensional space model of the towed body is generated to run a simulation model; and S03, the binocular camera is calibrated.The application solves the underwater topographic survey problem in the near-shore high-flow-speed turbid water area, and realizes efficient, low-cost and high-precision surveying and mapping operation.

Description

Technical Field

[0001] This invention relates to the field of computer processing technology, and in particular to a method for positioning and attitude measurement of a towed unmanned aerial vehicle (UAV) based on binocular vision positioning. Background Technology

[0002] Traditional marine surveying techniques primarily rely on ship platforms (including manned and unmanned vessels) combined with Global Navigation Satellite Systems (GNSS) and sonar equipment for water depth measurement. Underwater gliders, autonomous underwater vehicles (AUVs), or remotely operated underwater vehicles (ROVs) can also be used as observation platforms to carry sonar equipment for detection tasks. Furthermore, airborne lidar depth sounding technology has also seen initial applications in practical production, providing a rapid supplementary means for marine surveying.

[0003] However, the aforementioned existing technologies still have many limitations in practical applications:

[0004] 1. Traditional shipborne surveying methods are significantly affected by weather and sea conditions. Ship operations are often unable to proceed during and for a considerable period after severe weather (such as cold air or typhoons) due to safety regulations; in areas with strong tidal currents, the effective window of opportunity for operation is extremely limited. Taking the Jianshan section of the Qiantang River as an example, due to the influence of tidal currents, the safe operating time for ships is usually less than 4 hours per day, which severely restricts the efficiency of data acquisition.

[0005] 2. Underwater gliders and AUVs are generally suitable for areas with a water depth greater than 100 meters; ROVs, on the other hand, have strict requirements on ocean current speed, generally requiring the current speed not to exceed 4 knots. However, the current speed in my country's nearshore areas, especially in the coastal waters, is generally high. For example, the current speed in Hangzhou Bay often exceeds 6 knots, making it difficult for the above-mentioned equipment to operate effectively and limiting its application in nearshore high-current environments.

[0006] 3. While airborne lidar depth sounding boasts high efficiency, it requires extremely high water transparency. Currently, only some offshore areas in the South China Sea meet the applicable water quality conditions, severely limiting its applicability and making it difficult to implement in most nearshore turbid waters.

[0007] In summary, existing technologies have significant shortcomings in terms of efficiency, environmental adaptability, and applicability, and there is a lack of a technical means to effectively and flexibly detect water depth in nearshore, shallow, turbid, and high-velocity waters. Summary of the Invention

[0008] To address the aforementioned technical problems, the present invention provides a method for positioning and attitude measurement of a towed UAV based on binocular vision positioning, the method comprising the following steps:

[0009] S01. Construct a measurement system, the measurement system including a drone and a towed body connected to the drone by a cable; the drone is equipped with a GNSS module, an IMU module and a binocular camera, and the upper surface of the towed body is provided with a coded pattern and equipped with a sonar sensor;

[0010] S02. Establish a towed body coordinate system with the acoustic center of the sonar sensor as the origin, obtain the three-dimensional coordinates of the feature points on the towed body in the towed body coordinate system, and generate a three-dimensional space model of the towed body for simulation.

[0011] S03. Calibrate the binocular camera and obtain its interior orientation elements, distortion parameters, and exterior orientation element deviation values ​​between the binocular camera system and the GNSS / IMU system.

[0012] S04. The UAV tows the towed body and simultaneously records the following data sequence: the towed body image acquired by the binocular camera corresponding to each timestamp, the UAV position and attitude data acquired by the GNSS / IMU module, and the water depth data acquired by the sonar sensor.

[0013] S05. Process the two images of the towed body acquired at each time stamp, extract the towed body region through image matching and segmentation, perform corresponding point matching, and generate a dense three-dimensional point cloud of the towed body at the current time through stereo image pair forward intersection calculation based on the camera parameters calibrated in step S01.

[0014] S06. Iteratively register the dense 3D point cloud with the entity model pre-made in step S01, correct the dense 3D point cloud based on the entity model, and fuse them to generate a high-precision image entity model.

[0015] S07. Based on the image entity model, calculate the three-dimensional position and attitude angle of the towed body in the global coordinate system at the current moment;

[0016] S08. Based on the three-dimensional position and attitude angle of the towed body calculated in step S03, and combined with the fixed installation position of the sonar sensor in the coordinate system of the towed body, the water depth data synchronously collected in step S02 is reduced to the global coordinate system to obtain the three-dimensional coordinates of the underwater terrain.

[0017] Preferably, the detailed 3D modeling of the towed body specifically includes:

[0018] The processed towed body is scanned no less than three times using a high-resolution laser scanner to obtain its color point cloud data;

[0019] The average value of the point cloud data from multiple scans is calculated, and a point cloud model is built using the mesh method;

[0020] The actual coordinates of feature points on the towed body are measured using a total station, and the point cloud model is corrected based on the actual coordinates to obtain the solid model.

[0021] Preferably, the calibration of the binocular camera specifically includes:

[0022] The interior orientation elements and distortion parameters of the left and right monocular cameras were obtained using the Zhang Zhengyou calibration method.

[0023] The binocular camera and GNSS / IMU module are fixedly connected to the UAV to form a binocular measurement system;

[0024] Image data was acquired in a ground calibration field with known coordinates, and the exterior orientation elements of the binocular camera were solved by bundle adjustment of the regional network.

[0025] A fixed deviation value is calculated between the exterior orientation element obtained by post-processing and the position and attitude data synchronously recorded by the GNSS / IMU module. This deviation value is used to correct the exterior orientation element directly measured by the GNSS / IMU in subsequent operations in real time.

[0026] Preferably, the processing of the left and right trailing body images specifically includes the following sub-steps:

[0027] S31. Using the distortion parameters obtained in step S01, perform distortion correction on the left and right original images;

[0028] S32. Using the pre-stored towed body template image as a reference, a fast approximate nearest neighbor search algorithm is used to match in the corrected image to locate the area where the towed body is located; then, an image segmentation algorithm based on edge detection is used to accurately segment the local image of the towed body from the image.

[0029] S33. On the segmented local images of the left and right dragging bodies, the SURF algorithm is used for feature extraction and corresponding point matching.

[0030] Preferably, in step S32, the image segmentation algorithm based on edge detection uses the Canny operator, and its process includes Gaussian filtering for noise reduction, calculation of gradient magnitude and direction, non-maximum suppression, and double-threshold edge connection.

[0031] Preferably, in step S33, the SURF algorithm adds the sign of the Hessian matrix trace as a feature variable to the feature descriptor, and only performs similarity calculation on feature points with the same sign during matching.

[0032] Preferably, the ICP registration of the dense 3D point cloud and the solid model in step S03 specifically includes: calculating the centroid of the dense 3D point cloud and the point cloud of the solid model, and transforming the two point clouds to the centroid coordinate system;

[0033] Calculate the matrix H between the two point clouds after transformation, and perform singular value decomposition on H to solve for the optimal rotation matrix R and translation vector T;

[0034] The above process is executed iteratively until the preset number of iterations or the threshold of the change in the transformation matrix is ​​met.

[0035] The present invention has at least the following beneficial effects:

[0036] 1. By adopting an innovative operation mode of using an unmanned aerial vehicle (UAV) platform to tow an underwater towed body, the limitations of ship-based surveying due to adverse hydrological and meteorological conditions such as wind, waves, and tides are fundamentally overcome. In particular, it enables all-weather, high-efficiency continuous measurement in near-shore high-velocity waters.

[0037] 2. By setting simple visual markers on the towed body and using conventional sensors on the UAV for positioning and attitude measurement, the expensive high-precision GNSS / IMU equipment on the towed body is avoided, and there is no need to rely on large operating vessels or expensive AUV / ROV platforms, which significantly reduces the hardware investment and maintenance costs of the entire system.

[0038] 3. By setting simple visual markers on the towed body and using conventional sensors on the UAV for positioning and attitude measurement, the expensive high-precision GNSS / IMU equipment on the towed body is avoided, and there is no need to rely on large operating vessels or expensive AUV / ROV platforms, which significantly reduces the hardware investment and maintenance costs of the entire system. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 The flowchart shows a method for positioning and attitude measurement of a towed UAV based on binocular vision localization, provided in Embodiment 1 of the present invention.

[0041] Figure 2 This is a flowchart of step S02 provided in Embodiment 1 of the present invention;

[0042] Figure 3The flowchart shows steps S03-S08 provided in Embodiment 1 of the present invention. Detailed Implementation

[0043] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0044] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or server that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or devices.

[0045] Example 1

[0046] This embodiment provides a method for positioning and attitude measurement of a towed UAV based on binocular vision localization. The method includes the following steps: Figure 1 As shown:

[0047] S01. Construct a measurement system, which includes a drone and a towed body connected to the drone via cables. The drone is equipped with a GNSS module, an IMU module, and a binocular camera. The towed body has coded patterns on its upper surface and is equipped with sonar sensors.

[0048] Specifically, the drone is equipped with GNSS, an attitude measurement system, and a binocular camera. The drone's upper surface is coated with coded images and it carries a small towed body with a single beam or other sonar probe. The drone is connected to the towed body via a signal cable.

[0049] S02. Establish a towed body coordinate system with the acoustic center of the sonar sensor as the origin, obtain the three-dimensional coordinates of the feature points on the towed body in the towed body coordinate system, and generate a three-dimensional spatial model of the towed body to run the simulation model.

[0050] Furthermore, such as Figure 2 As shown, detailed 3D modeling of the towed body specifically includes:

[0051] The processed towed body is scanned no less than three times using a high-resolution laser scanner to obtain its color point cloud data;

[0052] The average value of the point cloud data from multiple scans is calculated, and a point cloud model is built using the mesh method;

[0053] The actual coordinates of feature points on the towed body are measured using a total station, and the point cloud model is corrected based on the actual coordinates to obtain the solid model.

[0054] Specifically, the interior orientation elements and camera distortion parameters of each monocular camera are parameters that describe the relative position between the imaging center and the image, including principal distance (f) and principal point coordinates (u0, v0), which are important parameters in photogrammetry; distortion mainly corrects radial distortion and tangential distortion.

[0055] Here, Zhang Zhengyou's calibration method is used to obtain the interior orientation elements and distortion model of the monocular camera. The interior orientation elements and camera distortion parameters can be represented by matrix A:

[0056] ;

[0057] In the above formula, Represents the principal point coordinates. This includes parameters that incorporate distortion and focal length.

[0058] First, prepare a chessboard calibration board (chessboard calibration board diagram.png). The size of the chessboard grid on the calibration board is preset.

[0059] Secondly, obtain an image of the chessboard calibration board with a coordinate system, and detect the coordinates of each corner point. ;

[0060] Then, calculate the coordinates of each corner point in the calibrated coordinate system, denoted as (u, v, w=0), and then perform the following calculations:

[0061] ;

[0062] ;

[0063] ;

[0064] in , The above matrix is ​​a homogeneous matrix with 8 unknown elements. Each corner point on the calibration board can provide two constraint equations. Only 4 corner points are needed to find matrix M for an image. When an image has more than 4 corner points, the least squares method can be used to find the optimal M.

[0065] Next, we obtain the intrinsic parameter matrix A:

[0066] ;

[0067] Given that the two columns of a rotation matrix have the property of being orthogonal to each other, i.e.:

[0068] ;

[0069] Depend on , It can be known that:

[0070] ;

[0071] ;

[0072] remember Then there is

[0073] ;

[0074] ;

[0075] ;

[0076] remember ,in Then we have:

[0077] ;

[0078] The above is recorded as:

[0079] ;

[0080] Each calibration board image can provide more than one constraint relationship, which contains two constraint equations. Since vector b has 6 parameters, three calibration board images are needed to solve for vector b. When there are more than 3 images, the least squares method can be used to obtain vector b, then matrix B, and finally the intrinsic parameter matrix A.

[0081] ;

[0082] ;

[0083] ;

[0084] ;

[0085] ;

[0086] At this point, the interior orientation elements of the two monocular cameras can be calculated separately.

[0087] S03. Calibrate the binocular camera and obtain its interior orientation elements, distortion parameters, and exterior orientation element deviation values ​​between the binocular camera system and the GNSS / IMU system.

[0088] Furthermore, the binocular camera is calibrated, specifically including:

[0089] The interior orientation elements and distortion parameters of the left and right monocular cameras were obtained using the Zhang Zhengyou calibration method.

[0090] A binocular measurement system is formed by fixing a binocular camera and a GNSS / IMU module to a UAV.

[0091] Image data was acquired in a ground calibration field with known coordinates, and the exterior orientation elements of the binocular camera were solved by bundle adjustment of the regional network.

[0092] The fixed deviation value between the exterior orientation elements obtained from post-processing and the position and attitude data synchronously recorded by the GNSS / IMU module is calculated and used as the exterior orientation element deviation value, which is used to correct the exterior orientation elements directly measured by the GNSS / IMU in subsequent operations in real time.

[0093] Specifically, two monocular cameras are securely mounted on the drone and combined with GNSS and IMU equipment to form a binocular measurement system.

[0094] Establish a calibration field on the ground, that is, set up no less than 8 control points, and measure the coordinates of each control point. The repeatability of the coordinates should be better than ±1cm for both plane and elevation.

[0095] Acquire image data of the calibration field and simultaneously record the time, location, and attitude information of the acquired images.

[0096] The exterior orientation elements of the camera are solved using the bundle method.

[0097] The equations for collinearity are given as follows:

[0098] ;

[0099] For the ground calibration field, since the positions of each measuring point are precisely known, there are only 6 exterior orientation elements as unknowns in the above formula.

[0100] Expanding the above expression into a single term, we get:

[0101] ;

[0102] In its general form, it is:

[0103] ;

[0104] The other image from the binocular camera is denoted as:

[0105] ;

[0106] For control points, a single acquisition by a binocular camera requires solving for twelve exterior orientation elements.

[0107] Therefore, calibration requires at least four control points on each of the two images acquired, resulting in 12 unknowns to be solved and 16 error equations to be generated.

[0108] Represented in matrix form, we have:

[0109] ;

[0110] in, The error equation is listed for the left image point. The error equations listed for the right image points, It is a column matrix composed of the exterior orientation elements of the left image. It is a column matrix composed of the exterior orientation elements of the right image. for The corresponding error equation constant term, for The corresponding error equation has a constant term.

[0111] ;

[0112] ;

[0113] ;

[0114] ;

[0115] ;

[0116] Represented in matrix form, we have

[0117] ;

[0118] Its normal equation is:

[0119] ;

[0120] By solving this equation, the precise exterior orientation elements of each image can be obtained.

[0121] S04. The drone tows the towed object and records the following data sequence simultaneously: the image of the towed object obtained by the binocular camera corresponding to each timestamp, the drone's position and attitude data obtained by the GNSS / IMU module, and the water depth data obtained by the sonar sensor.

[0122] Specifically, drones are used to tow a vehicle via cables for exploration. The exploration mainly records water depth measurement data, timestamp data, and synchronously acquired towed vehicle image data.

[0123] The position and attitude of the images acquired by the monocular camera are recorded using GNSS and attitude sensors on the UAV, and the water depth is recorded using the sonar equipment of the towed vehicle. The 1pps system of the GNSS equipment is used to synchronize the time of image acquisition by the monocular camera and the time of water depth acquisition by the sonar equipment. In this way, the following sequence data can be obtained:

[0124] ;

[0125] In the above formula, time represents the time when the image was acquired, photo represents the acquired image of the dragged object, and x, y, and z are the spatial coordinates of the camera. The images represent the roll angle, pitch angle, and yaw angle, with the subscript 'l' representing the left-side image, the subscript 'r' representing the right-side image, and 'depth' representing the water depth.

[0126] S05. Process the two towed images acquired at each time stamp, extract the towed area through image matching and segmentation, perform corresponding point matching, and generate a dense three-dimensional point cloud of the towed body at the current time through stereo image pair forward intersection calculation based on the camera parameters calibrated in step S01.

[0127] Further processing of the two towed images involves the following sub-steps:

[0128] S31. Using the distortion parameters obtained in step S01, perform distortion correction on the left and right original images;

[0129] S32. Using the pre-stored towed body template image as a reference, a fast approximate nearest neighbor search algorithm is used to match in the corrected image to locate the area where the towed body is located; then, an image segmentation algorithm based on edge detection is used to accurately segment the local image of the towed body from the image (the image segmentation algorithm based on edge detection uses the Canny operator, and its process includes Gaussian filtering for noise reduction, calculation of gradient magnitude and direction, non-maximum suppression, and double threshold edge connection).

[0130] S33. On the segmented local images of the left and right dragging bodies, the SURF algorithm (which adds the sign of the Hessian matrix trace to the feature descriptor as a feature variable, and only calculates similarity for feature points with the same sign during matching) is used for feature extraction and matching of corresponding points.

[0131] Specifically, this involves calculating the deviation between the image's exterior orientation elements and the GNSS and IMU records.

[0132] For calibrated equipment, the post-processed exterior orientation elements should be consistent with the position and attitude recorded by GNSS and IMU. However, due to a fixed deviation between the two, they are not identical. This fixed difference can be obtained by subtracting the two values. Since more than one image is observed, the difference for each image is calculated sequentially, and the average value is taken as the final correction, denoted as . .

[0133] Let the exterior orientation elements of each image obtained from post-processing be:

[0134] ;

[0135] Let the exterior orientation elements obtained from the GNSS and IMU records of the UAV be:

[0136] ;

[0137] Then we have:

[0138] ;

[0139] The correction number can then be expressed as:

[0140] ;

[0141] Therefore, the exterior orientation elements of the image can be obtained as follows:

[0142] ;

[0143] In the above formula, This represents the exterior orientation elements after image correction, which can be directly used for calculations; This refers to the exterior orientation elements obtained using the GNSS and IMU equipment of the UAV. Represents the correction number for external orientation elements.

[0144] After completing the above steps, any image acquired by the drone through the camera will have precise interior and exterior orientation elements, which can meet the needs of subsequent image processing.

[0145] Furthermore, the images of the left and right frames are processed as follows:

[0146] First, the distortion parameters are obtained in advance, and then the image is corrected.

[0147] ;

[0148] and This represents the image data before and after the correction.

[0149] Image matching technology is used to determine the range of the dragging body image of the left and right frames and to segment it. First, the k-means tree algorithm of FLANN is used for priority search.

[0150] In the preliminary work, photos of the towed objects were acquired. Photos containing all the QR codes were used as the reference photos for matching, and then matching was performed on each set of observation data. Furthermore, based on prior conditions, it is known that an image can contain exactly one image of a towed object.

[0151] Considering that the pose of the towed object may change, the FLANN algorithm is used for image matching to find the towed object from the corrected monocular camera.

[0152] Some of the detected matches may be incorrect positive examples. This is because the k-value used in the kNN matching is 2 (finding two points in the training set). The first match is the nearest neighbor, and the second match is the second nearest neighbor. Intuitively, a correct match will be closer to the first neighbor. In other words, an incorrect match results in two neighbors with similar distances. Therefore, we can judge the quality of the match by looking at the difference in their distances. We can then discard the bad matches and retain the good ones.

[0153] Furthermore, image segmentation techniques are used to segment out local images of the towed object from the image;

[0154] After matching, rectangular image data containing the towed body was obtained. Considering the significant difference between the color of the towed body and the water color, an edge detection-based segmentation method was used to separate the local image of the towed body from the rectangular image data. Specifically, the Canny operator was used, and the steps are as follows:

[0155] First, use a kill filter to remove noise from the image:

[0156] The two-dimensional Gaussian formula is:

[0157] ;

[0158] Then the gradient image and angle image are calculated.

[0159] Based on this, non-maximum suppression is performed on the gradient image, and interference from edge coarseness and weak edges is proposed.

[0160] Finally, edge concatenation is performed using double thresholds to obtain the edges.

[0161] Image segmentation algorithms based on edge detection attempt to solve the segmentation problem by detecting edges that contain different regions. It is arguably one of the first methods conceived and most extensively studied. Typically, the grayscale values ​​of pixels change drastically at the boundaries of different regions. If the image is transformed from the spatial domain to the frequency domain using a Fourier transform, the edges correspond to the high-frequency components, making this a very simple edge detection algorithm.

[0162] Edge detection techniques can generally be categorized into sequential edge detection and parallel edge detection based on their processing methods. Sequential edge detection determines whether a pixel belongs to a detection edge based on the verification results of previous pixels. Parallel edge detection, on the other hand, determines whether a pixel belongs to a detection edge based on the currently being detected pixel and its neighboring pixels.

[0163] The simplest edge detection method is the parallel differential operator method, which utilizes the discontinuity of pixel values ​​in adjacent regions and uses first- or second-order derivatives to detect edge points. In recent years, methods based on surface fitting, boundary curve fitting, reaction-diffusion equations, serial boundary lookup, and deformation models have also been proposed.

[0164] To use FLANN matching, we need to pass in two dictionaries as parameters. These determine the algorithm to use and other related parameters. The first is `IndexParams`, where we are using a k-means tree search algorithm. The second dictionary is `SearchParams`, which specifies the number of recursive traversals. Higher values ​​result in more accurate results, but also consume more time.

[0165] S06. Iteratively register the dense 3D point cloud with the pre-made entity model in step S01, correct the dense 3D point cloud based on the entity model, and fuse them to generate a high-precision image entity model.

[0166] Furthermore, the above-mentioned ICP registration of dense 3D point cloud and solid model specifically includes: calculating the centroid of dense 3D point cloud and solid model point cloud, and transforming the two point clouds to the centroid coordinate system;

[0167] Calculate the matrix H between the two point clouds after transformation, and perform singular value decomposition on H to solve for the optimal rotation matrix R and translation vector T;

[0168] The above process is executed iteratively until the preset number of iterations or the threshold of the change in the transformation matrix is ​​met.

[0169] Specifically, the integral image is first calculated. The sum of the pixels in the rectangular regions formed by the image from the starting point to each point is stored in memory as an element of an array. After traversing all pixels of the image to obtain the integral image, the sum of the pixel gray levels in any rectangular region only requires three addition and subtraction operations, which can be used for subsequent box filters.

[0170] Then, an approximation of the Hessian matrix is ​​obtained:

[0171] For an image f(x, y), its Hessian matrix is ​​as follows:

[0172] ;

[0173] The Hessian matrix after Gaussian filtering can be expressed as:

[0174] ;

[0175] When the discriminant of the Hessian matrix reaches a local maximum, it is determined whether the current point is brighter or darker than other points in the surrounding neighborhood, thereby locating the key point.

[0176] Its discriminant can be written as:

[0177] ;

[0178] in:

[0179] ;

[0180] ;

[0181] Use a box filter and modify the discriminant as follows:

[0182] ;

[0183] The scale space is represented by constructing a scale space pyramid of the image. The SURF algorithm does not require downsampling to obtain images of different sizes to build the pyramid. Instead, it uses box filtering and integral images to continuously increase the box filtering template and quickly calculate the response image of the box filtering through the integral image.

[0184] After establishing the scale space, key points need to be searched and located. Each pixel in the response image processed by box filtering is compared with 26 pixels in its 3D neighborhood. If it is a maximum value, it is considered a local feature point of that region. Then, 3D linear interpolation is used to obtain sub-pixel level feature points, while removing some points smaller than a given threshold to make the feature points detected by extreme values ​​more robust.

[0185] To ensure rotation invariance of the feature descriptor, a principal direction needs to be assigned to each feature point. Therefore, with the feature point as the center and 6s(s) as the scale of the feature point, the calculation process is as follows: Calculating the Haar wavelet response of an image within a region of radius ( ) is essentially performing gradient calculations on the image, but using an integral image to improve the efficiency of gradient calculation. The scale of the Haar wavelet used to calculate the gradient is 4s, and the scan step size is s.

[0186] use The response values ​​of the Haar wavelet are weighted using a Gaussian function. To determine the principal direction, a wavelet is designed centered at the feature point with an angle of . The fan-shaped window, rotated at a certain angle Rotate the window and evaluate the Haar wavelet response values ​​within the window. By accumulating the sums, we obtain a vector. :

[0187] ;

[0188] The principal direction is the direction corresponding to the maximum accumulated value of the Haar response, i.e. When a peak value exceeds 80% of the main peak value, the corresponding direction is considered an auxiliary direction for that feature point. A feature point may be assigned multiple directions, which can enhance the robustness of the matching.

[0189] The Haar wavelet response is calculated within a rectangular region. Centered on the feature point, a 20s × 20s neighborhood is divided into 4 × 4 sub-blocks along the main direction. The response value of each sub-block is calculated using a 2s Haar template. Then, the horizontal, horizontal absolute, vertical, and vertical absolute values ​​of the response values ​​are statistically analyzed to form a feature vector. First, the 20s window is divided into 4 × 4 sub-windows, each 5s × 5s in size. The response value of each sub-window is calculated using a 2s Haar wavelet. Then, centered on the feature point, the response is calculated using... Gaussian kernel function Perform weighted calculations; finally, statistically analyze the weighted response values ​​of each sub-block to obtain the vector for each sub-block:

[0190] ;

[0191] Since there are 4×4 sub-blocks, the feature dimension of the feature descriptor is 4×4×4=64.

[0192] During feature point detection, the sign of the trace of the Hessian matrix is ​​recorded and used as a variable in the feature vector. This does not increase the computational load because the trace of the Hessian matrix has already been calculated during feature point detection. During feature matching, this variable can effectively save search time because only two feature points with the same sign can match; similarity calculations are not performed for feature points with different signs.

[0193] Simply put, feature points can be divided into two groups based on the sign of their response values: one group has a positive Laplace response and the other has a negative Laplace response. During matching, only feature points in the same group with the same sign can be matched with each other, which saves time in feature point matching.

[0194] After matching, for the left and right images observed at time t, n corresponding points can be obtained, denoted as:

[0195] ;

[0196] In the above formula, time represents the observation time, index represents the sequence number, x and y represent the image coordinates, and the subscripts l and r represent the left and right images of the stereo camera, respectively.

[0197] S07. Based on the image entity model, calculate the three-dimensional position and attitude angle of the towed body in the global coordinate system at the current moment.

[0198] Specifically, combining equations 1.39 and 1.46, the geographic coordinates of each corresponding point are calculated using the forward intersection formula for stereo image pairs, based on known interior and exterior orientation elements. A photogrammetric coordinate system is then established on the ground. , The axis and the heading are basically consistent, and Horizontal plane. Construct an image space coordinate system through the left image S01. Their axes are respectively with The axes are parallel; similarly, an image space coordinate system is constructed on the right image Sr. Their axes are respectively with The axes are parallel.

[0199] Suppose that the points with the same name are in The coordinates are Its image space coordinates are and Like spatial auxiliary coordinates and Then we have:

[0200] ;

[0201]

[0202] R is calculated based on the exterior orientation elements of the left and right images.

[0203] Right camera station S02 is on the left camera station The coordinates can be represented as:

[0204] ;

[0205] We can see that the following relationship exists:

[0206] ;

[0207] Rewrite the above formula and use the value of B obtained from formula 48 to find the value of N:

[0208] ;

[0209] Finally, the geographical coordinates of the corresponding points were obtained as follows:

[0210] ;

[0211] By intersecting the two images from the front, we can obtain the dense point cloud coordinates of the dragged body at time t, denoted here as . .

[0212] S08. Based on the three-dimensional position and attitude angle of the towed body obtained in step S03, and combined with the fixed installation position of the sonar sensor in the coordinate system of the towed body, the water depth data synchronously collected in step S02 is reduced to the global coordinate system to obtain the three-dimensional coordinates of the underwater terrain.

[0213] Specifically, dense point cloud adjustment is used to calculate the position and attitude data of the towed body. The accuracy of the dense point cloud (denoted as Ps) obtained using forward intersection is generally lower than that of the point cloud of the "entity model" (denoted as Pt). Therefore, the points of the "entity model" are used as ground truth. The dense point cloud obtained from forward intersection is matched and corrected using the ICP algorithm to establish high-precision dense point cloud data. Information from the "entity model" is then used to supplement data for unmeasured areas. The resulting mixed data can be used to create a new towed body model for the "image entity model". The main steps of the ICP algorithm are as follows:

[0214] First, calculate the centroids of the dense point cloud Ps and the solid point cloud Pt, respectively. ;

[0215] Transform Ps and Pt to the centroid coordinate system:

[0216] ;

[0217] Calculate matrix H:

[0218] ;

[0219] Performing SVD decomposition on matrix H yields... Calculate using the following formula :

[0220] ;

[0221] Calculate t according to the formula.

[0222]

[0223] Repeat the steps until 100 iterations or and The change in any matrix is ​​less than 0.01.

[0224] Using the "image solid model", a local hull coordinate system is established and compared with the standard hull coordinate system to calculate the center position and rotation angle of the towed body:

[0225]

[0226] ;

[0227] Calculate water depth data.

[0228] Using the ship's coordinate system, the water depth data is converted to the user-specified coordinate system. The specific coordinates of the water depth points are:

[0229] (1.58)

[0230] The above formulas are used to project water depth with attitude onto the correct geographical location.

[0231] Example 2

[0232] This invention provides a non-transitory computer-readable storage medium storing at least one instruction or at least one program segment, which is loaded and executed by a processor to implement the following steps:

[0233] A measurement system is constructed, which includes a drone and a towed body connected to the drone by a cable; the drone is equipped with a GNSS module, an IMU module and a binocular camera, and the towed body has a coded pattern on its upper surface and is equipped with a sonar sensor.

[0234] A towed body coordinate system is established with the acoustic center of the sonar sensor as the origin. The three-dimensional coordinates of the feature points on the towed body in the towed body coordinate system are obtained, and a three-dimensional spatial model of the towed body is generated to run the simulation model.

[0235] The binocular camera is calibrated to obtain its interior orientation elements, distortion parameters, and exterior orientation element deviation values ​​between the binocular camera system and the GNSS / IMU system.

[0236] The drone tows the towed object and simultaneously records the following data sequence: the image of the towed object acquired by the binocular camera at each timestamp, the drone's position and attitude data acquired by the GNSS / IMU module, and the water depth data acquired by the sonar sensor.

[0237] The two towed images acquired at each time stamp are processed, the towed area is extracted by image matching and segmentation, the same point is matched, and based on the camera parameters calibrated in step S01, the dense three-dimensional point cloud of the towed body at the current time is generated by forward intersection calculation of stereo image pairs.

[0238] The dense 3D point cloud is iteratively registered with the pre-made solid model, the dense 3D point cloud is corrected based on the solid model, and then fused to generate a high-precision image solid model.

[0239] Based on the image entity model, the three-dimensional position and attitude angle of the towed body in the global coordinate system at the current moment are calculated.

[0240] Based on the calculated three-dimensional position and attitude angle of the towed body, and combined with the fixed installation position of the sonar sensor in the towed body coordinate system, the synchronously collected water depth data is reduced to the global coordinate system to obtain the three-dimensional coordinates of the underwater terrain.

[0241] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0242] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is used as an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0243] Example 3

[0244] This invention provides an electronic device, including a processor and a memory, wherein the memory stores at least one instruction or at least one program segment, and the at least one instruction or the at least one program segment is loaded and executed by the processor to implement the following steps:

[0245] A measurement system is constructed, which includes a drone and a towed body connected to the drone by a cable; the drone is equipped with a GNSS module, an IMU module and a binocular camera, and the towed body has a coded pattern on its upper surface and is equipped with a sonar sensor.

[0246] A towed body coordinate system is established with the acoustic center of the sonar sensor as the origin. The three-dimensional coordinates of the feature points on the towed body in the towed body coordinate system are obtained, and a three-dimensional spatial model of the towed body is generated to run the simulation model.

[0247] The binocular camera is calibrated to obtain its interior orientation elements, distortion parameters, and exterior orientation element deviation values ​​between the binocular camera system and the GNSS / IMU system.

[0248] The drone tows the towed object and simultaneously records the following data sequence: the image of the towed object acquired by the binocular camera at each timestamp, the drone's position and attitude data acquired by the GNSS / IMU module, and the water depth data acquired by the sonar sensor.

[0249] The two towed images acquired at each time stamp are processed, the towed area is extracted by image matching and segmentation, the same point is matched, and based on the camera parameters calibrated in step S01, the dense three-dimensional point cloud of the towed body at the current time is generated by forward intersection calculation of stereo image pairs.

[0250] The dense 3D point cloud is iteratively registered with the pre-made solid model, the dense 3D point cloud is corrected based on the solid model, and then fused to generate a high-precision image solid model.

[0251] Based on the image entity model, the three-dimensional position and attitude angle of the towed body in the global coordinate system at the current moment are calculated.

[0252] Based on the calculated three-dimensional position and attitude angle of the towed body, and combined with the fixed installation position of the sonar sensor in the towed body coordinate system, the synchronously collected water depth data is reduced to the global coordinate system to obtain the three-dimensional coordinates of the underwater terrain.

[0253] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A method for positioning and attitude measurement of a towed UAV based on binocular vision localization, characterized in that, The method includes the following steps: S01. Construct a measurement system, the measurement system including a drone and a towed body connected to the drone by a cable; the drone is equipped with a GNSS module, an IMU module and a binocular camera, and the upper surface of the towed body is provided with a coded pattern and equipped with a sonar sensor; S02. Establish a towed body coordinate system with the acoustic center of the sonar sensor as the origin, obtain the three-dimensional coordinates of the feature points on the towed body in the towed body coordinate system, and generate a three-dimensional space model of the towed body for simulation. The specific steps of generating the three-dimensional spatial model of the towed body and running the simulation model include: The processed towed body is scanned no less than three times using a high-resolution laser scanner to obtain its color point cloud data; The average value of the point cloud data from multiple scans is calculated, and a point cloud model is built using the mesh method; The actual coordinates of the feature points on the towed body are measured using a total station, and the point cloud model is corrected based on the actual coordinates to obtain the solid model. S03. Calibrate the binocular camera and obtain its interior orientation elements, distortion parameters, and exterior orientation element deviation values ​​between the binocular camera system and the GNSS / IMU system. S04. The UAV tows the towed body and simultaneously records the following data sequence: the towed body image acquired by the binocular camera corresponding to each timestamp, the UAV position and attitude data acquired by the GNSS / IMU module, and the water depth data acquired by the sonar sensor. S05. Process the two towed body images acquired at each timestamp, extract the towed body region through image matching and segmentation, perform corresponding point matching, and generate a dense three-dimensional point cloud of the towed body at the current time through stereo image pair forward intersection calculation based on the calibrated camera parameters. S06. Iteratively register the dense 3D point cloud with the pre-made entity model, correct the dense 3D point cloud based on the entity model, and fuse them to generate a high-precision image entity model. S07. Based on the image entity model, calculate the three-dimensional position and attitude angle of the towed body in the global coordinate system at the current moment; S08. Based on the three-dimensional position and attitude angle of the towed body, and combined with the fixed installation position of the sonar sensor in the coordinate system of the towed body, the synchronously collected water depth data is converted to the global coordinate system to obtain the three-dimensional coordinates of the underwater terrain.

2. The method for positioning and attitude measurement of a towed UAV based on binocular vision localization according to claim 1, characterized in that, The calibration of the binocular camera specifically includes: The interior orientation elements and distortion parameters of the left and right monocular cameras were obtained using the Zhang Zhengyou calibration method. The binocular camera and GNSS / IMU module are fixedly connected to the UAV to form a binocular measurement system; Image data was acquired in a ground calibration field with known coordinates, and the exterior orientation elements of the binocular camera were solved by bundle adjustment of the regional network. A fixed deviation value is calculated between the exterior orientation element obtained by post-processing and the position and attitude data synchronously recorded by the GNSS / IMU module. This deviation value is used to correct the exterior orientation element directly measured by the GNSS / IMU in subsequent operations in real time.

3. The method for positioning and attitude measurement of a towed UAV based on binocular vision positioning according to claim 1, characterized in that, The processing of the left and right trailing images specifically includes the following sub-steps: S31. Using the distortion parameters obtained in step S01, perform distortion correction on the left and right original images; S32. Using the pre-stored towed body template image as a reference, a fast approximate nearest neighbor search algorithm is used to match in the corrected image to locate the area where the towed body is located; then, an image segmentation algorithm based on edge detection is used to accurately segment the local image of the towed body from the image. S33. On the segmented local images of the left and right dragging bodies, the SURF algorithm is used for feature extraction and corresponding point matching.

4. The method for positioning and attitude measurement of a towed UAV based on binocular vision positioning according to claim 3, characterized in that, In step S32, the image segmentation algorithm based on edge detection uses the Canny operator, and its process includes Gaussian filtering for noise reduction, calculation of gradient magnitude and direction, non-maximum suppression, and double-threshold edge connection.

5. The method for positioning and attitude measurement of a towed UAV based on binocular vision positioning according to claim 3, characterized in that, In step S33, the SURF algorithm adds the sign of the Hessian matrix trace as a feature variable to the feature descriptor, and only performs similarity calculation on feature points with the same sign during matching.

6. The method for positioning and attitude measurement of a towed UAV based on binocular vision positioning according to claim 1, characterized in that, The step S06, which involves iteratively registering the dense 3D point cloud with the solid model, specifically includes: Calculate the centroids of the dense 3D point cloud and the point cloud of the solid model, and transform the two point clouds to the centroid coordinate system; Calculate the matrix H between the two point clouds after transformation, and perform singular value decomposition on H to solve for the optimal rotation matrix R and translation vector T; The above process is executed iteratively until the preset number of iterations or the threshold of the change in the transformation matrix is ​​met.

7. A non-transitory computer-readable storage medium, wherein the non-transitory computer-readable storage medium stores at least one instruction or at least one program segment, characterized in that, The at least one instruction or the at least one program segment is loaded and executed by the processor to implement the UAV towed body positioning and attitude measurement method based on binocular vision positioning as described in any one of claims 1-6.

8. An electronic device, characterized in that, It includes a processor and a memory, wherein the memory stores at least one instruction or at least one program, and the at least one instruction or at least one program is loaded and executed by the processor to implement the UAV towed body positioning and attitude measurement method based on binocular vision positioning as described in any one of claims 1-6.

Images