Unmanned aerial vehicle complex environment positioning system and method based on double laser radars

Abstract

The invention provides an unmanned aerial vehicle complex environment positioning system and method based on double laser radars, and the system comprises a positioning data frame construction module which is used for constructing a multi-sensor fusion positioning data frame based on the double laser radars installed on an unmanned aerial vehicle in combination with an inertial measurement unit (IMU), a global positioning system (GPS) and a visual sensor; the degradation environment detection module is used for carrying out degradation environment detection on a preset complex environment based on a degradation environment detection algorithm and optimizing a positioning data framework based on a degradation environment detection result; the data processing module is used for filtering the positioning data in the optimized positioning data framework based on a point cloud filtering algorithm and an ICP algorithm and performing fusion registration and pose estimation to obtain the pose of the unmanned aerial vehicle and an environment map; and the positioning module is used for obtaining a positioning result of the unmanned aerial vehicle in a preset complex environment based on the pose of the unmanned aerial vehicle and the environment map. According to the invention, high-precision positioning and map construction in a complex environment can be realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV navigation and positioning, and particularly relates to a UAV complex environment positioning system and method based on dual lidars. Background Art

[0002] In complex enclosed environments (such as warehouses, tunnels, ship cargo holds, etc.), traditional UAV inspection technologies face problems such as poor positioning accuracy and poor environmental adaptability. Most of the existing technologies rely on the fusion scheme of a single lidar and an inertial measurement unit (IMU). However, in an environment with sparse features, the scanning angle and range of the lidar are limited, which easily leads to positioning drift and error accumulation. At the same time, the existing solutions rely strongly on environmental features. In the face of a degraded environment (such as a long straight corridor, a symmetric cabin, etc.), the single lidar scheme often fails to effectively provide sufficient environmental information. Improving the positioning accuracy of UAVs in complex and degraded environments and reducing error accumulation have become key issues in UAV inspection technologies. There is an urgent need for a new solution that can overcome the challenges in degraded environments and improve the positioning and mapping capabilities of UAVs in these environments. Summary of the Invention

[0003] To overcome the deficiencies in the prior art, the present invention proposes a UAV complex environment positioning system and method based on dual lidars. This method realizes high-precision positioning and three-dimensional map construction in a complex degraded environment through the collaborative work of dual lidars, combined with an improved ICP algorithm and multi-sensor data fusion technology.

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

[0005] A UAV complex environment positioning system based on dual lidars, comprising:

[0006] A positioning data framework construction module, configured to collect positioning data of a preset complex environment based on dual lidars installed on a UAV, in combination with an inertial measurement unit IMU, a global positioning system GPS, and a vision sensor, and construct a multi-sensor fusion positioning data framework;

[0007] A degraded environment detection module, configured to perform degraded environment detection on the preset complex environment based on a degraded environment detection algorithm, and optimize the positioning data framework based on the degraded environment detection result;

[0008] A data processing module, configured to filter, fuse and register, and perform pose estimation on the positioning data in the optimized positioning data framework based on a point cloud filtering algorithm and an ICP algorithm, to obtain the UAV pose and an environmental map;

[0009] A positioning module, configured to obtain the positioning result of the drone in a preset complex environment based on the pose of the drone and the environmental map.

[0010] Preferably, the positioning data collected by the positioning data framework construction module includes point cloud data of the preset complex environment, IMU drone pose data, GPS global position data, and image data.

[0011] Preferably, the degraded environment detection module includes:

[0012] A point cloud feature extraction unit, configured to extract features from the point cloud data to obtain line feature density, plane feature proportion, and feature distribution variance;

[0013] A degraded discrimination function construction unit, configured to construct a degraded discrimination function based on the line feature density, the plane feature proportion, and the feature distribution variance;

[0014] A degraded environment detection unit, configured to obtain a degraded environment detection result based on the degraded discrimination function and a preset threshold;

[0015] A positioning mode switching unit, configured to activate an anti-degradation positioning mode based on the degraded environment detection result and complete the optimization of the positioning data framework.

[0016] Preferably, the positioning mode switching unit includes:

[0017] A degradation level discrimination sub-unit, configured to calculate the feature index values of the extracted point cloud data features, IMU drone pose data features, and visual features to obtain the degradation level of the preset complex environment;

[0018] A weight adjustment sub-unit, configured to dynamically adjust the weights of each sensor based on the degradation level;

[0019] A multi-modal fusion sub-unit, configured to obtain an optimized positioning data framework based on the sensors with adjusted weights.

[0020] Preferably, the data processing module includes:

[0021] A point cloud filtering unit, configured to filter the point cloud data collected by the dual lidar based on an anti-specular reflection point cloud filtering algorithm to obtain filtered point cloud data;

[0022] An ICP processing unit, configured to use the ICP algorithm to perform fusion registration on the filtered point cloud data, and optimize the drone pose estimation in combination with the degraded environment detection result to obtain the final drone pose and environmental map; wherein, the drone pose estimation includes IMU drone pose data and GPS global position data.

[0023] Preferably, the ICP processing unit includes:

[0024] A point cloud registration sub-unit, configured to perform fusion registration on the filtered point cloud data to obtain point cloud matching pairs;

[0025] An initial pose acquisition sub-unit, configured to obtain an initial pose estimate of the UAV based on the pose transformation matrix of the IMU UAV pose data and the GPS global position data;

[0026] A matching point weight acquisition sub-unit, configured to obtain point cloud matching point weights based on the Euclidean distance of the point cloud matching pairs;

[0027] An IMU data correction sub-unit, configured to correct the IMU UAV pose data based on the degradation environment detection result and the pose graph to obtain IMU UAV pose correction data;

[0028] A UAV pose optimization sub-unit, configured to obtain the final UAV pose based on the IMU UAV pose correction data and the initial pose estimate.

[0029] The present invention also provides a method for positioning a UAV in a complex environment based on dual lidars. Applying the system, it includes:

[0030] Based on the dual lidars installed on the UAV, in combination with an inertial measurement unit IMU, a global positioning system GPS, and a vision sensor, collect positioning data of a preset complex environment, and construct a positioning data framework for multi-sensor fusion;

[0031] Perform degradation environment detection on the preset complex environment based on a degradation environment detection algorithm, and optimize the positioning data framework based on the degradation environment detection result;

[0032] Perform filtering, fusion registration, and pose estimation on the positioning data in the optimized positioning data framework based on a point cloud filtering algorithm and an ICP algorithm to obtain the UAV pose and the environmental map;

[0033] Obtain the positioning result of the UAV in the preset complex environment based on the UAV pose and the environmental map.

[0034] Preferably, the collected positioning data includes point cloud data, IMU UAV pose data, GPS global position data, and image data of a preset complex environment.

[0035] Compared with the prior art, the beneficial effects of the present invention are:

[0036] Through the collaborative work of dual lidars, in combination with an improved ICP algorithm and multi-sensor data fusion technology, the present invention realizes high-precision positioning and three-dimensional map construction in a complex degradation environment.

[0037] The unmanned aerial vehicle (UAV) of the present invention includes two lidars, which are respectively installed at the front end and the rear end of the UAV. The front lidar is mainly used to scan the environment in front of the UAV, and the rear lidar is used to scan the environment behind. Through complementary scanning, the two lidars cover the environment in front of and behind the UAV, ensuring that complete environmental information can be provided in complex degraded environments. Through this dual-lidar layout, the positioning error caused by the limited perspective of a single lidar or the scarcity of environmental features is avoided.

[0038] In degraded environments, such as environments with sparse features like long straight corridors and symmetrical compartments, single-lidar systems cannot provide sufficient environmental information, resulting in unstable positioning or error accumulation. However, through the collaborative work of the two lidars in the present invention, multi-perspective environmental information can be provided, effectively avoiding positioning failures caused by perspective limitations or the lack of environmental features. At the same time, the complementary scanning of the two lidars can significantly improve the integrity of environmental mapping, thereby enhancing the accuracy of the 3D map.

[0039] Furthermore, the present invention proposes an improved ICP algorithm for the registration and pose optimization of point cloud data. In degraded environments, the lidar point cloud is initially registered by combining IMU data, and then the pose estimation is further optimized through the ICP algorithm to improve the positioning accuracy. This method can maintain a high positioning accuracy in complex environments, and the positioning error can be controlled within ±5 cm, significantly improving the positioning reliability of the UAV in complex environments.

[0040] The dual-lidar layout scheme of the present invention improves the positioning accuracy, further ensuring that the UAV can construct a complete environmental map in an environment with sparse features, providing more reliable data support for subsequent path planning and mission execution. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the technical solutions of the present invention, the following briefly introduces the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0042] Figure 1 It is a schematic structural diagram of a UAV complex environment positioning system based on dual lidars according to an embodiment of the present invention;

[0043] Figure 2 It is a data fusion processing flowchart according to an embodiment of the present invention;

[0044] Figure 3 It is a comparison diagram of the dual-lidar point cloud registration effect according to an embodiment of the present invention; among them, (a) is the single-lidar point cloud registration effect diagram; (b) is the dual-lidar point cloud registration effect diagram;

[0045] Figure 4 Schematic diagram of three-dimensional point cloud map construction in the embodiment of the present invention (local structure of the hull);

[0046] Figure 5 Radar scan map in the embodiment of the present invention; among them, (a) is the scan map with missing point cloud in the deck area due to the occlusion of the ship's side during single radar scanning; (b) is the scan map that completely covers the vertical and horizontal planes after the fusion of two radars. Detailed implementation manners

[0047] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0048] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific implementation manners.

[0049] Embodiment 1

[0050] As Figure 1 shown, a UAV complex environment positioning system based on dual lidars includes: a positioning data framework construction module, a degraded environment detection module, a data processing module, and a positioning module.

[0051] The positioning data framework construction module is used to collect positioning data of a preset complex environment based on the dual lidars installed on the UAV, in combination with an inertial measurement unit IMU, a global positioning system GPS, and a vision sensor, and construct a multi-sensor fusion positioning data framework. The positioning data framework of multi-sensor fusion also includes data collected by other auxiliary sensors (such as barometers, etc.).

[0052] Furthermore, the implementation manner lies in that the positioning data collected by the positioning data framework construction module includes point cloud data of a preset complex environment, IMU UAV pose data, GPS global position data, and image data.

[0053] In this embodiment, the dual lidar installed on the drone includes: a first lidar and a second lidar. The first lidar is installed at an inclined angle at the front of the drone and is used to scan the hull facade and complex structures; the second lidar is installed at the same inclined angle at the rear of the drone and is used to achieve high-precision positioning and modeling. Specifically, the installation angles of the dual lidar satisfy: the scanning plane of the first lidar forms an angle of 30° - 60° with the horizontal direction, covering the hull facade within 5 - 20 meters in front of the drone; the scanning plane of the second lidar forms an angle of 120° - 150° with the horizontal direction, covering the hull facade within 5 - 20 meters behind the drone; there is a partial overlapping area between the scanning ranges of the two to achieve data complementary verification.

[0054] The complementary verification is achieved through feature point matching and point cloud superposition. The core idea is to use the overlapping part of the scanning areas of the two lidars for data consistency verification and compensation. Assume that the point cloud collected by the front lidar is P1 = {p1, p2,..., p n}, and the point cloud collected by the rear lidar is P2 = {p1, p2,..., p m}. The overlapping area between the two is O = P1 ∩ P2. Use the feature extraction algorithm to extract the feature points in the overlapping area:

[0055] F1 = FeatureExtract(P1),

[0056] F2 = FeatureExtract(P2),

[0057] Then, through the nearest neighbor search algorithm (such as FLANN), match the feature points of the two point clouds:

[0058] M = Match(F1F2),

[0059] Next, use the ICP algorithm to estimate the rigid transformation of the matching point pairs:

[0060] (R, t) = ICP(M),

[0061] p' i = R·p i + t,

[0062] If the matching error meets the threshold:

[0063]

[0064] Then it is considered that the registration is successful; otherwise, adjust the weight parameters or use multi-frame superposition optimization. Among them, t represents the translation vector, and ε represents the threshold.

[0065] Fuse the point cloud according to the registration result:

[0066] P fused = α * P1 + βP2.

[0067] Where α and β are fusion weight coefficients, which are adjusted according to sensor accuracy or data quality.

[0068] The positioning data framework of multi-sensor fusion takes dual lidars as the core, combines an inertial measurement unit (IMU), a global positioning system (GPS), and a vision sensor to form a multi-redundant information fusion system. The dual lidars achieve synchronous acquisition of point cloud data through a hardware trigger and time synchronization mechanism, ensuring complementary spatial coverage and data consistency. The IMU measures acceleration and angular velocity in real time, provides pose change information and performs smoothing correction through Kalman filtering. The GPS provides global position information to assist in positioning. The vision sensor extracts image feature points and matches them with the laser point cloud for correction, enhancing the robustness of positioning.

[0069] A degradation environment detection module is used to detect degradation environments (such as long straight corridors, symmetric structures) in a preset complex environment based on a degradation environment detection algorithm, and optimize the positioning data framework based on the degradation environment detection results.

[0070] A further implementation is that the degradation environment detection module includes:

[0071] A point cloud feature extraction unit is used to extract features from point cloud data to obtain line feature density, plane feature proportion, and feature distribution variance; specifically, based on point cloud curvature analysis, plane and straight line features in the environment are identified. First, curvature calculation is performed on the acquired point cloud data, and a covariance matrix is constructed using each point and its neighborhood point set {p i}:

[0072]

[0073] Where is the centroid of the neighborhood point set:

[0074]

[0075] The covariance matrix is subjected to eigenvalue decomposition to obtain eigenvalues λ1 ≤ λ2 ≤ λ3 and corresponding eigenvectors. The curvature is defined as:

[0076]

[0077] If the curvature κ is less than a preset threshold, the area is considered a plane feature; if the curvature is large and satisfies the linear feature condition (such as λ1 << λ2 = λ3), it is judged as a straight line feature. By analyzing the curvature distribution and eigenvector direction, plane and straight line features in the environment are finally identified, providing accurate feature input for subsequent positioning and mapping algorithms.

[0078] By statistical analysis of feature distribution, determine whether the current environment is a degraded environment. Specifically, the analysis of the degraded environment determines the environmental characteristics by extracting and analyzing point cloud features, mainly using the following feature indicators:

[0079] Line feature density:

[0080]

[0081] Among them, N l represents the number of line features extracted per unit area, and A is the unit area. A higher line feature density indicates that there may be degraded scenarios such as long straight corridors.

[0082] Ratio of planar features:

[0083]

[0084] Among them, N f is the number of planar feature points, and N t is the total number of feature points. A higher ratio of planar features reflects the symmetry of the environment or the lack of obvious features.

[0085] Variance of feature distribution:

[0086]

[0087] Among them, d i is the distance between feature points, μ d is the mean value of feature point distances, and σ d being smaller indicates that the feature distribution is concentrated and it is easy to have degradation problems.

[0088] The degraded discrimination function construction unit is used to construct a degraded discrimination function based on the line feature density, the ratio of planar features, and the variance of feature distribution; specifically, the above feature indicators are combined to construct a degraded discrimination function:

[0089] D = w1ρ l + w2P f + w3σ d ,

[0090] Among them, w1, w2, and w3 are weight coefficients, which are determined by optimizing the training set.

[0091] The degraded environment detection unit is used to obtain the degraded environment detection result based on the degraded discrimination function and a preset threshold; in this embodiment, when the discriminant function value exceeds the set threshold (D ≥ θ), it is considered that the environment has degraded features and the anti-degradation positioning mode is activated.

[0092] The positioning mode switching unit is used to activate the anti-degradation positioning mode based on the degraded environment detection result and complete the optimization of the positioning data framework.

[0093] In a further embodiment, the anti-degradation positioning mode aims to improve the positioning accuracy through multi-modal data fusion and anti-degradation algorithms. The positioning mode switching unit includes:

[0094] A degradation level discrimination subunit, configured to calculate the characteristic index values of the extracted point cloud data features, IMU UAV pose data features, and visual features, and obtain the degradation level of a preset complex environment;

[0095] A weight adjustment subunit, configured to dynamically adjust the weights of each sensor based on the degradation level; specifically, a weight adjustment strategy is adopted to increase the weights of the IMU and visual sensors during the matching process and reduce the dependence of the laser point cloud on the matching accuracy.

[0096] P = α1P ICP +β1P IMU +γ1P Vision ,

[0097] where α1 + β1 + γ1 = 1, and the coefficients are dynamically adjusted according to the degradation level.

[0098] In this embodiment, a multi-feature compensation strategy is further included: in a degraded area such as a long straight corridor, dense point cloud sampling and a convolutional feature extraction network are used for feature enhancement.

[0099] F enhanced = CNN(F raw ),

[0100] A feature enhancement matrix is extracted through a deep convolutional network to improve the expression ability of feature points.

[0101] A multi-modal fusion subunit, configured to obtain an optimized positioning data framework based on the sensors with adjusted weights.

[0102] Generally speaking, the switching mechanism is that when the value of the degradation discrimination function exceeds the threshold, it switches to the anti-degradation positioning mode, and the process is as follows: ① Feature extraction: extract point cloud, IMU, and visual features; ② Degradation detection: calculate the characteristic index values and discriminate the degradation level; ③ Mode switching: adjust the sensor weights according to the degradation level; ④ Multi-modal fusion: output a high-precision pose based on the matching algorithm with weight combination.

[0103] Specifically, in a degraded environment, it switches to a positioning mode based on IMU and vision assistance to suppress positioning drift.

[0104] In a degraded environment, due to the sparse or overly regular distribution of lidar feature points, the reliability of the point cloud-based positioning algorithm decreases, and positioning drift is likely to occur. Therefore, it switches to a positioning mode based on IMU and vision assistance to effectively suppress the drift phenomenon. The attitude transformation matrix of the UAV is obtained through the IMU

[0105] T IMU and angular velocity ω and acceleration a:

[0106]

[0107] where T Prev is the pose matrix at the previous moment, Δt is the time interval, is the skew-symmetric matrix form of the angular velocity. Meanwhile, use the vision sensor to extract the key-frame image feature points, perform feature matching, and calculate the pose transformation matrix T vision :

[0108] T vision = PnP(features),

[0109] To improve the positioning accuracy, use the Kalman filter to fuse the IMU and vision-solved poses:

[0110] T fusion = K * T vision + (1 - K) * T IMU ,

[0111] where K is the adaptive fusion coefficient, which is automatically adjusted according to the positioning quality:

[0112]

[0113] By switching to the IMU and vision-assisted positioning mode, the drift caused by the lidar can be suppressed in the degraded environment, ensuring the positioning stability and accuracy of the UAV.

[0114] The data processing module is used to filter, fuse and register, and estimate the pose of the positioning data in the optimized positioning data framework based on the point cloud filtering algorithm and the ICP algorithm, to obtain the UAV pose and the environmental map.

[0115] A further implementation manner is that the data processing module includes: a point cloud filtering unit and an ICP processing unit.

[0116] A point cloud filtering unit is used to filter the point cloud data collected by dual lidars based on an anti-specular reflection point cloud filtering algorithm to obtain the filtered point cloud data. In this embodiment, in a complex environment, the point cloud data collected by lidars often contains noise and redundant points, and directly using this data will affect the positioning accuracy and mapping effect. The point cloud filtering algorithm is used to preprocess the original point cloud to extract effective environmental features. First, voxel filtering is used to downsample the point cloud, dividing the space into regular grids, and the points in each grid are replaced by the centroid, thereby reducing the number of point clouds and the computational complexity. Next, statistical filtering (removing outliers) is adopted. Statistical filtering judges whether the points with too large distances are noise points by calculating the average distance from each point to its neighboring points:

[0117]

[0118] If the mean distance exceeds the threshold, it is considered a noise point and is removed. Finally, radius filtering is used to further clean isolated points, that is, the number of neighboring points within a set radius of each point is counted. If the number of neighboring points is insufficient, the point is judged as noise. After multi-level filtering processing, the point cloud data is denser and more accurate, effectively improving the robustness of positioning and mapping. Then, effective environmental features are extracted from the point cloud to enhance the robustness of positioning and mapping. The filtered point cloud data has high density and accuracy, and the structural characteristics of the point cloud are further analyzed through the feature extraction algorithm. First, normal vector estimation is used to calculate the surface normal of each point:

[0119]

[0120] where N i is the set of neighboring points of point p i . The normal vector is used to describe the local geometric characteristics of the point cloud surface.

[0121] Next, the planarity or edge nature of the point cloud is judged through curvature analysis:

[0122]

[0123] A larger curvature value indicates that the point is located at the edge or mutation area, while a smaller curvature value indicates that the point is located in the plane area. During the feature extraction process, the feature points can be encoded through feature descriptors (such as FPFH or SHOT) for subsequent registration and matching. Finally, the set of feature points constructs the skeleton of the environmental map, which helps to improve the positioning and navigation accuracy of the UAV in a complex environment.

[0124] In this embodiment, it also includes identifying specular reflection noise points based on echo intensity and geometric consistency detection;

[0125] In lidar point cloud processing, due to the specular reflection effect, noise points with abnormal intensities or position offsets may be generated. To identify these noise points, first, echo intensity analysis is performed on each point. Assume that for a point P in the lidar point cloud i the echo intensity is I i , then the mean μ I and variance σ I of the echo intensities of the point set are calculated to determine its abnormality:

[0126]

[0127] When ΔI i is greater than the set threshold T I , it is considered that the point has the characteristic of abnormal intensity. At the same time, the geometric consistency between this point and its neighboring points is calculated. Assume that the neighboring point set of point P i is N(P i ), then the normal vector consistency is calculated:

[0128]

[0129] where n i and n j are the normal vectors of point P i and its neighboring points. When the consistency angle θ is greater than the set threshold T θ , its noise property is further verified. Through comprehensive judgment of echo intensity and normal vector consistency, specular reflection noise points are identified and removed. Furthermore, effective environmental features are retained.

[0130] The ICP processing unit is used to perform fusion registration on the filtered point cloud data using the ICP algorithm, and optimize the UAV pose estimation in combination with the results of degraded environment detection to obtain the final UAV pose and environmental map; among them, the UAV pose estimation includes IMU UAV pose data and GPS global position data.

[0131] A further implementation manner is that the ICP processing unit includes:

[0132] The point cloud registration sub-unit is used to perform fusion registration on the filtered point cloud data to obtain point cloud matching pairs; specifically, before performing ICP registration, the point clouds from the front and rear lidars are respectively denoised and downsampled to improve the calculation efficiency and registration accuracy. Assume that the point cloud of the front lidar is P f , and the point cloud of the rear lidar is P r .

[0133] The initial pose acquisition subunit is used to obtain the initial pose estimation of the UAV based on the pose transformation matrix of the IMU UAV pose data and the GPS global position data. Specifically, since the dual lidar is installed at the front and rear of the UAV, the initial pose estimation can be obtained through IMU and GPS data:

[0134] T0 = T IMU *T GPS ,

[0135] where T IMU and T GPS are the pose transformation matrices provided by IMU and GPS respectively.

[0136] The matching point weight acquisition subunit is used to obtain the point cloud matching point weight based on the Euclidean distance of the point cloud matching pair. On the basis of the traditional ICP algorithm, weighted matching and degenerate environment discrimination are introduced: for the point cloud matching pair (p i , q j ), the matching point weight w ij is defined as:

[0137]

[0138] where d(p i , q j ) is the Euclidean distance, and smaller distances are given larger weights.

[0139] The IMU data correction subunit is used to correct the IMU UAV pose data based on the degenerate environment detection result and the pose graph to obtain the IMU UAV pose correction data. In this embodiment, the optimized objective function of the improved ICP algorithm is:

[0140]

[0141] where R and t are the rotation matrix and the translation vector respectively, and the optimal solution is obtained by minimizing this objective function. In a degenerate environment (such as a long corridor or a symmetric structure), the point cloud features are prone to degenerate, resulting in unstable ICP convergence. Use the degenerate detection algorithm to detect the environmental features:

[0142] λ min <ε ====> degenerate environment,

[0143] where λ min is the minimum eigenvalue of the covariance matrix, and ε is the threshold. When a degenerate environment is detected, the pose graph is used to optimize and fuse the IMU information for correction:

[0144] T opt = α2T ICP + (1 - α2)T IMU ,

[0145] Among them, α2 is a weight coefficient, which is dynamically adjusted based on the degree of environmental degradation.

[0146] The UAV pose optimization subunit is used to obtain the final UAV pose based on the IMU UAV pose correction data and the initial pose estimation. The final fusion registration obtains the accurate UAV pose:

[0147] T final = T opt * T0,

[0148] In this embodiment, the IMU data is fused through Kalman filtering to optimize the pose estimation result.

[0149] Kalman filtering is a linear minimum variance estimation method. It uses the basic statistical characteristics of the observed value and the estimated value in a recursive manner in the state space description of the system to make predictions about the trend of a dynamic system with uncertain information. Kalman filtering is only applicable to linear systems, while most systems are nonlinear. Extended Kalman filtering extends the application scenario of Kalman filtering from linear to nonlinear problems. The main idea of extended Kalman filtering is to linearly approximate the nonlinear function and use the idea of approximation for higher-order terms to solve the nonlinear problem. The specific steps are as follows: Select 19-dimensional state variables, as follows:

[0150]

[0151] In the formula, G P I , G V I , G R I respectively represent the position, velocity, and attitude of the UAV in the global system, b ω , b a respectively represent the gyro zero bias and accelerometer zero bias of the IMU, G g represents the gravity vector. Construct the state prediction formula as follows:

[0152] Quaternion prediction:

[0153]

[0154] In the formula, G R I (k) represents the quaternion at time k, ω m (k) represents the three-axis angular velocity measured by the gyroscope at time k, b ω represents the zero bias of the three axes of the gyroscope, Δt represents the time interval between two consecutive frames of the IMU, Δt = τ i+1 - τ i .

[0155] Position prediction:

[0156]

[0157] Velocity prediction:

[0158] G V I (k) = G V I (k - 1)*Δt + a m (k)*Δt,

[0159] Gravity vector prediction: G Let g be the gravity vector, which is considered not to change with time. Therefore

[0160]

[0161] Mean square error prediction:

[0162]

[0163] In the formula, Φ is the state transition matrix, Γ is the system noise matrix, and Q is the system noise mean square error matrix. According to the state prediction equation, the state transition matrix Φ k,k-1 Can be expressed as:

[0164]

[0165] In the formula, let G R I = [q0, q1, q2, q3], then:

[0166]

[0167] System noise matrix Γ k-1 Is expressed as:

[0168]

[0169] In the formula:

[0170]

[0171] System noise mean square error matrix Q k-1 Is expressed as:

[0172] Q = diag([n bωx , n bωy , n bωz , n bax , n bay , n baz , b ωx , b ωy , b ωz, b ax , b ay , b ωz ),

[0173] Filter gain calculation

[0174]

[0175] where H is the measurement matrix and R is the measurement noise matrix.

[0176] State estimation

[0177]

[0178] where z = G P I , G V I , G R I T is the measured quantity, which are the position, velocity, and attitude of the UAV in the global coordinate system, respectively.

[0179] Mean square error estimation

[0180] P k|k = (I - K k H k )P k|k-1 ,

[0181] Through the improved ICP algorithm, combined with the degradation detection results, the pose estimation is optimized to ensure high-precision positioning and registration in complex degraded environments.

[0182] The positioning module is used to obtain the positioning result of the UAV in the preset complex environment based on the UAV pose and the environmental map.

[0183] In this embodiment, the process of constructing the environmental map includes:

[0184] The preset complex environment is sensed and collected by various sensors carried by the UAV. The sensors include two lidars, a visual camera, an inertial measurement unit (IMU), etc. During the flight of the UAV, the lidar scans the environment for three-dimensional point cloud data, and combines the image information obtained by the visual camera and the attitude and position information provided by the IMU to complete the comprehensive perception of the environmental space features.

[0185] Subsequently, the collected sensor data is input into the mapping algorithm for processing. The mapping algorithm includes but is not limited to the SLAM algorithm. Through the SLAM algorithm, the sensor data of the UAV in the flight path is subjected to feature extraction, data association, loop detection, and optimization, and finally an environmental map that can reflect the spatial structure features of the preset complex environment is generated.

[0186] The environmental map not only contains spatial location information, but can also overlay semantic information as needed to label specific objects or areas in the environment, further improving the autonomous positioning and navigation capabilities of the drone in complex environments. Obtain the positioning result of the drone in the preset complex environment.

[0187] In summary, the present invention realizes high-precision positioning and mapping in complex degraded environments by mounting two lidars on a quadrotor drone, which are respectively installed at the front and rear ends of the drone. The front lidar is mainly responsible for scanning the environment in front of the drone, while the rear lidar covers the environment behind. This front-rear complementary layout can effectively expand the scanning range of the lidar, ensure comprehensive environmental information in complex environments, and thus enhance the positioning accuracy and mapping integrity.

[0188] In the specific implementation process, the drone synchronously collects the point cloud data of the environment through the dual lidars. These point cloud data will be fused with the inertial measurement unit (IMU) and other auxiliary sensors (such as barometers, etc.) to construct a positioning framework for multi-sensor collaborative work. After being processed, the point cloud data provided by the dual lidars can effectively identify features such as planes, straight lines, and corners in the environment, assisting the drone to achieve precise three-dimensional map construction.

[0189] In degraded environments, such as long straight corridors, symmetric compartments, etc., a single lidar is prone to positioning drift or error accumulation due to limited viewing angles or scarce environmental features. However, the present invention can effectively avoid the above problems through the multi-view scanning provided by the dual lidars, improving the robustness of positioning. In such an environment, the dual lidars can enhance the integrity of environmental information through complementary scanning areas, thus ensuring the stable flight of the drone in complex environments.

[0190] To further improve the positioning accuracy, the present invention adopts an improved ICP algorithm. This algorithm combines the IMU data to perform preliminary registration on the lidar point cloud, and then optimally registers the point cloud through the ICP algorithm to accurately estimate the pose of the drone. In complex environments, this improved ICP algorithm can further improve the drone positioning accuracy by reducing error propagation, ensuring that the positioning error remains within ±5 cm.

[0191] During the entire inspection mission, the drone can not only perform precise path planning by real-time collecting and updating the environmental point cloud data, but also generate a high-precision three-dimensional map for subsequent tasks (such as structural assessment, environmental monitoring, etc.). The drone can perform positioning through the fusion of dual lidar and IMU data in the absence of GPS signals, ensuring stable operation in closed or GPS-unavailable environments.

[0192] Through this implementation method, the collaborative work of the dual lidars enables the UAV to perform efficient positioning and mapping in complex environments with scarce features, effectively overcoming the limitations of traditional single-lidar solutions in similar environments. The solution of the present invention can be widely applied to inspection tasks in various enclosed complex environments, especially those occasions that require precise positioning and environmental reconstruction, such as ship cargo holds, tunnels, etc.

[0193] Embodiment II

[0194] The present invention also provides a method for positioning a UAV in a complex environment based on a dual lidar, and an application system, including:

[0195] Based on the dual lidars installed on the UAV, combined with an inertial measurement unit (IMU), a global positioning system (GPS), and a vision sensor, collect positioning data of a preset complex environment, and construct a positioning data framework for multi-sensor fusion;

[0196] Perform degraded environment detection on the preset complex environment based on a degraded environment detection algorithm, and optimize the positioning data framework based on the degraded environment detection result;

[0197] Perform filtering, fusion registration, and pose estimation on the positioning data in the optimized positioning data framework based on a point cloud filtering algorithm and an ICP algorithm to obtain the UAV pose and the environmental map;

[0198] Based on the UAV pose and the environmental map, obtain the positioning result of the UAV in the preset complex environment.

[0199] A further implementation method lies in that the collected positioning data includes point cloud data of the preset complex environment, IMU UAV pose data, GPS global position data, and image data.

[0200] Embodiment III

[0201] This embodiment provides the specific hardware configuration and application steps of the system:

[0202] Hardware configuration example:

[0203] UAV: A quadrotor platform with a wheelbase of 600 mm and a load capacity ≥ 2 kg;

[0204] Lidar: A 16-line mechanical lidar (horizontal field of view 360°, vertical field of view ±15°);

[0205] Processor: An embedded industrial computer (Intel NUC, running the ROS system).

[0206] Detailed description of the implementation steps:

[0207] Dual-radar data acquisition: The forward radar scans the hull facade, and the bottom radar scans the deck plane;

[0208] Degraded environment detection: Based on point cloud curvature analysis, identify long straight corridors or symmetric structures;

[0209] Point cloud registration and pose optimization: Use an improved ICP algorithm to fuse dual-radar data and output a high-precision pose. The schematic diagram of the effect is as Figures 3 - 5 shown.

[0210] Figure 3 This is the comparison diagram of the dual-radar point cloud registration effect in the embodiment of the present invention; among them, (a) is the effect diagram of single-radar point cloud registration; (b) is the effect diagram of dual-radar point cloud registration;

[0211] Figure 4 This is the schematic diagram of the three-dimensional point cloud map construction in the embodiment of the present invention (local structure of the hull);

[0212] Figure 5 This is the radar scan diagram in the embodiment of the present invention; among them, (a) is the scan diagram with the point cloud of the deck area missing due to the occlusion of the ship's side during single-radar scanning; (b) is the scan diagram with the facade and plane completely covered after dual-radar fusion.

[0213] Industrial application scenarios:

[0214] Taking the detection of a 30-meter-long bulk carrier as an example: The drone takes off from the deck, the forward radar scans the ship's side and the cargo hatch, and the bottom radar continuously constructs the deck map. When entering the enclosed cargo hold, the GPS signal is lost, and the system automatically switches to the pure lidar-IMU fusion mode, realizes in-cabin positioning through dual-radar cross-verification, and finally generates a three-dimensional report including the detection results of the deformation of the cargo hold wall

[0215] This technology can be applied to tasks such as ship hull corrosion detection, cargo hold volume calculation, and mast structure safety assessment. Compared with manual detection, the efficiency is increased by more than 3 times, and the positioning accuracy reaches ±5 cm, meeting the specification requirements of the IMO (International Maritime Organization) for ship detection.

[0216] The present invention reduces the positioning failure rate in complex environments through the spatial complementarity of dual radars; the fusion algorithm takes both computational efficiency and accuracy into account and is suitable for real-time operation on embedded platforms; the system can be extended to other industrial detection scenarios (such as storage oil tanks, wind turbines).

[0217] The above-described embodiments are only descriptions of the preferred embodiments of the present invention, and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. An unmanned aerial vehicle complex environment positioning system based on dual lidar, characterized in that, Including: A positioning data framework construction module, which is used to collect positioning data of a preset complex environment based on dual lidars installed on a drone, in combination with an inertial measurement unit (IMU), a global positioning system (GPS), and a vision sensor, and construct a multi-sensor fusion positioning data framework; A degraded environment detection module, which is used to detect the degraded environment of the preset complex environment based on a degraded environment detection algorithm, and optimize the positioning data framework based on the degraded environment detection result; A data processing module, which is used to filter, fuse and register, and pose estimation of the positioning data in the optimized positioning data framework based on a point cloud filtering algorithm and an ICP algorithm, to obtain the drone pose and the environmental map; A positioning module, which is used based on the drone pose and the environmental map.

2. The system according to claim 1, wherein The positioning data collected by the positioning data framework construction module includes point cloud data of a preset complex environment, IMU drone pose data, GPS global position data, and image data.

3. The system according to claim 2, wherein The degraded environment detection module includes: A point cloud feature extraction unit, which is used to extract features from the point cloud data to obtain line feature density, plane feature ratio, and feature distribution variance; A degraded discrimination function construction unit, which is used to construct a degraded discrimination function based on the line feature density, the plane feature ratio, and the feature distribution variance; A degraded environment detection unit, which is used to obtain a degraded environment detection result based on the degraded discrimination function and a preset threshold; A positioning mode switching unit, which is used to activate an anti-degradation positioning mode based on the degraded environment detection result and complete the optimization of the positioning data framework.

4. The system according to claim 3, wherein The positioning mode switching unit includes: A degradation level discrimination sub-unit, which is used to calculate the feature index values of the extracted point cloud data features, IMU drone pose data features, and vision features to obtain the degradation level of the preset complex environment; A weight adjustment sub-unit, which is used to dynamically adjust the weights of each sensor based on the degradation level; A multi-modal fusion sub-unit, which is used to obtain an optimized positioning data framework based on the sensors with adjusted weights.

5. The system according to claim 3, wherein The data processing module includes: A point cloud filtering unit, which is used to filter the point cloud data collected by the dual lidars based on an anti-specular reflection point cloud filtering algorithm to obtain filtered point cloud data; An ICP processing unit, which is used to perform fusion registration on the filtered point cloud data using the ICP algorithm, and optimize the drone pose estimation in combination with the degraded environment detection result to obtain the final drone pose and the environmental map; wherein, the drone pose estimation includes IMU drone pose data and GPS global position data.

6. The system according to claim 5, wherein The ICP processing unit includes: A point cloud registration sub-unit, which is used to perform fusion registration on the filtered point cloud data to obtain point cloud matching pairs; An initial pose acquisition sub-unit, which is used to obtain an initial pose estimation of the drone based on the pose transformation matrix of the IMU drone pose data and the GPS global position data; A matching point weight acquisition sub-unit, which is used to obtain point cloud matching point weights based on the Euclidean distance of the point cloud matching pairs; The IMU data correction subunit is used to correct the pose data of the IMU drone based on the degradation environment detection result and the pose graph to obtain the pose correction data of the IMU drone; The drone pose optimization subunit is used to obtain the final drone pose based on the pose correction data of the IMU drone and the initial pose estimation.

7. A method for positioning an unmanned aerial vehicle in a complex environment based on dual lidar, applying the system according to any one of claims 1-6, characterized in that, It includes: Based on the dual lidar installed on the drone, combined with the inertial measurement unit IMU, the global positioning system GPS, and the vision sensor, collect the positioning data of the preset complex environment and construct a multi-sensor fusion positioning data framework; Perform degradation environment detection on the preset complex environment based on the degradation environment detection algorithm, and optimize the positioning data framework based on the degradation environment detection result; Perform filtering, fusion registration, and pose estimation on the positioning data in the optimized positioning data framework based on the point cloud filtering algorithm and the ICP algorithm to obtain the drone pose and the environmental map; Based on the drone pose and the environmental map, obtain the positioning result of the drone in the preset complex environment.

8. The method according to claim 7, wherein The collected positioning data includes the point cloud data, IMU drone pose data, GPS global position data, and image data of the preset complex environment.

Images