A navigation method of a UAV laser-inertial odometer
By employing methods such as lidar point cloud transformation, downsampling, and feature frame extraction, combined with an IMU motion model, the problem of insufficient positioning accuracy of UAVs in complex environments was solved, achieving high-precision autonomous navigation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- BEIJING INST OF TECH
- Filing Date
- 2023-12-27
- Publication Date
- 2026-07-14
AI Technical Summary
In complex environments, when GNSS is denied and the optical environment does not meet the operating conditions of the camera sensor, the UAV positioning will have a large error, and the positioning accuracy of existing lidar is insufficient.
A method is adopted to construct a local map by transforming LiDAR point clouds, downsampling, extracting feature frames, and constructing multiple key frames. Combined with an IMU motion model, frame-map matching is used to achieve UAV state estimation.
It achieves high-precision autonomous navigation under conditions of GNSS rejection and unsatisfactory optical environment, reduces positioning errors, and improves the accuracy of UAV navigation.
Smart Images
Figure CN117804440B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a navigation method for a laser-inertial odometry system for unmanned aerial vehicles (UAVs), belonging to the field of aircraft control technology. Background Technology
[0002] In complex environments, GNSS rejection and optical conditions that do not meet the operating requirements of camera sensors often occur. In such cases, lidar is generally used for positioning. However, compared to satellite positioning and visual positioning, traditional lidar positioning can result in significant errors in UAV positioning.
[0003] Therefore, it is necessary to further study the design methods of existing UAV laser-inertial odometry to improve the accurate autonomous navigation of UAVs in GNSS environments. Summary of the Invention
[0004] To overcome the above problems, the inventors conducted in-depth research and proposed a navigation method for UAV laser-inertial odometry, comprising the following steps:
[0005] S1. Transform the lidar point cloud by converting all the point clouds in a frame of lidar point cloud to the lidar coordinate system corresponding to the beginning of that frame.
[0006] S2. Downsample the lidar point cloud to reduce the number of lidar point clouds;
[0007] S3. Extract the feature frames of each frame in the lidar point cloud. The feature frames include edge points and planar points. The edge points are points in the point cloud with a surface smoothness greater than an edge threshold, and the planar points are points in the point cloud with a surface smoothness less than a planar threshold.
[0008] Compare the change in the state of the aircraft in the corresponding time frame of the LiDAR point cloud with the selection threshold, and take the feature frame corresponding to the second frame in the consecutive frames where the change is greater than the selection threshold as the key frame.
[0009] S4. Construct a local map using multiple key frames. Based on the local map, perform frame-to-map matching using the feature frames of the current moment to obtain the UAV status at the next moment.
[0010] S5. The drone flies based on the current drone status obtained at that moment.
[0011] In a preferred embodiment, in S1, an IMU motion model is used to estimate the coordinate transformation relationship between two frames of the lidar point cloud. Based on the coordinate transformation relationship, all point clouds in a frame of lidar point cloud are transformed to the lidar coordinate system corresponding to the start time of that frame.
[0012] In a preferred embodiment, the conversion process is represented as follows:
[0013]
[0014] in, This indicates the index of a point in the lidar point cloud. This indicates the start time of the lidar point cloud frame. Indicates the first point in the lidar point cloud The time corresponding to each point Indicates the first point in the lidar point cloud The coordinates obtained when a point is transformed to the coordinate system at the beginning of this frame. Indicates the first point in the lidar point cloud The points at The original coordinates of the moment. express Time-based lidar coordinate system The lidar coordinate system at the initial moment of this frame The rotation matrix, express Time's up The centroid translation vector of the drone.
[0015] In a preferred embodiment, in S2, during the downsampling process, points with an intensity below a threshold are filtered out, where the intensity is represented as:
[0016]
[0017] in, This represents the point cloud reflection intensity value. The distance from the point to the lidar. Let be the normalized intensity of the reflection at a point.
[0018] In a preferred embodiment, in S2, during the downsampling process, points with duplicate voxel indices are filtered out, so that each voxel index corresponds to only one point.
[0019] In a preferred embodiment, in S3, the surface smoothness is expressed as:
[0020]
[0021] in, Indicates the first The first frame Surface smoothness at each point, This represents the set of points where the laser beam returns within the same frame. express In addition to Other points, Indicates the first The coordinates of a point in the lidar coordinate system.
[0022] In a preferred embodiment, in step S3, a frame of lidar point cloud is divided into geometrically equal parts. The system divides the LiDAR point cloud into segments and sets edge and planar thresholds to ensure that each segment can contain a maximum of only a few points. edge points and Plane point.
[0023] In a preferred embodiment, S4 includes the following steps:
[0024] S41. Transform the feature frame to the world coordinate system;
[0025] S42. Construct a local map using consecutive keyframes prior to the current time.
[0026] S43. Obtain the Euclidean distance between the edge point in the current frame and the edge line of that point in the local map, and the Euclidean distance between the plane point in the current frame and the plane corresponding to that point in the local map. Sum the two Euclidean distances to construct the error equation.
[0027] S44. Minimize the error equation to obtain the UAV state transition matrix;
[0028] S45. Use the UAV state transition matrix to transform the UAV state at the previous moment to obtain the UAV state at the current moment.
[0029] In a preferred embodiment, in S43, the Euclidean distance between the edge point in the current frame and the edge line of that point in the local map is... Represented as:
[0030]
[0031] Euclidean distance between a plane point in the current frame and the plane corresponding to that point in the local map Represented as:
[0032]
[0033] in, Indicates the current moment. The first point in the lidar point cloud at the current moment The coordinates of the edge points For local maps Mid-edge point The two edge points on the corresponding edge lines; The first point in the lidar point cloud at the current moment The coordinates of a point on the plane Local maps Mid-plane point A point on the corresponding plane and a line.
[0034] In a preferred embodiment, in step S44, the UAV state transition matrix corresponding to minimizing the error equation is obtained. , represented as:
[0035] .
[0036] The beneficial effects of this invention include:
[0037] (1) High-precision autonomous navigation can be achieved by relying solely on lidar and IMU sensors;
[0038] (2) It can meet the high-precision navigation requirements under the conditions of GNSS rejection and optical environment that does not meet the working conditions of camera sensor. Attached Figure Description
[0039] Figure 1 A schematic flowchart of a navigation method for a UAV laser-inertial odometry according to a preferred embodiment of the present invention is shown.
[0040] Figure 2 A three-dimensional trajectory comparison diagram is shown for Example 1, Comparative Example 1, and Comparative Example 2;
[0041] Figure 3 A comparison diagram of the x-direction trajectories of Example 1, Comparative Example 1, and Comparative Example 2 is shown.
[0042] Figure 4 A comparison diagram of the y-direction trajectories of Example 1, Comparative Example 1, and Comparative Example 2 is shown.
[0043] Figure 5 The diagram shows a comparison of the z-direction trajectories of Example 1, Comparative Example 1, and Comparative Example 2.
[0044] Figure 6 This shows the motion deviation of Example 1 and Comparative Example 1 relative to Comparative Example 2 in the x-axis direction;
[0045] Figure 7 This shows the motion deviation of Example 1 and Comparative Example 1 relative to Comparative Example 2 in the y-axis direction;
[0046] Figure 8 The motion deviation of Example 1 and Comparative Example 1 relative to Comparative Example 2 in the z-axis direction is shown. Detailed Implementation
[0047] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Through these descriptions, the features and advantages of the present invention will become clearer and more apparent.
[0048] The term “exemplary” as used herein means “serving as an example, embodiment, or illustration.” Any embodiment illustrated herein as “exemplary” is not necessarily to be construed as superior to or better than other embodiments. Although various aspects of embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless specifically indicated otherwise.
[0049] A navigation method for a UAV using a laser-inertial odometry, as provided by the present invention, is as follows: Figure 1 As shown, it includes the following steps:
[0050] S1. Transform the lidar point cloud by converting all the point clouds in a frame of lidar point cloud to the lidar coordinate system corresponding to the beginning of that frame.
[0051] S2. Downsample the lidar point cloud to reduce the number of lidar point clouds;
[0052] S3. Extract the feature frames of each frame in the lidar point cloud. The feature frames include edge points and planar points. The edge points are points in the point cloud with a surface smoothness greater than an edge threshold, and the planar points are points in the point cloud with a surface smoothness less than a planar threshold.
[0053] Compare the change in the state of the aircraft in the corresponding time frame of the LiDAR point cloud with the selection threshold, and take the feature frame corresponding to the second frame in the consecutive frames where the change is greater than the selection threshold as the key frame.
[0054] S4. Construct a local map using multiple key frames. Based on the local map, perform frame-to-map matching using the feature frames of the current moment to obtain the UAV status at the next moment.
[0055] S5. The drone flies based on the current drone status obtained at that moment.
[0056] According to the present invention, the lidar coordinate system refers to a coordinate system in which the origin is located at the geometric center of the lidar, the x-axis points forward, the z-axis points upward, and the y-axis forms a right-handed coordinate system with the z-axis and x-axis. The world coordinate system refers to a coordinate system that coincides with the lidar coordinate system at the initial moment.
[0057] In the lidar point cloud, the point cloud obtained after one complete scan coverage by the lidar is called a frame, denoted as . ,use Let i represent the set of all point clouds in the first k scans. If i∈ Then the coordinates of point i in the lidar coordinate system are expressed as: The coordinates in the world coordinate system are represented as .
[0058] Since the output frequency of the IMU is higher than that of the lidar, multiple frames of IMU data exist within the time frame of a lidar frame. In this invention, the lidar point cloud data is transformed using S1. Furthermore, during the transformation process, it is assumed that the UAV moves at a constant speed within a single frame of IMU data. Therefore, the position and attitude changes at each moment in a single frame of lidar data can be obtained using linear interpolation. This yields the coordinate transformation matrix of each moment in a single frame of lidar point cloud data relative to the start time of that frame, thus transforming all the lidar point cloud data in a single frame back to the start time of that frame. Specifically:
[0059] In S1, the IMU motion model is used to estimate the coordinate transformation relationship between two frames of the lidar point cloud. Based on the coordinate transformation relationship, all the point clouds in a frame of lidar point cloud are transformed to the lidar coordinate system corresponding to the beginning of that frame.
[0060] The conversion process is represented as follows:
[0061]
[0062] in, This indicates the index of a point in the lidar point cloud. This indicates the start time of the lidar point cloud frame. Indicates the first point in the lidar point cloud The time corresponding to each point Indicates the first point in the lidar point cloud The coordinates obtained when a point is transformed to the coordinate system at the beginning of this frame. Indicates the first point in the lidar point cloud The points at The original coordinates of the moment. express Time-based lidar coordinate system The lidar coordinate system at the initial moment of this frame The rotation matrix, express Time's up The centroid translation vector of the drone.
[0063] The rotation matrix It is obtained by estimating the IMU measurements, specifically by integrating the IMU motion model.
[0064] The IMU motion model is represented as follows:
[0065]
[0066]
[0067]
[0068] in, Indicates the initial time. Indicates time interval, express The rotation matrix of the lidar coordinate system L relative to the world coordinate system W at time t. express The measured value of angular acceleration at time. express The bias of the gyroscope in the IMU at any given time. This indicates the measurement noise of the gyroscope. express The speed of the drone at all times This represents the local gravitational acceleration. express The measured value of acceleration at any given time. express The bias of the accelerometer in the IMU at any given moment. This indicates the measurement noise of the accelerometer. express The position vector of the lidar coordinate system L relative to the world coordinate system W at any given time.
[0069] The bias of the IMU is caused by factors such as temperature, zero drift, and vibration.
[0070] The number of point clouds obtained from a single frame of LiDAR scanning is enormous. It is obviously impossible to perform calculations on all LiDAR point clouds. Therefore, before processing and calculating the point clouds, it is necessary to first perform downsampling on the LiDAR point clouds. The purpose of downsampling on the LiDAR point clouds is to filter out points with low information content and low measurement reliability, while retaining points with high information content and high reliability, thereby improving computational efficiency and ensuring the reliability of the processing results.
[0071] In S2, preferably, during the downsampling process, points with intensity below a threshold are filtered out, where the intensity is expressed as:
[0072]
[0073] in, This represents the point cloud reflection intensity value. The distance from the point to the lidar. Let be the normalized intensity of the reflection at a point.
[0074] Furthermore, when the normalized intensity of reflection at a certain point is lower than the threshold When this point is selected, preferably, the threshold value is used to filter out the selected point. The value can be set according to the standard of filtering out 10%.
[0075] Preferably, during the downsampling process, points with duplicate voxel indices are screened out, so that each voxel index corresponds to only one point.
[0076] The voxel index is expressed as:
[0077]
[0078]
[0079]
[0080]
[0081]
[0082]
[0083]
[0084] in, Indicates the first The voxel index of each point, ( , , ) is the first The coordinates of the points This represents the minimum value of all points along the x-axis. This represents the maximum value of all points along the x-axis. This represents the minimum value of all points along the y-axis. This represents the maximum value of all points along the y-axis. This represents the minimum value of all points along the z-axis. This represents the maximum value of all points along the y-axis. This indicates the preset value for the map grid size.
[0085] More preferably, after obtaining the voxel indices of all points, all points are sorted according to the voxel indices. Then, for points with the same voxel indices, one point is retained and the others are discarded using a random method, thereby eliminating points with duplicate voxel indices.
[0086] In S3, the surface smoothness is expressed as:
[0087]
[0088] in, Indicates the first The first frame Surface smoothness at each point, This represents the set of points where the laser beam returns within the same frame. express In addition to All other points, Indicates the first The coordinates of a point in the lidar coordinate system.
[0089] In a preferred embodiment, a frame of lidar point cloud is divided into geometrically equal parts. The system divides the LiDAR point cloud into segments and sets edge and planar thresholds to ensure that each segment can contain a maximum of only a few points. edge points and Planar points, thereby avoiding feature point aggregation and enabling feature points to be generated uniformly in a single frame of lidar point cloud.
[0090] More preferably, Set it to 3-5, for example, 4; Set it to 1-3, for example, 2. Set it to 3-5, for example, 4.
[0091] In a preferred embodiment, the change in state refers to the change in the flight distance and flight direction of the aircraft;
[0092] Preferably, in the selection threshold, the flight distance is set to 1m and the flight direction is set to 10°.
[0093] S4 includes the following steps:
[0094] S41. Transform the feature frame to the world coordinate system;
[0095] S42. Construct a local map using consecutive keyframes prior to the current time.
[0096] S43. Obtain the Euclidean distance between the edge point in the current frame and the edge line of that point in the local map, and the Euclidean distance between the plane point in the current frame and the plane corresponding to that point in the local map. Sum the two Euclidean distances to construct the error equation.
[0097] S44. Minimize the error equation to obtain the UAV state transition matrix;
[0098] S45. Use the UAV state transition matrix to transform the UAV state at the previous moment to obtain the UAV state at the current moment.
[0099] In S42, the constructed local map is represented as follows:
[0100]
[0101] in, This refers to the time before the current time. For the construction of a local map, This represents the consecutive n keyframes preceding the current moment. Preferably, 25 consecutive keyframes are used to construct a local map.
[0102] In S43, the Euclidean distance between an edge point in the current frame and the edge line of that point in the local map is... Represented as:
[0103]
[0104] Euclidean distance between a plane point in the current frame and the plane corresponding to that point in the local map Represented as:
[0105]
[0106] in, Indicates the current moment. The first point in the lidar point cloud at the current moment The coordinates of the edge points For local maps Mid-edge point The two edge points on the corresponding edge lines; The first point in the lidar point cloud at the current moment The coordinates of a point on the plane Local maps Mid-plane point A point on the corresponding plane and a line.
[0107] The error equation is expressed as:
[0108] .
[0109] In S44, the UAV state transition matrix corresponding to minimizing the error equation is obtained. , represented as:
[0110]
[0111] This is a nonlinear optimization problem that can be solved using various methods. Those skilled in the art can solve it based on experience, and no limitation is made in this invention. For example, the Gauss-Newton method can be used:
[0112] make
[0113]
[0114] This least squares problem can then be written as:
[0115]
[0116] Where f is the error function of F.
[0117] To solve this least squares problem using Gauss-Newton's method, we first define a... It is related to the state transition matrix. The relationship is:
[0118]
[0119] in This transforms a six-dimensional vector into a 4×4 matrix. The specific conversion formula is as follows:
[0120]
[0121] in The symbol represents the cross product matrix of three-dimensional vectors, so the above nonlinear optimization function can be calculated in the following way:
[0122]
[0123] in It represents the points in four-dimensional form before calculating the cross product matrix. Convert to three-dimensional form .
[0124] Jacobian matrix of edge point error function for:
[0125]
[0126] in:
[0127]
[0128] Similarly, we can also obtain the Jacobian matrix of the plane point error equation. :
[0129]
[0130] Through two Jacobian matrices and We can get each The corresponding Jacobian matrix ,make
[0131]
[0132]
[0133] The solution to the least squares problem can then be transformed into a linear solution.
[0134]
[0135] in, , There are no intermediate variables.
[0136] The maximum number of iterations and the convergence threshold are set. The UAV state transition matrix can be obtained when the difference between the coordinate transformation matrices obtained from two iterations is less than the convergence threshold or the number of iterations reaches the set maximum number of iterations. .
[0137] In a preferred embodiment, using As the initial value for iteration in the Gauss-Newton method ,in, express The rotation matrix from the lidar coordinate system of this frame to the lidar coordinate system at the initial moment of this frame. express The centroid translation vector of the frame drone.
[0138] In S45, the transformation is represented as:
[0139]
[0140] in, The current state of the drone is represented as follows: This represents the drone's status at the previous moment.
[0141] The UAV state includes the rotation matrix of the current lidar coordinate system L relative to the world coordinate system W, the position vector of the current lidar coordinate system L relative to the world coordinate system W, the current velocity, and the current IMU bias, expressed as:
[0142]
[0143] in, This represents the rotation matrix of the lidar coordinate system relative to the world coordinate system at the current moment. This represents the position vector of the lidar coordinate system relative to the world coordinate system at the current moment. Indicates the speed at the current moment. This indicates the IMU bias at the current moment.
[0144] It should be understood that the various forms of processes shown above can be used to rearrange, add, or delete steps. For example, the steps described in this disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein. Example
[0145] Example 1
[0146] A 20m*5m rectangular path flight test was conducted using a DJI F450 Innovation M210 RTK drone. The drone was equipped with GPS, LiDAR, a camera, and an IMU sensor, and navigation was performed using a laser-inertial odometry method. The test included the following steps:
[0147] S1. Transform the lidar point cloud by converting all the point clouds in a frame of lidar point cloud to the lidar coordinate system corresponding to the beginning of that frame.
[0148] S2. Downsample the lidar point cloud to reduce the number of lidar point clouds;
[0149] S3. Extract the feature frames of each frame in the lidar point cloud. The feature frames include edge points and planar points. The edge points are points in the point cloud with a surface smoothness greater than an edge threshold, and the planar points are points in the point cloud with a surface smoothness less than a planar threshold.
[0150] Compare the change in the state of the aircraft in the corresponding time frame of the LiDAR point cloud with the selection threshold, and take the feature frame corresponding to the second frame in the consecutive frames where the change is greater than the selection threshold as the key frame.
[0151] S4. Construct a local map using multiple key frames. Based on the local map, perform frame-to-map matching using the feature frames of the current moment to obtain the UAV status at the next moment.
[0152] S5. The drone flies based on the current drone status obtained at that moment.
[0153] In S1, the IMU motion model is used to estimate the coordinate transformation relationship between two frames of the lidar point cloud. Based on the coordinate transformation relationship, all the point clouds in a frame of lidar point cloud are transformed to the lidar coordinate system corresponding to the beginning of that frame.
[0154] The conversion process is represented as follows:
[0155]
[0156] The rotation matrix The IMU motion model is obtained by integrating the IMU motion model, which is expressed as follows:
[0157]
[0158]
[0159]
[0160] In S2, during the downsampling process, points with intensity below a threshold are filtered out, where the intensity is represented as:
[0161]
[0162] Furthermore, points with duplicate voxel indices are filtered out, so that each voxel index corresponds to only one point.
[0163] In S3, the surface smoothness is expressed as:
[0164]
[0165] In S3, a frame of LiDAR point cloud is divided into four geometrically equal parts. By setting edge thresholds and planar thresholds, each part of the LiDAR point cloud can generate a maximum of two edge points and four planar points. In the threshold selection, the flight distance is set to 1m and the flight direction is set to 10°.
[0166] S4 includes the following steps:
[0167] S41. Transform the feature frame to the world coordinate system;
[0168] S42. Construct a local map using consecutive keyframes prior to the current time.
[0169] S43. Obtain the Euclidean distance between the edge point in the current frame and the edge line of that point in the local map, and the Euclidean distance between the plane point in the current frame and the plane corresponding to that point in the local map. Sum the two Euclidean distances to construct the error equation.
[0170] S44. Minimize the error equation to obtain the UAV state transition matrix;
[0171] S45. Use the UAV state transition matrix to transform the UAV state at the previous moment to obtain the UAV state at the current moment.
[0172] In S42, a local map is constructed using 25 consecutive key frames. In S43, the Euclidean distance between an edge point in the current frame and the edge line of that point in the local map is calculated. Represented as:
[0173]
[0174] Euclidean distance between a plane point in the current frame and the plane corresponding to that point in the local map Represented as:
[0175]
[0176] In S44, the UAV state transition matrix corresponding to minimizing the error equation is obtained. , represented as:
[0177]
[0178] The solution is obtained using the Gauss-Newton method.
[0179] Comparative Example 1
[0180] The same experiment as in Example 1 was conducted, except that GPS navigation was used.
[0181] Comparative Example 2
[0182] The same experiment as in Example 1 was conducted, except that a DJI visual odometry was used.
[0183] The DJI visual odometry in Comparative Example 2 is the drone's built-in odometry, combining measurements from GPS, camera, and IMU. It has the highest accuracy and can be used as a reference standard, i.e., the "true value." The results from Example 1, Comparative Example 1, and Comparative Example 2 are compared, and the results are as follows: Figure 2-8 As shown, where,
[0184] Figure 2 A three-dimensional trajectory comparison diagram is shown. Figure 3 A comparison diagram of trajectories in the x-direction is shown. Figure 4 A comparison diagram of trajectories in the y-direction is shown. Figure 5 A comparison diagram of trajectories in the z-direction is shown. Figure 6 The motion deviation of Example 1 and Comparative Example 1 relative to Comparative Example 2 in the x-axis direction is shown. Figure 7 The motion deviation of Example 1 and Comparative Example 1 relative to Comparative Example 2 in the y-axis direction is shown. Figure 8 The motion deviation of Example 1 and Comparative Example 1 relative to Comparative Example 2 in the z-axis direction is shown.
[0185] from Figure 2-8 As can be seen, the results obtained in Example 1 almost completely coincide with the true value (Comparative Example 2) in the x and y directions, with a maximum deviation of no more than 0.2m. In the z-axis direction, the results obtained in Example 1 show some fluctuations compared with the true value, but the fluctuation range is small, with a maximum deviation of no more than 0.5m. In contrast, the results obtained in Comparative Example 1 show a maximum deviation of more than 1m from the true value in the x and y directions. Although the deviation in the z-axis direction is small, the overall effect of Comparative Example 1 is worse than that of Example 1. Therefore, the method in Example 1 can achieve relatively accurate navigation in a GNSS-free environment.
[0186] The present invention has been described above with reference to preferred embodiments; however, these embodiments are merely exemplary and illustrative. Various substitutions and modifications can be made to the present invention based on these embodiments, all of which fall within the scope of protection of the present invention.
Claims
1. A navigation method using a laser-inertial odometry system for unmanned aerial vehicles (UAVs), characterized in that, Includes the following steps: S1. Transform the lidar point cloud by converting all the point clouds in a frame of lidar point cloud to the lidar coordinate system corresponding to the beginning of that frame. S2. Downsample the lidar point cloud to reduce the number of lidar point clouds; S3. Extract the feature frames of each frame in the lidar point cloud. The feature frames include edge points and planar points. The edge points are points in the point cloud with a surface smoothness greater than an edge threshold, and the planar points are points in the point cloud with a surface smoothness less than a planar threshold. Compare the change in the state of the aircraft in the corresponding time frame of the LiDAR point cloud with the selection threshold, and take the feature frame corresponding to the second frame in the consecutive frames where the change is greater than the selection threshold as the key frame. S4. Construct a local map using multiple key frames. Based on the local map, perform frame-to-map matching using the feature frames of the current moment to obtain the UAV status at the next moment. S5. The drone flies based on the current drone status obtained at that moment; In S3, the surface smoothness is expressed as: in, Indicates the first The first frame Surface smoothness at each point, This represents the set of points where the laser beam returns within the same frame. express In addition to Other points, Indicates the first The coordinates of a point in the lidar coordinate system; S4 includes the following steps: S41. Transform the feature frame to the world coordinate system; S42. Construct a local map using consecutive keyframes prior to the current time. S43. Obtain the Euclidean distance between the edge point in the current frame and the edge line of that point in the local map, and the Euclidean distance between the plane point in the current frame and the plane corresponding to that point in the local map. Sum the two Euclidean distances to construct the error equation. S44. Minimize the error equation to obtain the UAV state transition matrix; S45. Use the UAV state transition matrix to transform the UAV state at the previous moment to obtain the UAV state at the current moment; In S43, the Euclidean distance between an edge point in the current frame and the edge line of that point in the local map is... Represented as: Euclidean distance between a plane point in the current frame and the plane corresponding to that point in the local map Represented as: in, Indicates the current moment. The first point in the lidar point cloud at the current moment The coordinates of the edge points For local maps Mid-edge point The two edge points on the corresponding edge lines; The first point in the lidar point cloud at the current moment The coordinates of a point on the plane Local maps mid-plane point A point on the corresponding plane and a line; In S44, the UAV state transition matrix corresponding to minimizing the error equation is obtained. , represented as: 。 2. The navigation method for UAV laser-inertial odometry according to claim 1, characterized in that, In S1, the IMU motion model is used to estimate the coordinate transformation relationship between two frames of the lidar point cloud. Based on the coordinate transformation relationship, all the point clouds in a frame of lidar point cloud are transformed to the lidar coordinate system corresponding to the beginning of that frame.
3. The navigation method for UAV laser-inertial odometry according to claim 2, characterized in that, The conversion process is represented as follows: in, This indicates the index of a point in the lidar point cloud. This indicates the start time of the lidar point cloud frame. Indicates the first point in the lidar point cloud The time corresponding to each point Indicates the first point in the lidar point cloud The coordinates obtained when a point is transformed to the coordinate system at the beginning of this frame. Indicates the first point in the lidar point cloud The points at The original coordinates of the moment. express Time-based lidar coordinate system The lidar coordinate system at the initial moment of this frame The rotation matrix, express Time's up The centroid translation vector of the drone.
4. The navigation method for UAV laser-inertial odometry according to claim 1, characterized in that, In S2, during the downsampling process, points with intensity below a threshold are filtered out, where the intensity is represented as: in, This represents the point cloud reflection intensity value. The distance from the point to the lidar. Let be the normalized intensity of the reflection at a point.
5. The navigation method for UAV laser-inertial odometry according to claim 1, characterized in that, In S2, during the downsampling process, points with duplicate voxel indices are filtered out, so that each voxel index corresponds to only one point.
6. The navigation method for UAV laser-inertial odometry according to claim 1, characterized in that, In S3, a frame of LiDAR point cloud is divided into geometrically equal parts. The system divides the LiDAR point cloud into segments and sets edge and planar thresholds to ensure that each segment can contain a maximum of only a few points. edge points and Plane point.