Method and system for improving unmanned aerial vehicle state estimation and mapping robustness

Abstract

A method for improving unmanned aerial vehicle state estimation and mapping robustness comprises map construction and state updating, and the map construction comprises the following steps: S1, obtaining real-time data through a laser radar scanner and an IMU sensor carried on an unmanned aerial vehicle, performing forward propagation on real-time data through an IMU kinematics equation to predict a predicted pose of the unmanned aerial vehicle system; s2, correcting the laser point cloud by using the predicted pose to obtain corrected point cloud data; s3, performing voxel map construction by using the corrected point cloud data; through a pose prediction and correction mechanism, efficient voxel map construction and real-time state feedback and adjustment, the robustness of unmanned aerial vehicle state estimation and mapping is significantly improved. Even in a complex and dynamic environment, the unmanned aerial vehicle can keep stable pose estimation and accurate map construction, and a solid foundation is provided for subsequent tasks such as autonomous navigation and obstacle avoidance.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method and system for improving the robustness of unmanned aerial vehicle state estimation and mapping. Background Art

[0002] In the state estimation and mapping of LiDAR-Inertial Odometry (LIO) drones, point cloud registration is a core step, which is used to fuse point cloud data acquired at different times or perspectives into a consistent three-dimensional spatial representation. Existing point cloud registration technologies mainly include feature-based, geometry-based, and voxel-based methods. Compared with other methods, the advantages of voxel-based methods are: fast calculation speed and robustness to noise.

[0003] The existing point cloud registration algorithms based on voxel models mainly include the NDT algorithm and the GICP algorithm. The NDT algorithm uses a point-to-distribution method for registration. First, the map is divided into several voxels, and the point cloud in each voxel is modeled as a Gaussian distribution. The residual from the source point to the map voxel distribution is optimized to estimate the pose. GICP is a single-distribution-to-single-distribution registration mode. The source point cloud is also divided into voxels and modeled as a Gaussian distribution. Then, the residual from the source point voxel distribution to the target voxel distribution is optimized to estimate the pose. The statistical information in the voxel where the source point is located is used to smooth out some of the noise effects.

[0004] However, the existing technology has certain objective shortcomings: the point-to-distribution registration mode does not utilize the statistical information around the source point and is sensitive to noise; the single-distribution-to-single-distribution registration mode ignores the relationship between neighboring voxels, resulting in the entire map being divided into multiple discrete voxels. This ultimately affects the robustness of the drone in state estimation and mapping. Summary of the invention

[0005] In view of the above-mentioned defects, the purpose of the present invention is to propose a method and system for improving the robustness of drone state estimation and mapping, so as to solve the problem that existing drones do not reasonably utilize the statistical information around the source point, resulting in low robustness during mapping or state updating.

[0006] To achieve this purpose, the present invention adopts the following technical solution: a method for improving the robustness of drone state estimation and mapping, including map construction and state update, wherein map construction includes the following steps:

[0007] Step S1: Real-time data is obtained through the laser radar scanner and IMU sensor mounted on the drone. The real-time data is forward propagated through the IMU kinematic equation to predict the predicted position and posture of the drone system;

[0008] Step S2: Correcting the laser point cloud using the predicted pose to obtain corrected point cloud data;

[0009] Step S3: constructing a voxel map using the corrected point cloud data;

[0010] The status update steps are as follows:

[0011] Step S4: Obtain the state error, and adjust the predicted posture according to the state error to obtain the real state of the UAV.

[0012] Preferably, the formula for obtaining the predicted posture in step S1 is as follows:

[0013]

[0014] in They are the rotation prediction, translation prediction and velocity prediction of the IMU in the world coordinate system at time k, are the gyroscope bias and accelerometer bias at time k respectively;

[0015] in

[0016]

[0017] Exp() is the exponential mapping to the SO(3) group, is the rotation prediction of the IMU in the world coordinate system at time k-1, w k is the gyroscope reading at time k, Δt is the integration time interval between two adjacent IMU frames, is the translation prediction of the IMU in the world coordinate system at time k-1, is the velocity prediction of the IMU in the world coordinate system at time k-1, is the velocity prediction of the IMU in the world coordinate system at time k, g is the gravitational acceleration, a k is the accelerometer reading at time k, is the gyro deviation at time k-1, is the accelerometer bias at time k-1.

[0018] Preferably, the specific steps of step S2 are:

[0019] The coordinates P of the point at time k in the laser radar scanning cycle k Go to period k s The specific formula for transformation is as follows:

[0020]

[0021] Where “*” represents the matrix multiplication operator, P kis the representation of the laser point in the laser radar coordinate system at time k before distortion correction; P′ is the point cloud data of the corrected point in the laser radar coordinate system, It is the external parameter transformation matrix from the IMU coordinate system to the lidar coordinate system. is the period k s The transformation matrix from the world coordinate system to the IMU coordinate system at the moment, is the transformation matrix from the IMU coordinate system to the world coordinate system at time k, is the external parameter transformation matrix from the lidar coordinate system to the IMU coordinate system, where To predict the pose, interpolate according to the timestamp.

[0022] Preferably, the specific steps of step S3 are as follows:

[0023] Step S31: mapping the corrected point cloud data into a hash index through a hash function, and dividing the point cloud data into different voxels according to the hash index;

[0024] Step S32: triangulate the scene graph scanned by the laser radar to obtain a number of triangular descriptors, and construct a triangular description conformation set;

[0025] Step S33: Project each triangle descriptor in the triangle description set to the historical voxel map in the world coordinate system using the predicted pose.

[0026] Preferably, the step S31 of dividing the point cloud data into different voxels according to the hash index specifically comprises: comparing the hash indexes of the point cloud data, and if there are the same hash indexes, dividing the point cloud data with the same hash index into the same voxel;

[0027] The formula for mapping point cloud data to hash index is as follows:

[0028] hash-index=(g x n x )Xor(g y n y )Xor(g z n z )Mod(M max );

[0029]

[0030] P x ′,P y ′,P z ′ are the x-coordinate, y-coordinate and z-coordinate of the corrected point cloud data, v is the size of the voxel, floor() is rounded down, M max is the maximum number of points stored in a voxel, nx ,n y ,n z They are large prime numbers, Mod is the modulus operation, Xor is the exclusive or operation, g x ,g y ,g z are the x-coordinate, y-coordinate, and z-coordinate of the voxel where the laser point is located;

[0031] The specific steps of triangulating the scene graph scanned by the laser radar in step S32 are as follows:

[0032] Step S321: in the voxels constructed by the laser radar of the current frame, the centroid coordinates of each voxel are obtained as vertices, and the vertices are stored in a vertex collection;

[0033] Step S322: sequentially obtaining a vertex in the vertex collection as a first vertex, obtaining two vertices closest to the first vertex and marking them as second vertices, and using the first vertex and the two second vertices to form the triangle descriptor;

[0034] Step S323: removing the first vertex and the second vertex from the vertex collection;

[0035] Step S324: repeat steps S322 to S324 until the number of vertex sets is less than 3;

[0036] The specific formula of step S33 is as follows:

[0037]

[0038] is the representation of the centroid of the triangle descriptor in the world coordinate system, is the representation of the centroid of the triangle descriptor in the lidar coordinate system, is the transformation matrix from the laser radar to the IMU coordinate system;

[0039]

[0040] is the representation of the triangular descriptor covariance in the world coordinate system, is the representation of the triangular descriptor covariance in the lidar coordinate system, is the rotation prediction of the IMU in the world coordinate system at time k, is the rotation matrix from the laser radar to the IMU coordinate system, and “*” represents the matrix multiplication operator.

[0041] Preferably, step S3 further includes the following steps:

[0042] Step S34: construct an i-kd tree with the centroid coordinates of the triangle descriptor in world coordinates As coordinates in the i-kd tree;

[0043] The centroid coordinates The formula for obtaining is as follows:

[0044]

[0045] in is the nominal state of the IMU forward propagation The transformation matrix composed of rotation prediction and translation prediction is is the transformation matrix from the laser radar to the IMU coordinate system, It is the centroid of the triangle descriptor in the lidar coordinate system.

[0046] Preferably, after constructing the i-kd tree, the following steps need to be performed:

[0047] Step A: Calculate each triangle descriptor ΔQ in the source point cloud of the current frame separately n and its K nearest neighbor historical triangle descriptor set searched in the historical voxel map The covariance distribution distance S between n,j , and select the covariance distribution distance S n,j Minimum historical triangle descriptor ΔM j As the triangle descriptor ΔQ in the current frame n to associate and form (ΔQ n ,ΔM j )'s associated pair;

[0048] Step B: Determine the associated pair (ΔQ n ,ΔM j ) n,j Is it less than the distance threshold Sτ? If so, keep the associated pair (ΔQ n ,ΔM j ), if it is not less than, then associate the pair (ΔQ n ,ΔM j ) to delete;

[0049] The distance threshold S τ The formula for obtaining is as follows:

[0050]

[0051] γ v is the mean of the Gaussian distribution of laser points on the voxel surface, v is the size of the voxel, and m is the number of laser points occupied on the voxel boundary;

[0052]

[0053] D k ={jj<k};

[0054] γ k is the mean of all predicted pose errors before time k; D k is the posture error δ(ΔT before the current time k) i ), |()| represents the number of elements in the set, δ(ΔT i ) is the IMU predicted attitude error.

[0055] Preferably, the specific steps of step S4 are as follows:

[0056] The formula for obtaining the state error in the ath iteration is set as:

[0057]

[0058] where δX k is the state error, is δX k The inverse of the prediction covariance matrix of , mum is the number of associated pairs, is the number of associated pairs (ΔQ n ,ΔM j ), is the diagonal matrix of eigenvalues;

[0059] Use the conventional iterative error state Kalman filter method to find the optimal solution to the maximum a posteriori problem

[0060] The optimal solution of the state error Projected into the manifold space, the predicted pose of the UAV system is corrected.

[0061] A system for improving the robustness of drone state estimation and mapping, using the method for improving the robustness of drone state estimation and mapping, comprising: a prediction module, a correction module, a mapping module and a state update module;

[0062] The prediction module is used to obtain real-time data through the lidar scanner and IMU sensor mounted on the drone. The real-time data is forward propagated through the IMU kinematic equation to predict the predicted position and posture of the drone system.

[0063] The correction module is used to correct the laser point cloud using the predicted posture to obtain corrected point cloud data;

[0064] The mapping module is used to construct voxel maps using the rectified point cloud data;

[0065] The status update steps are as follows:

[0066] The state update module is used to obtain the state error, and adjust the predicted posture through the state error to obtain the real state of the UAV.

[0067] One of the above technical solutions has the following advantages or beneficial effects: through the posture prediction and correction mechanism, efficient voxel map construction and real-time state feedback and adjustment, the robustness of drone state estimation and mapping is significantly improved. Even in complex and dynamic environments, drones can maintain stable posture estimation and accurate map construction, providing a solid foundation for subsequent autonomous navigation, obstacle avoidance and other tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 is a flow chart of an embodiment of the method of the present invention.

[0069] Figure 2 It is a schematic diagram of the structure of an embodiment of the system of the present invention. DETAILED DESCRIPTION

[0070] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions from beginning to end. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0071] In the description of the embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0072] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, the meaning of "plurality" is two or more. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0073] like Figures 1-2 As shown, a method for improving the robustness of drone state estimation and mapping includes map construction and state update, wherein map construction includes the following steps:

[0074] Step S1: Real-time data is obtained through the laser radar scanner and IMU sensor mounted on the drone. The real-time data is forward propagated through the IMU kinematic equation to predict the predicted position and posture of the drone system;

[0075] Step S2: Correcting the laser point cloud using the predicted pose to obtain corrected point cloud data;

[0076] Step S3: constructing a voxel map using the corrected point cloud data;

[0077] The status update steps are as follows:

[0078] Step S4: Obtain the state error, and adjust the predicted posture according to the state error to obtain the real state of the UAV.

[0079] First, the IMU's kinematic equations are used to predict the drone's position and posture. This step can quickly respond to the drone's dynamic changes. Subsequently, the laser point cloud is corrected using the predicted position and posture, removing point cloud distortion caused by the drone's motion and improving the accuracy and usability of the point cloud data. This prediction and correction mechanism significantly enhances the system's ability to adapt to complex dynamic environments.

[0080] The voxel map is constructed using the corrected point cloud data. Compared with the traditional point cloud map, the voxel map is more efficient in storage space, while retaining enough environmental information for subsequent path planning, obstacle avoidance and other tasks. The voxelization process can also further smooth the noise and improve the map quality.

[0081] In addition, through the posture prediction and correction mechanism, efficient voxel map construction, and real-time state feedback and adjustment, the robustness of drone state estimation and mapping has been significantly improved. Even in complex and dynamic environments, drones can maintain stable posture estimation and accurate map construction, providing a solid foundation for subsequent autonomous navigation, obstacle avoidance and other tasks.

[0082] Preferably, the formula for obtaining the predicted posture in step S1 is as follows:

[0083]

[0084] in They are the rotation prediction, translation prediction and velocity prediction of the IMU in the world coordinate system at time k, are the gyroscope bias and accelerometer bias at time k respectively;

[0085] in

[0086]

[0087] Exp() is the exponential mapping to the SO(3) group, is the rotation prediction of the IMU in the world coordinate system at time k-1, w k is the gyroscope reading at time k, Δt is the integration time interval between two adjacent IMU frames, is the translation prediction of the IMU in the world coordinate system at time k-1, is the velocity prediction of the IMU in the world coordinate system at time k-1, is the velocity prediction of the IMU in the world coordinate system at time k, g is the gravitational acceleration, a k is the accelerometer reading at time k, is the gyro deviation at time k-1, is the accelerometer bias at time k-1.

[0088] Preferably, the specific steps of step S2 are:

[0089] The coordinates P of the point at time k in the laser radar scanning cycle k Go to period k s The specific formula for transformation is as follows:

[0090]

[0091] Where “*” represents the matrix multiplication operator, P k is the representation of the laser point in the laser radar coordinate system at time k before distortion correction; P′ is the point cloud data of the corrected point in the laser radar coordinate system, It is the external parameter transformation matrix from the IMU coordinate system to the lidar coordinate system. is the period k s The transformation matrix from the world coordinate system to the IMU coordinate system at the moment, is the transformation matrix from the IMU coordinate system to the world coordinate system at time k, is the external parameter transformation matrix from the lidar coordinate system to the IMU coordinate system, where To predict the pose, interpolate according to the timestamp.

[0092] Preferably, the specific steps of step S3 are as follows:

[0093] Step S31: mapping the corrected point cloud data into a hash index through a hash function, and dividing the point cloud data into different voxels according to the hash index;

[0094] Step S32: triangulate the scene graph scanned by the laser radar to obtain a number of triangular descriptors, and construct a triangular description conformation set;

[0095] Step S33: Project each triangle descriptor in the triangle description set to the historical voxel map in the world coordinate system using the predicted pose.

[0096] Existing algorithms usually use sparse point cloud data within a single voxel in the source point cloud to model the surrounding environment. In contrast, the present invention makes full use of the statistical information around the source point, reduces the matching errors caused by noise, and improves accuracy. In addition, since it can make full use of the statistical information around the source point, the triangular descriptor proposed in this patent can still achieve high registration accuracy even with sparse point cloud data.

[0097] Preferably, the step S31 of dividing the point cloud data into different voxels according to the hash index specifically comprises: comparing the hash indexes of the point cloud data, and if there are the same hash indexes, dividing the point cloud data with the same hash index into the same voxel;

[0098] The formula for mapping point cloud data to hash index is as follows:

[0099] hash-index=(g x n x )Xor(g y n y )Xor(g z n z )Mod(M max );

[0100]

[0101] P x ′,P y ′,P z ′ are the x-coordinate, y-coordinate and z-coordinate of the corrected point cloud data, v is the size of the voxel, floor() is rounded down, M max is the maximum number of points stored in a voxel, n x ,n y, n z They are large prime numbers, Mod is the modulus operation, Xor is the exclusive or operation, g x ,g y ,g z are the x-coordinate, y-coordinate, and z-coordinate of the voxel where the laser point is located;

[0102] The specific steps of triangulating the scene graph scanned by the laser radar in step S32 are as follows:

[0103] Step S321: in the voxels constructed by the laser radar of the current frame, the centroid coordinates of each voxel are obtained as vertices, and the vertices are stored in a vertex collection;

[0104] Step S322: sequentially obtaining a vertex in the vertex collection as a first vertex, obtaining two vertices closest to the first vertex and marking them as second vertices, and using the first vertex and the two second vertices to form the triangle descriptor;

[0105] Step S323: removing the first vertex and the second vertex from the vertex collection;

[0106] Step S324: repeat steps S322 to S324 until the number of vertex sets is less than 3;

[0107] The specific formula of step S33 is as follows:

[0108]

[0109] is the representation of the centroid of the triangle descriptor in the world coordinate system, is the representation of the centroid of the triangle descriptor in the lidar coordinate system, is the transformation matrix from the laser radar to the IMU coordinate system; is the nominal state of the IMU forward propagation The transformation matrix composed of the rotation prediction and translation prediction.

[0110]

[0111] is the representation of the triangular descriptor covariance in the world coordinate system, is the representation of the triangular descriptor covariance in the lidar coordinate system, is the rotation prediction of the IMU in the world coordinate system at time k, is the rotation matrix from the laser radar to the IMU coordinate system, and “*” represents the matrix multiplication operator.

[0112] In step S33, the IMU forward propagation is used to predict the posture The predicted pose information Project each triangle descriptor in the triangle description set ΔQuery into the historical voxel map in the world coordinate system.

[0113] The historical voxel map is a collection of triangulated descriptors of accumulated lidar scans. ΔM m is the mth triangle descriptor in the historical voxel map, and S is the total number of triangle descriptors.

[0114] Preferably, step S3 further includes the following steps:

[0115] Step S34: construct an i-kd tree with the centroid coordinates of the triangle descriptor in world coordinates As coordinates in the i-kd tree;

[0116] The centroid coordinates The formula for obtaining is as follows:

[0117]

[0118] in is the nominal state of the IMU forward propagation The transformation matrix composed of rotation and translation, is the transformation matrix from the laser radar to the IMU coordinate system, It is the centroid of the triangle descriptor in the lidar coordinate system.

[0119] In order to quickly query and match the triangle descriptor, the i-kd tree can be used to maintain all the triangle descriptors of the historical voxel map. It is rotation invariant, so it is used as the coordinate for constructing i-kdtree.

[0120] Preferably, after constructing the i-kd tree, the following steps need to be performed:

[0121] Step A: Calculate each triangle descriptor ΔQ in the source point cloud of the current frame separately n and its K nearest neighbor historical triangle descriptor set searched in the historical voxel map The covariance distribution distance S between n,j , and select the covariance distribution distance S n,j Minimum historical triangle descriptor ΔM j As the triangle descriptor ΔQ in the current frame n to associate and form (ΔQ n ,ΔM j )'s associated pair;

[0122] Step B: Determine the associated pair (ΔQ n ,ΔM j ) n,j Is it less than the distance threshold Sτ? If so, keep the associated pair (ΔQ n ,ΔM j ), if it is not less than, then associate the pair (ΔQ n ,ΔM j ) to delete;

[0123] During the search process, the triangular descriptor set ΔQuery obtained by the lidar scan of the current frame is used to search for the historical voxel map The data association process is completed by finding the nearest neighbor through i-kd tree.

[0124] Specifically, for each triangle descriptor ΔQ in ΔQuery n , using its centroid coordinates exist Find the nearest neighbor in the constructed i-kd tree.

[0125] In large-scale map scenes, there may be multiple similar triangle structures. This causes the i-kd tree to search for ΔQ n The K nearest neighbors of Too many, thus affecting the operation of the system. At the same time, the existing technology usually calculates the Euclidean distance between matching points to screen out abnormal matches, which cannot effectively distinguish similar triangular structures in large-scale map scenes, resulting in abnormal data association. In contrast, the hierarchical global abnormal association pruning strategy proposed in this patent fully takes into account the statistical covariance distribution distance. The point cloud within the voxel is fitted as a Gaussian distribution, and the covariance distribution distance between the matching triangular descriptors is evaluated to remove outliers, which enhances the robustness of the algorithm to noise. In this regard, a hierarchical global abnormal association pruning strategy is proposed. First, multiple association pairs are obtained, and then the distance between the association pairs is judged to determine whether to retain the association pair.

[0126] The distance threshold S τ The formula for obtaining is as follows:

[0127]

[0128] γ v is the mean of the Gaussian distribution of laser points on the voxel surface, v is the size of the voxel, and m is the number of laser points occupied on the voxel boundary;

[0129]

[0130] D k ={jj<k};

[0131] γ k is the mean of all predicted pose errors before time k; D k is the posture error δ(ΔT before the current time k) i ), |()| represents the number of elements in the set, δ(ΔT i ) is the IMU predicted attitude error.

[0132] In traditional cases, the distance threshold S τIt is set by the user's experience and is a fixed value that cannot be adaptively adjusted according to specific scenarios and data characteristics. In contrast, the present invention enhances the adaptability of data association by adaptively adjusting the maximum distance threshold of the covariance distribution allowed for data association by considering the error of IMU predicted posture and voxel discretization error.

[0133] Preferably, the specific steps of step S4 are as follows:

[0134] The formula for obtaining the state error in the ath iteration is set as:

[0135]

[0136] where δX k is the state error, is δX k The inverse of the prediction covariance matrix of , mum is the number of associated pairs, is the number of associated pairs (ΔQ n ,ΔM j ), is the diagonal matrix of eigenvalues;

[0137] Use the conventional iterative error state Kalman filter method to find the optimal solution to the maximum a posteriori problem

[0138] The optimal solution of the state error Projected into the manifold space, the predicted pose of the UAV system is corrected.

[0139] The specific correction is to directly adopt the optimal solution and predicted pose Just add them together, no further description will be given here.

[0140] The present invention can achieve accurate estimation of the drone state by decomposing the real state of the drone system into predicted posture and state error, and using the Kalman filter to iteratively estimate the error state in Euclidean space. This method can effectively deal with the uncertainty in the system state and improve the accuracy of state estimation. Finally, by projecting the state error into the manifold space and correcting the predicted posture of the drone system, it can ensure that the real state is accurately maintained. This method of optimizing state correction can further improve the accuracy and reliability of state estimation.

[0141] A system for improving the robustness of drone state estimation and mapping, using the method for improving the robustness of drone state estimation and mapping, comprising: a prediction module, a correction module, a mapping module and a state update module;

[0142] The prediction module is used to obtain real-time data through the lidar scanner and IMU sensor mounted on the drone. The real-time data is forward propagated through the IMU kinematic equation to predict the predicted position and posture of the drone system.

[0143] The correction module is used to correct the laser point cloud using the predicted posture to obtain corrected point cloud data;

[0144] The mapping module is used to construct voxel maps using the rectified point cloud data;

[0145] The status update steps are as follows:

[0146] The state update module is used to obtain the state error, and adjust the predicted posture through the state error to obtain the real state of the UAV.

[0147] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "illustrative embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0148] Although the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

Claims

1. A method for improving the robustness of drone state estimation and mapping, characterized in that: Including map construction and status update, where map construction includes the following steps: Step S1: Real-time data is obtained through the laser radar scanner and IMU sensor mounted on the drone. The real-time data is forward propagated through the IMU kinematic equation to predict the predicted position and posture of the drone system; Step S2: Correcting the laser point cloud using the predicted pose to obtain corrected point cloud data; Step S3: constructing a voxel map using the corrected point cloud data; The status update steps are as follows: Step S4: Obtain the state error, and adjust the predicted posture according to the state error to obtain the real state of the UAV.

2. The method for improving the robustness of drone state estimation and mapping according to claim 1, characterized in that: The formula for obtaining the predicted pose in step S1 is as follows: in They are the rotation prediction, translation prediction and velocity prediction of the IMU in the world coordinate system at time k, are the gyroscope bias and accelerometer bias at time k respectively; in Exp() is the exponential mapping to the SO(3) group, is the rotation prediction of the IMU in the world coordinate system at time k-1, w k is the gyroscope reading at time k, Δt is the integration time interval between two adjacent IMU frames, is the translation prediction of the IMU in the world coordinate system at time k-1, is the velocity prediction of the IMU in the world coordinate system at time k-1, is the velocity prediction of the IMU in the world coordinate system at time k, g is the gravitational acceleration, a k is the accelerometer reading at time k, is the gyro deviation at time k-1, is the accelerometer bias at time k-1.

3. The method for improving the robustness of drone state estimation and mapping according to claim 1, characterized in that: The specific steps of step S2 are: The coordinates P of the point at time k in the laser radar scanning cycle k Go to period k s The specific formula for transformation is as follows: Where "*" represents the matrix multiplication operator, P k is the representation of the laser point in the laser radar coordinate system at time k before distortion correction; P′ is the point cloud data of the corrected point in the laser radar coordinate system, It is the external parameter transformation matrix from the IMU coordinate system to the lidar coordinate system. is the period k s The transformation matrix from the world coordinate system to the IMU coordinate system at the moment, is the transformation matrix from the IMU coordinate system to the world coordinate system at time k, is the external parameter transformation matrix from the lidar coordinate system to the IMU coordinate system, where To predict the pose, interpolate according to the timestamp.

4. The method for improving the robustness of drone state estimation and mapping according to claim 1, characterized in that: The specific steps of step S3 are as follows: Step S31: mapping the corrected point cloud data into a hash index through a hash function, and dividing the point cloud data into different voxels according to the hash index; Step S32: triangulate the scene graph scanned by the laser radar to obtain a number of triangular descriptors, and construct a triangular description conformation set; Step S33: Project each triangle descriptor in the triangle description set to the historical voxel map in the world coordinate system using the predicted pose.

5. The method for improving the robustness of drone state estimation and mapping according to claim 4, characterized in that: The step S31 of dividing the point cloud data into different voxels according to the hash index specifically includes: comparing the hash indexes of the point cloud data, and if there are the same hash indexes, dividing the point cloud data with the same hash index into the same voxel; The formula for mapping point cloud data to hash index is as follows: hash-index=(g x n x )Xor(g y n y )Xor(g z n z )Mod(M max ); P′ x ,P′ y ,P′ z are the x-coordinate, y-coordinate, and z-coordinate of the corrected point cloud data, v is the size of the voxel, floor() is rounded down, and M max is the maximum number of points stored in a voxel, n x ,n y ,n z They are large prime numbers, Mod is the modulus operation, Xor is the exclusive or operation, g x ,g y ,g z are the x-coordinate, y-coordinate, and z-coordinate of the voxel where the laser point is located; The specific steps of triangulating the scene graph scanned by the laser radar in step S32 are as follows: Step S321: in the voxels constructed by the laser radar of the current frame, the centroid coordinates of each voxel are obtained as vertices, and the vertices are stored in a vertex collection; Step S322: sequentially obtain a vertex in the vertex collection as the first vertex, obtain two vertices closest to the first vertex, mark them as second vertices, and use the first vertex and the two second vertices to form the triangle descriptor; Step S323: removing the first vertex and the second vertex from the vertex collection; Step S324: repeat steps S322 to S324 until the number of vertex sets is less than 3; The specific formula of step S33 is as follows: is the representation of the centroid of the triangle descriptor in the world coordinate system, is the representation of the centroid of the triangle descriptor in the lidar coordinate system, is the transformation matrix from the laser radar to the IMU coordinate system; is the representation of the triangular descriptor covariance in the world coordinate system, is the representation of the triangular descriptor covariance in the lidar coordinate system, is the rotation prediction of the IMU in the world coordinate system at time k, is the rotation matrix from the laser radar to the IMU coordinate system, and "*" represents the matrix multiplication operator.

6. The method for improving the robustness of drone state estimation and mapping according to claim 4, characterized in that: Step S3 also includes the following steps: Step S34: construct an i-kd tree with the centroid coordinates of the triangle descriptor in world coordinates As coordinates in i-kdtree; The centroid coordinates The formula for obtaining is as follows: in is the nominal state of the IMU forward propagation The transformation matrix composed of rotation and translation, is the transformation matrix from the laser radar to the IMU coordinate system, It is the centroid of the triangle descriptor in the lidar coordinate system.

7. The method for improving the robustness of drone state estimation and mapping according to claim 6, characterized in that: After building the i-kd tree, you need to perform the following steps: Step A: Calculate each triangle descriptor ΔQ in the source point cloud of the current frame separately n and its K nearest neighbor historical triangle descriptor set searched in the historical voxel map The covariance distribution distance S between n,j , and select the covariance distribution distance S n,j Minimum historical triangle descriptor ΔM j As the triangle descriptor ΔQ in the current frame n to associate and form (ΔQ n ,ΔM j )'s associated pair; Step B: Determine the associated pair (ΔQ n ,ΔM j ) n,j Is it less than the distance threshold S? τ , if it is less than, then keep the associated pair (ΔQ n ,ΔM j ), if it is not less than, then associate the pair (ΔQ n ,ΔM j ) to delete; The distance threshold S τ The formula for obtaining is as follows: γ v is the mean of the Gaussian distribution of laser points on the voxel surface, v is the size of the voxel, and m is the number of laser points occupied on the voxel boundary; D k ={j|j<k}; γ k is the mean of all predicted pose errors before time k; D k is the posture error δ(ΔT before the current time k) i ), |()| represents the number of elements in the set, δ(ΔT i ) is the IMU predicted attitude error.

8. The method for improving the robustness of drone state estimation and mapping according to claim 1, characterized in that: The specific steps of step S4 are as follows: The formula for obtaining the state error in the ath iteration is set as: where δX k is the state error, is δX k The inverse of the prediction covariance matrix of , mum is the number of associated pairs, is the number of associated pairs (ΔQ n ,ΔM j ), is the diagonal matrix of eigenvalues; Use the conventional iterative error state Kalman filter method to find the optimal solution to the maximum a posteriori problem The optimal solution of the state error Projected into the manifold space, the predicted pose of the UAV system is corrected.

9. A system for improving the robustness of drone state estimation and mapping, using the method for improving the robustness of drone state estimation and mapping as claimed in any one of claims 1 to 8, characterized in that: include: Prediction module, correction module, mapping module and status update module; The prediction module is used to obtain real-time data through the lidar scanner and IMU sensor mounted on the drone. The real-time data is forward propagated through the IMU kinematic equation to predict the predicted position and posture of the drone system. The correction module is used to correct the laser point cloud using the predicted posture to obtain corrected point cloud data; The mapping module is used to construct voxel maps using the rectified point cloud data; The status update steps are as follows: The state update module is used to obtain the state error, and adjust the predicted posture through the state error to obtain the real state of the UAV.

Images