High-precision positioning method for desilting equipment in limited space below wharf

Abstract

The invention discloses a high-precision positioning method for dredging equipment in a limited space below a wharf. A Kalman gain adaptive real-time updating fusion high-precision positioning method based on depth camera-IMU tight coupling positioning and multi-line laser radar-IMU tight coupling is established. According to the method, firstly, offline correction and joint external parameter calculation are carried out on a depth camera, an IMU and a multi-line laser radar sensor, then time synchronization is carried out on obtained corrected data, independent positioning system optimization is carried out according to a special scene below a wharf, a depth camera-IMU tight coupling system and a multi-line laser radar-IMU tight coupling system are established, and the depth camera-IMU tight coupling system and the multi-line laser radar-IMU tight coupling system are integrated. And finally, in order to realize high-precision positioning, an abnormal state processing method is provided, and self-adaptive Kalman fusion positioning system design is carried out on the two kinds of positioning, so that high-precision positioning of the dredging equipment in the limited space below the wharf is realized.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent dredging, in particular to high-precision positioning of intelligent dredging equipment in a narrow confined space, and specifically relates to a high-precision positioning method for dredging equipment in a confined space under a dock. Background Art

[0002] With the rapid development of automatic control technology, unmanned driving technology and robotics technology, the development of unmanned intelligent dredging and desilting equipment has a profound impact. At present, the sedimentation problem under the dock is an urgent problem to be solved in the dredging industry, and it is also a major arduous task for the intelligent dredging industry. The narrow space under the dock has extremely high requirements for the control accuracy of intelligent dredging equipment. However, the extremely high control accuracy depends on a high-precision positioning system. How to achieve high-precision positioning of dredging equipment in the narrow and confined space under the dock is a critical technical issue.

[0003] In the prior art, wireless communication technology is often used in outdoor environments to achieve accurate positioning of mobile targets, mainly including Bluetooth positioning, WIFI positioning, UWB positioning, GPS positioning and other positioning methods. However, in the narrow and confined space with complex structure under the dock, it is not suitable to use the above method to build a positioning network due to site limitations and water environment. Because in the complex scene under the dock, even if there are more columns (i.e. pile foundations) and water surface reflections (i.e. water environment), the base station cannot be installed or is inconvenient to install, and the base station operation will be affected by multipath interference, resulting in poor signal and inability to position. At present, there is an urgent need for a new high-precision positioning method for dredging equipment in confined spaces under docks. Summary of the invention

[0004] The purpose of the present invention is to provide a high-precision positioning method for dredging equipment in a confined space under a wharf. According to the limited space and high similarity structured characteristics under the wharf, a depth camera combined with an IMU tightly coupled positioning system and a multi-line laser radar combined with an IMU tightly coupled positioning system are designed using a multi-line laser radar, a depth camera, an IMU (i.e., Inertial Measuring Unit) and other sensing devices, and an improved simultaneous positioning and mapping SLAM (i.e., Simultaneous Localization and Mapping) algorithm. A Kalmanson coupling fusion strategy for the two positioning systems is formulated, which solves the problem that the dredging equipment cannot be autonomously positioned in a confined space under the wharf during the construction process because traditional positioning methods such as GPS positioning and UWB positioning cannot be used. Among them, the depth camera combined with an IMU tightly coupled positioning system can realize output positioning through visual input, and the multi-line laser radar combined with an IMU tightly coupled positioning system can realize output positioning through laser input. The Kalmanson coupling fusion strategy for the two positioning systems can further improve the system robustness and system positioning accuracy.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] A high-precision positioning method for dredging equipment in a confined space under a dock, comprising:

[0007] S1: Sensor internal calibration.

[0008] Because high-precision positioning is required, the data acquired by the sensor is not accurate enough, and the sensor's own parameters need to be calibrated (internal parameters) to determine the sensor's noise level, data zero drift level, etc., so as to compensate for the sensor's input and output data.

[0009] Sensor intrinsic calibration includes determining the IMU attitude sensor intrinsic parameters and depth camera distortion parameters. The IMU attitude sensor intrinsic parameters include IMU random walk noise parameters and bias error; the depth camera distortion parameters include radial distortion and tangential distortion parameters.

[0010] Furthermore, the sensor internal parameter calibration described in step S1 includes the following contents:

[0011] S1_1: IMU sensor internal parameter calibration.

[0012] The present invention establishes a posture output model composed of IMU bias error and random walk error, calibrates the random walk parameters using the Allan variance method, calibrates the bias error parameters using the six-plane method, and calibrates and compensates the IMU acceleration and angular velocity output using the IMU model established by the present invention and the calibration results.

[0013] S1_2: Depth camera intrinsic calibration.

[0014] The present invention uses a chessboard to calibrate the distortion parameters of a depth camera, and uses Zhang's calibration method to perform image calibration based on the established radial and tangential distortion calibration models.

[0015] S2: Sensor external parameter calibration.

[0016] External parameter calibration is mainly to determine the relative installation position parameters between sensors, that is, the installation position of the depth camera and IMU, and the installation position of the lidar and IMU.

[0017] The present invention uses a two-dimensional code calibration plate to determine the rotation and translation change parameters between the depth camera and the IMU and between the multi-line laser radar and the IMU, and then determines the relative rotation and translation matrix.

[0018] Furthermore, the external parameter calibration described in step S2 includes the following contents:

[0019] S2_1: Depth camera and IMU joint external parameter calibration.

[0020] The present invention develops a two-dimensional code calibration plate with no less than 6 rows and columns, and writes a YAML description file to describe the physical parameters of the calibration plate, record the static data parameters of the joint rotation and translation changes of the depth camera and IMU, and use the Kalibr algorithm to solve the rotation and translation matrix after sufficient movement.

[0021] S2_2: Joint external parameter calibration of multi-line lidar and IMU.

[0022] The present invention performs slow translation and rotation transformation operations on a multi-line laser radar-IMU positioning system in an empty room on multiple planes, records data, and performs rotation and translation matrix calibration using a target-free calibration method.

[0023] S3: Sensor time synchronization.

[0024] After calibrating the sensor and determining the sensor's internal and external parameters, it is necessary to synchronize the sensor output data in terms of data time, which is also a relatively important step.

[0025] The present invention performs time synchronization between the depth camera and IMU, and the multi-line laser radar and IMU data, eliminates the time difference of data reaching the CPU, provides time synchronization guarantee for the positioning system coupling, avoids data offset and large errors, and thus affects the positioning accuracy of the system.

[0026] S4: Establish a tightly coupled positioning system between the depth camera and IMU.

[0027] The present invention utilizes the high-frequency characteristics of IMU data to perform short-term posture prediction between depth camera image frames, uses image frames for long-term posture prediction, and outputs the posture of the depth camera-IMU positioning system in a tightly coupled manner.

[0028] Furthermore, the establishment of a tightly coupled positioning system between the depth camera and the IMU described in step S4 mainly includes:

[0029] S4_1: IMU integration between image frames.

[0030] The present invention establishes an IMU short-time pre-integration model to decouple the IMU measurement results from the absolute posture and estimate the posture changes between image frames.

[0031] S4_2: Image feature point extraction.

[0032] The present invention innovatively invents a rectangular corner point discrimination method, which can effectively speed up the extraction of feature points.

[0033] The present invention converts an image frame into a grayscale image, traverses the pixel points on the image, extracts brightness data of the pixel points at a fixed distance within the upper, lower, left, and right rectangular range centered on the pixel point, sets a brightness threshold, and determines that if a large number of pixel points have brightness greater than the threshold, they are corner points, thereby quickly extracting feature points on the image.

[0034] S4_3: Estimate the motion position based on the image feature points.

[0035] The present invention assumes that the image changes with time and the grayscale value of the pixel at the same spatial point in the grayscale image is constant when estimating the position, and then uses the feature points found in step S4_2 and the motion of the feature points between adjacent image frames to estimate the lateral and longitudinal motion speeds of the camera.

[0036] S4_4: Estimate motion posture based on image feature points.

[0037] The present invention defines an image as a number of small blocks when estimating the posture, and defines the moment of each small block, and then the direction vector of the image block movement can be solved through its centroid.

[0038] S4_5: IMU data and image positioning data fusion strategy.

[0039] Determine whether the image feature points extracted in step S4_2 are complete; if the image feature points are complete, initialize the IMU integration and calculate the positioning system pose according to step S4_1; otherwise, do not initialize and discard the current image frame.

[0040] S4_6: Generate environmental perception information map.

[0041] Record historical image frames, depth image frames, and pose change data to incrementally generate environment maps.

[0042] S4_7: Image frame loop detection optimization.

[0043] Using the recorded environment map, a similarity evaluation function is constructed to perform loop detection and calibration, and the final optimized pose is output.

[0044] S5: Establish a multi-line lidar IMU tightly coupled positioning system.

[0045] The present invention innovatively utilizes multi-line laser radar to extract three-dimensional environmental features, compresses the features in two dimensions, solves the posture through scanning matching, and utilizes the high-frequency characteristics of IMU data to perform distortion correction on the inter-frame motion of the multi-line laser radar.

[0046] Furthermore, the establishment of the multi-line laser radar IMU tightly coupled positioning system described in step S5 mainly includes:

[0047] S5_1: Multi-line LiDAR point cloud segmentation.

[0048] According to the actual positioning scenario under the dock, ripples on the turbid horizontal surface will cause unreliable feature recognition to the multi-line laser radar positioning system. The present invention proposes a threshold filtering method to perform point cloud segmentation on the multi-line laser radar, that is, to determine whether the angle between horizontal adjacent points on the cloud is less than or equal to the threshold angle. When the angle between horizontal adjacent points on the point cloud is less than or equal to the threshold angle, it is a point on the wave surface. When the adjacent angle is greater than the threshold angle, it is a point on the same object, thereby realizing the segmentation of the point cloud.

[0049] S5_2: Multi-line LiDAR feature point extraction.

[0050] First, the Euclidean distance between adjacent points in the point cloud is used to determine whether the adjacent point clouds are continuous. If they are continuous, their curvature can be calculated, and the curvature threshold is set based on experience to determine whether their curvature is less than or equal to the curvature threshold. If it is less than or equal to the curvature threshold, it is a flat point. If it is greater than the curvature threshold, it is a corner point, that is, a feature point.

[0051] S5_3: Two-dimensional compression of point cloud features.

[0052] Since there is a relationship between the space below the dock and the tide level, that is, the three-dimensional space state will be affected by the tide level, which may cause the vertical direction features of the radar scan to fluctuate greatly with the rise and fall of the tide level, the present invention compresses the three-dimensional features into two dimensions, thereby avoiding the influence of the vertical features affected by the tide level change on the positioning. Specifically, the feature point cloud extracted in step S5_2 is removed from the distance information in the vertical direction to obtain the feature point cloud of the two-dimensional plane.

[0053] S5_4: Scan matching to solve the pose.

[0054] According to the two-dimensional feature point cloud obtained in step S5_3, the optimal pose solver is constructed using the least squares method to solve the optimal pose.

[0055] S5_5: IMU point cloud motion distortion correction.

[0056] Due to the high similarity of the scenes under the dock and the influence of the corridor effect, the IMU pre-integration method is used to predict the pose between multi-line lidar frames to prevent pose jumps.

[0057] S5_6: Incremental map building.

[0058] The feature point cloud is incrementally saved according to the position and posture of the multi-line lidar-IMU positioning system to generate a two-dimensional map, and the obstacle boundary in the grid map is set according to the threshold.

[0059] S5_7: Multi-line laser radar loop correction.

[0060] According to the characteristic two-dimensional point cloud, the matrix area, corner point density, and arc diameter feature vectors are constructed and saved as historical data. The Euclidean distance between the current feature vector and the historical feature vector is used to determine whether there is a loop. If there is a loop, loop correction is performed. After loop detection, the corrected posture is output.

[0061] S6: Establish a depth camera-IMU and multi-line lidar-IMU multi-sensor loosely coupled fusion positioning system.

[0062] The present invention innovatively proposes a loosely coupled fusion positioning method. The loosely fused system can improve the system redundancy, robustness and stability of the system. Specifically, according to the posture output by the depth camera-IMU positioning system and the multi-line laser radar-IMU positioning system, the positioning posture data is loosely coupled, which further improves the reliability and stability of the positioning data and improves the redundancy of the positioning system.

[0063] Furthermore, the establishment of the depth camera-IMU and multi-line laser radar-IMU multi-sensor loosely coupled fusion positioning system described in step S6 includes the following contents:

[0064] S6_1: System abnormal state switching.

[0065] The visual positioning system is greatly affected by the ambient light. Overexposure or insufficient light will seriously affect the positioning effect of the visual positioning system and produce large deviations. The present invention innovatively designs a switching strategy when the positioning system is in an abnormal state, that is, when the Euclidean distance between the output pose of the depth camera-IMU positioning system and the pose of the multi-line laser radar-IMU positioning system is greater than or equal to the set threshold, the depth camera-IMU positioning system is unreliable and only outputs the optimal pose estimate of the radar positioning system. If it is less than the set threshold, data fusion is performed using data fusion.

[0066] S6_2: Design of fusion filter for Kalman-based depth camera-IMU positioning system and lidar-IMU positioning system.

[0067] The present invention innovatively designs an adaptive Kalman filter for the system and automatically updates the Kalman filter parameters. Specifically, the fusion filter in the present invention is designed using the Kalman data fusion method, and the median of the posture at the previous moment is used as the mean to obtain the error of the posture change at the current moment, thereby constructing a real-time mean and variance solver to solve the real-time variance, so as to continuously update the Kalman gain, realize adaptive real-time optimal solution, output the optimal posture estimate, and realize high-precision positioning under the dock.

[0068] Compared with the prior art, the technical solution of the present invention has at least the following beneficial effects:

[0069] For example, based on the limited space and highly similar structured characteristics under the dock, the present invention utilizes multi-line lidar, depth camera, IMU and other sensing equipment, and adopts an improved simultaneous positioning and mapping SLAM algorithm to design a depth camera combined with IMU tightly coupled positioning system and a multi-line lidar combined with IMU tightly coupled positioning system, and formulates a Kalmanson coupling fusion strategy for the two positioning systems, which solves the problem that dredging equipment cannot perform autonomous and high-precision positioning in the confined space during the construction process under the dock when traditional positioning methods such as GPS positioning and UWB positioning cannot be used.

[0070] For another example, the present invention only utilizes multi-line lidar, depth camera, IMU and other sensor equipment, and does not require the installation of additional base stations. It performs non-contact environmental perception and positioning. Compared with the traditional positioning method of installing a large number of base stations, it greatly reduces the positioning cost and the complexity of positioning system deployment, facilitates implementation, and makes intelligent control of intelligent dredging equipment under the dock possible.

[0071] For another example, the present invention utilizes multi-line lidar, depth camera, IMU and other sensor equipment to establish a real-time environmental map based on the real-time data fed back, while achieving high-precision positioning. This provides the prerequisite for planning the construction path of dredging equipment in the narrow space under the dock.

[0072] For another example, the present invention designs a loosely coupled fusion strategy for two positioning systems for the special scene under the dock, which improves the redundancy of the system and greatly improves the reliability and stability of the positioning system. Compared with the traditional method of setting up base stations, which has the problem of multipath interference, the present invention is not affected by it.

[0073] For another example, the present invention adopts a multi-line lidar-IMU tightly coupled positioning system and a depth camera-IMU tightly coupled positioning system, utilizing the high-frequency characteristics of the IMU to complement the advantages of the camera or the depth camera, and not just the fusion at the data level, thereby achieving higher-precision positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] In order to more clearly illustrate the high-precision positioning method of dredging equipment in the confined space under the dock proposed by the present invention, the technical solution of the present invention will be described below through the accompanying drawings. For other ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0075] Figure 1 This is a principle block diagram of a tightly coupled positioning system of a depth camera and an IMU in an embodiment of the present invention;

[0076] Figure 2 This is a principle block diagram of a multi-line laser radar and IMU tightly coupled positioning system in an embodiment of the present invention;

[0077] Figure 3 It is a principle block diagram of a loosely coupled positioning system of a multi-line laser radar-IMU positioning system and a depth camera-IMU positioning system in an embodiment of the present invention;

[0078] Figure 4 A flowchart of a high-precision positioning method according to an embodiment of the present invention;

[0079] Figure 5 Schematic diagram of the definition of the angle θ of adjacent points of the laser radar in an embodiment of the present invention, wherein A and B are adjacent points of the laser radar, and OA and OB are laser radar rays;

[0080] Figure 6 This is a diagram showing the positioning effect of using the high-precision positioning method in an embodiment of the present invention. DETAILED DESCRIPTION

[0081] In order to make the purpose, features and beneficial effects of the present invention more obvious and easy to understand, the following is a brief description of the present invention in conjunction with the attached Figures 1 to 6 The specific implementation of the present invention is described in detail. It should be understood that the specific implementation described below is only used to explain the present invention, rather than to limit the present invention.

[0082] For technologies, methods and devices known to those skilled in the art, this article may not be discussed in detail, but where appropriate, the technologies, methods and devices should be considered as part of the authorization specification. In all examples shown and discussed here, any specific values ​​should be interpreted as merely exemplary and not as limiting. Therefore, other examples of the exemplary embodiments may have different values.

[0083] An embodiment of the present invention provides a high-precision positioning method for dredging equipment in a confined space under a dock.

[0084] Specifically, the high-precision positioning method may include the following steps:

[0085] S1: sensor internal calibration;

[0086] S2: sensor external parameter calibration;

[0087] S3: sensor time synchronization;

[0088] S4: Establish a tightly coupled positioning system between the depth camera and IMU;

[0089] S5: Establish a multi-line lidar IMU tightly coupled positioning system;

[0090] S6: Establish a depth camera-IMU and multi-line lidar-IMU multi-sensor loosely coupled fusion positioning system.

[0091] Because high-precision positioning needs to be achieved, but the data obtained by the sensor is not accurate enough, it is necessary to calibrate the sensor's own parameters (i.e., internal parameters) to determine the sensor's noise level, data zero drift level, etc., so as to compensate for the sensor's input and output data.

[0092] In the specific implementation of step S1, it is necessary to perform internal calibration on the IMU sensor and the depth camera respectively, and compensate and correct the sensor output data according to the different intrinsic parameters of different hardware devices, so as to provide more accurate positioning information.

[0093] Furthermore, the sensor intrinsic parameter calibration described in step S1 includes: determining the IMU attitude sensor intrinsic parameters and the depth camera distortion parameters. The IMU attitude sensor intrinsic parameters include the IMU random walk noise parameters and the bias error; the depth camera distortion parameters include the radial distortion and the tangential distortion parameters.

[0094] Specifically, the sensor internal parameter calibration described in step S1 is achieved by the following steps:

[0095] S1_1: IMU sensor internal parameter calibration.

[0096] The present invention establishes a posture output model composed of IMU bias error and random walk error, calibrates the random walk parameters using the Allan variance method, calibrates the bias error parameters using the six-plane method, and calibrates and compensates the IMU acceleration and angular velocity output using the IMU model established by the present invention and the calibration results.

[0097] Specifically, record the data of the IMU data packet when it is stationary for at least 2 hours. Use the Allen variance method to calibrate the random walk noise of the IMU, use the six-face method to calibrate the bias error, determine the bias error and random error, and compensate the accelerometer and gyroscope outputs according to the following formula.

[0098]

[0099] in, is the output value of the accelerometer, is the gyroscope output value, a W (t) is the actual output value of acceleration at the current moment, ω b (t) is the actual output value of the gyroscope at the current moment, b a (t) is the acceleration random walk bias value, b g (t) is the gyroscope angular velocity random walk bias value, g W is the measured value of gravitational acceleration, n a (t) is the random noise when the acceleration is stationary, n g (t) is the random noise of the gyroscope when it is stationary, is the rotation matrix.

[0100] S1_2: Depth camera intrinsic calibration.

[0101] The present invention uses a chessboard to calibrate the distortion parameters of a depth camera, and uses Zhang's calibration method to perform image calibration based on the established radial and tangential distortion calibration models.

[0102] Specifically, the depth camera is calibrated with a calibration plate, which is a two-dimensional black and white chessboard. The spacing between the black and white chessboard should be no less than 2 cm, and the number of rows and columns of the chessboard should be no less than 7. The calculation formula of the depth camera distortion correction model is as follows:

[0103]

[0104] Among them, the radial distortion parameter is a 1 、a 2 、a 3 ; The tangential distortion parameter is b 1 、b 2 ; r is a 1 The parameters are mainly used to correct the distortion area in the center of the image. 2The parameters are mainly used to correct the edge area of ​​the image. 3 The parameter corrects the area with larger overall image distortion, b 1 , b 2 The parameters are used to correct the error caused by the camera lens installation. Different parameter correction models are selected according to the imaging conditions of the actual image. The distortion parameters are determined by Zhang’s calibration method, (x new ,y new ) is the pixel coordinate after distortion, and (x, y) is the ideal pixel coordinate.

[0105] S2: Sensor external parameter calibration.

[0106] Sensor external parameter calibration mainly includes depth camera and IMU external parameter calibration, multi-line laser radar and IMU external parameter calibration, which determines the external parameters of depth camera and IMU, multi-line laser radar and IMU, that is, the rotation and translation transformation matrix from the coordinate system of the depth camera and multi-line laser radar to the coordinate system of the IMU, so as to provide accurate external reference information for high-precision positioning and improve the fusion positioning accuracy. The relative positions of the multi-line laser radar, depth camera and IMU are fixed during calibration.

[0107] Specifically, the sensor external parameter calibration described in step S2 is achieved by the following steps:

[0108] S2_1: Depth camera and IMU joint external parameter calibration.

[0109] Make a QR code calibration board for joint calibration. The number of rows and columns of the calibration board should be no less than 6. Write a description YAML description file based on the QR code calibration board. During calibration, record image data and IMU data for no less than 2 minutes, and fully include the rotation and translation operations of the three axes of the IMU to make it fully move; turn off the depth camera structured light to avoid the influence of structured light on the image. The image publishing frequency of the left and right cameras is 20Hz, and the IMU publishing frequency is 200Hz. After full movement, use the Kalibr algorithm to obtain the relative rotation and translation matrix of the depth camera binoculars and IMU.

[0110] In the specific implementation, using the Kalibr algorithm to obtain the relative rotation and translation matrices of the depth camera binoculars and IMU is a conventional technical means in the field and will not be repeated here.

[0111] S2_2: Joint external parameter calibration of multi-line lidar and IMU.

[0112] The present invention performs slow translation and rotation transformation operations on a multi-line laser radar and an IMU positioning system in an empty room on multiple planes, records data, and performs rotation and translation matrix calibration using a target-free calibration method.

[0113] Specifically, the multi-line lidar and IMU data are positioned in an empty multi-plane room, and the IMU is moved and rotated slowly enough in the three axes, and the rotation matrix and translation matrix are determined using the target-free calibration method.

[0114] S3: Sensor time synchronization.

[0115] After the sensor internal and external parameters are determined by calibrating the sensor, the sensor output data needs to be synchronized in terms of data time. The present invention synchronizes the depth camera with the IMU, the multi-line laser radar with the IMU data, eliminates the time difference of the data reaching the CPU, provides time synchronization guarantee for the positioning system coupling, avoids data offset and large errors, and thus affects the positioning accuracy of the system.

[0116] Specifically, the depth camera, multi-line lidar, and IMU sensor work independently, and their data output frequencies are different. When the data is collected to the CPU, there is always a time difference Δτ between the sensors, which will cause irreversible position errors between the sensors.

[0117] In order to reduce the impact on positioning, it is assumed that within a short time Δτ, the feature points of the depth camera image move at a uniform speed between two frames. The speed of the feature point l in the k frame and the reprojection error equation are:

[0118]

[0119]

[0120] in, Represents the pixel position vector of the feature point on the image, is the camera world coordinate system position vector, is the pose transformation matrix, P w is the landmark point in the world coordinate system, π(·) is the function of converting camera coordinates to pixel coordinates, V l k represents the velocity of feature point l in frame k, Represents the reprojection error of feature point l in frame k.

[0121] Introducing the delay Δτ, we get:

[0122]

[0123] Set at c i The depth value of feature point l on the frame image is γ i , converted to the world coordinate system:

[0124]

[0125] c i Pixel coordinates on the frame image, P l Represents the position vector of feature point l in world coordinates, is the pose transformation matrix, is the camera's world coordinate position vector.

[0126] P l Projection to c i The pixel plane of the frame is measured as The reprojection error is then:

[0127]

[0128] Among them, Δτ affects the i-th and j-th frame measurement systems:

[0129]

[0130]

[0131] Then the reprojection error considering the influence of Δτ is updated as:

[0132]

[0133] The camera position and delay are optimized by minimizing the error function at runtime.

[0134] In the present invention, the multi-line laser radar and IMU adopt hardware clock synchronization, that is, PTP network clock synchronization. The specific implementation process is a conventional technical means in the field, which will not be repeated here.

[0135] S4: Establish a tightly coupled positioning system between the depth camera and IMU.

[0136] Based on the low-frequency characteristics of the depth camera and the high-frequency characteristics of the IMU and the long-term drift of the IMU, the IMU sensor is used for short-term pose prediction and the depth camera is used for long-term pose prediction.

[0137] Because IMU data is high frequency, it can be used for short-term prediction. IMU is generally around 200Hz, that is, there are 200 data in 1 second, while the camera is generally 30Hz, about 30 times in 1 second. Then within 1 / 30 second, the camera has no data, but the IMU has data. Because IMU data is 1 / 200 second at a time, which is much less than 1 / 30, the high-frequency characteristics of IMU are used to make short-term predictions of the camera motion pose within 1 / 30 second. Because IMU has drift in long-term operation, the data will slowly distort, resulting in inaccurate long-term predictions, but the camera will not, so the camera image frame is used for pose prediction on a long-term scale.

[0138] Specifically, the establishment of the tightly coupled positioning system between the depth camera and the IMU described in step S4 is achieved by the following steps:

[0139] S4_1: IMU integration between image frames.

[0140] An IMU short-time pre-integration model is established to decouple the IMU measurement results from the absolute posture and estimate the posture change between image frames. Specifically, the linear velocity and angular velocity obtained by the IMU are integrated to obtain the displacement value and angle value, but the integration operation is time-consuming. Therefore, the pre-integration idea is adopted to decouple the IMU measurement results from the absolute posture, thereby avoiding repeated integration operations. The IMU pre-integration model is as follows:

[0141]

[0142] in, See step S1_1 for acquisition. is the IMU short-time Δt attitude estimation, P imu is the IMU short-time Δt position estimate, V imu It is the velocity estimation within a short time Δt of IMU.

[0143] S4_2: Image feature point extraction.

[0144] The present invention innovatively invents a rectangular corner point discrimination method, which can effectively speed up the extraction of feature points. The present invention converts an image frame into a grayscale image, traverses the pixel points on the image, extracts brightness data of the pixel points within a fixed distance in the upper, lower, left, and right rectangular range centered on the pixel point, sets a brightness threshold, and determines that if there are a large number of pixel points with brightness greater than the threshold, they are corner points, thereby quickly extracting feature points on the image.

[0145] Specifically, the color image is converted into a grayscale image, and an arbitrary pixel point δ is selected on the image, and its brightness is represented by I δ , the rectangular discrimination method is used to extract feature corners, that is, the threshold τ is set, and the pixel point δ is taken as the center, and the pixel points Q on the upper, lower, left and right sides of the rectangle with a distance of δ of 2 are symmetrically selected. If the pixel points are greater than the threshold I δ +τ or less than I δ If the number of pixels of -τ is not less than 3 / 4Q, the point is considered a feature point. By traversing the pixels of the entire image, the feature points of the entire image can be found.

[0146] S4_3: Estimate the motion position based on the image feature points.

[0147] The present invention assumes that the image changes with time and the grayscale value of the pixel at the same spatial point in the grayscale image is constant when estimating the position, and then uses the feature points found in step S4_2 and the motion of the feature points between adjacent image frames to estimate the lateral and longitudinal motion speeds of the camera.

[0148] Specifically, assuming that the image changes over time, and the grayscale value of the pixel at the same spatial point in the grayscale image is constant, the grayscale of the image can be expressed as I(x, y, t), and when the camera moves for Δt time, there will be:

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

[0150] After Taylor series expansion, it can be expressed as:

[0151]

[0152] Where dx / dt and dy / dt are the speeds of the x-axis and y-axis, respectively, denoted by v x 、v y , The gradient of the feature point on the image in the x-axis and y-axis directions is denoted by I x ,I y .

[0153] When the camera moves, assuming that each feature point has the same movement mode, then:

[0154]

[0155] in

[0156] S4_4: Estimate motion posture based on image feature points.

[0157] The present invention defines an image as a number of small blocks when estimating the posture, and defines the moment of each small block, and the direction vector of the image block movement can be solved through its centroid.

[0158] Specifically, the image is divided into several fixed small blocks, and the moment in each small image block C is defined as:

[0159]

[0160] Then its centroid is:

[0161]

[0162] Then the image block direction vector can be expressed as:

[0163] θ cam =arctan(m01 / m 10 )

[0164] S4_5: IMU data and image positioning data fusion strategy.

[0165] Determine whether the image feature points extracted in step S4_2 are complete; if the image feature points are complete, initialize the IMU integration and calculate the positioning system pose according to step S4_1; otherwise, do not initialize and discard the current image frame. That is, when a new image frame arrives and the image frame feature points are complete, restart the IMU integration and obtain the positioning system pose according to step S4_1, otherwise do not initialize and discard the current image frame.

[0166] S4_6: Generate environmental perception information map.

[0167] Record historical image frames, depth image frames, and pose change data to incrementally generate environment maps.

[0168] Specifically, the depth point cloud image of the environmental information obtained by the depth camera during the entire movement process is combined with the position and posture trajectory path, that is, each image frame accumulates and saves the depth image data according to the position and posture of the current frame to generate a 3D image point cloud map.

[0169] S4_7: Image frame loop detection optimization.

[0170] Using the recorded environment map, a similarity evaluation function is constructed to perform loop detection and calibration, and the final optimized pose is output.

[0171] Due to the inevitable error accumulation between image frames in the long-term pose estimation process, it is impossible to build a globally consistent trajectory and map. When a loop appears in the map, the image frames will have extremely high similarity. Using the similarity judgment function:

[0172]

[0173] π q , π d is the gray value of the feature point of the feature image. When the score of 5 consecutive image key frames is more than 3 times the score between adjacent frames, it is judged as a loop, and the trajectory posture is corrected using scale changes, and the corrected posture P is output. τ (p xτ ,p yτ ,θ τ ).

[0174] S5: Establish a multi-line lidar IMU tightly coupled positioning system.

[0175] The present invention innovatively utilizes multi-line laser radar to extract three-dimensional environmental features, compresses the features in two dimensions, solves the posture through scanning matching, and utilizes the high-frequency characteristics of IMU data to perform distortion correction on the inter-frame motion of the multi-line laser radar.

[0176] Specifically, the establishment of the multi-line laser radar IMU tightly coupled positioning system described in step S5 is achieved by the following steps:

[0177] S5_1: Multi-line LiDAR point cloud segmentation.

[0178] Since the fluctuation of water surface ripples will affect the recognition of the environment by the laser radar point cloud, the fluctuation will bring unreliable environmental features, that is, the turbid wave surface in two consecutive scans will form unreliable features. Therefore, effective point cloud segmentation can effectively avoid the influence of unreliable features on the positioning system. In order to speed up the processing speed of the point cloud, the present invention adopts the threshold filtering method to perform point cloud segmentation. For two adjacent points in the point cloud, the angle is α. If α≤10°, they are considered to be points on the wave surface. If the angle α≥60° ​​and α≤120°, they are considered to be points on the same object. Please refer to the attached diagram for the definition of the angle. Figure 5 .

[0179] Where O is the center point of the lidar, and A and B are adjacent points on the radar point cloud.

[0180] S5_2: Multi-line LiDAR feature point extraction.

[0181] Feature extraction first needs to determine whether the point is a continuous point. Specifically, the Euclidean distance between adjacent points in the point cloud can be used to determine whether the adjacent point clouds are continuous. If they are continuous, their curvature can be calculated, and the curvature threshold can be set based on experience to determine whether their curvature is less than or equal to the curvature threshold. If it is less than or equal to the curvature threshold, it is a flat point. If it is greater than the curvature threshold, it is a corner point, that is, a feature point.

[0182] The continuous point discrimination formula proposed by the present invention is:

[0183]

[0184] Among them, p i is the current point on the point cloud, p i+1 、p i-1 is the neighboring point on the current point cloud. If ρ∈(0.75,1.25), then point p is considered i is a continuous point on the point cloud. If it is a continuous point, the curvature calculation formula is defined as follows:

[0185]

[0186] Where k is the curvature, p i is the current point on the point cloud, pj is the 5 adjacent points before and after, and S is the set of points before and after the current point. When k≤0.25, it is considered flat, and when k>0.25, it is considered p i It is a corner point.

[0187] S5_3: Two-dimensional compression of point cloud features.

[0188] Since there is a relationship between the space below the dock and the tide level, that is, the three-dimensional spatial state will be affected by the tide level, which may cause the vertical direction characteristics of the radar scan to fluctuate greatly with the rise and fall of the tide level, the present invention compresses the three-dimensional characteristics into two dimensions, thereby avoiding the influence of the vertical characteristics affected by the tide level changes on positioning.

[0189] Specifically, according to the point cloud features obtained in steps S5_1 and S5_2, the plane points in the horizontal direction are removed from the point cloud, and the extracted corner points are projected onto a two-dimensional plane to obtain a two-dimensional plane feature point cloud.

[0190] p(x,y,z)=p(x,y,z)| z＝0

[0191] Among them, p(x,y,z) is a point on the point cloud.

[0192] S5_4: Scan matching to solve the pose.

[0193] According to the two-dimensional point cloud of the frame obtained by S5_3, the optimal solution formula for the pose is constructed using the least squares method as follows:

[0194]

[0195] in, represents the position and posture of the dredging equipment in the two-dimensional plane, p x , p y is the two-dimensional horizontal and vertical coordinates in the world coordinate system, θ is the rotation angle relative to the Y axis of the world coordinate system, represents the initial pose, represents the optimal pose for this scan matching, Indicates the coordinates of the laser radar coordinate system converted to the world coordinate system. i,x 、s i,y is a point in the radar coordinate system. x 、p y is the initial position. M represents the occupancy rate in the map generated by scan matching.

[0196] S5_5: IMU point cloud motion distortion correction.

[0197] Due to the special structure under the wharf, the similarity of different positions is high and there is a corridor effect. Therefore, the IMU pre-integration value is used as the prediction of the inter-frame pose of the multi-line lidar to prevent the pose jump caused by the high similarity of the pile foundation structure under the wharf.

[0198] S5_6: Incremental map building.

[0199] The feature point cloud is incrementally saved according to the position and posture of the multi-line lidar-IMU positioning system to generate a two-dimensional map, and the obstacle boundary in the grid map is set according to the threshold.

[0200] Specifically, if the occupancy rate M(s i,x ,s i,y ) is greater than 0.4, then the grid map is set to black. If the occupancy rate M(s i,x ,s i,y ) is less than 0.4, the grid is set to white and the two-dimensional feature point cloud is saved incrementally.

[0201] S5_7: Multi-line laser radar loop correction.

[0202] According to the characteristic two-dimensional point cloud, the matrix area, corner point density, and arc diameter feature vectors are constructed and saved as historical data. The Euclidean distance between the current feature vector and the historical feature vector is used to determine whether there is a loop. If there is a loop, loop correction is performed. After loop detection, the corrected posture is output.

[0203] Specifically, the feature point cloud p(x, y, z) compressed by S5_3 is vectorized according to the line-surface geometric relationship, such as the rectangular area S, the corner point density α, and the arc diameter d, and represented as F where F i (S i ,α i , d i ) represents the feature vector of the i-th frame. The feature vectors of all image frames are recorded in the historical database, where F j (S j ,α j ,d j ) is the feature vector of the nth frame in the historical data. The i-th frame F i (S i ,α i ,d i ) and F j (S j ,α j ,d j ) distance is defined as:

[0204]

[0205] when When , it is considered that there is a loop in the i-th frame. Where L is a threshold. When the threshold L<0.25, the posture is calibrated.

[0206] Experiments have shown that when the threshold L ≥ 0.25, the loop detection effect is poor and will bring additional computational overhead. Therefore, the threshold L is set to < 0.25, and loop detection is performed to calibrate the posture. When L ≥ 0.25, it is an open loop state and no loop detection is performed.

[0207] S6: Establish a depth camera-IMU and multi-line lidar-IMU multi-sensor loosely coupled fusion positioning system.

[0208] The present invention innovatively proposes a loose coupling method. The loosely fused system can improve system redundancy, robustness and stability. Specifically, according to the position output by the depth camera-IMU positioning system and the multi-line laser radar-IMU positioning system, that is, the P obtained in step S4 τ (p xτ ,p yτ ,θ τ ) and step S5 obtained Loose coupling of positioning posture data can be performed to further improve the reliability and stability of positioning data and increase the redundancy of the positioning system.

[0209] S6_1: System abnormal state switching.

[0210] The visual positioning system is seriously affected by light and other factors. For example, overexposure or insufficient light will seriously affect the visual positioning effect, while the lidar positioning system is more reliable. Therefore, when the visual positioning system has a large deviation, it is necessary to remove the visual positioning system in time, that is, Where β is the deviation coefficient, which is generally set to 1. At this time, the system outputs the laser radar positioning, that is:

[0211]

[0212] Where P out is the positioning system output, P fusion Output the fusion results of vision and radar positioning.

[0213] S6_2: Adaptive fusion design of Kalman-based depth camera-IMU positioning system and lidar-IMU positioning system.

[0214] The present invention innovatively designs an adaptive Kalman filter for the system and automatically updates the Kalman filter parameters.

[0215] Specifically, according to P τ , The fused estimate is defined as:

[0216] P fusion =P 1 +K(P 2 -P 1 )

[0217] Among them, P 1 Output pose P for the depth camera-IMU positioning system τ , P 2 Output pose for multi-line lidar-IMU positioning system K is the Kalman gain, P fusion is the fusion output result. fusion The variance of is as follows:

[0218]

[0219] When the variance is minimum, dσ 2 / dK=0, that is, when the Kalman gain So that the variance σ 2 Minimum, at this time the positioning estimate value P fusion The variance of 2 .in is the depth camera-IMU positioning system variance, is the variance of the multi-line lidar-IMU positioning system, and its real-time update formula is as follows.

[0220]

[0221] in, represents the mean of the first j multi-line lidar-IMU positioning errors, represents the mean positioning error of the current j depth cameras-IMU, E 1i , and E 2i Represents the positioning error of the i-th frame output.

[0222] The present invention uses the Kalman data fusion method to design a fusion filter, takes the median of the posture at the previous moment as the mean, and obtains the error of the posture change at the current moment, thereby constructing a real-time mean and variance solver, solving the real-time variance, and continuously updating the Kalman gain to achieve adaptive real-time optimal solution, output the optimal posture estimate, and achieve high-precision positioning under the dock.

[0223] Although specific embodiments have been described above, these embodiments are not intended to limit the scope of the present disclosure, even when only a single embodiment is described with respect to specific features. The feature examples provided in the present disclosure are intended to be illustrative rather than limiting, unless otherwise stated. In specific implementations, the technical features of one or more dependent claims may be combined with the technical features of the independent claims, based on actual needs and where technically feasible, and the technical features from the corresponding independent claims may be combined in any appropriate manner rather than just by the specific combinations listed in the claims.

[0224] Although the present invention is disclosed as above, the present invention is not limited thereto. Any person skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the scope defined by the claims.

Claims

1. A high-precision positioning method for dredging equipment in a confined space under a dock, characterized in that: The following steps are involved: S1: sensor internal calibration; S2: sensor external parameter calibration; S3: sensor time synchronization; S4: Establish a tightly coupled positioning system between the depth camera and IMU; S5: Establish a multi-line lidar IMU tightly coupled positioning system; S6: Establish a depth camera-IMU and multi-line lidar-IMU multi-sensor loosely coupled fusion positioning system.

2. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 1, characterized in that: The sensor internal parameter calibration described in step S1 includes the following steps: S1_1: Use the Allen variance method and six-plane calibration method to determine the random walk noise and bias error parameters of the IMU for calibration. Compensate the IMU accelerometer and gyroscope outputs; S1_2: Prepare a calibration plate to calibrate the depth camera internal parameters, and use the following correction model to calibrate the camera distortion; The calibration plate is a two-dimensional black and white chessboard, the spacing between the black and white chessboard should be no less than 2cm, and the number of rows and columns of the chessboard should be no less than 7.

3. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 1, characterized in that: The sensor external parameter calibration described in step S2 includes: S2_1: Make a QR code calibration plate for joint calibration. The number of rows and columns of the calibration plate is not less than 6, and it is described in YAML. During calibration, the image publishing frequency is set to 20Hz, the IMU publishing frequency is set to 200Hz, and the structured light is turned off when recording data. The rotation and translation operations of the three axes of the IMU are included, and sufficient movement is performed. The rotation and translation matrix parameters of the depth camera and IMU are obtained using the Kalibr algorithm; S2_2: In an open environment, use the target-free calibration method to calibrate the multi-line lidar and IMU to determine the rotation and translation matrix parameters.

4. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 1, characterized in that: The sensor time synchronization described in step S3 includes: assuming that the feature points of the depth camera image move at a uniform speed between two frames within a short time Δτ, and using the constructed reprojection error solution formula affected by Δτ to solve the time synchronization.

5. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 1, characterized in that: The step S4 of establishing a tightly coupled positioning system between a depth camera and an IMU includes: S4_1: IMU integration between image frames; S4_2: Image feature point extraction; S4_3: Estimate motion position based on image feature points; S4_4: Estimating motion posture based on image feature points; S4_5: Fusion of IMU data and image positioning data; S4_6: Environmental perception information map generation; S4_7: Image frame loop detection optimization.

6. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 5, characterized in that: The image feature point extraction described in step S4_2 includes extracting image features using a rectangular corner point discrimination method; The rectangular corner point identification method traverses the pixel points on the image, extracts the brightness data of the pixel points within a fixed distance within the upper, lower, left, and right rectangular range centered on the pixel point, sets a brightness threshold, and determines that the pixel points are corner points if the number of pixel points with brightness greater than the threshold is large.

7. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 5, characterized in that: The step S4_3 of estimating the motion position based on the image feature points includes: assuming that the image changes over time, and the grayscale value of the pixel at the same spatial point in the grayscale image is constant, and the motion mode of each feature point is the same, thereby solving the motion speed of the feature point.

8. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 1, characterized in that: The establishment of the multi-line laser radar IMU tightly coupled positioning system described in step S5 includes: S5_1: multi-line lidar point cloud segmentation; S5_2: Multi-line laser radar feature point extraction; S5_3: 2D compression of point cloud features; S5_4: Scan matching to solve the pose; S5_5: IMU point cloud motion distortion correction; S5_6: incremental map building; S5_7: Multi-line laser radar loop correction.

9. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 8, characterized in that: The multi-line laser radar point cloud segmentation described in step S5_1 includes: multi-line laser radar point cloud segmentation based on threshold filtering, that is, when the angle between horizontally adjacent points on the point cloud is less than the threshold angle, it is considered to be a point on the wave surface; when the adjacent angle is greater than the threshold angle, it is considered to be a point on the same object.

10. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 8, characterized in that: The continuous point discrimination formula for extracting the multi-line laser radar feature points in step S5_2 is: And define the curvature formula of continuous points in the point cloud as 11. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 1, characterized in that: The step S6 of establishing a depth camera-IMU and multi-line laser radar-IMU multi-sensor loosely coupled fusion positioning system includes: S6_1: System abnormal state switching; S6_2: Adaptive fusion design of Kalman-based depth camera-IMU positioning system and lidar-IMU positioning system.

12. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 11, characterized in that: The system abnormal state switching described in step S6_1 includes: when the Euclidean distance between the output posture of the depth camera-IMU positioning system and the posture of the multi-line lidar-IMU positioning system is greater than a set threshold, only the optimal posture estimate of the radar positioning system is output; if it is less than the set threshold, data fusion is performed using a data fusion method.

13. The high-precision positioning method for dredging equipment in a confined space under a dock according to claim 11, characterized in that: The adaptive fusion design of the Kalman-based depth camera-IMU positioning system and the lidar-IMU positioning system described in step S6_2 includes: constructing a real-time mean and variance solver, updating the Kalman gain in real time, and outputting the optimal posture.

Images