Indoor unmanned aerial vehicle positioning and navigation method based on tight coupling of laser radar and IMU

Abstract

The invention discloses an indoor unmanned aerial vehicle positioning and navigation method based on tight coupling of a laser radar and an IMU, and the method comprises the steps: collecting data based on the laser radar and the IMU, and carrying out the down-sampling processing of the data; packaging the radar data to be processed currently and the IMU data, synchronizing the radar data and the IMU data, and confirming that the radar data and the IMU data are aligned in time sequence to obtain radar and IMU data streams; taking the obtained radar and IMU data streams as input to carry out forward propagation and integration to obtain rough pose estimation, then carrying out motion compensation based on back propagation, and removing radar point cloud distortion to obtain a current state; evaluating whether the current state estimation is accurate or not based on residual calculation; and if not, estimating pose transformation by using iterative error Kalman filtering until the increment is smaller than a threshold value, then mapping according to the pose and updating the feature map, and carrying out unmanned aerial vehicle positioning navigation. According to the method, radar and IMU data are fused, and high-precision low-drift tight coupling mapping is realized through time sequence alignment, filtering, motion compensation and iterative optimization.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-sensor fusion SLAM, and particularly relates to an indoor UAV positioning and navigation method based on the tight coupling of lidar and IMU. Background Technique

[0002] As an important component of new energy vehicles, special-shaped pipes need to meet various requirements such as hydrolysis resistance, high temperature resistance, and lightweight. Parts such as battery packs, motors, and controllers are connected by cooling water pipes. The cooling water therein serves as a carrier of heat. When the working temperature of the parts is too high, the cooling water takes out the excess heat and dissipates it through radiators, battery coolers, etc. When the working temperature of the parts is too low, the cooling water can bring heat from parts such as water heaters and water-cooled condensers.

[0003] In the application of indoor UAV autonomous navigation, SLAM (Simultaneous Localization and Mapping) technology is crucial. In an indoor environment where GPS signals cannot be effectively covered, UAVs need to rely on SLAM technology to achieve autonomous positioning and navigation. SLAM technology estimates the motion trajectory of the UAV in real time through sensors installed on the carrier and constructs an environmental map, providing accurate positioning information and environmental perception for the UAV.

[0004] As a sensor capable of actively detecting the surrounding environment, lidar has the characteristics of high measurement accuracy, strong resistance to light interference, and good environmental adaptability, and thus has been widely used in the field of SLAM. Lidar emits laser pulses and receives reflected echoes, and can generate accurate three-dimensional point cloud data for constructing the geometric information of the environment. However, when lidar data is used alone for SLAM, it is easily affected by distortion caused by motion, especially when the UAV is flying rapidly and the attitude changes greatly, which may affect the accuracy of pose estimation. Therefore, it is extremely meaningful to study the tight coupling fusion technology of lidar and IMU. Summary of the Invention

[0005] The purpose of the present invention is to overcome the deficiencies of the prior art. To achieve the above purpose, an indoor UAV positioning and navigation method based on the tight coupling of lidar and IMU is adopted to solve the problems raised in the above background technique.

[0006] An indoor UAV positioning and navigation method based on the tight coupling of lidar and IMU includes the following steps:

[0007] Step S1, collect data based on lidar and IMU, and perform downsampling processing on the data;

[0008] Step S2: Pack the radar data and IMU data to be processed, synchronize the radar data and IMU data, confirm that they are aligned in time sequence, and obtain the radar and IMU data streams;

[0009] Step S3, using the obtained radar and IMU data streams as input for forward propagation, integrating to obtain a rough pose estimate, and then performing motion compensation based on back propagation to remove radar point cloud distortion and obtain the current state;

[0010] Step S4: evaluate whether the current state estimation is accurate based on residual calculation; if the residual does not meet the preset threshold range, use the iterative error Kalman filter to estimate the posture transformation until the increment of each iteration is less than the threshold, then build a map based on the posture and update the feature map to perform drone positioning and navigation.

[0011] As a further solution of the present invention: the specific steps in step S1 include:

[0012] Step S11, collecting data through IMU, and performing timestamp increment verification and buffer management on the collected IMU data to confirm the IMU data timing;

[0013] Step S12: collecting data through a solid-state laser radar, and performing time stamp storage and real-time downsampling on the collected radar data;

[0014] Step S13: downsample each frame of radar point cloud by fusion of voxel filtering and pass-through filtering, extract key feature points, and output high-quality feature point cloud.

[0015] As a further solution of the present invention: the specific steps in step S2 include:

[0016] Step S21, detecting whether there is data in the radar data and IMU data buffers in step S1, if there is no data, it indicates synchronization failure;

[0017] When there is data, the first frame of radar data in the radar buffer and the timestamp are taken out and stored in the radar part and time part of the data synchronization packaging area. The scanning time n is counted, and the start time of this frame of radar data is added to the time difference between the last point of this frame of radar data and the first point to calculate the end time and store it. Then, according to the mean time of this radar scanning time, n Update the average scan time, sample incremental average calculation method, smooth adjustment of the average scan time at the end of each new scan, more accurately reflect the actual radar scan time interval; calculate the new average scan time mean n+1 , the formula is:

[0018]

[0019] Where, Tnew Indicates the first scan time;

[0020] Step S22: Determine whether the timestamp of the latest IMU data is greater than the end time of this frame of lidar in step S21;

[0021] If so, store the IMU with a timestamp earlier than the lidar end timestamp in the IMU part of the data synchronization and packaging area, confirm that each frame of IMU data is within the time range of this frame of lidar scan, and then remove the processed data and timestamp from the data buffer. Then the synchronization and packaging are completed, and enter the next synchronization;

[0022] Otherwise, it means that not enough IMU data is collected, indicating that the synchronization fails and continue to collect and resynchronize.

[0023] As a further solution of the present invention: The specific steps in step S3 include:

[0024] Step S31: Construct a 24-dimensional state vector and initialize the covariance matrix to provide the initial state and error model, where the state vector includes pose, extrinsic parameters, bias, and gravity;

[0025] Among them, the covariance calculation formula is:

[0026]

[0027] In the formula, n represents the count, V n Represents the covariance of all current data items, A n Represents the mean of all data items, X n Represents the current input data, the current acceleration covariance or the angular velocity covariance;

[0028] Step S32: Stitch cross-frame IMU data and sort the point cloud timestamps to determine the continuity of the time sequence;

[0029] Step S33: Use median integration to correct the IMU bias, propagate the pose forward based on the extended Kalman filter, and output a rough pose estimate;

[0030] Step S34: Inherit the state of the last frame of IMU to the next frame, maintain the cross-frame state transfer chain, ensure seamless connection between the end state of the S3.5 backward compensation and the start state initialized in the next frame, and eliminate the pose accumulation error;

[0031] Step S35: Back-project the point cloud based on the IMU integration trajectory to the end of the scan to eliminate the motion distortion caused by the lidar scan time difference, and obtain the undistorted point cloud observation data.

[0032] As a further solution of the present invention: The specific steps in step S4 include:

[0033] Step S41, quantifying the position deviation between the point cloud observation data and the map features after compensation through the residual model, iteratively optimizing and defining the objective function, and converting the observation error into a state correction amount;

[0034] Step S42, linearize the observation equation and calculate the Jacobian matrix, establish the gradient relationship between the state estimation and the residual, obtain directional correction information, and drive the state to iterate towards the optimal solution;

[0035] Step S43, using iterative error Kalman filtering to dynamically adjust the Kalman gain, minimize the residual until convergence, output high-precision pose and update the global map;

[0036] Among them, the formula for iteratively updating the Kalman gain is:

[0037] K=AH T (HAH T +R) -1

[0038] Among them, A is the covariance matrix updated in the iterative process, R is the observed noise covariance, and H is the Jacobian matrix;

[0039] Iterative Error Kalman Filter improves state estimation by minimizing the combined error of prior state estimate and observation residual The accuracy of the iteration formula is:

[0040]

[0041] Compared with the prior art, the present invention has the following technical effects:

[0042] By adopting the above technical solution, the processing speed is improved and the redundant points caused by the glass environment are processed. In terms of extracting feature points, the downsampling method of fusing voxel filtering and through filtering is effectively solved.

[0043] Dynamic time synchronization of multiple frames of data between IMU and LiDAR frames is performed to reduce the impact of time error on state estimation through integration and compensation. Each radar point is converted between the IMU coordinate system and the world coordinate system, and motion distortion is dynamically compensated, which can significantly reduce the impact of motion distortion in fast motion.

[0044] Using a tightly coupled algorithm, this method directly combines the high-frequency, short-time interval motion information of the IMU with the spatial data of the lidar point cloud. The advantages and disadvantages of the two sensors complement each other. This method reduces the cumulative error and drift in the loosely coupled scheme, and has significant improvements in accuracy and computational efficiency compared to traditional methods.

[0045] Iterative error Kalman filtering is used to optimize the IMU and lidar, and the iterative convergence is judged by setting a threshold to ensure the optimal pose estimation, thereby improving the overall registration accuracy and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] The specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings:

[0047] Figure 1 This is a schematic diagram of the steps of the indoor drone positioning and navigation method according to the embodiment disclosed in the present application;

[0048] Figure 2 This is a flow chart of the indoor drone SLAM technology of the disclosed embodiment of the present application. DETAILED DESCRIPTION

[0049] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0050] Please refer to Figure 1 and Figure 2 In an embodiment of the present invention, a method for positioning and navigating an indoor UAV based on tight coupling of a laser radar and an IMU comprises the following steps:

[0051] Step S1: Collect data based on LiDAR and IMU, and downsample the data.

[0052] In the specific implementation steps, data is collected through the solid-state laser radar mid-360 and the built-in ICM40609. Due to the huge amount of radar point cloud data, down-sampling is performed through a method that combines voxel filtering and pass-through filtering to reduce the number of points and propose feature points;

[0053] The specific steps in step S1 include:

[0054] Step S11, collecting data through IMU, and performing timestamp increment verification and buffer management on the collected IMU data to confirm the IMU data timing;

[0055] Specifically, each frame of data collected by the IMU is stored in an IMU data buffer for subsequent processing. The timestamp of each frame of IMU data received and processed cannot be smaller than the timestamp of the previous frame of IMU data, otherwise the IMU data buffer is cleared. If the timestamp becomes smaller, it means that the time sequence of the IMU message has been reversed, which may be due to abnormal IMU data or device restart;

[0056] Step S12: Collect data through a solid-state lidar, and perform timestamp storage and real-time downsampling on the collected lidar data;

[0057] Specifically, the lidar data is received in a similar way to the above IMU data. At the same time, the timestamp of each frame of lidar data is stored in a time buffer. However, before storing each frame of lidar data in a lidar data buffer, the lidar data is downsampled once;

[0058] Step S13: Downsample each frame of lidar point cloud through a combined voxel filter and passthrough filter, extract key feature points, and output high-quality feature point clouds;

[0059] Specifically, downsample each frame of lidar point cloud through a combined voxel filter and passthrough filter. First, limit the x, y, and z of each point (the range is set according to a specific scenario), and only retain the points in a specific area. Since it is indoors and the overall range is limited, redundant points can be effectively removed for indoor glass walls. Then set the size of the voxel. Here, a voxel grid filter is used, and the side length of each voxel is 0.05 meters. Filter the point cloud processed above, and group adjacent points in the point cloud into a voxel, thereby reducing the number of points and retaining features. Smaller voxels will retain more points, and larger voxels will result in greater compression of the point cloud.

[0060] Step S2: Package the currently processed lidar data and IMU data, and synchronize the lidar data and IMU data to confirm alignment in time sequence to obtain lidar and IMU data streams;

[0061] In the specific implementation steps, package the currently processed lidar data and IMU data, and synchronize the lidar data and IMU data to ensure alignment in time for subsequent fusion processing. That is, synchronize a set of data including a frame of point cloud and all IMU data within the scanning time of this frame of point cloud;

[0062] The specific steps in Step S2 include:

[0063] Step S21: Detect whether there is data in the lidar data and IMU data buffers in Step S1. If there is no data, it means the synchronization fails;

[0064] When there is data, take out the first frame of lidar data and timestamp in the lidar buffer and store them in the lidar part and time part of the data synchronization packaging area. Count the scanning time n, and use the start time of this frame of lidar data plus the time difference between the last point and the first point of this frame of lidar data to calculate the end time and store it. Then, according to the mean of the current lidar scan time nUpdate the average scan time. By sampling the incremental average value calculation method, the average scan time is smoothly adjusted at the end of each new scan, which can more accurately reflect the actual radar scan time interval. Calculate the new average scan time mean using Equation (1). n+1 , the formula is:

[0065]

[0066] In the formula, T new represents the first scan time;

[0067] Step S22: Determine whether the timestamp of the latest IMU data is greater than the end time of this frame of radar in Step S21;

[0068] If so, store the IMU with a timestamp earlier than the radar end timestamp in the IMU part of the data synchronization packaging area, confirm that each frame of IMU data is within the time range of this frame of radar scan, and then remove the processed data and timestamp from the data buffer. Then the synchronization packaging is completed and enter the next synchronization;

[0069] Otherwise, it means that not enough IMU data has been collected, indicating that the synchronization fails and continue to collect and resynchronize.

[0070] Step S3: Use the obtained radar and IMU data streams as inputs for forward propagation, integrate to obtain a rough pose estimate, and then perform motion compensation based on backpropagation to remove the distortion of the radar point cloud and obtain the current state;

[0071] In the specific implementation steps, use the IMU data in the packaging area of Step S22 as the input for forward propagation, integrate to obtain a rough pose estimate, and perform motion compensation through backpropagation to remove the distortion of the radar point cloud;

[0072] The specific steps in Step S3 include:

[0073] Step S31: Construct a 24-dimensional state vector and initialize the covariance matrix to provide the initial state and error model, where the state vector includes pose, extrinsic parameters, bias, and gravity;

[0074] Specifically, ensure that the data in the packaging area exists, initialize the IMU, and initialize a 24x1-dimensional state vector x = W p I T , W q I T , I q L T , I t L T ,W v I T , b ω T , b a T , W g T T , where W p I T , W v I T , W q I T represents the position, velocity, and attitude from the IMU coordinate system to the world coordinate system. Here, the world coordinate system refers to the coordinate system of the first frame of point cloud as the world coordinate system. I q L T , I t L T represents the rotation and translation from the lidar coordinate system to the IMU coordinate system, b ω T , b a T represents the biases of the IMU gyroscope and accelerometer. W g T represents the gravitational acceleration. First, use the acceleration value and angular velocity value of the first frame as the initial mean, and at the same time, use the start time of the lidar corresponding to the current IMU frame as the initial time. Then traverse all IMU data within the scan time of this frame of point cloud and calculate the mean and covariance. The mean sampling adopts an incremental average calculation method in step 2.1;

[0075] Among them, the covariance calculation formula is:

[0076]

[0077] In the formula, n represents the count, V n represents the covariance of all current data items, A n represents the mean of all data items, X n represents the current input data, the current acceleration covariance, or the angular velocity covariance;

[0078] ​Then obtain the current state. First, update the 24x1-dimensional state vector x, calculate the gravity direction, normalize it, and store it in the current state. Use the mean angular velocity calculated above as the gyroscope bias of the current state and store it. Also store the rotation and translation extrinsic parameters from the radar coordinate system to the IMU coordinate system in the current state. Then, initialize and obtain the covariance matrix p between the 24x24-dimensional true state and the estimated state, and finally transmit it and record the last IMU data in the current radar frame. The initialization process should be executed only once when the IMU data is received for the first time, which can be ensured by limiting the maximum number of iterations.

[0079] Step S32: Stitch cross-frame IMU data and sort point cloud timestamps to determine the continuity of the time sequence;

[0080] Specifically, take out the IMU queue of the current radar frame, the tail IMU time, and the timestamps at the start and end of the radar point cloud. Add the last tail IMU of the previous radar frame to the head IMU of the current frame, and reorder the points according to the timestamp of each point in the point cloud of the current radar frame to obtain the previous state vector x i-1 As the initial state vector for this IMU prediction, it includes the time interval, the previous frame's acceleration, the previous frame's angular velocity, the previous frame's velocity, the previous frame's position, and the previous frame's rotation matrix, which are stored in a list;

[0081] Step S33: Use median integration to correct the IMU bias and propagate the pose forward based on the extended Kalman filter to output a rough pose estimate;

[0082] Specifically, the data directly measured by the IMU is affected by bias and noise and has errors. Therefore, it is necessary to traverse the estimated IMU measurements of this time and perform integration. Note that the first data of the IMU in this radar frame traversed at this time is the last IMU data of the previous radar frame, and the last data is the second-to-last IMU data of this radar frame, because the last frame is reserved for the next radar frame as the first IMU data.

[0083] First, obtain the IMU data of the current frame and the next frame, and determine whether the timestamp of the next frame of IMU is earlier than the end time of the previous frame of lidar. If so, skip this loop (i.e., ignore the IMU data of this frame) and continue to process the IMU data with the next timestamp. For each pair of adjacent IMU data, use median integration to calculate the average values of angular velocity and acceleration. Correct the acceleration by dividing it by the initial value of acceleration and multiplying by the gravitational acceleration constant, and adjust the acceleration based on the gravitational value. Then calculate the time interval dt between the current two frames of IMU data. If the IMU data of the current frame is earlier than the end time of the previous lidar frame, calculate the time difference starting from the end time of the previous lidar frame. Next, perform the forward propagation of the extended Kalman filter. There are three input quantities, taking the average acceleration and angular velocity obtained through the above median integration as inputs, as well as the configured 12x12-dimensional white noise covariance matrix Q and the time interval dt between the two frames of IMU data as inputs, and the output is the 24x1-dimensional rough state vector of the next frame of IMU And the covariance matrix between the true state and the estimated state of 24x24 dimensions

[0084] The rough state vector is obtained according to Equation (3):

[0085]

[0086] The noise quantity in Equation (3) is 0 during the propagation process, and f is the motion formula, which represents the state increment equation obtained based on the measurement values of the IMU in the current state. The operator has the following meaning:

[0087]

[0088] In Equation (4), Μ is a manifold in an n-dimensional space, and × represents an operation. The specific meaning of the operator is determined by the form of the operand in front of the operator. If it is an n-dimensional vector, direct addition and subtraction operations are performed. If it is an n-dimensional matrix, the operation is performed through Equation (4) above;

[0089] The covariance of the error between the true state and the estimated state is obtained from Equation (5):

[0090]

[0091] In Equation (5), is the Jacobian matrix of the current state , and F e is the Jacobian matrix of the system noise. During the forward propagation process and F e are also continuously updated;

[0092] Step S34: Inherit the last-frame IMU state to the next frame, maintain the cross-frame state transfer chain, ensure seamless connection between the end state of the S3.5 backward compensation and the start state initialized in the next frame, and eliminate the pose accumulation error;

[0093] Specifically, after the forward propagation is completed, update the IMU state to the integrated state, that is, extract the estimated state vector of the next-frame IMU in step S33, and update the angular velocity and acceleration in the world coordinate system measured in the next frame. Obtain the time interval between the next-frame IMU time and the start time of this radar. Store this time interval, the updated acceleration and angular velocity, as well as the velocity, position, and rotation matrix obtained by updating the state vector, into a list. Since it is emphasized in step S33 that the tail data is the IMU data of the second-to-last frame in this radar frame, the last-frame IMU data is also supplemented and processed. Similar to the above, obtain the time interval, then perform forward propagation to obtain the covariance matrix of the error between the new state vector and the true state and the estimated state, and then update the state vector. Finally, save the last-frame IMU data for use as the first-frame IMU data in the next-frame radar, and also save the measurement time of the last radar point cloud in this frame for reference in the IMU data of the next-frame radar;

[0094] Step S35: Back-project the point cloud based on the IMU integration trajectory to the end of the scan to eliminate the motion distortion caused by the lidar scan time difference and obtain the undistorted point cloud observation data.

[0095] Specifically, since a frame of lidar data consists of many points, and these points are obviously not measured at the same time, it is necessary to compensate for the motion error caused by the time difference. To solve the problem that the obtained data is the sampling of all points at the same moment, start the backward propagation, that is, motion compensation. Determine whether the point cloud is empty by judging whether the first point cloud data in this frame of radar is equal to the last point cloud data.

[0096] Mainly perform backward propagation based on the fact that the pose of the point cloud in the world coordinate system before compensation is the same as that of the point cloud in the world coordinate system after compensation. Start traversing forward from the last element of the IMU data list in step S34, and extract the acceleration and angular velocity of the current frame of IMU and the rotation matrix, velocity, and position of the previous frame respectively. Since the point cloud data is sorted, also traverse forward from the last point cloud in this radar frame, and judge whether the time of the point cloud is greater than the time of the previous-frame IMU. If so, perform compensation; otherwise, skip to the IMU of the frame before the previous frame for compensation.

[0097] Compensation start: First, transfer this time point to the IMU coordinate system, and then transfer it to the world coordinate system according to the rotation and offset of the IMU at this time point; then project this point to the position at the end time of this frame of lidar scan. First, transfer it to the IMU coordinate system, and then transfer it to the world coordinate system according to the rotation and offset of the IMU at the end time of this point. Focus on the fact that the pose of the point cloud before compensation in the world coordinate system is the same as the pose of the point cloud after compensation in the world coordinate system, and inversely obtain the position where this point is projected to the end time of this frame of lidar scan.

[0098] The pose equality formula before and after compensation can be obtained from Equation (6):

[0099]

[0100] In Equation (6) represents the rotation of the IMU at the time when the point is located and the rotation of the IMU at the end time where the point is to be projected; represents the position of the lidar coordinate system at the time when the point is located and the position of the lidar coordinate system at the end time where the point is to be projected; represents the world position of the IMU at the time when the point is located and the world position of the IMU at the end time where the point is to be projected;

[0101] The position where this point is projected to the end time of this frame of lidar scan can be obtained from Equation (7):

[0102]

[0103] The motion distortion of the lidar has been compensated up to this point.

[0104] Step S4: Evaluate whether the current state estimation is accurate based on the residual calculation; when the residual does not meet the preset threshold range, use the iterative error Kalman filter to estimate the pose transformation until the increment of each iteration is less than the threshold, and then build a map based on the pose and update the feature map for UAV positioning and navigation.

[0105] Specifically, calculate the residual. The purpose of residual calculation is to evaluate whether the current state estimation is accurate and whether it needs to be corrected according to new observation data. When the residual is large, it means that there is a large deviation between the current state estimation and the actual observation data, and the filter will adjust the state variables to reduce this deviation. Here, use the iterative error Kalman filter to estimate the pose transformation until the increment of each iteration is less than the threshold, and then build a map based on the pose and update the feature map;

[0106] The specific steps in Step S4 include:

[0107] Step S41: Quantify the pose deviation between the compensated point cloud observation data and the map features through the residual model, iteratively optimize the defined objective function, and convert the observation error into a state correction amount;

[0108] Specifically, first convert the compensated feature points to the global coordinate system, and then calculate the residual. According to the following formula:

[0109]

[0110] In formulas (8) and (9), represents the pose transformation to be solved, represents the world coordinates of the compensated points, w E j represents a point on a plane or an edge, and G j is the normal vector of the plane or the edge, represents the residual; the calculation of the above residual is similar to that in LOAM, and the point-plane distance and point-line distance are used as the residuals.

[0111] Step S42: Linearize the observation equation and calculate the Jacobian matrix, establish the gradient relationship between the state estimate and the residual, obtain the directional correction information, and drive the state to iterate towards the optimal solution;

[0112] Specifically, theoretically, using the true value x of the state quantity k and the true value of the radar point after removing the ranging noise its residual should be zero, that is its h(x,n) is a function of the observation equation. Linearize it and expand it around (that is, at because ):

[0113]

[0114] In formula (10), is the Jacobian matrix of h(x,n) with respect to v j comes from the original measurement noise

[0115] Step S43: Use the iterative error Kalman filter to dynamically adjust the Kalman gain, minimize the residual until convergence, output the high-precision pose and update the global map;

[0116] Among them, the formula for iteratively updating the Kalman gain is:

[0117] K = AH T (HAH T + R) -1 (11)

[0118] Among them, A is the covariance matrix updated during the iteration, R is the noise covariance of the observation, and H is the Jacobian matrix; ​

[0119] Iterative error Kalman filtering improves state estimation by minimizing the combined error of the prior state estimate and the observation residual. The accuracy is as follows; the iterative formula is:

[0120]

[0121] In each iteration, the filter adjusts the state estimate based on the observation residual using the Kalman gain K. This iterative process is repeated until it is less than a threshold, indicating convergence. Obtain the optimal state estimate, convert it into the optimal estimate from the IMU coordinate system to the world coordinate system, then transform each feature point into the world coordinate system, and then insert it into the global map frame by frame. Finally, it achieves small drift, high positioning accuracy, much better mapping effect than loose coupling, and improved efficiency.

[0122] Although the embodiments of the present invention have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents and should be included within the protection scope of the present invention.

Claims

1. A method for indoor UAV positioning and navigation based on tight coupling of laser radar and IMU, characterized in that: The following steps are involved: Step S1, collecting data based on laser radar and IMU, and downsampling the data; Step S2: Pack the radar data and IMU data to be processed, synchronize the radar data and IMU data, confirm that they are aligned in time sequence, and obtain the radar and IMU data streams; Step S3, using the obtained radar and IMU data streams as input for forward propagation, integrating to obtain a rough pose estimate, and then performing motion compensation based on back propagation to remove radar point cloud distortion and obtain the current state; Step S4: evaluate whether the current state estimation is accurate based on residual calculation; if the residual does not meet the preset threshold range, use the iterative error Kalman filter to estimate the posture transformation until the increment of each iteration is less than the threshold, then build a map based on the posture and update the feature map to perform drone positioning and navigation.

2. According to claim 1, a method for indoor UAV positioning and navigation based on tight coupling of laser radar and IMU is characterized in that: The specific steps in step S1 include: Step S11, collecting data through IMU, and performing timestamp increment verification and buffer management on the collected IMU data to confirm the IMU data timing; Step S12: collecting data through a solid-state laser radar, and performing time stamp storage and real-time downsampling on the collected radar data; Step S13: downsample each frame of radar point cloud by fusion of voxel filtering and pass-through filtering, extract key feature points, and output high-quality feature point cloud.

3. According to claim 1, a method for indoor UAV positioning and navigation based on tight coupling of laser radar and IMU, characterized in that: The specific steps in step S2 include: Step S21, detecting whether there is data in the radar data and IMU data buffers in step S1, if there is no data, it indicates synchronization failure; When there is data, the first frame of radar data in the radar buffer and the timestamp are taken out and stored in the radar part and time part of the data synchronization packaging area. The scanning time n is counted, and the start time of this frame of radar data is added to the time difference between the last point of this frame of radar data and the first point to calculate the end time and store it. Then, according to the mean time of this radar scanning time, n Update the average scan time, sample incremental average calculation method, smooth adjustment of the average scan time at the end of each new scan, more accurately reflect the actual radar scan time interval; calculate the new average scan time mean n+1 , the formula is: Where, T new Indicates the first scan time; Step S22, determine whether the latest IMU data timestamp is greater than the radar end time of this frame in step S21; If yes, the IMU with a timestamp earlier than the radar end timestamp is stored in the IMU part of the data synchronization packaging area, confirming that each frame of IMU data is within the time range of this frame of radar scanning, and then remove the processed data and timestamp from the data buffer, then the synchronization packaging is completed and enter the next synchronization; Otherwise, it means that not enough IMU data has been collected, indicating synchronization failure and continuing to collect and resynchronize.

4. According to claim 1, a method for indoor UAV positioning and navigation based on tight coupling of laser radar and IMU is characterized in that: The specific steps in step S3 include: Step S31, constructing a 24-dimensional state vector and initializing a covariance matrix to provide an initial state and error model, wherein the state vector includes posture, external parameters, deviation, and gravity; The covariance calculation formula is: In the formula, n represents the count, V n Represents the covariance of all current data items, A n represents the mean of all data items, X n Represents the current input data, current acceleration covariance or angular velocity covariance; Step S32, splicing cross-frame IMU data and sorting point cloud timestamps to determine the continuity of the time series; Step S33, using median integral to correct IMU deviation, and outputting a rough pose estimate based on the extended Kalman filter forward propagation pose; Step S34, inherit the IMU state of the last frame to the next frame, maintain the cross-frame state transfer chain, ensure that the end state of S3.5 reverse compensation is seamlessly connected with the starting state of the next frame initialization, and eliminate the cumulative error of posture; Step S35, based on the IMU integral trajectory, the point cloud is back-projected to the scanning end time, so as to eliminate the motion distortion caused by the laser radar scanning time difference and obtain the distortion-free point cloud observation data.

5. According to claim 1, a method for indoor UAV positioning and navigation based on tight coupling of laser radar and IMU, characterized in that: The specific steps in step S4 include: Step S41, quantifying the position deviation between the point cloud observation data and the map features after compensation through the residual model, iteratively optimizing and defining the objective function, and converting the observation error into a state correction amount; Step S42, linearize the observation equation and calculate the Jacobian matrix, establish the gradient relationship between the state estimation and the residual, obtain directional correction information, and drive the state to iterate towards the optimal solution; Step S43, using iterative error Kalman filtering to dynamically adjust the Kalman gain, minimize the residual until convergence, output high-precision pose and update the global map; Among them, the formula for iteratively updating the Kalman gain is: K=AH T (HAH T +R) -1 Among them, A is the covariance matrix updated in the iterative process, R is the observed noise covariance, and H is the Jacobian matrix; Iterative Error Kalman Filter improves state estimation by minimizing the combined error of prior state estimate and observation residual The accuracy of the iteration formula is:

Images