Collaborative three-dimensional mapping method for air-ground heterogeneous unmanned platform

Abstract

The invention relates to a collaborative three-dimensional mapping method for an air-ground heterogeneous unmanned platform, belongs to the technical field of three-dimensional scene map construction, and solves the problems of low efficiency and poor precision of an existing single unmanned platform in a three-dimensional mapping process. The method comprises the following steps: acquiring laser radar point cloud data, IMU data and images shot by a camera of an air-ground heterogeneous unmanned platform, and respectively carrying out data fusion to obtain a single-carrier point cloud map of each unmanned platform; wherein in the data fusion process, point cloud downsampling based on distance sensing is carried out on laser radar point cloud data; performing two-point cloud registration on each single-carrier point cloud map by using an improved generalized ICP (Inductively Coupled Plasma) algorithm to obtain a registered point cloud rigid body transformation matrix; according to the improved generalized ICP algorithm, when point cloud registration is carried out, a distance measurement method based on point cloud normal angle invariant features is used for searching for nearest neighbor points; and performing coordinate transformation on each single-carrier point cloud map based on the registered point cloud rigid body transformation matrix to obtain a fused map.

Description

Technical Field

[0001] The present invention relates to the technical field of three-dimensional scene map construction, and in particular to a method for collaborative three-dimensional mapping by air-ground heterogeneous unmanned platforms. Background Art

[0002] A 3D high-precision map is a 3D digital model that describes spatial objects and environments. It is used to represent the geographical features of the physical world and has urgent application needs in many fields, such as environmental monitoring and assessment, emergency disaster prevention and mitigation, urban planning and construction, archaeology and cultural heritage protection. For example, after a disaster occurs, a 3D map can provide rescue teams with real-time and accurate geographic information to help rescuers quickly locate and launch rescue operations; through 3D scanning and modeling, historical sites and cultural relics can be digitally protected and restored.

[0003] At present, it is common to use ground mobile unmanned vehicles or aerial drones equipped with cameras, lidar, IMU and other sensors to build maps. In the absence of prior environmental information, the surrounding environment data is collected in real time through the movement of the vehicle itself and the map is built. Compared with some traditional surveying, exploration and mapping methods, unmanned platform carrier mapping has greatly improved the efficiency of mapping unknown environments.

[0004] Existing methods use ground unmanned vehicles or aerial drone platforms to build maps, which makes it difficult to form a complete scene map. When facing complex scenes, such as narrow and rugged areas in historical site surveys, and ruined areas in emergency disaster prevention and mitigation, using ground unmanned vehicles or aerial drone platforms to build maps has problems such as low map building efficiency and poor accuracy, and even the inability to complete map construction due to the inability of the carrier to pass. Summary of the invention

[0005] In view of the above analysis, an embodiment of the present invention aims to provide a method for collaborative three-dimensional mapping of air-ground heterogeneous unmanned platforms, so as to solve the problems of low mapping efficiency and poor accuracy in the three-dimensional mapping process of existing single-type unmanned platform carriers.

[0006] The purpose of the present invention is mainly achieved through the following technical solutions:

[0007] The present invention provides a method for collaborative three-dimensional mapping of air-ground heterogeneous unmanned platforms, comprising the following steps:

[0008] The laser radar point cloud data, IMU data and camera images of three unmanned platforms with heterogeneity in the air and on the ground are obtained and data fusion is performed to obtain a single-carrier point cloud map of each unmanned platform; wherein, in the data fusion process, the laser radar point cloud data is downsampled based on distance perception;

[0009] Performing pairwise point cloud registration on each of the single-carrier point cloud maps using an improved generalized ICP algorithm to obtain three point cloud rigid body transformation matrices after registration; wherein the improved generalized ICP algorithm uses a distance metric method based on the point cloud normal angle invariance feature to search for the nearest neighbor point when performing point cloud registration;

[0010] The point clouds in each of the single-carrier point cloud maps are subjected to coordinate transformation based on the three registered point cloud rigid body transformation matrices to obtain a fused map.

[0011] Furthermore, the data fusion obtains a single-carrier point cloud map of each unmanned platform, including:

[0012] Performing motion distortion compensation on the laser radar point cloud data of each unmanned platform based on the IMU data to obtain compensated laser radar point cloud data;

[0013] Constructing a fused observation residual based on the compensated lidar point cloud data and the image taken by the camera;

[0014] Based on the fused observation residual and IMU data, Kalman filtering is performed to obtain updated postures of each unmanned platform;

[0015] Performing distance-aware point cloud downsampling on the compensated laser radar point cloud data to obtain downsampled laser radar point cloud data;

[0016] Based on the updated posture of each unmanned platform, the downsampled lidar point cloud data is dynamically spatially divided and the map is incrementally updated to obtain a point cloud map of each single carrier.

[0017] Furthermore, the performing distance-aware point cloud downsampling on the compensated lidar point cloud data includes:

[0018] Calculate the maximum distance between each point of each laser radar point cloud and the laser radar position after compensation at time t;

[0019] Taking the laser radar position at time t as the center, the maximum distance is evenly divided into a number of sub-intervals, and uniform sampling is performed in each of the sub-intervals;

[0020] The sampling point data within each of the subintervals is used as the laser radar point cloud data after downsampling at time t.

[0021] Furthermore, the maximum distance between each point of each laser radar point cloud after compensation at time t and the laser radar position is calculated using the following formula:

[0022]

[0023] Among them, L trepresents the laser radar position coordinates at time t; p t,i Represents the position coordinates of the i-th point of the compensated lidar point cloud at time t.

[0024] Furthermore, the number of sampling points in each sub-interval is calculated using the following formula:

[0025] M t,k =-D(1-p t,k ) γ log(p t,k )

[0026]

[0027] Among them, M t,k represents the number of sampling points in the kth interval at time t; D represents the constant factor of the expected number of sampling point clouds; γ represents the constant suppression factor; R t,k Represents the left boundary distance of the kth interval at time t; R tmax represents the maximum distance between each lidar point cloud and the lidar position after compensation at time t; K represents the number of intervals that evenly divide the maximum distance between each lidar point cloud and the lidar position at time t.

[0028] Furthermore, the step of constructing a fused observation residual based on the compensated lidar point cloud data and the image captured by the camera includes:

[0029] The point cloud observation residual of each point in the compensated lidar point cloud is constructed using the following formula:

[0030]

[0031] in, Represents the point cloud observation residual of the i-th point; represents the transpose of the normal vector of the fitted plane of the i-th point in the lidar point cloud; w pi represents the coordinates of the i-th point in the lidar point cloud; w qi Represents the coordinates of the center point of the fitting plane of the i-th point in the lidar point cloud;

[0032] Project each point of the compensated LiDAR point cloud onto the previous frame image taken by the camera, and use the optical flow method to obtain the coordinates of each point in the point cloud in the current frame image;

[0033] Project each point in the compensated LiDAR point cloud data to the current frame image taken by the camera, and use the following formula to construct the visual observation residual of each point in the LiDAR point cloud:

[0034]

[0035] in, represents the visual observation residual of the i-th point; ρ j,i represents the coordinates of the i-th point in the current frame image obtained by the optical flow method; π() represents the image projection model; (R j ,t j ) represents the rotation matrix and translation vector of the current frame image relative to the laser radar; Represents the coordinates of the i-th point in the laser radar;

[0036] The compensated lidar point cloud observation residual and the visual observation residual are combined to obtain a fused observation residual.

[0037] Furthermore, the distance measurement method based on the invariant feature of the point cloud normal angle is used to search for the nearest neighbor point, and the nearest neighbor point is searched using the following formula:

[0038]

[0039] Among them, (x h ,y h ,z h ), f h They represent the coordinates and eigenvalues ​​of the hth point in a single carrier point cloud to be registered; (x g ,y g ,z g ), f g They respectively represent the coordinates and eigenvalues ​​of the g-th point in another single-carrier point cloud to be registered; λ represents the weighting coefficient.

[0040] Furthermore, the eigenvalue of the hth point in a single carrier point cloud to be registered is obtained using the following formula:

[0041]

[0042] Among them, n h Represents the normal vector of the hth point in a single carrier point cloud to be registered; n m Represents the normal vector of the mth point in a single carrier point cloud to be registered; d hm represents the Euclidean distance between the hth point and the mth point in a single carrier point cloud to be registered; M represents the number of neighboring points of the hth point in a single carrier point cloud to be registered;

[0043] Use the following formula to get the eigenvalue of the g-th point in another single-carrier point cloud to be registered:

[0044]

[0045] Among them, n g Represents the normal vector of the g-th point in another single-carrier point cloud to be registered; n nRepresents the normal vector of the nth point in another single-carrier point cloud to be registered; d gn It represents the Euclidean distance between the g-th point and the n-th point in another single-carrier point cloud to be registered; N represents the number of neighboring points of the g-th point in another single-carrier point cloud to be registered.

[0046] Furthermore, each of the single-carrier point cloud maps is subjected to pairwise point cloud registration using an improved generalized ICP algorithm, which also includes: in the initial stage of registration, performing maximum likelihood estimation on the two single-carrier point cloud maps to be registered to obtain an initial rigid body transformation matrix; and performing point cloud registration using an improved generalized ICP algorithm based on the initial rigid body transformation matrix.

[0047] Further, coordinate transformation is performed on the point clouds in each of the single-carrier point cloud maps based on the three registered point cloud rigid body transformation matrices to obtain a fused map, including:

[0048] Perform coordinate transformation on each of the single-carrier point cloud maps based on the three registered point cloud rigid body transformation matrices to unify them into any one of the point cloud map coordinate systems;

[0049] The three point cloud maps after coordinate conversion are merged to obtain a fused map.

[0050] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0051] 1. The present invention proposes a three-dimensional mapping method for collaborative mapping of heterogeneous unmanned platforms in the air and on the ground. Through the collaborative mapping of ground unmanned platforms (unmanned vehicles) and aerial unmanned platforms (drones), the accuracy and efficiency of complex scene mapping are improved.

[0052] 2. The present invention introduces visual observation into the existing Kalman filter framework, projects the point cloud in the local map to the previous frame to form projection points, and then calculates the matching points of the projection points in the current frame through the optical flow method, and then calculates the visual residual model between the projection points and the matching points of the current frame. On this basis, the Kalman filter update stage will comprehensively utilize the point cloud observation residual and visual residual information to better adapt to complex environments such as weak textures, irregularities, and closed environments with a single geometric structure.

[0053] 3. In the process of single-carrier data fusion, the present invention proposes a distance-aware point cloud downsampling method, which consciously compresses the number of long-distance point clouds while reducing the amount of point cloud data, thereby reducing the impact of remote noise points on subsequent multi-platform map alignment.

[0054] 4. In the process of point cloud registration, the present invention proposes an efficient point cloud registration method that combines the point cloud normal angle invariant feature and KD tree acceleration. Compared with the original registration algorithm, it is more robust and more efficient.

[0055] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The drawings are only for the purpose of illustrating specific embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0057] Figure 1 It is a schematic diagram of the process of a method for collaborative three-dimensional mapping of air-ground heterogeneous unmanned platforms in an embodiment of the present invention;

[0058] Figure 2 Schematic diagram of the process of distance-aware point cloud downsampling in an embodiment of the present invention. DETAILED DESCRIPTION

[0059] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0060] A specific embodiment of the present invention discloses a method for collaborative 3D mapping of heterogeneous unmanned platforms on the ground and air, such as Figure 1 As shown, the steps S1 to S3 are included:

[0061] Step S1, obtaining the lidar point cloud data, IMU data and camera images of three unmanned platforms with heterogeneity in the air and on the ground, and performing data fusion respectively to obtain a single-carrier point cloud map of each unmanned platform; wherein, in the data fusion process, the lidar point cloud data is subjected to point cloud downsampling based on distance perception.

[0062] Specifically, this embodiment uses multiple types of unmanned platform carriers in the air and on the ground for collaborative mapping. Unmanned vehicles are stable and good at detecting near-ground areas, while drones are flexible and good at detecting air and overhead areas. By comprehensively using ground unmanned platforms (unmanned vehicles) and aerial unmanned platforms (drones), the shortcomings of single unmanned platform carriers in complex scene mapping in the prior art are overcome. Exemplarily, this embodiment obtains three unmanned platforms with heterogeneity in the air and on the ground, which can be two unmanned vehicle platforms and one drone platform or two drone platforms and one unmanned vehicle platform.

[0063] Each unmanned platform is equipped with a lidar sensor, an IMU sensor and a camera. By fusing the data from the three sensors, the error of a single sensor can be corrected and the robustness of the system can be enhanced.

[0064] Specifically, the laser radar point cloud data is a data set consisting of a series of three-dimensional coordinate points obtained by the laser radar sensor carried by the unmanned platform. Each point represents a spatial position and can be located in three-dimensional space through the coordinate value; the laser radar sensor will periodically emit a laser beam and measure the time it takes for the laser beam to return to calculate the distance between the target object and the laser radar. Through the rotation or scanning mechanism, the point cloud data of the target object at different angles can be obtained.

[0065] The IMU data is the acceleration and angular velocity data of the unmanned platform acquired by the IMU sensor carried by the unmanned platform, and is used to provide real-time posture information of the unmanned platform.

[0066] Furthermore, the data fusion obtains a single-carrier point cloud map of each unmanned platform, including steps S101 to S105:

[0067] Step S101: performing motion distortion compensation on the laser radar point cloud data of each unmanned platform based on the IMU data to obtain compensated laser radar point cloud data.

[0068] Specifically, the motion distortion compensation includes: pre-integrating the acceleration and angular velocity of the IMU data to obtain the position and posture of the unmanned platform; pre-processing the lidar point cloud data to improve the accuracy of the lidar point cloud data compensation. Exemplarily, the preprocessing includes filtering out abnormal values, etc.; for each point in the lidar point cloud, using the IMU data to calculate the position of the point relative to the unmanned platform coordinate system at the scanning time; based on the posture change of the unmanned platform, inversely transforming the position of the point to compensate for the impact of the platform movement; applying the calculated inverse transformation to each point of the point cloud data to obtain compensated lidar point cloud data.

[0069] Step S102: construct a fused observation residual based on the compensated lidar point cloud data and the image taken by the camera.

[0070] Specifically, the fused observation residual combines the laser point cloud observation residual and the visual observation residual to improve the accuracy and robustness of positioning and mapping in multi-sensor systems. This fusion method takes advantage of the respective advantages of lidar and camera, namely the high accuracy of lidar in three-dimensional space measurement and the powerful ability of camera in image recognition and texture analysis.

[0071] Furthermore, the point cloud observation residual of each point in the compensated lidar point cloud is constructed using the following formula:

[0072]

[0073] in, Represents the point cloud observation residual of the i-th point; Represents the transpose of the normal vector of the fitted plane at the i-th point in the lidar point cloud; Represents the coordinates of the i-th point in the lidar point cloud; Represents the coordinates of the center point of the fitted plane of the i-th point in the lidar point cloud.

[0074] Specifically, after a frame of lidar point cloud is collected, the original point cloud is filtered according to the echo information and point cloud intensity information of the lidar point cloud, the GICP observation equation of the lidar is established, the current frame is matched with the map, and the point-to-surface distance observation residual is calculated.

[0075] Furthermore, each point of the compensated lidar point cloud is projected onto the previous frame image captured by the camera, and the coordinates of each point in the point cloud in the current frame image are obtained using the optical flow method;

[0076] Project each point in the compensated LiDAR point cloud data to the current frame image taken by the camera, and use the following formula to construct the visual observation residual of each point in the LiDAR point cloud:

[0077]

[0078] in, represents the visual observation residual of the i-th point; ρ j,i represents the coordinates of the i-th point in the current frame image obtained by the optical flow method; π() represents the image projection model; (R j ,t j ) represents the rotation matrix and translation vector of the current frame image relative to the laser radar; Represents the coordinates of the i-th point in the lidar.

[0079] Specifically, visual observation is introduced into the compensated lidar point cloud to better adapt to complex environments such as weak texture, irregular and single geometric structure enclosed environments, so as to achieve complementary advantages between lidar and vision.

[0080] After projecting each point of the compensated lidar point cloud onto the previous frame image of the camera to obtain the projection point, the matching point of the projection point in the current frame is calculated by the optical flow method. At the same time, each point of the compensated lidar point cloud is projected onto the current frame image of the camera. The error between the projection point of each point in the current frame image and the matching point is the visual observation residual.

[0081] Furthermore, the compensated lidar point cloud observation residual and the visual observation residual are combined to obtain a fused observation residual.

[0082] Step S103: Based on the fused observation residual and IMU data, Kalman filtering is performed to obtain an updated posture of each of the unmanned platforms.

[0083] Specifically, the Kalman filter includes a prediction phase and an update phase. In the prediction phase of the Kalman filter, the algorithm predicts the state quantity based on the acceleration and angular velocity of the unmanned platform input from the IMU sensor according to the motion model of the IMU, and predicts and updates the covariance matrix of the state quantity. When the filter is updated, the Kalman gain is calculated based on the fusion observation residual, and the state quantity is updated.

[0084] Step S104: performing distance-aware point cloud downsampling on the compensated lidar point cloud data to obtain downsampled lidar point cloud data.

[0085] Specifically, the amount of point cloud data generated by large-scale complex scenes is large, and point clouds far away from the lidar are generally easily distorted and become noise points, which will affect the efficient registration of subsequent multi-platform collaborative map construction. Therefore, unlike the uniform downsampling and random downsampling in the prior art, this embodiment proposes a point cloud downsampling method based on distance perception, which consciously compresses the number of long-distance point clouds while reducing the amount of point cloud data.

[0086] Further, such as Figure 2 As shown, the point cloud downsampling based on distance perception of the compensated laser radar point cloud data includes steps S1041 to S1043:

[0087] Step S1041, calculate the maximum distance between each point of each laser radar point cloud after compensation at time t and the laser radar position.

[0088] Specifically, the formula for calculating the maximum distance between each point and the lidar position is as follows:

[0089]

[0090] Among them, L t represents the laser radar position coordinates at time t; p t,i Represents the position coordinates of the i-th point of the compensated lidar point cloud at time t.

[0091] Specifically, the maximum distance between each point of the lidar point cloud and the lidar position represents the distance of the farthest point that the lidar can detect at that moment. Obtaining this distance can help determine the sampling range of the point cloud data. At the same time, points far away from the lidar may be affected by more noise and errors. Calculating the maximum distance between each point of the lidar point cloud and the lidar position can selectively eliminate these points that may be more affected during downsampling, thereby improving data quality.

[0092] Step S1042: Taking the laser radar position at time t as the center, divide the maximum distance into several sub-intervals, and perform uniform sampling in each sub-interval.

[0093] Furthermore, the number of sampling points in each sub-interval is calculated using the following formula:

[0094] M t,k =-D(1-p t,k ) γ log(p t,k )

[0095]

[0096] Among them, M t,k represents the number of sampling points in the kth interval at time t; D represents the constant factor of the expected number of sampling point clouds, and D can be 80000 by way of example; γ represents the constant suppression factor, and γ can be 1 by way of example; R t,k Represents the left boundary distance of the kth interval at time t; R tmax represents the maximum distance between each lidar point cloud and the lidar position after compensation at time t; K represents the number of intervals that evenly divide the maximum distance between each lidar point cloud and the lidar position at time t.

[0097] Specifically, by considering the distance between each point in the point cloud data and the lidar, we can ensure that denser sampling is obtained near the lidar (i.e., the area of ​​interest), while reducing the sampling density at a greater distance. This helps to improve the overall data quality and reduce the impact of noise and errors at a distance.

[0098] Step S1043: Use the sampling point data in each of the sub-intervals as the down-sampled laser radar point cloud data at time t.

[0099] Specifically, the point cloud data sampled in all subintervals are combined to form the downsampled lidar point cloud data at that moment. The amount of processed point cloud data is reduced, but it still contains the key information in the original point cloud data.

[0100] Step S105: Based on the updated posture of each unmanned platform, the downsampled lidar point cloud data is dynamically spatially divided and the map is incrementally updated to obtain each single-carrier point cloud map.

[0101] Specifically, based on the current position and posture of each unmanned platform, the downsampled laser radar point cloud data space is divided into different areas for dynamic space division. That is, according to the current position and moving direction of the unmanned platform, the space is divided into different areas to facilitate the management and update of map data, and the map is incrementally updated according to the result of dynamic space division.

[0102] To improve computational efficiency, an incremental KD tree (ikd-Tree) can be used, for example, to perform dynamic spatial partitioning of point cloud data and incremental map updates. Specifically, the incremental KD tree grows dynamically by merging new point cloud scans at the odometer rate. In order to prevent the size of the map from being unconstrained, in this embodiment, only map points in a cube of length L around the current position of the laser radar are retained in the incremental KD tree. For example, L is 100-200 meters. Specifically, the initial map area is initialized as a cube of length L, which is centered on the initial laser radar position. The detection area of ​​the laser radar is a detection sphere centered on the current position of the laser radar. The radius of the detection sphere is the laser radar field of view (FOV) range * relaxation parameter, where the relaxation parameter is greater than 1. When the laser radar moves to a new position where the detection sphere touches the map boundary, the map area moves in the direction of the expansion of the laser radar detection area to include new detection data. The distance moved by the map area is a preset constant value. In this way, the expansion of the map area is fixed, which is beneficial to maintaining the locality of the map data while avoiding excessive accumulation of data. When the lidar leaves an area and the point cloud data in that area is no longer needed, these data are removed from the incremental KD tree through a frame-by-frame deletion operation.

[0103] Step S2, using the improved generalized ICP algorithm to perform pairwise point cloud registration on each of the single-carrier point cloud maps to obtain the three point cloud rigid body transformation matrices after registration; wherein the improved generalized ICP algorithm uses a distance measurement method based on the invariant angle feature of the point cloud normal to search for the nearest neighbor point when performing point cloud registration.

[0104] Specifically, the generalized ICP algorithm is a point cloud data alignment and matching method based on the ICP algorithm. The registration steps of the algorithm start with initializing a rigid body transformation matrix, iteratively finding corresponding point pairs between two sets of point clouds, calculating the error metric, removing outlier points that do not meet the conditions, and optimizing the transformation parameters to minimize the distance error between the point clouds until the termination condition is met, that is, the preset number of iterations is reached or the distance between point pairs is less than the preset threshold. Finally, the rigid body transformation matrix that optimally aligns the two sets of point clouds is output. Among them, the rigid body transformation matrix includes a rotation matrix and a translation vector.

[0105] It should be noted that in the initial stage of registration in this embodiment, maximum likelihood estimation is performed on two single-vehicle point cloud maps to be registered to obtain an initial rigid body transformation matrix; based on the initial rigid body transformation matrix, an improved generalized ICP algorithm is used for point cloud registration.

[0106] Specifically, the maximum likelihood estimation is a method for estimating probability model parameters. It is to select parameter values such that the probability (likelihood function) of the observed data appearing under these parameter values is the largest. In this embodiment, the maximum likelihood estimation is used to obtain the initial rigid body transformation matrix as the rough matching result of point cloud registration, providing a good initial rigid body transformation matrix for the fine registration stage. The advantage of this method is that considering the uncertainty of point cloud data, the registration accuracy and robustness are improved by optimizing the log-likelihood function.

[0107] Specifically, in this embodiment, during the maximum likelihood estimation, the local Gaussian distribution is obtained for each point in the two point clouds to be registered. Among them, one of the point clouds to be registered is denoted as the point cloud set A = {a0, a1, …, a W}, and the other point cloud to be registered is denoted as the point cloud set B = {b0, b1, …, b W}; the Gaussian distribution of the points is constructed as: Among them, represents the estimated position of the u-th point in the point cloud set A; represents the covariance matrix of the u-th point in the point cloud set A, reflecting its uncertainty; represents the estimated position of the v-th point in the point cloud set B; represents the covariance matrix of the v-th point in the point cloud set B, reflecting its uncertainty.

[0108] For the point cloud set A, find the points in the other point cloud B to be registered that are the closest to it. These points form a local neighbor set {b v ‖a u -b v ‖ < r}, where r is a preset distance threshold; and calculate the error between the points in the point cloud A after pose transformation and these neighboring points. The error formula is: Where T represents the pose transformation matrix.

[0109] Based on the conversion error, the pose transformation matrix T that maximizes the logarithmic likelihood is estimated using the following formula, that is, a pose transformation matrix T is found that can minimize the error between all points in the point cloud set A and their corresponding points in the point cloud geometry B after transformation:

[0110]

[0111] Among them, W u Indicates the number of neighboring points of the u-th point in point cloud A; a u represents the u-th point in the point cloud set A; b v Represents the vth point in the point cloud set B, and is point a u neighboring points.

[0112] The obtained pose transformation matrix T is used as a rough registration result to provide the initial rigid body transformation matrix for the subsequent generalized ICP algorithm.

[0113] It should be noted that when the generalized ICP algorithm iteratively searches for corresponding point pairs between two sets of point clouds, in order to speed up the registration efficiency of large-scale complex scenes, this embodiment uses the NanoFLANN method to construct a KD tree. When constructing the KD tree, its nearest neighbor retrieval method is to search for the nearest neighbor based on the distance measurement method with the invariant angle feature of the point cloud normal.

[0114] Specifically, NanoFLANN is a lightweight C++ library for constructing KD trees with different topological data sets. In point cloud registration, one point cloud to be registered is used as a reference point cloud, and NanoFLANN is used to construct a KD tree of the reference point cloud. Then, each point in the other point cloud to be registered is queried to find the nearest point in the reference point cloud. It should be noted that in this embodiment, one of the two point clouds to be registered is randomly selected as the reference point cloud, which is not limited in this embodiment.

[0115] Furthermore, the distance measurement method based on the invariant feature of the point cloud normal angle is used to search for the nearest neighbor point, and the nearest neighbor point is searched using the following formula:

[0116]

[0117] Among them, (x h ,y h ,z h ), f h They represent the coordinates and eigenvalues ​​of the hth point in a single carrier point cloud to be registered; (x g ,y g ,z g ), f gThey respectively represent the coordinates and eigenvalues ​​of the g-th point in another single-carrier point cloud to be registered; λ represents the weighting coefficient, and its value range is between 0 and 1; among them, for points with high point cloud data density, their eigenvalues ​​can provide more information, and at this time λ selects a larger value close to 1; conversely, for points with low point cloud data density or point cloud data with large noise, over-reliance on eigenvalues ​​may lead to incorrect matching. Therefore, λ selects a smaller value close to 0.

[0118] Specifically, the distance measurement method based on the invariant angle feature of the point cloud normal not only considers the Euclidean distance between points, but also the normal direction information of the points. For areas with weak texture or single geometric structure in the point cloud data, relying solely on the Euclidean distance is prone to find wrong corresponding points, while the normal information can help distinguish these areas and improve the accuracy and robustness of the registration.

[0119] Furthermore, the eigenvalue of the hth point in a single carrier point cloud to be registered is obtained using the following formula:

[0120]

[0121] Among them, n h Represents the normal vector of the hth point in a single carrier point cloud to be registered; n m Represents the normal vector of the mth point in a single carrier point cloud to be registered; d hm represents the Euclidean distance between the hth point and the mth point in a single carrier point cloud to be registered; M represents the number of neighboring points of the hth point in a single carrier point cloud to be registered;

[0122] Use the following formula to get the eigenvalue of the g-th point in another single-carrier point cloud to be registered:

[0123]

[0124] Among them, n g Represents the normal vector of the g-th point in another single-carrier point cloud to be registered; n n Represents the normal vector of the nth point in another single-carrier point cloud to be registered; d gn It represents the Euclidean distance between the g-th point and the n-th point in another single-carrier point cloud to be registered; N represents the number of neighboring points of the g-th point in another single-carrier point cloud to be registered.

[0125] Specifically, the point cloud normal angle invariant feature value is created by combining the angle information of the normal vector and the spatial distance information. It is stable in the local neighborhood of the point cloud and will not change due to the overall rotation or scale change of the point cloud. In point cloud registration, this feature can help the algorithm identify point pairs that still match even after the point cloud is rotated or scaled, thereby improving the robustness and accuracy of the registration.

[0126] Step S3, performing coordinate transformation on the point clouds in each of the single-carrier point cloud maps based on the three registered point cloud rigid body transformation matrices to obtain a fused map, including:

[0127] Each of the single-carrier point cloud maps is transformed into a unified point cloud map coordinate system based on the three registered point cloud rigid body transformation matrices;

[0128] The three point cloud maps after coordinate conversion are merged to obtain a fused map.

[0129] Specifically, for each single-carrier point cloud map, the point cloud rigid body transformation matrix obtained through the registration process is used to adjust the point coordinates in each point cloud so that they are aligned with a common coordinate system. In this way, point cloud data from different carriers can be merged together to form a unified, continuous 3D map that contains the environmental information captured by all carriers, thus providing a comprehensive perspective to understand the entire scene.

[0130] Exemplarily, when the three air-ground heterogeneous unmanned platforms are the first unmanned vehicle, the second unmanned vehicle and the unmanned aerial vehicle, respectively, the three point cloud rigid body transformation matrices are: the point cloud rigid body transformation matrix of the first unmanned vehicle and the second unmanned vehicle, the point cloud rigid body transformation matrix of the first unmanned vehicle and the unmanned aerial vehicle, and the point cloud rigid body transformation matrix of the second unmanned vehicle and the unmanned aerial vehicle. Select any one of the three unmanned platforms as the reference point cloud for coordinate transformation. Exemplarily, the unmanned aerial vehicle is selected as the unified reference point cloud for coordinates. Based on the point cloud rigid body transformation matrix of the first unmanned vehicle and the unmanned aerial vehicle, the coordinates of each point in the point cloud map of the first unmanned vehicle are transformed into the coordinate system of the unmanned aerial vehicle; based on the point cloud rigid body transformation matrix of the second unmanned vehicle and the unmanned aerial vehicle, the coordinates of each point in the point cloud map of the second unmanned vehicle are transformed into the coordinate system of the unmanned aerial vehicle; at this point, the coordinates of each point in the point cloud of the three unmanned platforms are unified into the coordinate system of the unmanned aerial vehicle, and the three point cloud maps of the unified coordinate system are superimposed to complete the fusion, and a complete environmental map registration and splicing result is obtained.

[0131] In summary, the method for collaborative 3D mapping of air-ground heterogeneous unmanned platforms according to the embodiment of the present invention has the following beneficial effects:

[0132] 1. The present invention proposes a three-dimensional mapping method for collaborative mapping of heterogeneous unmanned platforms in the air and on the ground. Through the collaborative mapping of ground unmanned platforms (unmanned vehicles) and aerial unmanned platforms (drones), the accuracy and efficiency of complex scene mapping are improved.

[0133] 2. The present invention introduces visual observation into the existing Kalman filter framework, projects the point cloud in the local map to the previous frame to form projection points, and then calculates the matching points of the projection points in the current frame through the optical flow method, and then calculates the visual residual model between the projection points and the matching points of the current frame. On this basis, the Kalman filter update stage will comprehensively utilize the point cloud observation residual and visual residual information to better adapt to complex environments such as weak textures, irregularities, and closed environments with a single geometric structure.

[0134] 3. In the process of single-carrier data fusion, the present invention proposes a distance-aware point cloud downsampling method, which consciously compresses the number of long-distance point clouds while reducing the amount of point cloud data, thereby reducing the impact of remote noise points on subsequent multi-platform map alignment.

[0135] 4. In the process of point cloud registration, the present invention proposes an efficient point cloud registration method that combines the point cloud normal angle invariant feature and KD tree acceleration. Compared with the original registration algorithm, it is more robust and more efficient.

[0136] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A method for collaborative 3D mapping between air-ground heterogeneous unmanned platforms, characterized in that: The steps include: The laser radar point cloud data, IMU data and camera images of three unmanned platforms with heterogeneity in the air and on the ground are obtained and data fusion is performed to obtain a single-carrier point cloud map of each unmanned platform; wherein, in the data fusion process, the laser radar point cloud data is downsampled based on distance perception; Performing pairwise point cloud registration on each of the single-carrier point cloud maps using an improved generalized ICP algorithm to obtain three point cloud rigid body transformation matrices after registration; wherein the improved generalized ICP algorithm uses a distance metric method based on the point cloud normal angle invariance feature to search for the nearest neighbor point when performing point cloud registration; The point clouds in each of the single-carrier point cloud maps are subjected to coordinate transformation based on the three registered point cloud rigid body transformation matrices to obtain a fused map.

2. The method according to claim 1, characterized in that: The data fusion obtains a single-carrier point cloud map of each unmanned platform, including: Performing motion distortion compensation on the laser radar point cloud data of each unmanned platform based on the IMU data to obtain compensated laser radar point cloud data; Constructing a fused observation residual based on the compensated lidar point cloud data and the image taken by the camera; Based on the fused observation residual and IMU data, Kalman filtering is performed to obtain updated postures of each unmanned platform; Performing distance-aware point cloud downsampling on the compensated laser radar point cloud data to obtain downsampled laser radar point cloud data; Based on the updated posture of each unmanned platform, the downsampled lidar point cloud data is dynamically spatially divided and the map is incrementally updated to obtain a point cloud map of each single carrier.

3. The method according to claim 2, characterized in that: The step of performing distance-aware point cloud downsampling on the compensated laser radar point cloud data includes: Calculate the maximum distance between each point of each laser radar point cloud and the laser radar position after compensation at time t; Taking the laser radar position at time t as the center, the maximum distance is evenly divided into a number of sub-intervals, and uniform sampling is performed in each of the sub-intervals; The sampling point data within each of the subintervals is used as the laser radar point cloud data after downsampling at time t.

4. The method according to claim 3, characterized in that: The maximum distance between each point of each laser radar point cloud and the laser radar position after compensation at time t is calculated using the following formula: Among them, L t represents the laser radar position coordinates at time t; p t,i Represents the position coordinates of the i-th point of the compensated lidar point cloud at time t.

5. The method according to claim 4, characterized in that: Use the following formula to calculate the number of sampling points in each sub-interval: M t,k =-D(1-p t,k ) γ log(p t,k ) Among them, M t,k represents the number of sampling points in the kth interval at time t; D represents the constant factor of the expected number of sampling point clouds; γ represents the constant suppression factor; R t,k Represents the left boundary distance of the kth interval at time t; R tmax represents the maximum distance between each lidar point cloud and the lidar position after compensation at time t; K represents the number of intervals that evenly divide the maximum distance between each lidar point cloud and the lidar position at time t.

6. The method according to claim 2, characterized in that: The step of constructing a fused observation residual based on the compensated laser radar point cloud data and the image captured by the camera includes: The point cloud observation residual of each point in the compensated lidar point cloud is constructed using the following formula: in, Represents the point cloud observation residual of the i-th point; represents the transpose of the normal vector of the fitted plane of the i-th point in the lidar point cloud; w pi Represents the coordinates of the i-th point in the lidar point cloud; Represents the coordinates of the center point of the fitting plane of the i-th point in the lidar point cloud; Project each point of the compensated LiDAR point cloud onto the previous frame image taken by the camera, and use the optical flow method to obtain the coordinates of each point in the point cloud in the current frame image; Project each point in the compensated LiDAR point cloud data to the current frame image taken by the camera, and use the following formula to construct the visual observation residual of each point in the LiDAR point cloud: in, represents the visual observation residual of the i-th point; ρ j,i represents the coordinates of the i-th point in the current frame image obtained by the optical flow method; π() represents the image projection model; (R j ,t j ) represents the rotation matrix and translation vector of the current frame image relative to the laser radar; Represents the coordinates of the i-th point in the laser radar; The compensated lidar point cloud observation residual and the visual observation residual are combined to obtain a fused observation residual.

7. The method according to claim 1, characterized in that: The distance metric method based on the point cloud normal angle invariant feature is used to search for the nearest neighbor point, and the nearest neighbor point is searched using the following formula: Among them, (x h ,y h ,z h ), f h They represent the coordinates and eigenvalues ​​of the hth point in a single carrier point cloud to be registered; (x g ,y g ,z g ), f g They respectively represent the coordinates and eigenvalues ​​of the g-th point in another single-carrier point cloud to be registered; λ represents the weighting coefficient.

8. The method according to claim 7, characterized in that: Use the following formula to get the eigenvalue of the hth point in a single carrier point cloud to be registered: Among them, n h Represents the normal vector of the hth point in a single carrier point cloud to be registered; n m Represents the normal vector of the mth point in a single carrier point cloud to be registered; d hm represents the Euclidean distance between the hth point and the mth point in a single carrier point cloud to be registered; M represents the number of neighboring points of the hth point in a single carrier point cloud to be registered; Use the following formula to get the eigenvalue of the g-th point in another single-carrier point cloud to be registered: Among them, n g Represents the normal vector of the g-th point in another single-carrier point cloud to be registered; n n Represents the normal vector of the nth point in another single-carrier point cloud to be registered; d gn It represents the Euclidean distance between the g-th point and the n-th point in another single-carrier point cloud to be registered; N represents the number of neighboring points of the g-th point in another single-carrier point cloud to be registered.

9. The method according to claim 1, characterized in that: Using an improved generalized ICP algorithm to perform pairwise point cloud registration on each of the single-carrier point cloud maps, it also includes: in the initial stage of registration, performing maximum likelihood estimation on the two single-carrier point cloud maps to be registered to obtain an initial rigid body transformation matrix; and performing point cloud registration based on the improved generalized ICP algorithm of the initial rigid body transformation matrix.

10. The method according to any one of claims 1 to 9, characterized in that: The point clouds in each of the single-carrier point cloud maps are subjected to coordinate transformation based on the three registered point cloud rigid body transformation matrices to obtain a fused map, including: Perform coordinate transformation on each of the single-carrier point cloud maps based on the three registered point cloud rigid body transformation matrices to unify them into any one of the point cloud map coordinate systems; The three point cloud maps after coordinate conversion are merged to obtain a fused map.

Images