A tightly coupled and lightweight real-time positioning and mapping method

Abstract

The present invention proposes a tightly coupled lightweight real-time positioning and mapping method, including obtaining the state prediction value of the unmanned motion system through the inertial measurement unit IMU, and obtaining the original point cloud of the unmanned motion system through the laser radar; preprocessing the original point cloud; performing motion compensation on the preprocessed original point cloud through the state prediction value to obtain a dedistorted point cloud; combining the state prediction value and the dedistorted point cloud, updating the state of the unmanned motion system through Kalman filtering to obtain an optimal state estimate; and updating the map according to the optimal state estimate. The method proposed by the present invention, based on Kalman filtering, tightly couples the laser radar and the inertial navigation, and can achieve accurate and real-time motion estimation and mapping.

Description

Technical Field

[0001] The invention belongs to the field of positioning and mapping of unmanned systems. Background Art

[0002] For autonomous driving tasks, high-precision positioning information is particularly important as an input for subsequent decision-making and planning modules. In the field of positioning, especially in high-speed driving mission scenarios, high-frequency self-motion estimation is essential. To this end, technicians have proposed active sensors such as lidar to complete this task, the so-called simultaneous localization and mapping (SLAM). Some key advantages of typical 3D lidar include a wide horizontal field of view and invariance to ambient lighting conditions, but lidar-based navigation systems are sensitive to the surrounding environment. In addition, motion distortion and the sparsity of point clouds can degrade the performance of the algorithm in some challenging scenarios, such as wide open areas.

[0003] Recent studies have shown that the shortcomings of LiDAR can be compensated by fusing IMU. Unlike LiDAR, IMU is not sensitive to the environment. It provides accurate short-term motion constraints and usually works at high frequencies (e.g., 100Hz-500Hz). These features can help LiDAR navigation systems recover point clouds from highly dynamic motion distortions, thereby improving accuracy. Summary of the invention

[0004] The present invention aims to solve one of the technical problems in the related art at least to a certain extent.

[0005] To this end, the purpose of the present invention is to propose a tightly coupled lightweight real-time positioning and mapping method to help the lidar navigation system recover point clouds from highly dynamic motion distortions, thereby improving accuracy.

[0006] To achieve the above-mentioned purpose, the first embodiment of the present invention proposes a tightly coupled lightweight real-time positioning and mapping method, including:

[0007] Acquire a state prediction value of the unmanned motion system through an inertial measurement unit (IMU), and acquire an original point cloud of the unmanned motion system through a laser radar;

[0008] Preprocessing the original point cloud; performing motion compensation on the preprocessed original point cloud through the state prediction value to obtain a dedistorted point cloud;

[0009] Combining the state prediction value and the dedistorted point cloud, updating the state of the unmanned motion system through Kalman filtering to obtain an optimal state estimate;

[0010] The map is updated according to the optimal state estimate.

[0011] In addition, the tightly coupled lightweight real-time positioning and mapping method according to the above embodiment of the present invention may also have the following additional technical features:

[0012] Furthermore, in one embodiment of the present invention, obtaining a state prediction value of the unmanned motion system by using an inertial measurement unit IMU includes:

[0013] Collect acceleration and angular velocity data of unmanned motion systems in real time through IMU;

[0014] By integrating the acceleration and angular velocity data and combining the state estimation and motion equation at the previous moment, the position, velocity and posture information of the unmanned motion system at the current moment are obtained, which are used as the initial value for subsequent optimization; at the same time, according to the noise model of the unmanned motion system, the covariance matrix of the state estimation is updated to quantify the uncertainty of the prediction.

[0015] Furthermore, in one embodiment of the present invention, the preprocessing of the original point cloud includes:

[0016] Performing point cloud segmentation and feature extraction on the original point cloud; wherein,

[0017] The point cloud segmentation includes reprojecting the original point cloud into a range image, marking the points on the scan line between the preset angle range as ground points, and not participating in the subsequent segmentation; segmenting the non-ground points of the range image into multiple clusters, wherein the clusters with fewer points are removed as noise, and the points in the remaining clusters are marked as segmentation points;

[0018] The feature extraction includes extracting features from ground points and segmentation points. Specifically, for the point cloud P at time t, t Every point P in i The 5 points on the left and right sides of the distance image are combined into a point set S, and the roughness is calculated by the following formula:

[0019]

[0020] Among them, r i Indicates P i Distance to sensor, r j Indicates P j Distance to the sensor;

[0021] The distance image is horizontally divided into a number of sub-images; a number of points with the largest roughness are selected from the segmentation points in each row of the sub-images as edge feature points, and a number of points with the smallest roughness are selected from the ground points or segmentation points in each row of the sub-images as plane feature points.

[0022] Furthermore, in one embodiment of the present invention, performing motion compensation on the preprocessed original point cloud using the state prediction value includes:

[0023] The point cloud is transformed according to the state prediction value, and the point cloud is projected back to the position at the acquisition time, thereby removing the distortion of the point cloud in one frame.

[0024] Further, in one embodiment of the present invention, the state of the unmanned motion system is updated by Kalman filtering to obtain an optimal state estimation, including:

[0025] Comparing the dedistorted point cloud with the predicted state to obtain a measurement residual;

[0026] According to the covariance matrix of the measurement residual and the measurement noise, the Kalman gain is calculated to weigh the predicted value and the measured value;

[0027] Using the Kalman gain, the predicted value and the measurement residual are weighted averaged to obtain an optimized state estimate, and the covariance matrix is ​​updated at the same time;

[0028] Continuously optimize the state estimation of the point cloud; ultimately, through Kalman filtering, the state estimation of the unmanned motion system is continuously converged to the optimal value.

[0029] Furthermore, in one embodiment of the present invention, updating the map according to the optimal state estimation includes:

[0030] According to the optimal state estimation, the radar feature point Convert to the world coordinate system to get

[0031] in, represents the optimal estimate of the world coordinates of the radar feature point, Indicates t k The optimal pose estimation when I T L Indicates the relative position of IMU and radar, Indicates that the feature points obtained by the radar j-th scan are converted to t k The position in the radar coordinate system at that time;

[0032] The feature points Add to an existing map.

[0033] To achieve the above objectives, the second embodiment of the present invention proposes a tightly coupled lightweight real-time positioning and mapping device, comprising the following modules:

[0034] An acquisition module is used to acquire a state prediction value of the unmanned motion system through an inertial measurement unit IMU, and to acquire an original point cloud of the unmanned motion system through a laser radar;

[0035] A motion compensation module is used to preprocess the original point cloud; the preprocessed original point cloud is motion compensated by the state prediction value to obtain a dedistorted point cloud;

[0036] A state update module, used to combine the state prediction value and the dedistorted point cloud, and update the state of the unmanned motion system through Kalman filtering to obtain an optimal state estimate;

[0037] A map updating module is used to update the map according to the optimal state estimation.

[0038] To achieve the above-mentioned purpose, the third aspect of the present invention proposes a computer device, characterized in that it includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, it implements a tightly coupled lightweight real-time positioning and mapping method as described above.

[0039] To achieve the above-mentioned objectives, the fourth aspect of the present invention proposes a computer-readable storage medium on which a computer program is stored, characterized in that when the computer program is executed by a processor, a tightly coupled lightweight real-time positioning and mapping method as described above is implemented.

[0040] The tightly coupled lightweight real-time positioning and mapping method proposed in the embodiment of the present invention first obtains the state prediction value through the IMU for state propagation, and preprocesses the input radar point cloud, including point cloud segmentation and feature extraction. The preprocessed point cloud is then motion compensated through the state prediction value to remove distortion. Next, the state is updated through the Kalman filter to obtain the optimal state estimate. Finally, the map is updated and managed, and the updated map is used to register new points in the next round. Based on Kalman filtering, this method tightly couples the lidar and inertial navigation, and can achieve accurate and real-time motion estimation and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The above and / or additional aspects and advantages of the present invention will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0042] Figure 1 A schematic flow chart of a tightly coupled lightweight real-time positioning and mapping method provided by an embodiment of the present invention.

[0043] Figure 2A schematic diagram of the time relationship between IMU input and radar point cloud input within a radar frame provided by an embodiment of the present invention.

[0044] Figure 3 A flowchart of a tightly coupled lightweight real-time positioning and mapping method provided by an embodiment of the present invention.

[0045] Figure 4 A schematic diagram of a tightly coupled lightweight real-time positioning and mapping device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0046] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0047] The tightly coupled lightweight real-time positioning and mapping method according to an embodiment of the present invention is described below with reference to the accompanying drawings.

[0048] Figure 1 A schematic flow chart of a tightly coupled lightweight real-time positioning and mapping method provided by an embodiment of the present invention.

[0049] like Figure 1 As shown, the tightly coupled lightweight real-time positioning and mapping method includes the following steps:

[0050] S101: obtaining a state prediction value of the unmanned motion system through an inertial measurement unit IMU, and obtaining an original point cloud of the unmanned motion system through a laser radar;

[0051] S102: preprocessing the original point cloud; performing motion compensation on the preprocessed original point cloud through the state prediction value to obtain a dedistorted point cloud;

[0052] S103: combining the state prediction value and the dedistorted point cloud, updating the state of the unmanned motion system through Kalman filtering to obtain an optimal state estimation;

[0053] S104: updating the map according to the optimal state estimation.

[0054] Furthermore, in one embodiment of the present invention, obtaining a state prediction value of the unmanned motion system by using an inertial measurement unit IMU includes:

[0055] Collect acceleration and angular velocity data of unmanned motion systems in real time through IMU;

[0056] By integrating the acceleration and angular velocity data and combining the state estimation and motion equation at the previous moment, the position, velocity and posture information of the unmanned motion system at the current moment are obtained, which are used as the initial value for subsequent optimization; at the same time, according to the noise model of the unmanned motion system, the covariance matrix of the state estimation is updated to quantify the uncertainty of the prediction.

[0057] Specifically, first define the state x of the unmanned system as:

[0058]

[0059] Among them, the I system is the IMU coordinate system, which is also the unmanned system coordinate system; the G system is the world coordinate system, and the first IMU coordinate system is taken as the G system. G R I , G p I and They are the attitude, position and speed of the unmanned system in the world coordinates. and are the biases of the IMU angular velocity and acceleration measurements, G g is the unknown gravitational acceleration in the world coordinate system. The IMU input u is

[0060]

[0061] Among them, ω m Represents the IMU's measurement of angular velocity, a m Indicates the acceleration measured by IMU. Within a radar frame, the time relationship between IMU input and radar point cloud input is as follows: Figure 2 shown.

[0062] like Figure 2 As shown, the acquisition time of the kth radar frame is [t k-1 ,t k ]; In the kth frame, the time of IMU input is recorded as τ 1 ,τ 2 ,…τ i ,τ i+1 ,…; the radar scanning time is recorded as ρ 1 ,ρ 2 ,…ρ j-1 ,ρ j ,…ρ m , where ρ j-1 ∈[τ i-1 ,τ i ).

[0063] Once the IMU input state is received, the state is propagated once. Press to spread:

[0064]

[0065] in, Indicates that at time τ i+1 The state of propagation, Indicates that at time τ i The state of propagation, Δt = τ i+1 -τ i , represents the time difference between two consecutive IMU inputs, u i Indicates the time τ i IMU measurements, Indicates t k-1 The optimal estimate of the state at time represents an operation defined,

[0066]

[0067] Covariance of state errors Press to spread:

[0068]

[0069] in, express The covariance of the errors, express The covariance of the errors, express The covariance of the errors, and F w represents an operation defined,

[0070]

[0071]

[0072] in, Represents τ i The posture of the unmanned system in the world coordinates at the moment; Δt = τ i+1 -τ i , indicating the time difference; Represents τ i IMU angular velocity estimation at this moment; Represents τ i IMU acceleration estimate at that moment.

[0073] Furthermore, in one embodiment of the present invention, the preprocessing of the original point cloud includes:

[0074] Performing point cloud segmentation and feature extraction on the original point cloud; wherein,

[0075] The point cloud segmentation includes reprojecting the original point cloud into a range image, marking the points on the scan line between the preset angle range as ground points, and not participating in the subsequent segmentation; segmenting the non-ground points of the range image into multiple clusters, wherein the clusters with fewer points are removed as noise, and the points in the remaining clusters are marked as segmentation points;

[0076] The feature extraction includes extracting features from ground points and segmentation points. Specifically, for the point cloud P at time t, t Every point P in i The 5 points on the left and right sides of the distance image are combined into a point set S, and the roughness is calculated by the following formula:

[0077]

[0078] Among them, r i Indicates P i Distance to sensor, r j Indicates P j Distance to the sensor;

[0079] The distance image is horizontally divided into a number of sub-images; a number of points with the largest roughness are selected from the segmentation points in each row of the sub-images as edge feature points, and a number of points with the smallest roughness are selected from the ground points or segmentation points in each row of the sub-images as plane feature points.

[0080] Specifically, the point cloud segmentation converts a 3D point cloud into a 2D image, with a horizontal resolution of 360° / radar horizontal resolution, a vertical resolution of the number of radar lines, and a pixel value of the distance from the point to the sensor. Points on the scan line that appear within a certain angle range (e.g., [-15°, -1°]) are marked as ground points and do not participate in subsequent segmentation.

[0081] Subsequently, the non-ground points in the range image are segmented into many clusters, the clusters with fewer points are removed as noise, and the points in the remaining clusters are marked as segmentation points.

[0082] Furthermore, in one embodiment of the present invention, performing motion compensation on the preprocessed original point cloud using the state prediction value includes:

[0083] The point cloud is transformed according to the state prediction value, and the point cloud is projected back to the position at the acquisition time, thereby removing the distortion of the point cloud in one frame.

[0084] Specifically, we can get the following three formulas through propagation calculation: Convert the original point cloud from its own acquisition time to the end time of a frame:

[0085]

[0086] in, Indicates that the unmanned system is in ρ j-1 Time in I k The position in the coordinate system, Indicates that the unmanned system is in ρ j Time in I k The position in the coordinate system, Indicates that the unmanned system is in ρ j Time in I k The speed in the coordinate system, Δt represents the time interval between two radar scans, Indicates that the unmanned system is in ρ m Time in I k The position in the coordinate system.

[0087]

[0088] in, Indicates that the unmanned system is in ρ j-1 Time in I k The velocity in the coordinate system, Indicates that the unmanned system is in ρ j Time in I k The velocity in the coordinate system, Indicates that the unmanned system is in ρ j Time in I k The posture in the coordinate system, Indicated in τ i-1 The IMU measures the acceleration at represents the estimation of the IMU acceleration measurement bias in the kth radar frame, Δt represents the time interval between two radar scans, Indicates the number of radar frames in I k The gravitational acceleration in the coordinate system, Indicates that the unmanned system is in ρ m Time in I k The speed in the coordinate system.

[0089]

[0090] in, Indicates that the unmanned system is in ρ j-1 Time in I k The posture in the coordinate system, Indicates that the unmanned system is in ρ j Time in I k The posture in the coordinate system, Indicated in τ i-1 The IMU measures the angular velocity at time, represents the estimation of the IMU angular velocity measurement bias in the kth radar frame, Δt represents the time interval between two radar scans, Indicates that the unmanned system is in ρ m Time in I k The posture in the coordinate system.

[0091] Obtain After that, we get the unmanned system in ρ j relative to t k The pose transformation at time:

[0092]

[0093] Thus, the characteristic points obtained by the radar in the jth scan are converted to t k The position in the radar coordinate system at time:

[0094]

[0095] Among them, the relative posture of IMU and radar is known I T L , Indicates that the unmanned system is in ρ j relative to t k The posture change when Represents the feature points obtained by the j-th radar scan.

[0096] Further, in one embodiment of the present invention, the state of the unmanned motion system is updated by Kalman filtering to obtain an optimal state estimation, including:

[0097] Comparing the dedistorted point cloud with the predicted state to obtain a measurement residual;

[0098] According to the covariance matrix of the measurement residual and the measurement noise, the Kalman gain is calculated to weigh the predicted value and the measured value;

[0099] Using the Kalman gain, the predicted value and the measurement residual are weighted averaged to obtain an optimized state estimate, and the covariance matrix is ​​updated at the same time;

[0100] Continuously optimize the state estimation of the point cloud; ultimately, through Kalman filtering, the state estimation of the unmanned motion system is continuously converged to the optimal value.

[0101] First, set the initial value.

[0102] Let κ = -1, κ represents the number of iterations, Indicates t k The state of propagation.

[0103] Second, iterative calculation.

[0104] Let κ=κ+1, where κ represents the number of iterations.

[0105] Recalculate in At the end of the last iteration, we get Calculated by IMU state prediction.

[0106] Then, calculate

[0107] Next, calculate the residual

[0108] The original feature points are converted into Convert to the world coordinate system to get

[0109]

[0110] in, represents the world coordinates of the feature point obtained by the j-th radar scan calculated by the κ-th iteration of the Kalman filter, represents the t calculated by the K-th iteration of the Kalman filter k The posture at that time, I T L Indicates the relative position of IMU and radar, Indicates that the feature points obtained by the radar j-th scan are converted to t k For each lidar feature point, the residual is defined as the world coordinate estimated by the feature point. Distance to the nearest plane (or edge) in the map:

[0111]

[0112] in, is the residual, G j is the normal vector (or direction vector) of the corresponding plane (or edge), represents the world coordinates of the feature point estimate, Denotes a point on the corresponding plane (or edge). The nearest point in the map is searched by constructing a KD-tree of the points in the latest map. Only residuals with a norm below a certain threshold are considered, and residuals above this threshold are either outliers or newly observed points.

[0113] Then, calculate in is the residual, v j The measurement noise from the radar, represents the t calculated by the K-th iteration of the Kalman filter k Error in state.

[0114] Next, calculate the state update

[0115] make R = diag(R 1 ,…R m ),

[0116] calculate

[0117] K=PH T (HPH T +R) -1 ,

[0118]

[0119] in, represents the t calculated by the κ+1th iteration of the Kalman filter k The state of represents the t calculated by the kth iteration of the Kalman filter k The state of represents the t calculated by the K-th iteration of the Kalman filter k The residual error when Indicates that at time t k The state of the spread.

[0120] K=(H T R -1 H+P -1 ) -1 H T R -1 ,

[0121] Finally, convergence is determined.

[0122] if (∈ is a set small value), then the iteration stops and the optimal estimate of the output state is

[0123] Furthermore, in one embodiment of the present invention, updating the map according to the optimal state estimation includes:

[0124] According to the optimal state estimation, the radar feature points Convert to the world coordinate system to get

[0125]

[0126] in, represents the optimal estimate of the world coordinates of the radar feature point, Indicates t k The optimal pose estimation when I T L Indicates the relative position of IMU and radar, Indicates that the feature points obtained by the radar j-th scan are converted to t k The position in the radar coordinate system at that time;

[0127] The feature points Add to an existing map.

[0128] The above is a complete tightly coupled lightweight real-time positioning and mapping method process. Figure 3 It is a schematic diagram of the technical route of the present invention.

[0129] The tightly coupled lightweight real-time positioning and mapping method proposed in the embodiment of the present invention obtains the state prediction value through the IMU for state propagation, and preprocesses the input radar point cloud, including point cloud segmentation and feature extraction. The preprocessed point cloud is then motion compensated through the state prediction value to remove distortion. Next, the state is updated through the Kalman filter to obtain the optimal state estimate. Finally, the map is updated and managed, and the updated map is used to register new points in the next round. Based on Kalman filtering, this method tightly couples the lidar and inertial navigation, which can achieve accurate and real-time motion estimation and mapping.

[0130] Compared with the prior art, the advantages of the present invention are

[0131] 1) First, the state prediction value is obtained through the IMU for state propagation, and the input radar point cloud is preprocessed, including point cloud segmentation and feature extraction. The preprocessed point cloud is then motion compensated through the state prediction value to remove distortion. Next, the state is updated through the Kalman filter to obtain the optimal state estimate. Finally, the map is updated and managed, and the updated map is used to register new points in the next round. This method tightly couples the lidar and inertial navigation, which can achieve accurate and real-time motion estimation and mapping.

[0132] 2) Before point cloud segmentation, the ground points are marked so that they do not participate in subsequent segmentation, which greatly reduces the amount of calculation.

[0133] 3) The motion compensation method is to remove the distortion of the preprocessed point cloud through the state prediction value.

[0134] 4) The back-end optimization adopts the Kalman filtering method, so that the optimal estimate of the current state is only related to the variables at the previous moment, which improves the real-time performance of the calculation.

[0135] In order to implement the above embodiments, the present invention also proposes a tightly coupled lightweight real-time positioning and mapping device.

[0136] Figure 4 A schematic diagram of the structure of a tightly coupled lightweight real-time positioning and mapping device provided by an embodiment of the present invention.

[0137] like Figure 4 As shown, the tightly coupled lightweight real-time positioning and mapping device includes: an acquisition module 100, a motion compensation module 200, a state update module 300, and a map update module 400, wherein:

[0138] The acquisition module is used to obtain the state prediction value of the unmanned motion system through the inertial measurement unit IMU and the original point cloud of the unmanned motion system through the laser radar;

[0139] The motion compensation module is used to preprocess the original point cloud; the preprocessed original point cloud is motion compensated through the state prediction value to obtain the dedistorted point cloud;

[0140] The state update module is used to combine the state prediction value and the dedistorted point cloud, and update the state of the unmanned motion system through Kalman filtering to obtain the optimal state estimation;

[0141] The map update module is used to update the map according to the optimal state estimation.

[0142] To achieve the above-mentioned purpose, the third aspect of the present invention proposes a computer device, characterized in that it includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, it implements the tightly coupled lightweight real-time positioning and mapping method as described above.

[0143] To achieve the above-mentioned purpose, the fourth aspect of the present invention proposes a computer-readable storage medium on which a computer program is stored, characterized in that when the computer program is executed by a processor, the tightly coupled lightweight real-time positioning and mapping method as described above is implemented.

[0144] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. 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 representations of the above terms do 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. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0145] 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 at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0146] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A tightly coupled lightweight real-time positioning and mapping method, characterized in that: The following steps are involved: Acquire a state prediction value of the unmanned motion system through an inertial measurement unit (IMU), and acquire an original point cloud of the unmanned motion system through a laser radar; Preprocessing the original point cloud; Performing motion compensation on the preprocessed original point cloud by using the state prediction value to obtain a dedistorted point cloud; Combining the state prediction value and the dedistorted point cloud, updating the state of the unmanned motion system through Kalman filtering to obtain an optimal state estimate; updating a map according to the optimal state estimate; The step of performing motion compensation on the preprocessed original point cloud through the state prediction value to obtain a dedistorted point cloud includes: According to the state prediction value, the point cloud is transformed and the point cloud is projected back to the position at the acquisition time, thereby removing the distortion of the point cloud in one frame; The preprocessed original point cloud is subjected to motion compensation by using the state prediction value to obtain a dedistorted point cloud, specifically comprising: The following three formulas are used to calculate the propagation Convert the original point cloud from its own acquisition time to the end time of a frame, Indicates that the feature points obtained by the radar j-th scan are converted to t k The position in the radar coordinate system at time , the three equations are: in, Indicates that the unmanned motion system is in ρ j-1 Time in I k The position in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The position in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The speed in the coordinate system, Δt represents the time interval between two radar scans, Indicates that the unmanned motion system is in ρ m Time in I k Position in coordinate system; in, Indicates that the unmanned motion system is in ρ j-1 Time in I k The velocity in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The velocity in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The posture in the coordinate system, Indicated in τ i-1 The IMU measures the acceleration at represents the estimation of the IMU acceleration measurement bias in the kth radar frame, Δt represents the time interval between two radar scans, Indicates the number of radar frames in I k The gravitational acceleration in the coordinate system, Indicates that the unmanned motion system is in ρ m Time in I k The velocity in the coordinate system; in, Indicates that the unmanned motion system is in ρ j-1 Time in I k The posture in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The posture in the coordinate system, Indicated in τ i-j The IMU measures the angular velocity at time, represents the estimation of the IMU angular velocity measurement bias in the kth radar frame, Δt represents the time interval between two radar scans, Indicates that the unmanned motion system is in ρ m Time in I k The posture in the coordinate system; Obtain Then, we get the unmanned motion system in ρ j relative to t k The pose transformation at time: Thus, the characteristic points obtained by the radar in the jth scan are converted to t k The position in the radar coordinate system at time: Among them, the relative pose of IMU and radar is known I T L , Indicates that the unmanned motion system is in ρ j relative to t k The posture change when Represents the feature points obtained by the j-th radar scan; The step of updating the state of the unmanned motion system by using Kalman filtering to obtain an optimal state estimation includes: Comparing the dedistorted point cloud with the predicted state to obtain a measurement residual; According to the covariance matrix of the measurement residual and the measurement noise, the Kalman gain is calculated to weigh the predicted value and the measured value; Using the Kalman gain, the predicted value and the measurement residual are weighted averaged to obtain an optimized state estimate, and the covariance matrix is ​​updated at the same time; Continuously optimize the state estimation of the point cloud; finally, through Kalman filtering, the state estimation of the unmanned motion system is continuously converged to the optimal value; The updating of the state of the unmanned motion system by using Kalman filtering to obtain an optimal state estimation specifically includes: First, set the initial value. Let κ = -1, k represents the number of iterations, Indicates t k The state of the spread; Secondly, iterative calculation, Let k = k + 1, where k represents the number of iterations; Recalculate in At the end of the last iteration, we get Calculated by IMU state prediction; Then, calculate Next, calculate the residual The original feature points are converted into Convert to the world coordinate system to get in, represents the world coordinates of the feature point obtained by the j-th radar scan calculated by the κ-th iteration of the Kalman filter, represents the t calculated by the K-th iteration of the Kalman filter k The posture at that time, I T L Indicates the relative position of IMU and radar, Indicates that the feature points obtained by the radar j-th scan are converted to t k The position in the radar coordinate system at that time, for each lidar feature point, the residual is defined as the world coordinate estimated by the feature point Distance to the nearest plane or edge in the map: in, is the residual, G j is the normal or direction vector corresponding to the plane or edge, represents the world coordinates of the feature point estimate, Represents a point on the corresponding plane or edge. The nearest point in the map is searched by constructing a KD-tree of the points in the latest map. Only residuals with norms below a certain threshold are considered. Residuals exceeding this threshold are outliers or newly observed points. Then, calculate in is the residual, v j The measurement noise from the radar, represents the t calculated by the K-th iteration of the Kalman filter k Error in time state; Next, calculate the state update make R=diag(R1,…R m ), calculate K=PH T (HPH T +R) -1 , in, represents the t calculated by the κ+1th iteration of the Kalman filter k The state of represents the t calculated by the K-th iteration of the Kalman filter k The state of represents the t calculated by the K-th iteration of the Kalman filter k The residual error when Indicates that at time t k The state of propagation, K=(H T R -1 H+P -1 ) -1 H T R -1 ; Finally, the convergence is judged. if ∈ is a set small value, the iteration is stopped and the optimal estimate of the output state is 2. The method according to claim 1, characterized in that The method of obtaining a state prediction value of the unmanned motion system by using an inertial measurement unit (IMU) includes: Collect acceleration and angular velocity data of unmanned motion systems in real time through IMU; By integrating the acceleration and angular velocity data and combining the state estimation and motion equation at the previous moment, the position, velocity and posture information of the unmanned motion system at the current moment are obtained, which are used as the initial value for subsequent optimization; at the same time, according to the noise model of the unmanned motion system, the covariance matrix of the state estimation is updated to quantify the uncertainty of the prediction.

3. The method according to claim 1, characterized in that The preprocessing of the original point cloud comprises: Performing point cloud segmentation and feature extraction on the original point cloud; wherein, The point cloud segmentation includes reprojecting the original point cloud into a range image, marking the points on the scan line between the preset angle range as ground points, and not participating in the subsequent segmentation; segmenting the non-ground points of the range image into multiple clusters, wherein the clusters with fewer points are removed as noise, and the points in the remaining clusters are marked as segmentation points; The feature extraction includes extracting features from ground points and segmentation points. Specifically, for the point cloud P at time t, t Every point P in i The 5 points on the left and right sides of the distance image are combined into a point set S, and the roughness is calculated by the following formula: Among them, r i Indicates P i Distance to sensor, r j Indicates P j Distance to the sensor; The distance image is horizontally divided into a number of sub-images; a number of points with the largest roughness are selected from the segmentation points in each row of the sub-images as edge feature points, and a number of points with the smallest roughness are selected from the ground points or segmentation points in each row of the sub-images as plane feature points.

4. The method according to claim 1, characterized in that: The updating of the map according to the optimal state estimation includes: According to the optimal state estimation, the radar feature point Convert to the world coordinate system to get in, represents the optimal estimate of the world coordinates of the radar feature point, Indicates t k The optimal pose estimation when I T L Indicates the relative position and posture of IMU and radar; The feature points Add to an existing map.

5. A tightly coupled lightweight real-time positioning and mapping device, characterized in that: Includes the following modules: An acquisition module is used to acquire a state prediction value of the unmanned motion system through an inertial measurement unit IMU, and to acquire an original point cloud of the unmanned motion system through a laser radar; A motion compensation module, used for preprocessing the original point cloud; Performing motion compensation on the preprocessed original point cloud by using the state prediction value to obtain a dedistorted point cloud; A state update module, used to combine the state prediction value and the dedistorted point cloud, and update the state of the unmanned motion system through Kalman filtering to obtain an optimal state estimate; A map updating module, used for updating the map according to the optimal state estimation; The step of performing motion compensation on the preprocessed original point cloud through the state prediction value to obtain a dedistorted point cloud includes: According to the state prediction value, the point cloud is transformed and the point cloud is projected back to the position at the acquisition time, thereby removing the distortion of the point cloud in one frame; The preprocessed original point cloud is subjected to motion compensation by using the state prediction value to obtain a dedistorted point cloud, specifically comprising: The following three formulas are used to calculate the propagation Convert the original point cloud from its own acquisition time to the end time of a frame, Indicates that the feature points obtained by the radar j-th scan are converted to t k The position in the radar coordinate system at time , the three equations are: in, Indicates that the unmanned motion system is in ρ j-1 Time in I k The position in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The position in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The speed in the coordinate system, Δt represents the time interval between two radar scans, Indicates that the unmanned motion system is in ρ m Time in I k Position in coordinate system; in, Indicates that the unmanned motion system is in ρ j-1 Time in I k The velocity in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The velocity in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The posture in the coordinate system, Indicated in τ i-1 The IMU measures the acceleration at represents the estimation of the IMU acceleration measurement bias in the kth radar frame, Δt represents the time interval between two radar scans, Indicates the number of radar frames in I k The gravitational acceleration in the coordinate system, Indicates that the unmanned motion system is in ρ m Time in I k The velocity in the coordinate system; in, Indicates that the unmanned motion system is in ρ j-1 Time in I k The posture in the coordinate system, Indicates that the unmanned motion system is in ρ j Time in I k The posture in the coordinate system, Indicated in τ i-1 The IMU measures the angular velocity at time, represents the estimation of the IMU angular velocity measurement bias in the kth radar frame, Δt represents the time interval between two radar scans, Indicates that the unmanned motion system is in ρ m Time in I k The posture in the coordinate system; Obtain Then, we get the unmanned motion system in ρ j relative to t k The pose transformation at time: Thus, the characteristic points obtained by the radar in the jth scan are converted to t k The position in the radar coordinate system at time: Among them, the relative pose of IMU and radar is known I T L , Indicates that the unmanned motion system is in ρ j relative to t k The posture change when Represents the feature points obtained by the j-th radar scan; The step of updating the state of the unmanned motion system by using Kalman filtering to obtain an optimal state estimation includes: Comparing the dedistorted point cloud with the predicted state to obtain a measurement residual; According to the covariance matrix of the measurement residual and the measurement noise, the Kalman gain is calculated to weigh the predicted value and the measured value; Using the Kalman gain, the predicted value and the measurement residual are weighted averaged to obtain an optimized state estimate, and the covariance matrix is ​​updated at the same time; Continuously optimize the state estimation of the point cloud; finally, through Kalman filtering, the state estimation of the unmanned motion system is continuously converged to the optimal value; The updating of the state of the unmanned motion system by using Kalman filtering to obtain an optimal state estimation specifically includes: First, set the initial value. Let κ = -1, κ represents the number of iterations, Indicates t k The state of the spread; Secondly, iterative calculation, Let κ = κ + 1, where κ represents the number of iterations; Recalculate in At the end of the last iteration, we get Calculated by IMU state prediction; Then, calculate Next, calculate the residual The original feature points are converted into Convert to the world coordinate system to get in, represents the world coordinates of the feature point obtained by the j-th radar scan calculated by the κ-th iteration of the Kalman filter, represents the t calculated by the K-th iteration of the Kalman filter k The posture at that time, I T L Indicates the relative position of IMU and radar, Indicates that the feature points obtained by the radar j-th scan are converted to t k The position in the radar coordinate system at that time, for each lidar feature point, the residual is defined as the world coordinate estimated by the feature point Distance to the nearest plane or edge in the map: in, is the residual, G j is the normal or direction vector corresponding to the plane or edge, represents the world coordinates of the feature point estimate, Represents a point on the corresponding plane or edge. The nearest point in the map is searched by constructing a KD-tree of the points in the latest map. Only residuals with norms below a certain threshold are considered. Residuals exceeding this threshold are outliers or newly observed points. Then, calculate in is the residual, v j The measurement noise from the radar, represents the t calculated by the K-th iteration of the Kalman filter k Error in time state; Next, calculate the state update make calculate K=PH T (HPH T +R) -1 , in, represents t calculated by the k+1th iteration of the Kalman filter k The state of represents the t calculated by the kth iteration of the Kalman filter k The state of represents the t calculated by the K-th iteration of the Kalman filter k The residual error when Indicates that at time t k The state of propagation, K=(H T R -1 H+P -1 ) -1 H T R -1 ; Finally, the convergence is judged. if ∈ is a set small value, the iteration is stopped and the optimal estimate of the output state is 6. A computer device, characterized in that: The invention comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the tightly coupled lightweight real-time positioning and mapping method as described in any one of claims 1 to 4 is implemented.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the tightly coupled lightweight real-time positioning and mapping method as described in any one of claims 1 to 4 is implemented.

Images