CN117387598A - Tightly-coupled lightweight real-time positioning and mapping method - Google Patents
Tightly-coupled lightweight real-time positioning and mapping method Download PDFInfo
- Publication number
- CN117387598A CN117387598A CN202311296493.3A CN202311296493A CN117387598A CN 117387598 A CN117387598 A CN 117387598A CN 202311296493 A CN202311296493 A CN 202311296493A CN 117387598 A CN117387598 A CN 117387598A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- state
- points
- unmanned
- motion system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000013507 mapping Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 claims abstract description 30
- 238000001914 filtration Methods 0.000 claims abstract description 26
- 238000007781 pre-processing Methods 0.000 claims abstract description 12
- 230000011218 segmentation Effects 0.000 claims description 25
- 238000000605 extraction Methods 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 11
- 238000010168 coupling process Methods 0.000 claims description 9
- 238000005859 coupling reaction Methods 0.000 claims description 9
- 230000008878 coupling Effects 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000004807 localization Effects 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000005303 weighing Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/3811—Point data, e.g. Point of Interest [POI]
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
- G06T19/20—Editing of 3D images, e.g. changing shapes or colours, aligning objects or positioning parts
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Software Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Geometry (AREA)
- Computer Graphics (AREA)
- Multimedia (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Architecture (AREA)
- Computer Hardware Design (AREA)
- General Engineering & Computer Science (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention provides a tightly-coupled lightweight real-time positioning and mapping method, which comprises the steps of acquiring a state predicted value of an unmanned motion system through an Inertial Measurement Unit (IMU) and acquiring an original point cloud of the unmanned motion system through a laser radar; preprocessing an original point cloud; performing motion compensation on the preprocessed original point cloud through a state predicted value to obtain a de-distorted point cloud; combining the state predicted value and the de-distorted point cloud, and carrying out state update on the unmanned motion system through Kalman filtering to obtain an optimal state estimation; and updating the map according to the optimal state estimation. According to the method provided by the invention, based on Kalman filtering, the laser radar and inertial navigation are tightly coupled, so that accurate and real-time motion estimation and mapping can be realized.
Description
Technical Field
The invention belongs to the field of positioning and mapping of unmanned systems.
Background
For automatic driving tasks, high-precision positioning information is particularly important as input of modules such as subsequent decision planning and the like. In the positioning field, particularly in the task scenario of high-speed driving, high-frequency motion estimation is indispensable. For this purpose, the skilled person proposes active sensors, such as lidar, to accomplish this task, 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, however, lidar-based navigation systems are sensitive to the surrounding environment. Furthermore, motion distortion and sparsity of the point cloud can degrade the performance of the algorithm in certain challenging scenarios (e.g., wide open areas).
Recent studies have shown that the shortfall of lidar can be remedied by fusing IMUs. Unlike lidar, IMUs are not sensitive to the environment. It provides accurate short-term motion constraints and typically operates at high frequencies (e.g., 100Hz-500 Hz). These functions can help the lidar navigation system recover the point cloud from highly dynamic motion distortions, thereby improving accuracy.
Disclosure of Invention
The present invention aims to solve at least one of the technical problems in the related art to some extent.
Therefore, the invention aims to provide a tightly-coupled lightweight real-time positioning and mapping method which is used for helping a laser radar navigation system to recover point clouds from highly dynamic motion distortion, thereby improving accuracy.
To achieve the above objective, an embodiment of a first aspect of the present invention provides a method for positioning and mapping in real time of tight coupling lightweight, including:
acquiring a state predicted value of the unmanned motion system through an Inertial Measurement Unit (IMU), and acquiring 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 through the state predicted value to obtain a de-distorted point cloud;
combining the state predicted value and the de-distortion point cloud, and carrying out state update on the unmanned motion system through Kalman filtering to obtain an optimal state estimation;
and updating the map according to the optimal state estimation.
In addition, the method for positioning and mapping in real time of tight coupling lightweight according to the above embodiment of the present invention may further have the following additional technical features:
further, in an embodiment of the present invention, the acquiring, by the inertial measurement unit IMU, a state prediction value of the unmanned motion system includes:
acquiring acceleration and angular velocity data of the unmanned motion system in real time through the IMU;
integrating the acceleration and angular velocity data, and combining the state estimation and the motion equation at the last moment to obtain the position, the velocity and the gesture information of the unmanned motion system at the current moment, and taking the position, the velocity and the gesture information as initial values of subsequent optimization; and meanwhile, updating a covariance matrix of state estimation according to a noise model of the unmanned motion system, and quantifying the uncertainty of prediction.
Further, in an embodiment of the present invention, the preprocessing the original point cloud includes:
performing point cloud segmentation and feature extraction on the original point cloud; wherein,
the point cloud segmentation comprises the steps of re-projecting an original point cloud into a distance image, marking points on a scanning line between preset angle ranges as ground points, and not participating in subsequent segmentation; dividing non-ground points of the distance image into a plurality of clusters, removing clusters with fewer points in the clusters as noise, and marking the points in the left clusters as dividing points;
the feature extraction comprises feature extraction from ground points and segmentation points, in particular to a point cloud P at the moment t t Each point P of (3) i The 5 points on the left and right in the distance image are combined into a point set S, and the roughness is calculated by the following formula:
wherein r is i Representing P i Distance to sensor, r j Representing P j Distance to the sensor;
dividing the distance image level into a plurality of sub-images; and selecting a plurality of points with the largest roughness from the segmentation points of each row in the sub-image as edge characteristic points, and selecting a plurality of points with the smallest roughness from the ground points or the segmentation points of each row in the sub-image as plane characteristic points.
Further, in an embodiment of the present invention, the motion compensation of the preprocessed original point cloud by the state prediction value includes:
and transforming the point cloud according to the state predicted value, and projecting the point cloud to the position of the extraction moment, so that the distortion of the point cloud in one frame is removed.
Further, in an embodiment of the present invention, the performing, by kalman filtering, the state update on the unmanned motion system to obtain the optimal state estimate includes:
comparing the undistorted point cloud with a predicted state to obtain a measurement residual;
calculating Kalman gain according to covariance matrixes of measurement residual errors and measurement noise, and weighing predicted values and measured values;
carrying out weighted average on the predicted value and the measurement residual error by using Kalman gain to obtain an optimized state estimation value, and updating a covariance matrix;
continuously optimizing the state estimation of the point cloud; and finally, continuously converging the state estimation of the unmanned motion system to an optimal value through Kalman filtering.
Further, in an embodiment of the present invention, the updating the map according to the optimal state estimation includes:
radar feature points are estimated according to the optimal stateConversion to world coordinate system>
Wherein,optimal estimate of world coordinates representing radar feature points,/->Representing t k The optimal pose estimation at the time is performed, I T L representing the relative pose of IMU and radar, < ->Indicating the jth scan of the radarFeature point transition to t k A position under a time radar coordinate system;
feature points are formedAdded to the existing map.
In order to achieve the above objective, a second aspect of the present invention provides a tightly coupled lightweight real-time positioning and mapping device, which includes the following modules:
the acquisition module is used for acquiring a state predicted value of the unmanned motion system through the Inertial Measurement Unit (IMU), and acquiring an original point cloud of the unmanned motion system through a laser radar;
the motion compensation module is used for preprocessing the original point cloud; performing motion compensation on the preprocessed original point cloud through the state predicted value to obtain a de-distorted point cloud;
the state updating module is used for carrying out state updating on the unmanned motion system through Kalman filtering by combining the state predicted value and the de-distortion point cloud to obtain an optimal state estimation;
and the map updating module is used for updating the map according to the optimal state estimation.
To achieve the above object, an embodiment of the present invention provides a computer device, which is characterized by comprising a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor implements a tightly coupled lightweight real-time positioning and mapping method as described above when executing the computer program.
To achieve the above object, a fourth aspect of the present invention provides a computer readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements a tightly coupled lightweight real-time positioning and mapping method as described above.
According to the tightly-coupled lightweight real-time positioning and mapping method provided by the embodiment of the invention, firstly, the state predicted value is obtained through the IMU to carry out state propagation, and meanwhile, the input radar point cloud is preprocessed, including point cloud segmentation and feature extraction. And then carrying out motion compensation on the preprocessed point cloud through the state predicted value to remove distortion. Next, the state is updated by kalman filtering to obtain an optimal state estimate. Finally, the map is updated and managed, and the updated map is used for registering new points in the next round. The method is based on Kalman filtering, and the laser radar and inertial navigation are tightly coupled, so that accurate and real-time motion estimation and mapping can be realized.
Drawings
The foregoing and/or additional aspects and advantages of the invention will become apparent and readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings, in which:
fig. 1 is a flow chart of a method for positioning and mapping in real time of tight coupling lightweight according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a time relationship between IMU input and radar point cloud input in a radar frame time provided in an embodiment of the present invention.
FIG. 3 is a flow chart of a method for tightly coupling lightweight real-time localization and mapping according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of a tightly coupled lightweight real-time positioning and mapping device according to an embodiment of the present invention.
Detailed Description
Embodiments of the present invention are described in detail below, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the drawings are illustrative and intended to explain the present invention and should not be construed as limiting the invention.
The following describes a tightly coupled lightweight real-time localization and mapping method of an embodiment of the present invention with reference to the accompanying drawings.
Fig. 1 is a flow chart of a method for positioning and mapping in real time of tight coupling lightweight according to an embodiment of the present invention.
As shown in fig. 1, the method for positioning and mapping the tight-coupling lightweight in real time comprises the following steps:
s101: acquiring a state predicted value of the unmanned motion system through an Inertial Measurement Unit (IMU), and acquiring an original point cloud of the unmanned motion system through a laser radar;
s102: preprocessing an original point cloud; performing motion compensation on the preprocessed original point cloud through a state predicted value to obtain a de-distorted point cloud;
s103: combining the state predicted value and the de-distortion point cloud, and carrying out state update on the unmanned motion system through Kalman filtering to obtain an optimal state estimation;
s104: and updating the map according to the optimal state estimation.
Further, in an embodiment of the present invention, the acquiring, by the inertial measurement unit IMU, a state prediction value of the unmanned motion system includes:
acquiring acceleration and angular velocity data of the unmanned motion system in real time through the IMU;
integrating the acceleration and angular velocity data, and combining the state estimation and the motion equation at the last moment to obtain the position, the velocity and the gesture information of the unmanned motion system at the current moment, and taking the position, the velocity and the gesture information as initial values of subsequent optimization; and meanwhile, updating a covariance matrix of state estimation according to a noise model of the unmanned motion system, and quantifying the uncertainty of prediction.
Specifically, first, a state x of the unmanned system is defined as:
the system I is an IMU coordinate system and also an 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 Andthe pose, position and speed of the unmanned system in world coordinates, respectively, +.>And->Respectively the offset of the IMU angular velocity and acceleration measurements, G g is the unknown gravitational acceleration in the world coordinate system. IMU input u is
Wherein omega m Representing measurements of the angular velocity of an IMU, a m Representing the measurement of acceleration by the IMU. The time relationship between IMU input and radar point cloud input during the time of one radar frame is shown in fig. 2.
As shown in FIG. 2, the acquisition time of the kth radar frame is [ t ] k-1 ,t k ]The method comprises the steps of carrying out a first treatment on the surface of the In the kth frame, the time of IMU input is noted τ 1 ,τ 2 ,…τ i ,τ i+1 …; the time of the radar scan is noted as ρ 1 ,ρ 2 ,…ρ j-1 ,ρ j ,…ρ m Wherein ρ is j-1 ∈[τ i-1 ,τ i )。
Receiving an IMU input state and transmitting the state oncePropagating as follows:
wherein,indicated at time tau i+1 Status of transmission, ++>Indicated at time tau i Propagation state, Δt=τ i+1 -τ i Representing the time difference between two consecutive inputs of IMU, u i Indicating time τ i IMU measurements,/->Representing t k-1 Optimal estimation of the time of day state ∈>A type of operation that is defined is represented,
covariance of state errorsPropagating as follows:
wherein,representation->Covariance of error,>representation->Covariance of error,>representation->Covariance of error,>and F w A type of operation that is defined is represented,
wherein,denoted τ i The gesture of the unmanned system in world coordinates at the moment; Δt=τ i+1 -τ i Representing a time difference;denoted τ i An IMU angular velocity estimation value at a moment; />Denoted τ i Time IMU acceleration estimate.
Further, in an embodiment of the present invention, the preprocessing the original point cloud includes:
performing point cloud segmentation and feature extraction on the original point cloud; wherein,
the point cloud segmentation comprises the steps of re-projecting an original point cloud into a distance image, marking points on a scanning line between preset angle ranges as ground points, and not participating in subsequent segmentation; dividing non-ground points of the distance image into a plurality of clusters, removing clusters with fewer points in the clusters as noise, and marking the points in the left clusters as dividing points;
the feature extraction comprises feature extraction from ground points and segmentation points, in particular to a point cloud P at the moment t t Each point P of (3) i The left and right 5 points in the distance image are combined into a pointSet S, roughness is calculated by:
wherein r is i Representing P i Distance to sensor, r j Representing P j Distance to the sensor;
dividing the distance image level into a plurality of sub-images; and selecting a plurality of points with the largest roughness from the segmentation points of each row in the sub-image as edge characteristic points, and selecting a plurality of points with the smallest roughness from the ground points or the segmentation points of each row in the sub-image as plane characteristic points.
Specifically, the point cloud segmentation changes a three-dimensional point cloud into a two-dimensional image, the horizontal resolution is 360 degrees/the radar horizontal resolution, the vertical resolution is the number of radar lines, and the pixel value is the distance from the point to the sensor. Points on the scan line that occur between a range of angles (e.g., -15 °, -1 ° ]) are marked as ground points and do not participate in subsequent segmentations.
Subsequently, the non-ground points of the distance image are segmented into a plurality of clusters, the clusters with fewer points are removed as noise, and the points in the remaining clusters are marked as segmented points.
Further, in an embodiment of the present invention, the motion compensation of the preprocessed original point cloud by the state prediction value includes:
and transforming the point cloud according to the state predicted value, and projecting the point cloud to the position of the extraction moment, so that the distortion of the point cloud in one frame is removed.
Specifically, the method is obtained by the propagation calculation of the following three formulasConverting the original point cloud from the self-acquisition time to the frame end time of one frame:
wherein,representing that unmanned system is at ρ j-1 At I k Position under coordinate system, +.>Representing that unmanned system is at ρ j At I k Position under coordinate system, +.>Representing that unmanned system is at ρ j At I k Speed in the coordinate system, Δt represents the time interval between two scans of the radar, +.>Representing that unmanned system is at ρ m At I k Position under the coordinate system.
Wherein,representing that unmanned system is at ρ j-1 At I k Speed in coordinate System, +.>Representing that unmanned system is at ρ j At I k Speed in coordinate System, +.>Representing that unmanned system is at ρ j At I k Pose in coordinate system, ++>Represented at τ i-1 Time IMU pair addingMeasurement of speed, +.>Representing an estimate of the IMU acceleration measurement bias in the kth radar frame, Δt represents the time interval between two scans of the radar, +.>Indicating that in the kth radar frame I k The acceleration of gravity in the coordinate system,representing that unmanned system is at ρ m At I k Speed in the coordinate system.
Wherein,representing that unmanned system is at ρ j-1 At I k Pose in coordinate system, ++>Representing that unmanned system is at ρ j At I k Pose in coordinate system, ++>Represented at τ i-1 Measurement of angular velocity of IMU, +.>Representing an estimate of the offset of the IMU angular velocity measurement in the kth radar frame, Δt representing the time interval between two scans of the radar,/for>Representing that unmanned system is at ρ m At I k Pose in coordinate system.
ObtainingAfter that, the unmanned system is obtained at ρ j Relative to t k Pose transformation at the time:
thereby obtaining the characteristic point obtained by the jth scanning of the radar and converting the characteristic point into t k Position under the time radar coordinate system:
wherein the relative pose of the IMU and the radar is known I T L ,Representing that unmanned system is at ρ j Relative to t k Posture change during time, the->And the characteristic points obtained by the jth scanning of the radar are represented.
Further, in an embodiment of the present invention, the performing, by kalman filtering, the state update on the unmanned motion system to obtain the optimal state estimate includes:
comparing the undistorted point cloud with a predicted state to obtain a measurement residual;
calculating Kalman gain according to covariance matrixes of measurement residual errors and measurement noise, and weighing predicted values and measured values;
carrying out weighted average on the predicted value and the measurement residual error by using Kalman gain to obtain an optimized state estimation value, and updating a covariance matrix;
continuously optimizing the state estimation of the point cloud; and finally, continuously converging the state estimation of the unmanned motion system to an optimal value through Kalman filtering.
First, an initial value is set.
Let k = -1,kappa indicates the number of iterations, < >>Representing t k A state of propagation.
And secondly, iterating the calculation.
Let k=k+1, where k represents the number of iterations.
RecalculatingWherein->At the end of the last iteration, get +.>And the IMU state prediction calculation is carried out.
Then, calculate
Next, the residual is calculated
The original feature points are pressed downConversion to world coordinate system>
Wherein,world coordinates representing feature points obtained by jth scan of radar calculated by kth iteration of Kalman filtering, < + >>Representing t calculated by the kth iteration of Kalman filtering k The position and the posture of the user during the process, I T L representing the relative pose of IMU and radar, < ->Representing the conversion of the characteristic point obtained by the jth scanning of the radar to t k Radar coordinate system. For each lidar feature point, defining the residual as the world coordinate of the feature point estimate +.>Distance from nearest plane (or edge) in the map:
wherein,is the residual error, G j Is the normal vector (or direction vector) of the corresponding plane (or edge), +.>World coordinates representing feature point estimates, +.>Representing a point on the corresponding plane (or edge). The nearest point in the map is searched by constructing a KD-tree of the latest map points. Considering only residuals with norms below a certain threshold, residuals exceeding the threshold are either alienConstant, or newly observed point.
Then, calculateWherein->Is the residual error, v j Measurement noise from radar, < >>Representing t calculated by the kth iteration of Kalman filtering k Error of time status.
Next, a state update is calculated
Order theR=diag(R 1 ,…R m ),
Calculation of
K=PH T (HPH T +R) -1 ,
Wherein,representing t calculated from the kth+1th iteration of Kalman filtering k Status of time->Representing t calculated by the kth iteration of Kalman filtering k Status of time->Representing t calculated by the kth iteration of Kalman filtering k Residual error at time->Indicated at time t k The state of propagation.
K=(H T R -1 H+P -1 ) -1 H T R -1 ,
And finally, judging convergence.
If it is(∈is a set small value), the iteration is stopped, the optimal estimate of the output state +.>
Further, in one embodiment of the present invention, updating the map according to the optimal state estimate includes:
radar feature points according to optimal state estimationConversion to world coordinate system>
Wherein,optimal estimate of world coordinates representing radar feature points,/->Representing t k Optimal pose estimation at time, I T L Representing the relative pose of IMU and radar, < ->Representing the conversion of the characteristic point obtained by the jth scanning of the radar to t k A position under a time radar coordinate system;
feature points are formedAdded to the existing map.
The above is a complete process flow of the tight coupling lightweight real-time positioning and mapping method, and fig. 3 is a schematic diagram of the technical route of the present invention.
According to the tightly-coupled lightweight real-time positioning and mapping method provided by the embodiment of the invention, the IMU is used for obtaining the state predicted value to perform state propagation, and meanwhile, the input radar point cloud is subjected to preprocessing operation, including point cloud segmentation and feature extraction. And then carrying out motion compensation on the preprocessed point cloud through the state predicted value to remove distortion. Next, the state is updated by kalman filtering to obtain an optimal state estimate. Finally, the map is updated and managed, and the updated map is used for registering new points in the next round. The method is based on Kalman filtering, and the laser radar and inertial navigation are tightly coupled, so that accurate and real-time motion estimation and mapping can be realized.
The invention has the advantages over the prior art that
1) Firstly, acquiring a state prediction value through an IMU to perform state propagation, and simultaneously performing preprocessing operation on input radar point clouds, including point cloud segmentation and feature extraction. And then carrying out motion compensation on the preprocessed point cloud through the state predicted value to remove distortion. Next, the state is updated by kalman filtering to obtain an optimal state estimate. Finally, the map is updated and managed, and the updated map is used for registering new points in the next round. The method tightly couples the laser radar and the inertial navigation, and can realize accurate and real-time motion estimation and mapping.
2) The ground points marked before the point cloud segmentation do not participate in the subsequent segmentation, so that the calculated amount is greatly reduced.
3) The motion compensation method is to remove distortion from the preprocessed point cloud by using state prediction values.
4) The back-end optimization adopts a Kalman filtering method, so that the optimal estimation of the current state is only related to the variable at the last moment, and the real-time performance of calculation is improved.
In order to realize the embodiment, the invention also provides a tightly-coupled lightweight real-time positioning and mapping device.
Fig. 4 is a schematic structural diagram of a tightly-coupled lightweight real-time positioning and mapping device according to an embodiment of the present invention.
As shown in fig. 4, the tightly-coupled lightweight real-time positioning and mapping apparatus includes: an acquisition module 100, a motion compensation module 200, a status update module 300, a map update module 400, wherein,
the acquisition module is used for acquiring a state predicted value of the unmanned motion system through the Inertial Measurement Unit (IMU), and acquiring an original point cloud of the unmanned motion system through the laser radar;
the motion compensation module is used for preprocessing the original point cloud; performing motion compensation on the preprocessed original point cloud through a state predicted value to obtain a de-distorted point cloud;
the state updating module is used for carrying out state updating on the unmanned motion system through Kalman filtering by combining the state predicted value and the undistorted point cloud to obtain the optimal state estimation;
and the map updating module is used for updating the map according to the optimal state estimation.
To achieve the above object, an embodiment of the present invention provides a computer device, which is characterized by comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor implements the tightly coupled lightweight real-time positioning and mapping method as described above when executing the computer program.
To achieve the above object, a fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the tightly-coupled lightweight real-time positioning and mapping method as described above.
In the description of the present specification, a description referring to terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms are not necessarily directed to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, the different embodiments or examples described in this specification and the features of the different embodiments or examples may be combined and combined by those skilled in the art without contradiction.
Furthermore, the terms "first," "second," and the like, are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include at least one such feature. In the description of the present invention, the meaning of "plurality" means at least two, for example, two, three, etc., unless specifically defined otherwise.
While embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives and variations may be made to the above embodiments by one of ordinary skill in the art within the scope of the invention.
Claims (9)
1. A method for positioning and mapping a tight coupling lightweight real-time image is characterized by comprising the following steps:
acquiring a state predicted value of the unmanned motion system through an Inertial Measurement Unit (IMU), and acquiring 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 through the state predicted value to obtain a de-distorted point cloud;
combining the state predicted value and the de-distortion point cloud, and carrying out state update on the unmanned motion system through Kalman filtering to obtain an optimal state estimation;
and updating the map according to the optimal state estimation.
2. The method according to claim 1, wherein the obtaining, by the inertial measurement unit IMU, a state prediction value of the unmanned motion system comprises:
acquiring acceleration and angular velocity data of the unmanned motion system in real time through the IMU;
integrating the acceleration and angular velocity data, and combining the state estimation and the motion equation at the last moment to obtain the position, the velocity and the gesture information of the unmanned motion system at the current moment, and taking the position, the velocity and the gesture information as initial values of subsequent optimization; and meanwhile, updating a covariance matrix of state estimation according to a noise model of the unmanned motion system, and quantifying the uncertainty of prediction.
3. The method of claim 1, wherein the preprocessing the original point cloud comprises:
performing point cloud segmentation and feature extraction on the original point cloud; wherein,
the point cloud segmentation comprises the steps of re-projecting an original point cloud into a distance image, marking points on a scanning line between preset angle ranges as ground points, and not participating in subsequent segmentation; dividing non-ground points of the distance image into a plurality of clusters, removing clusters with fewer points in the clusters as noise, and marking the points in the left clusters as dividing points;
the feature extraction comprises feature extraction from ground points and segmentation points, in particular to a point cloud P at the moment t t Each point P of (3) i The 5 points on the left and right in the distance image are combined into a point set S, and the roughness is calculated by the following formula:
wherein ri represents the distance Pi from the sensor and rj represents P j Distance to the sensor;
dividing the distance image level into a plurality of sub-images; and selecting a plurality of points with the largest roughness from the segmentation points of each row in the sub-image as edge characteristic points, and selecting a plurality of points with the smallest roughness from the ground points or the segmentation points of each row in the sub-image as plane characteristic points.
4. The method of claim 1, wherein the motion compensating the preprocessed source point cloud with the state prediction value comprises:
and transforming the point cloud according to the state predicted value, and projecting the point cloud to the position of the extraction moment, so that the distortion of the point cloud in one frame is removed.
5. The method of claim 1, wherein the performing a state update on the unmanned motion system by kalman filtering to obtain an optimal state estimate comprises:
comparing the undistorted point cloud with a predicted state to obtain a measurement residual;
calculating Kalman gain according to covariance matrixes of measurement residual errors and measurement noise, and weighing predicted values and measured values;
carrying out weighted average on the predicted value and the measurement residual error by using Kalman gain to obtain an optimized state estimation value, and updating a covariance matrix;
continuously optimizing the state estimation of the point cloud; and finally, continuously converging the state estimation of the unmanned motion system to an optimal value through Kalman filtering.
6. The method of claim 1, wherein updating the map based on the optimal state estimate comprises:
radar feature points are estimated according to the optimal stateConversion to world coordinate system>
Wherein,optimal estimate of world coordinates representing radar feature points,/->Representing the optimal pose estimate at tk, ITL representing the relative pose of IMU and radar, +.>Representing the position of a radar coordinate system when the characteristic point obtained by the jth scanning of the radar is converted to tk;
feature points are formedAdded to the existing map.
7. The tight coupling lightweight real-time positioning and mapping device is characterized by comprising the following modules:
the acquisition module is used for acquiring a state predicted value of the unmanned motion system through the Inertial Measurement Unit (IMU), and acquiring an original point cloud of the unmanned motion system through a laser radar;
the motion compensation module is used for preprocessing the original point cloud; performing motion compensation on the preprocessed original point cloud through the state predicted value to obtain a de-distorted point cloud;
the state updating module is used for carrying out state updating on the unmanned motion system through Kalman filtering by combining the state predicted value and the de-distortion point cloud to obtain an optimal state estimation;
and the map updating module is used for updating the map according to the optimal state estimation.
8. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the tightly coupled lightweight real-time localization and mapping method of any of claims 1-6 when the computer program is executed by the processor.
9. A computer readable storage medium, having stored thereon a computer program, which when executed by a processor implements the tightly coupled lightweight real-time localization and mapping method of any of claims 1-6.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311296493.3A CN117387598A (en) | 2023-10-08 | 2023-10-08 | Tightly-coupled lightweight real-time positioning and mapping method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311296493.3A CN117387598A (en) | 2023-10-08 | 2023-10-08 | Tightly-coupled lightweight real-time positioning and mapping method |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117387598A true CN117387598A (en) | 2024-01-12 |
Family
ID=89440219
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311296493.3A Pending CN117387598A (en) | 2023-10-08 | 2023-10-08 | Tightly-coupled lightweight real-time positioning and mapping method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117387598A (en) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111929699A (en) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | Laser radar inertial navigation odometer considering dynamic obstacles and mapping method and system |
CN114136311A (en) * | 2021-11-08 | 2022-03-04 | 上海应用技术大学 | Laser SLAM positioning method based on IMU pre-integration |
CN115326068A (en) * | 2022-10-17 | 2022-11-11 | 北京理工大学 | Design method and system for laser radar-inertial measurement unit fusion odometer |
CN115984463A (en) * | 2022-12-21 | 2023-04-18 | 泉州装备制造研究所 | Three-dimensional reconstruction method and system suitable for narrow roadway |
CN116452763A (en) * | 2023-04-17 | 2023-07-18 | 浙江大学 | Three-dimensional point cloud map construction method based on error Kalman filtering and factor graph |
CN116449384A (en) * | 2023-03-17 | 2023-07-18 | 天津大学 | Radar inertial tight coupling positioning mapping method based on solid-state laser radar |
CN116448100A (en) * | 2023-03-10 | 2023-07-18 | 华南理工大学 | Multi-sensor fusion type offshore unmanned ship SLAM method |
-
2023
- 2023-10-08 CN CN202311296493.3A patent/CN117387598A/en active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111929699A (en) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | Laser radar inertial navigation odometer considering dynamic obstacles and mapping method and system |
CN114136311A (en) * | 2021-11-08 | 2022-03-04 | 上海应用技术大学 | Laser SLAM positioning method based on IMU pre-integration |
CN115326068A (en) * | 2022-10-17 | 2022-11-11 | 北京理工大学 | Design method and system for laser radar-inertial measurement unit fusion odometer |
CN115984463A (en) * | 2022-12-21 | 2023-04-18 | 泉州装备制造研究所 | Three-dimensional reconstruction method and system suitable for narrow roadway |
CN116448100A (en) * | 2023-03-10 | 2023-07-18 | 华南理工大学 | Multi-sensor fusion type offshore unmanned ship SLAM method |
CN116449384A (en) * | 2023-03-17 | 2023-07-18 | 天津大学 | Radar inertial tight coupling positioning mapping method based on solid-state laser radar |
CN116452763A (en) * | 2023-04-17 | 2023-07-18 | 浙江大学 | Three-dimensional point cloud map construction method based on error Kalman filtering and factor graph |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112347840B (en) | Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method | |
CN113436260B (en) | Mobile robot pose estimation method and system based on multi-sensor tight coupling | |
JP6760114B2 (en) | Information processing equipment, data management equipment, data management systems, methods, and programs | |
CN105809687B (en) | A kind of monocular vision ranging method based on point information in edge in image | |
CN111795686B (en) | Mobile robot positioning and mapping method | |
CN112304307A (en) | Positioning method and device based on multi-sensor fusion and storage medium | |
JP6456141B2 (en) | Generating map data | |
CN111665512B (en) | Ranging and mapping based on fusion of 3D lidar and inertial measurement unit | |
CN111524194B (en) | Positioning method and terminal for mutually fusing laser radar and binocular vision | |
CN114013449B (en) | Data processing method and device for automatic driving vehicle and automatic driving vehicle | |
CN112444246B (en) | Laser fusion positioning method in high-precision digital twin scene | |
CN111862214B (en) | Computer equipment positioning method, device, computer equipment and storage medium | |
CN111623773B (en) | Target positioning method and device based on fisheye vision and inertial measurement | |
CN115272596A (en) | Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene | |
CN114494629A (en) | Three-dimensional map construction method, device, equipment and storage medium | |
CN115526914A (en) | Robot real-time positioning and color map fusion mapping method based on multiple sensors | |
CN114397642A (en) | Three-dimensional laser radar and IMU external reference calibration method based on graph optimization | |
CN117710476A (en) | Monocular vision-based unmanned aerial vehicle pose estimation and dense mapping method | |
WO2023226154A1 (en) | Autonomous localization method and apparatus, and device and computer-readable storage medium | |
CN115984463A (en) | Three-dimensional reconstruction method and system suitable for narrow roadway | |
CN115950414A (en) | Adaptive multi-fusion SLAM method for different sensor data | |
CN117387598A (en) | Tightly-coupled lightweight real-time positioning and mapping method | |
CN115355904A (en) | Slam method for Lidar-IMU fusion of ground mobile robot | |
CN114723920A (en) | Point cloud map-based visual positioning method | |
CN115344033A (en) | Monocular camera/IMU/DVL tight coupling-based unmanned ship navigation and positioning method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |