CN113466890B - Light laser radar inertial combination positioning method and system based on key feature extraction - Google Patents
Light laser radar inertial combination positioning method and system based on key feature extraction Download PDFInfo
- Publication number
- CN113466890B CN113466890B CN202110590374.3A CN202110590374A CN113466890B CN 113466890 B CN113466890 B CN 113466890B CN 202110590374 A CN202110590374 A CN 202110590374A CN 113466890 B CN113466890 B CN 113466890B
- Authority
- CN
- China
- Prior art keywords
- laser
- time
- inertial
- laser radar
- frame
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 90
- 238000000605 extraction Methods 0.000 title claims abstract description 32
- 230000033001 locomotion Effects 0.000 claims abstract description 92
- 238000001514 detection method Methods 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims description 117
- 238000005259 measurement Methods 0.000 claims description 46
- 239000013598 vector Substances 0.000 claims description 33
- 230000008569 process Effects 0.000 claims description 26
- 230000010354 integration Effects 0.000 claims description 23
- 238000004458 analytical method Methods 0.000 claims description 21
- 238000011156 evaluation Methods 0.000 claims description 17
- 238000006073 displacement reaction Methods 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000010276 construction Methods 0.000 abstract description 2
- 238000005457 optimization Methods 0.000 description 13
- 238000012937 correction Methods 0.000 description 12
- 238000004422 calculation algorithm Methods 0.000 description 10
- 238000013507 mapping Methods 0.000 description 9
- 230000001133 acceleration Effects 0.000 description 5
- 230000000295 complement effect Effects 0.000 description 5
- 238000011160 research Methods 0.000 description 5
- 238000012216 screening Methods 0.000 description 5
- 230000006835 compression Effects 0.000 description 4
- 238000007906 compression Methods 0.000 description 4
- 238000010168 coupling process Methods 0.000 description 4
- 230000007547 defect Effects 0.000 description 4
- 238000011158 quantitative evaluation Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000011002 quantification Methods 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 239000002699 waste material Substances 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 101000981685 Homo sapiens Protein Lines homolog 1 Proteins 0.000 description 1
- 102100024087 Protein Lines homolog 1 Human genes 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- CLOMYZFHNHFSIQ-UHFFFAOYSA-N clonixin Chemical compound CC1=C(Cl)C=CC=C1NC1=NC=CC=C1C(O)=O CLOMYZFHNHFSIQ-UHFFFAOYSA-N 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000010790 dilution Methods 0.000 description 1
- 239000012895 dilution Substances 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
- 239000000243 solution Substances 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- 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
- 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
- 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention provides a light laser radar inertial combination positioning method and system based on key feature extraction, comprising the following steps: compensating the motion distortion of the laser point cloud by using an inertial pre-integrated intermediate variable; extracting line and surface characteristics in a laser radar key frame to finish inter-frame characteristic matching; quantitatively evaluating the validity of the laser radar feature matching constraint; eliminating inefficient feature constraints; and selecting proper loop detection time, reducing the calculation cost on the premise of ensuring the precision, and realizing the construction of the three-dimensional point cloud and the high-precision positioning and posture estimation.
Description
Technical Field
The invention provides a lightweight laser radar inertial combination positioning method and system based on key feature extraction, and relates to the field of robot positioning and mapping. The invention can realize the construction of the three-dimensional point cloud and the high-precision positioning and attitude estimation by utilizing the combined system of the multi-line laser radar and the inertial measurement unit, thereby being applied to specific scenes such as mapping, automatic driving vehicle positioning and the like.
Background
The existing laser radar odometer method can be divided into two types, one type is based on iteration nearest neighbor points (iterative closest point, ICP), and because the method needs to align dense laser point clouds between two scans, the calculated amount is large, the precision is too dependent on the huge number of the laser point clouds, and related researches are less. In addition, an odometer method based on feature extraction is a mainstream solution. The most representative of these is the lom method, which is a typical odometer method based on geometric feature matching, and achieves efficient lidar localization by extracting feature points on edge lines and feature points on planes. Compared to ICP-based methods, the LOAM method extracts more sparse edge points and planar points, and therefore its computation is less extensive. However, the LOAM method requires motion estimation of the lidar itself to perform motion distortion correction of the point cloud, and this iterative correction method causes additional computational overhead.
The point cloud has motion distortion because the laser radar scan is not instantaneously completed for one week: when the laser radar moves within the time interval of one circle of scanning, the position of the reference coordinate system corresponding to each laser point obtained by ranging also changes along with the movement; at this time, if the coordinate system change is not compensated for by the motion information, the observed point cloud will exhibit distortion that is inconsistent with the real environment. To account for motion distortion of the point cloud, complementary characteristics between sensors, such as the introduction of a global positioning system (Global Positioning System, GPS), visual odometer or inertial measurement unit (Inertial Measurement Unit, IMU), may be utilized in addition to iterative corrections of the LOAM method. The inertial measurement unit is not easily interfered by the outside, has high short-time motion information precision and the like, and is widely used for a combined positioning system with the laser radar. Some fusion methods utilize a laser radar and an inertial measurement unit to respectively calculate displacement variation amounts at adjacent moments, which belong to loose coupling in fusion classification. The loose coupling method has low utilization efficiency on complementary characteristics among sensors, and is a 'tight coupling' method for fusing in the observation level, so that the robustness is stronger. The method for positioning the tight coupling of the laser radar and the inertial device has some research results at home and abroad, and the research adds inertial information into the optimization process of the rear end, for example, a iterative error state Kalman filter (iterative error-state Kalman filter) is designed by LINS so that the system can complete positioning estimation in real time; LIO-SAM controls the computational cost by key frame technique and factor graph method. Although these tightly coupled lidar inertial combined positioning methods exhibit acceptable computational efficiency, their front ends still use the same laser feature extraction concept as the LOAM method. That is, the real-time performance of the laser radar inertial combination positioning method can be further improved by selecting a more effective laser feature constraint subset.
Indeed, screening of laser features has been partially discussed in the prior art, but this approach of selection is mainly from a technical point of view, eliminating "invalid" points with some artificially established rules. In the prior art methods, the LOAM excludes points that lie in a plane parallel to the laser beam, as well as boundary points at the occluded area; loam_livox excludes lasing points of abnormal intensity; in LIO-mapping research, edge points are considered not to improve the performance of the odometer; the Lego-LOAM simultaneously retains the edge points and the plane points, and then extracts the laser characteristics as evenly as possible in all directions. The prior art just provides a laser characteristic screening method based on practical experience, and the prior art has not been discussed yet: how each pair of feature matches contributes to positioning accuracy is analyzed from a theoretical perspective, nor is the validity assessment of laser feature constraints quantified. The invention provides a quantification method based on a Bayesian estimation theory, so that the key characteristics are determined, and the method is used for a laser radar inertial odometer to further reduce the calculated amount and realize the light weight of the system.
The prior art has two significant drawbacks in terms of computational cost (real-time): when correcting point cloud motion distortion, body motion information estimated by using a laser radar is needed, the three items of motion information optimization estimation, inter-frame feature matching and motion distortion correction are put into one large loop for iteration in the conventional algorithm, so that the relatively accurate complementary characteristic of short-time motion information of an inertial sensor is not fully utilized, and the calculation cost of the whole odometer method is increased, as shown in the upper half part of fig. 1. The second obvious disadvantage is that the prior art only selects laser features which are considered to be reserved by researchers at the human experience level, lacks a quantification method based on theoretical analysis, and cannot quantitatively evaluate the effectiveness of constraint brought by each pair of laser feature matching to positioning estimation.
Disclosure of Invention
The invention provides a light laser radar inertial odometer method based on key feature extraction, which enhances the real-time performance of a combined system by extracting the most effective laser radar feature constraint subset. The method mainly comprises the following steps: 1) Compensating the motion distortion of the laser point cloud by utilizing an intermediate variable pre-integrated by an inertial measurement unit; 2) Extracting line and surface characteristics in key frames of the multi-line laser radar to finish inter-frame characteristic matching; 3) Quantitatively evaluating the validity of the laser radar feature matching constraint according to the Bayesian estimation theory and the definition of the position precision factor in satellite navigation; 4) According to the quantitative evaluation result, a selection algorithm for eliminating the low-efficiency characteristic constraint is designed; 5) And selecting proper loop detection time, and reducing the waste of calculated amount on the premise of ensuring the accuracy. Under the condition of Jie Juesuan force limitation, the method utilizes the combination of the multi-line laser radar and the inertial measurement to realize the mapping of the three-dimensional point cloud and the high-precision positioning and posture estimation.
The invention comprises the following three key technical characteristics:
1) The motion distortion of the laser point cloud is compensated by utilizing the intermediate variable pre-integrated by the inertial measurement unit, so that the complementary characteristic of short-time motion information of the inertial sensor is fully utilized, and unnecessary parts in the laser radar odometer are prevented from being added into the cycle, as shown in the lower part of the figure 1;
2) According to the Bayesian estimation theory and the definition of the position precision factors in satellite navigation, the effectiveness of the laser radar feature matching constraint can be quantitatively evaluated in the implementation of a laser radar inertial combination positioning system, and the evaluation method can be used for screening the low-efficiency laser radar feature matching constraint and judging whether the current time is proper loop detection time or not, so that the waste of calculation amount is avoided;
3) Creatively designs a selection algorithm for eliminating the low-efficiency feature constraint, and combines the quantitative evaluation results in the step 2) to select the most effective 'key feature' subset.
Aiming at the defects of the prior art, the invention provides a lightweight laser radar inertial positioning method based on key feature extraction, which comprises the following steps:
step 1, respectively obtaining multi-frame inertial data and multi-frame laser point clouds of a moving platform through an inertial measurement unit and a laser radar, and pre-integrating the inertial data to obtain an inertial residual error of the inertial measurement unit and a first covariance matrix corresponding to the inertial residual error;
step 2, compensating the motion distortion of the laser point cloud corresponding to the time frame by using the motion increment obtained in the pre-integration process of the inertial data, and calibrating the laser point cloud of each frame to obtain a calibrated point cloud;
Step 3, extracting line and plane characteristics in the calibration point clouds, and carrying out inter-frame characteristic matching on all the calibration point clouds to obtain a plurality of laser radar characteristic constraints, and laser residual errors and a second covariance matrix corresponding to the laser radar characteristic constraints;
step 4, quantitatively analyzing the influence of each laser radar characteristic constraint on the positioning result of the motion platform to obtain an analysis result of each laser radar characteristic constraint;
step 5, selecting characteristic constraint which enables the positioning result to be obviously improved in precision according to the analysis result as efficient characteristic constraint of the laser radar;
and 6, obtaining a positioning result of the motion platform according to the laser residual error and the second covariance matrix which correspond to the efficient characteristic constraint and the inertia residual error and the first covariance matrix which correspond to the inertia residual error, wherein the positioning result comprises the direction, the position, the speed and the zero offset of the inertia measurement unit.
The specific process of calibrating each frame of laser point cloud by using the intermediate quantity of inertia pre-integration in the step 2 comprises the following steps:
when compensating the motion distortion of the laser spot at the time t, the time t is determined by the following method k Laser radar coordinate system conversion mode by time t:
Wherein the rotation vector increment delta phi k,k+1 Comprising rotation angle and rotation axis, equivalent to the time t k By time t k+1 Is rotated by an increment Δr in the coordinate system of (2) k,k+1 The two variables obtained on the left side of the above are Δφ kt And Δp kt Respectively indicate time t k Rotational vector to time tIncrement and position increment, delta phi kj And Δp kj Respectively correspond to the slave time t k By time t j Rotation vector increment and position increment of (a);
for laser point cloudsAt each specific point observed at time t, Δφ is first applied kt And Δp kt And delta phi kj And Δp kj Respectively combining to obtain the time t from the time t to the time t j Motion delta information of (a); then the rotation quantity is multiplied by the left and then the position increment is added to map the point of the motion distortion to be corrected to t j And (5) a laser radar coordinate system at the moment to finish the calibration of the laser point cloud.
The light laser radar inertial positioning method based on key feature extraction comprises the following steps:
t is obtained by j Estimation error δp of time and position state j ：
δp j ＝(J T Σ -1 J) -1 J T Σ -1 r(χ j )
Wherein r (x) j ) Including all laser residualsl E laser feature set ∈ ->A set of multiple-frame laser point clouds>J is r (x) j ) Relative to p j Jacobian matrix of (a); Σ represents the weight. δp j The error covariance matrix of (a) is:
here cov (. Cndot.) represents covariance Function (J) T J) Is a 3*3 symmetric matrix, and represents r (x j ) Relative to p j The transpose of jacobian matrix J multiplied by itself, (·) -1 Inverting the matrix;
as a result of this analysis E (·) the following formula:
where tr (·) represents the operation of matrix tracing;
the step 5 comprises the following steps:
and removing low-efficiency characteristic constraints from the multiple laser radar characteristic constraints according to the analysis result E, a preset precision expansion factor lambda and an upper limit mu of an evaluation standard, and selecting the characteristic constraints with improved positioning result precision as the high-efficiency characteristic constraints of the laser radar.
The step 6 comprises the following steps of obtaining a positioning result of the motion platform:
wherein the residual error r 0 And its covariance matrix Σ 0 In order to set the value of the preset value,is the inertial residual, Σ I For the first covariance matrix,sigma is the laser inertia residual error L For the second covariance matrix,>for time t j Laser feature set of frame,/>Is a collection of multi-frame laser point clouds.
The light laser radar inertial positioning method based on key feature extraction, wherein the step 4 further comprises the following steps:
determining loop detection timing by:
Where tr (·) represents the operation of matrix tracing, J is the residual function r (x j ) Relative to p j Jacobian matrix of r (x) j ) Including all laser residualsAnd inertial residual->Sigma is the covariance matrix Sigma of the features of the laser L And an inertial covariance matrix Σ I A block diagonal matrix is formed;
if the current moment is judged to be the proper loop detection time, judging whether the current position is overlapped with the historical track or not through the displacement track recorded by the inertia measurement unit, and if so, continuing to perform the laser radar inter-frame matching in the step 3 so as to obtain the available laser characteristic constraint.
The invention also provides a light laser radar inertial positioning system based on key feature extraction, which comprises:
the module 1 is used for respectively obtaining multi-frame inertial data and multi-frame laser point clouds of a moving platform through an inertial measurement unit and a laser radar, and pre-integrating the inertial data to obtain an inertial residual error of the inertial measurement unit and a first covariance matrix corresponding to the inertial residual error;
the module 2 is used for compensating the motion distortion of the laser point cloud corresponding to the time frame by using the motion increment obtained in the pre-integration process of the inertial data, and calibrating the laser point cloud of each frame to obtain a calibrated point cloud;
The module 3 is used for obtaining a plurality of laser radar feature constraints and corresponding laser residual errors and a second covariance matrix by extracting line and surface features in the calibration point clouds and carrying out inter-frame feature matching on all the calibration point clouds;
the module 4 is used for quantitatively analyzing the influence of each laser radar characteristic constraint on the positioning result of the motion platform to obtain an analysis result of each laser radar characteristic constraint;
the module 5 is used for selecting the characteristic constraint which improves the precision of the positioning result according to the analysis result as the efficient characteristic constraint of the laser radar;
and the module 6 is used for obtaining a positioning result of the motion platform according to the laser residual error and the second covariance matrix which correspond to the efficient characteristic constraint and the inertia residual error and the first covariance matrix which correspond to the inertia residual error, wherein the positioning result comprises the direction, the position, the speed and the zero offset of the inertia measurement unit.
The specific process of calibrating each frame of laser point cloud by using the intermediate quantity of inertial pre-integration in the module 2 comprises the following steps:
when compensating the motion distortion of the laser spot at the time t, the time t is determined by the following method k Laser radar coordinate system conversion mode by time t:
Wherein the rotation vector increment delta phi k,k+1 Comprising rotation angle and rotation axis, equivalent to the time t k By time t k+1 Is rotated by an increment Δr in the coordinate system of (2) k,k+1 The two variables obtained on the left side of the above are Δφ kt And Δp kt Respectively indicate time t k Rotation vector increment and position increment by time t, delta phi kj And Δp kj Respectively correspond to the slave time t k By time t j Rotation vector increment and position increment of (a);
for laser point cloudsAt each specific point observed at time t, Δφ is first applied kt And Δp kt And delta phi kj And Δp kj Respectively combining to obtain the time t from the time t to the time t j Motion delta information of (a); then the rotation quantity is multiplied by the left and then the position increment is added to map the point of the motion distortion to be corrected to t j And (5) a laser radar coordinate system at the moment to finish the calibration of the laser point cloud.
The light laser radar inertial positioning system based on key feature extraction, wherein the module 4 comprises:
t is obtained by j Estimation error δp of time and position state j ：
δp j ＝(J T Σ -1 J) -1 J T Σ -1 r(χ j )
Wherein r (x) j ) Including all laser residualsl E laser feature set ∈ ->A set of multiple-frame laser point clouds>J is r (x) j ) Relative to p j Jacobian matrix of (a); Σ represents the weight. δp j The error covariance matrix of (a) is:
here cov (. Cndot.) represents the covariance function, (J) T J) Is a 3*3 symmetric matrix, and represents r (x j ) Relative to p j The transpose of jacobian matrix J multiplied by itself, (·) -1 Inverting the matrix;
as a result of this analysis E (·) the following formula:
where tr (·) represents the operation of matrix tracing;
the module 5 comprises:
and removing low-efficiency characteristic constraints from the multiple laser radar characteristic constraints according to the analysis result E, a preset precision expansion factor lambda and an upper limit mu of an evaluation standard, and selecting the characteristic constraints with improved positioning result precision as the high-efficiency characteristic constraints of the laser radar.
The light laser radar inertial positioning system based on key feature extraction, wherein the module 6 obtains the positioning result of the motion platform by the following formula:
wherein the residual error r 0 And its covariance matrix Σ 0 In order to set the value of the preset value,is the inertial residual, Σ I For the first covariance matrix,sigma is the laser inertia residual error L For the second covariance matrix,>for time t j Laser feature set of frame,/>Is a collection of multi-frame laser point clouds.
The light laser radar inertial positioning system based on key feature extraction, wherein the module 4 further comprises:
determining loop detection timing by:
where tr (·) represents the operation of matrix tracing, J is the residual function r (x j ) Relative to p j Jacobian matrix of r (x) j ) Including all laser residualsAnd inertial residual->Sigma is the covariance matrix Sigma of the features of the laser L And an inertial covariance matrix Σ I A block diagonal matrix is formed;
if the current moment is judged to be the proper loop detection time, judging whether the current position is overlapped with the historical track or not through the displacement track recorded by the inertia measurement unit, and if so, continuing to perform the laser radar inter-frame matching in the module 3 to obtain the available laser characteristic constraint.
The advantages of the invention are as follows:
compared with the prior art, the invention has the advantages of greatly improving the reduction of the calculation cost and the improvement of the real-time performance. Firstly, because the motion information provided by the inertial sensor is used in the process of correcting the motion distortion of the laser point cloud, the links of 'finishing feature extraction and matching from the current observation point cloud-estimating the motion information-finishing the motion distortion correction-finishing feature extraction and matching from the corrected laser point cloud' are avoided. In addition, in the optimization process, the dimension of the corresponding jacobian matrix is obviously reduced due to the extraction of the most effective subset matched with the laser features, so that the calculation cost is reduced under the condition that the expected positioning and mapping precision is ensured. Therefore, the lightweight laser radar inertial combination positioning method and system based on key feature extraction disclosed by the invention have better real-time performance than the prior art under the condition of limited calculation force.
Drawings
FIG. 1 is a flow chart comparison of a prior LOAM method and a method of the present invention;
FIG. 2 is a general flow chart of the present invention;
FIG. 3 is an exemplary diagram of the intermediate variable compensated laser point cloud motion distortion of the present invention;
FIG. 4 is a graph of a culling algorithm of the low-efficiency feature constraint of the present invention.
Detailed Description
The inventor finds that defects in the prior art are caused by two aspects when carrying out light weight research on a laser radar inertial combination positioning method, and firstly, when completing the motion distortion correction of laser point cloud, the complementary characteristic of accurate short-time motion information of an inertial sensor is not fully utilized; secondly, for laser feature extraction, the contribution of each pair of feature matching to positioning accuracy is not analyzed from a theoretical angle, and the validity assessment of laser feature constraint is not quantized. For the first defect reason, mainly because the use of inertial sensor information for laser point cloud motion distortion correction involves integration over manifold, it is necessary to demonstrate the rationality of using linear interpolation approximation in special euclidean groups that do not close to addition. For the second defect reason, the contribution of each pair of feature matches to positioning accuracy is theoretically quantified, the propagation of errors of nonlinear estimation approximation on covariance matrices needs to be deduced from Bayesian theory, and both of the two require more complex mathematical theory deductions, so that the prior art does not consider the two.
The inventor finds out through strict geographic theory deduction and experimental verification that the factors which limit the improvement of the inertial combination positioning instantaneity of the laser radar can be solved. The invention still uses the thought of LOAM method when extracting the initial laser characteristic, has extracted the point on edge line and level, then has introduced the brand-new combination positioning system to realize the flow: by extracting the most effective lidar feature constraint subset, the real-time performance of the combined system is enhanced. The method mainly comprises the following steps: 1) Compensating the motion distortion of the laser point cloud by utilizing an intermediate variable pre-integrated by an inertial measurement unit; 2) Extracting line and surface characteristics in key frames of the multi-line laser radar to finish inter-frame characteristic matching; 3) Quantitatively evaluating the validity of the laser radar feature matching constraint according to the Bayesian estimation theory and the definition of the position precision factor in satellite navigation; 4) According to the quantitative evaluation result, a selection algorithm for eliminating the low-efficiency characteristic constraint is designed; 5) And selecting proper loop detection time, and reducing the waste of calculated amount on the premise of ensuring the accuracy. Under the condition of Jie Juesuan force limitation, the method utilizes the combination of the multi-line laser radar and the inertial measurement to realize the mapping of the three-dimensional point cloud and the high-precision positioning and posture estimation.
In order to make the above features and effects of the present invention more clearly understood, the following specific examples are given with reference to the accompanying drawings.
The invention provides a lightweight laser radar inertial combined positioning system, which aims at estimating position and body motion information by using sensor observables, and in order to clearly describe the whole positioning process, three reference coordinate systems are defined at first:
1) The body coordinate system is denoted by symbol B in the following description, and its origin is at the center of the mobile platform (e.g., robot, unmanned vehicle), and for simplicity of description, the coordinate system of the inertial sensor is set to coincide with the body coordinate system in the present invention;
2) The lidar coordinate system is denoted by L, in which the laser point cloud is measured, and since the lidar and inertial sensors are strapdown on the mobile platform throughout the multisensor system, the transformation from the lidar coordinate system to the body coordinate system is considered to be known in advance by an outlier calibration.
3) The map coordinate system is denoted by M, coincides with the body coordinate system at the initial time, and is fixed. The map coordinate system is used for representing the track and the mapping result, the z-axis of the map coordinate system is defined to be perpendicular to the elliptic surface of the earth, and the other two coordinate systems are on a plane perpendicular to the z-axis and accord with the right-hand spiral rule.
The alphabetic abbreviation B, L, M of the three above coordinate systems, as appearing in the right superscript of the symbolic variable below, indicates that the current variable is projected on the corresponding seatAnd is indicated in the label. The laser radar inertial combination positioning system provided by the invention can be abstracted into a state estimation problem, and at t k The state vector to be estimated at the moment contains the direction R k Position p k Velocity v k Zero offset of inertial measurement unit
Fig. 2 is an overview of a flow chart of the present invention, and the implementation of each module in the flow chart will be separately described.
(1) Pre-integration of inertial measurement unit
The output of the inertial measurement unit is the motion of the body coordinate system relative to the inertial system, and the map coordinate system M can be regarded as an inertial system when the influence of the earth rotation is ignored. The pre-integration theory is expanded to the lie algebra from Forster in 2015, and the theory is directly used when the inertial measurement unit data are processed.
The output of the inertial measurement unit is the angular velocityAnd specific force->Wherein the upper wavy line indicates that this is an observed value containing noise and is not a true physical value; the subscript k indicates that this is t k Time observation quantity; the superscript B represents that these two motion properties are represented in the body coordinate system (B-system). Angular velocity observance +. >Gyro (gyroscillope) zero bias requiring subtraction of inertial measurement unit +.>One symbolRandom noise of Gaussian distribution>The angular velocity amount proximate to the true value will be obtained. Similarly, t k Moment of specific force->It is necessary to add the gravitational acceleration and subtract the accelerometer zero bias +.>And random noise->Acceleration in the inertial frame of reference is obtained.
In the pre-integration theory, the set acceleration and angular velocity remain constant for one inertia sampling period Δt. In the following description, Δt will be taken as the discrete time interval, e.g. t j -t i = (j-i) Δt. In the invention, the inertial sensor and the laser radar can complete time synchronization in advance, so that a tiny time interval between observations output by the inertial sensor and the laser radar can be ignored. Taking into account inertial sensor zero offsetThe influence of (a) at time t i And time t j The motion delta between them can be written as:
here [ DELTAR ij ,△v ij ,△p ij ]Not corresponding to a change in the amount of motion of the physical world, but rather independent of t i Pre-integration observations of time-of-day state vectors, ΔR ij Representing the slave time t i By time t j Is rotated by an increment of Deltav in the coordinate system of (2) ij Delta p for speed increment ij Is a position increment. Exp (·) in the above formula is an exponential mapping function of the prune group SO (3), For time t i Direction cosine matrix R of (2) i Transpose of R j For time t j Is a directional cosine matrix of (a). V in the second formula i And v j Respectively time t i And t j G represents the gravitational acceleration constant, ΔR ik Namely from time t i By time t k Is rotated by an increment of the coordinate system of (a). P in the third formula i And p j Respectively time t i And t j Is a v ik Namely from time t i By time t k Is a speed increment of (c).
Only the derivation of the pre-integration theory and the most relevant part of the system is given here, and the pre-integration theory can directly use the prior art, so that details of residual errors in the pre-integration paper are omittedAnd its covariance matrix sigma I Here we denote the Inertial Measurement Unit (IMU) with the capital letter I, the subscript being the time t i Key frame and time t j Key frames. This residual term will be input as a constraint to the optimization module to obtain the result of the state estimation.
(2) Key frame selection for lidar
In order to reduce the calculation cost of the whole laser radar inertial combination positioning system, the invention only focuses on a subset of laser radar observations, but does not perform position estimation and point cloud map update once at the moment of each laser radar observation, and the method is also commonly called a key frame selection technology. The reason for this is that when a mobile platform (such as a robot or an autopilot) carrying the lidar is in an approximately stationary or very slow state, the lidar observations of two adjacent frames are very close and do not provide more effective information, so that the invention only focuses on those 'key frames' with larger motion changes when implementing a combined positioning system.
At this time, the pre-integration technology of the inertial device can be used to avoid repeatedly calculating the high-dimensional matrix in the estimation process, and meanwhile, the motion information is provided for the selection of the key frames: when the motion between the current lidar coordinate system L and the previous keyframe exceeds a preset value, the current lidar coordinate system is considered to be a new keyframe and is thus added to the subsetThis subset->Representing up to time t n Is a set of key frames. Therefore, in the present invention, the total state vector outputted in the optimization module is +.>Here x i Representing t i State vector to be estimated at the moment.
(3) Using pre-integrated intermediate quantities of inertial measurement units for motion distortion correction of laser point clouds
The compensation of motion distortion of the laser point cloud by means of an intermediate variable pre-integrated by an inertial measurement unit, which will be described below, is one of the core technical points of the present invention.
In the present invention, the motion delta DeltaT is obtained using pre-integration k,k+1 ＝[ΔR k,k+1 |Δp k,k+1 ]To achieve motion distortion correction of key frames, where ΔR k,k+1 Namely from time t k By time t k+1 Is rotated by delta p k,k+1 Is the position increment during this time. In the permitIn the multi-laser radar mileage calculation method, a uniform motion model is often adopted. However, as shown in fig. 3, if the acceleration and angular rate are not constant over the laser radar scan range, linear interpolation of the motion estimation values using the laser radar odometer output will result in a larger error (corresponding to the gray dark portion in fig. 3 being the error between the linear interpolation and the true value). In contrast, the error in the present invention in performing the motion distortion correction using the intermediate variable in the pre-integration process is small due to the higher sampling frequency (corresponding to the dotted line of fig. 3, it is apparent that the area of the interval between the dotted line and the solid line is smaller than that of the gray dark portion).
Taking the example of compensating the motion distortion of the laser spot observed at the moment t, at the moment t k And time t k+1 ＝t k Increment of movement delta T between + delta T k,k+1 Can be obtained from an intermediate quantity of the pre-integration process, whose linear interpolation will compensate for the motion distortion at time t. In the following description, the following will be usedTo express laser point cloud data, which is the original observation point cloud before correction, the subscript thereof and the end time t of the whole scan j Consistent (as also expressed in figure 3). Then time t k The transformation mode of the laser radar coordinate system at the time t can be approximately calculated by the following formula:
wherein the rotation vector increment delta phi k,k+1 Comprising rotation angle and rotation axis, equivalent to the time t k By time t k+1 Is rotated by an increment Δr in the coordinate system of (2) k,k+1 Conversion between the two can be accomplished by the rotdrign rotation formula. The two variables obtained on the left side above are Δφ kt And Δp kt Respectively indicate time t k The rotation vector increment and the position increment by time t. For point cloudsMiddle inA specific point observed at time t is first of all Δφ kt And Δp kt And delta phi kj And Δp kj Respectively combining to obtain the time t from the time t to the time t j Motion delta information of (a); the point of the motion distortion to be corrected can be mapped to t by multiplying the rotation quantity and then adding the translation quantity (namely the position increment) j A lidar coordinate system of time. Delta phi as described above kj And Δp kj Can also be obtained during the pre-integration process, respectively corresponding to the slave time t k By time t j Rotation vector increment and position increment of (c).
(4) Selection, feature extraction and matching of laser radar
The laser characteristic extraction and matching method of the invention is the same as LOAM: edge points and plane points are selected by using curvature, and then corresponding edge lines and planes are found in other key frames. Because the LOAM method requires motion distortion correction using the motion information estimated by the lidar odometer, feature matching is divided into two different frequencies, a "frame-to-frame" matching and a "frame-to-map" matching. In the invention, the motion increment provided by inertial pre-integration and the coordinate system transformation in the state variable can replace the motion estimation obtained by the inter-frame matching, so that the characteristic matching from frame to sub-image is only needed. The subgraph comprises a series of key frames, and the distance between the key frames to be matched and the current frame is required to be relatively close; when a loop exists, the key frames associated with the loop are also added to the sub-graph.
After the motion distortion of the laser point cloud is corrected, the original observation point cloudTo be uniformly corrected to t j Time lidar coordinate system. For the point cloud with motion distortion correction, the laser characteristic set can be extracted according to the LOAM method>Its representation in the map coordinate system M is written as +.>In the following, when a physical quantity is represented by a symbol, the subscript j indicates that this point or point cloud belongs to the scan end time t j Corresponding laser radar frame, and similarly, subscript i indicates that it belongs to the scanning ending time t i And the corresponding laser radar frame. The superscript M indicates that this physical quantity is represented under the map coordinate system M. And the subscripts l, α, β, γ hereinafter are numerals for distinguishing different points, and are similar to numerals 1, 2, 3, 4.
For laser feature setsA point l, < +.>Representing its position in the map coordinate system; />And->Indicating that it is at t i The positions of two nearest points in a frame are scanned temporally. Since two points can determine the position of a line, t can be calculated by using the laser characteristic matching relationship j Edge feature point l in time frame and its position at t i Distance between edges (edge) corresponding to the moment ∈>The following formula. The tip cap label on the symbol here +.>Indicating that this variable is an estimated value and that there is a residual between the true value.
For laser feature setsPoint l,/on one plane of (c)>And->Indicating that it is at t i The positions of three non-collinear points on the corresponding plane in the time sweep. Since the non-collinear three points can determine the position of a plane, t can be calculated by using the laser characteristic matching relationship j Planar feature point l in time frame and its position at t i Distance between planes (planes) corresponding to the moment +.>
The estimated distance brought by the matching relation of the laser characteristic points can be calculated by the two formulas It and the corresponding true distance d= { d e ,d p The difference between the laser characteristic matching residual functions can be obtained>This residual will be added as a constraint to the optimization process to complete the position and motion information estimation. Residual function->The superscript L of (1) indicates that it originates from LiDAR (LiDAR), the subscript j is from +.>The value of (1) represents the time count in the key frame subset, and the subscript l is +_ from the laser feature set at the current time>And (3) the value of the characteristic point represents the count of the characteristic point. The calculation mode of the true distance is as follows: when t j Frame sum of time t i When the inter-frame rotation and translation estimation at the moment is free of errors, the characteristic edge points are located on the found corresponding edge lines, so that the real distance is 0 at the moment; the same feature plane point should also be on the corresponding plane found, at which point the true distance is also 0.
(5) Optimization and state estimation
In the present invention, the state vector to be estimated is as described in (2)In general, assuming that all observation noise accords with the gaussian distribution of zero mean, solving the maximum posterior probability estimation problem abstracted by the combined positioning system is equivalent to minimizing negative log likelihood, and the problem can be written as the following mahalanobis distance form:
in the optimization process of the invention, namely solving the weighted least square problem, the problem can be solved by using mature nonlinear optimization such as LM (Levenberg-Marquardt method) method, gauss Newton method and the like. Here residual r 0 And its covariance matrix sigma 0 This a priori information needs to be given when combined with global class sensors such as GPS, depending on the initialization; this can be ignored for most local positioning problems. Residual errorAnd its covariance matrix sigma I Obtained by a pre-integration process of the inertial measurement unit, the definition of which has been explained in detail in (1). Residual error->And its covariance matrix sigma L Corresponding to the laser characteristics, the corresponding variable definitions and calculation modes have been explained in detail in (4), e.g.>For time t j Laser feature set of frame. After that (6), the method for quantifying the validity of the laser feature matching constraint and the selection of key features will be described; suitable loop detection timing determination is described in (7).
(6) Quantification of laser feature availability and selection of key features
By matching the laser features between frames, a number of constraints provided by the lidar measurements can be obtained, their corresponding residual functionsWill be used for state variable estimation in the optimization. To reduce the amount of computation, the present invention will remove some unnecessary constraints within the acceptable accuracy performance, rather than using all the data. Aiming at judging whether each laser characteristic matching constraint provides inefficient information, the invention provides an evaluation method, which is one of the core technical points of the invention.
The estimation method is defined according to Bayesian estimation theory and position precision factors in satellite navigation. The accuracy of the laser radar odometer is not only dependent on the magnitude of the observed noise, but also on the geometrical distribution of the laser characteristic matching constraints, as analyzed in a physical sense. In the implementation process of the laser radar odometer, the line-of-sight direction (the line of sight) from the mobile platform to the laser feature point is quite similar to the line-of-sight from the mobile platform to the satellite in the satellite positioning field. In the satellite positioning field, the impact of the geometric distribution of the line of sight on the positioning accuracy can be quantified with a precision factor (Dilution of Precision, DOP), which represents the uncertainty of how much the observed noise affects the state estimation.
The state vector to be estimated in the lidar inertial odometer has been described in the above implementation stepsInertial residual as constraint +.>And laser characteristic residual->Since the two types of residuals are independent, their impact on state error can be discussed separately. Using a first order taylor series expansion, at t j Estimation error δp of time and position state j Can be expressed as:
δp j ＝(J T Σ -1 J) -1 J T Σ -1 r(χ j )
wherein r (x) j ) Including all laser feature residualsHere->The meaning is explained in (4). And J in the above formula is r (x) j ) Relative to p j Jacobian matrix of (a); Σ represents the weight, is equivalent to the laser characteristic covariance matrix described above, and is typically reduced to a unit matrix. δp as a result of iterative optimization j The error covariance matrix of (2) can be calculated by the following formula:
here cov (. Cndot.) represents the covariance function, (J) T J) Is a 3*3 symmetric matrix, representing r (x j ) Relative to p j The transpose of jacobian matrix J multiplied by itself, (·) -1 The matrix inversion is calculated. J is defined according to laser characteristic constraint T J depends on the relative geometrical distribution of feature matching. Here, r (x j ) The assumption that the elements are independently and equidistributed is set as variance
Based on the above theoretical basis, the present invention proposes an evaluation criterion E (·) that employs the following function:
/>
where tr (·) represents the operation of matrix tracing, which calculates the sum of the elements on the main diagonal of the matrix (diagonal from top left to bottom right). The evaluation method provided by the invention is different from the precision factor in the satellite positioning field in that the evaluation standard of the invention needs to calculate the bias of the residual function of the laser radar, and the jacobian matrix J of the precision factor in the satellite positioning field is directly formed by the sight vector. It should be noted that, in the effective measurement range of the laser radar adopting TOF ranging, the ranging error basically keeps constant, which accords with the assumption that all elements are independently and uniformly distributed; the distance measurement error of the visual odometer is proportional to the distance, and the important performance makes the laser radar odometer suitable for the evaluation standard proposed in the invention, and the visual odometer cannot be used.
Based on the proposed quantitative evaluation standard, the invention also provides a selection algorithm for eliminating the low-efficiency characteristic constraint, and the elimination method sets a precision expansion factor lambda which indicates how many times the tolerable state estimation error is amplified after the low-efficiency characteristic constraint is eliminated. At the same time, an upper limit mu of an evaluation standard is set, and the inefficiency is eliminated The evaluation function value after the characteristic is not allowed to exceed the upper limit so as to ensure that the positioning mapping accuracy is not seriously damaged. The rejection algorithm pseudo code of this inefficient feature constraint is shown in FIG. 4, whose result is subject to r (x j ) The order of laser feature matching in the middle, and thus the "key feature subset" selected is not necessarily optimal, but experiments have shown that this selection method is more efficient than random screening.
The rejection algorithm of the low-efficiency characteristic constraint provided by the invention is described as follows:
i. firstly, setting an accuracy expansion factor lambda, preferably lambda takes a value between 1.2 and 2; meanwhile, setting an upper limit mu of an evaluation standard, wherein the specific value of the upper limit mu is required to be determined according to a specific positioning scene;
the second step is to calculate the corresponding residual error according to the laser radar characteristic matchingIt is relative to p j Jacobian matrix J;
third step requires the residual error to be first takenThe elements in (a) are randomly ordered to obtain a new residual vector +.>Let->The method aims at avoiding the influence of the sequence of laser characteristic matching on a rejection algorithm as much as possible;
iv, calculating an evaluation function value corresponding to the current residual errorAttention to->The element sequence of the evaluation function value is not influenced;
v. in the calculation processUpper bounds of evaluation function values of (a)
Start cycle process, pairThe following steps are completed in turn:
a) Order theDeletion of->Is selected by the current cycle>Which represents that its corresponding feature matching constraint is deleted;
b) Deleting the corresponding row in the jacobian matrix J and then calculating
c) If it isLess than E max And mu, then let +.> And the corresponding row in the jacobian matrix J is kept deleted, which means that the laser characteristic matching corresponding to the residual element is deleted without excessively influencing the position precision, so that the current element can be removed as an inefficient characteristic constraint; otherwise, do not update->The rest areEach variable remains unchanged;
when the treatment is completedEnding the cycle, and calculating the compression rate, wherein the compression rate represents that the laser characteristic constraint which is still reserved accounts for the original percentage, and the smaller the value of the compression rate is, the more effective the screening result is;
ending the algorithm, returning the value of the compression rate and the resulting residual after elimination of the feature constraintsThis new residual will be used to optimize the process output state estimation results.
(7) Proper loop detection timing determination
When the mobile platform walks to the estimated position again and establishes a new relation with the existing laser feature points in the map, the mobile platform is regarded as a loop detection occurrence. When a loop is established, a drop in uncertainty of the state estimate may be desired. It is known that matching for loop detection is computationally expensive, and therefore it is wasteful if possible loop matches are detected at each instant. In addition, some laser feature matching results in a constraint that is not necessarily valid, in which case it is not necessary to perform the detection if the current state estimation error is small, such as when the odometer just starts to move, or when the last loop detection just ends.
Then when is the proper timing for loop back detection? In the invention, the necessary loop detection time is determined by setting a threshold vector. When the state estimation error exceeds this threshold, the accuracy of the estimation problem is considered to be corrected, at which time loop detection attempts are made at each key frame. The calculation of the state estimation error is similar to that in (6), but the residual function here does not only contain the laser characteristic residualAlso to include inertial residuals related to pre-integrationIn calculating the state error, the covariance matrix Σ is not necessarily assumed to be a unit matrix, and only the weighted partial calculation is carried out according to the sensor parameters.
The judgment formula of the loop detection time is as follows:
where tr (·) represents the operation of matrix tracing, J is the residual function r (x j ) Relative to p j Jacobian matrix of r (x) j ) Including all laser feature residualsAnd inertial residual->Where Σ corresponds to the characteristic covariance matrix Σ by the laser L And an inertial covariance matrix Σ I The specific definition of the block diagonal matrix is set forth in (4) and (1), respectively. When the value of the above judgment formula exceeds the manually set threshold, the judgment is judged to be proper loop detection time, and the manually set threshold is related to the precision required by the positioning task, and when the higher precision is required to be realized, the threshold is required to be set smaller.
If the current moment is judged to be the proper loop detection opportunity, the loop detection module starts to take effect: firstly, roughly judging whether the current position is possibly overlapped with a historical track through a displacement track recorded by an inertia measurement unit; if there is a coincidence, continuing the laser radar inter-frame matching as described in (4), except that this time the laser radar inter-frame matching does not occur within two frames of adjacent time, but occurs in the current frame and the history frame where the trajectory is considered to coincide, to obtain a new available laser feature matching constraint whose corresponding residual function is to be added to the optimization process to complete the state estimation; if it is determined that there is no overlap, it is stated that the current time does not form a loop with the historical track, and therefore no new residual function is available to be added to the optimization process.
The following is a system example corresponding to the above method example, and this embodiment mode may be implemented in cooperation with the above embodiment mode. The related technical details mentioned in the above embodiments are still valid in this embodiment, and in order to reduce repetition, they are not repeated here. Accordingly, the related technical details mentioned in the present embodiment can also be applied to the above-described embodiments.
The invention also provides a light laser radar inertial positioning system based on key feature extraction, which comprises:
the module 1 is used for respectively obtaining multi-frame inertial data and multi-frame laser point clouds of a moving platform through an inertial measurement unit and a laser radar, and pre-integrating the inertial data to obtain an inertial residual error of the inertial measurement unit and a first covariance matrix corresponding to the inertial residual error;
the module 2 is used for compensating the motion distortion of the laser point cloud corresponding to the time frame by using the motion increment obtained in the pre-integration process of the inertial data, and calibrating the laser point cloud of each frame to obtain a calibrated point cloud;
the module 3 is used for obtaining a plurality of laser radar feature constraints and corresponding laser residual errors and a second covariance matrix by extracting line and surface features in the calibration point clouds and carrying out inter-frame feature matching on all the calibration point clouds;
the module 4 is used for quantitatively analyzing the influence of each laser radar characteristic constraint on the positioning result of the motion platform to obtain an analysis result of each laser radar characteristic constraint;
the module 5 is used for selecting the characteristic constraint which obviously improves the precision of the positioning result as the efficient characteristic constraint of the laser radar according to the analysis result;
And the module 6 is used for obtaining a positioning result of the motion platform according to the laser residual error and the second covariance matrix which correspond to the efficient characteristic constraint and the inertia residual error and the first covariance matrix which correspond to the inertia residual error, wherein the positioning result comprises the direction, the position, the speed and the zero offset of the inertia measurement unit.
The specific process of calibrating each frame of laser point cloud by using the intermediate quantity of inertial pre-integration in the module 2 comprises the following steps:
when compensating the motion distortion of the laser spot at the time t, the time t is determined by the following method k Laser radar coordinate system conversion mode by time t:
wherein the rotation vector increment delta phi k,k+1 Comprising rotation angle and rotation axis, equivalent to the time t k By time t k+1 Is rotated by an increment Δr in the coordinate system of (2) k,k+1 The two variables obtained on the left side of the above are Δφ kt And Δp kt Respectively indicate time t k Rotation vector increment and position increment by time t, delta phi kj And Δp kj Respectively correspond to the slave time t k By time t j Rotation vector increment and position increment of (a);
for laser point cloudsAt each specific point observed at time t, Δφ is first applied kt And Δp kt And delta phi kj And Δp kj Respectively combining to obtain the time t from the time t to the time t j Motion delta information of (a); then the rotation quantity is multiplied by the left and then the position increment is added to map the point of the motion distortion to be corrected to t j And (5) a laser radar coordinate system at the moment to finish the calibration of the laser point cloud.
The light laser radar inertial positioning system based on key feature extraction, wherein the module 4 comprises:
t is obtained by j Estimation error δp of time and position state j ：
δp j ＝(J T Σ -1 J) -1 J T Σ -1 r(χ j )
Wherein r (χ) j ) Including all laser residualsl E laser feature set ∈ ->A set of multiple-frame laser point clouds>J is r (x) j ) Relative to p j Jacobian matrix of (a); Σ represents the weight. δp j The error covariance matrix of (a) is:
here cov (. Cndot.) represents the covariance function, (J) T J) Is a 3*3 symmetric matrix, r (X j ) Relative to p j The transpose of jacobian matrix J multiplied by itself, (·) -1 Inverting the matrix;
as a result of this analysis E (·) the following formula:
where tr (·) represents the operation of matrix tracing;
the module 5 comprises:
and removing low-efficiency characteristic constraints from the multiple laser radar characteristic constraints according to the analysis result E, a preset precision expansion factor lambda and an upper limit mu of an evaluation standard, and selecting the characteristic constraints with improved positioning result precision as the high-efficiency characteristic constraints of the laser radar.
The light laser radar inertial positioning system based on key feature extraction, wherein the module 6 obtains the positioning result of the motion platform by the following formula:
wherein the residual error r 0 And its covariance matrix Σ 0 In order to set the value of the preset value,is the inertial residual, Σ I For the first covariance matrix,sigma is the laser inertia residual error L For the second covariance matrix,>for time t j Laser feature set of frame,/>Is a collection of multi-frame laser point clouds.
The light laser radar inertial positioning system based on key feature extraction, wherein the module 4 further comprises:
determining loop detection timing by:
where tr (·) represents the operation of matrix tracing, J is the residual function r (x j ) Relative to p j Jacobian matrix, r (χ j ) Including all laser residualsAnd inertial residual->Sigma is the covariance matrix Sigma of the features of the laser L And an inertial covariance matrix Σ I A block diagonal matrix is formed;
if the current moment is judged to be the proper loop detection time, judging whether the current position is overlapped with the historical track or not through the displacement track recorded by the inertia measurement unit, and if so, continuing to perform the laser radar inter-frame matching in the module 3 to obtain the available laser characteristic constraint.
Claims (8)
1. The lightweight laser radar inertial positioning method based on key feature extraction is characterized by comprising the following steps of:
step 1, respectively obtaining multi-frame inertial data and multi-frame laser point clouds of a moving platform through an inertial measurement unit and a laser radar, and pre-integrating the inertial data to obtain an inertial residual error of the inertial measurement unit and a first covariance matrix corresponding to the inertial residual error;
step 2, compensating the motion distortion of the laser point cloud corresponding to the time frame by using the motion increment obtained in the pre-integration process of the inertial data, and calibrating the laser point cloud of each frame to obtain a calibrated point cloud;
step 3, extracting line and plane characteristics in the calibration point clouds, and carrying out inter-frame characteristic matching on all the calibration point clouds to obtain a plurality of laser radar characteristic constraints, and laser residual errors and a second covariance matrix corresponding to the laser radar characteristic constraints;
step 4, quantitatively analyzing the influence of each laser radar characteristic constraint on the positioning result of the motion platform to obtain an analysis result of each laser radar characteristic constraint;
step 5, selecting feature constraint which improves the precision of the positioning result as efficient feature constraint of the laser radar according to the analysis result;
step 6, according to the laser residual error and the second covariance matrix which correspond to the efficient characteristic constraint and the inertia residual error and the first covariance matrix which correspond to the inertia residual error, a positioning result of the motion platform is obtained, wherein the positioning result comprises the direction, the position, the speed and zero offset of the inertia measurement unit;
Wherein the step 4 comprises:
by the following stepsObtaining t j Estimation error δp of time and position state j ：
Wherein the method comprises the steps ofIncluding all laser residuals->l E laser feature set ∈ ->J epsilon set of multi-frame laser point clouds>J is->Relative to p j Jacobian matrix of (a); sigma represents the weight, p j For time t j Is (are) located>Is a state vector δp j The error covariance matrix of (a) is:
here cov (. Cndot.) represents the covariance function, (J) T J) Is a 3*3 symmetric matrix, which representsRelative to p j The transpose of jacobian matrix J multiplied by itself, (·) -1 Is a matrixInversion calculation, each variance is set as +.>
As the analysis result E, the following formula is given:
where tr (·) represents the operation of matrix tracing;
the step 5 comprises the following steps:
and removing low-efficiency characteristic constraints from the multiple laser radar characteristic constraints according to the analysis result E, a preset precision expansion factor lambda and an upper limit mu of an evaluation standard, and selecting the characteristic constraints with improved positioning result precision as the high-efficiency characteristic constraints of the laser radar.
2. The inertial positioning method of lightweight lidar based on key feature extraction as claimed in claim 1, wherein the specific process of calibrating the laser point cloud per frame using the intermediate amount of inertial pre-integration in step 2 comprises:
When compensating the motion distortion of the laser spot at the time t, the time t is determined by the following method k Laser radar coordinate system conversion mode by time t:
wherein the rotation vector incrementComprising rotation angle and rotation axis, equivalent to the time t k By time t k+1 Is rotated by an increment Δr in the coordinate system of (2) k,k+1 Two variables obtained on the left side of the above are +.>And Δp kt Respectively indicate time t k Rotation vector increment and position increment by time t, +.>And Δp kj Respectively correspond to the slave time t k By time t j Rotation vector increment and position increment, Δp of (a) k,k+1 For time t k By time t k+1 Position increment in;
for laser point cloudsEvery specific point observed at time t will first be +.>And Δp kt And->And Δp kj Respectively combining to obtain the time t from the time t to the time t j Motion delta information of (a); then the rotation quantity is multiplied by the left and then the position increment is added to map the point of the motion distortion to be corrected to t j And (5) a laser radar coordinate system at the moment to finish the calibration of the laser point cloud.
3. The inertial positioning method of light lidar based on key feature extraction of claim 1, wherein the step 6 comprises obtaining the positioning result of the motion platform by the following formula
Wherein the residual error r 0 And its covariance matrix Σ 0 In order to set the value of the preset value,is the inertial residual, Σ I For the first covariance matrix,>sigma is the laser inertia residual error L For the second covariance matrix,>for time t j Laser feature set of frame,/>For a set of multi-frame laser point clouds, the state vector to be estimated is +.> x i Represents t i State vector to be estimated at moment, at t i Direction of time R i Position p i Velocity v i Zero offset of inertial measurement unit>
4. The inertial positioning method of lightweight lidar based on key feature extraction of claim 1, wherein the step 4 further comprises:
determining loop detection timing by:
where tr (·) represents the operation of matrix tracing and J is the residual functionRelative to p j Is a jacobian matrix of (c),including all laser residuals->And inertial residual->Sigma is the covariance matrix Sigma of the features of the laser L And an inertial covariance matrix Σ I A block diagonal matrix is formed;
if the current moment is judged to be the proper loop detection time, judging whether the current position is overlapped with the historical track or not through the displacement track recorded by the inertia measurement unit, and if so, continuing to perform the inter-frame feature matching in the step 3 to obtain the available laser feature constraint.
5. Lightweight lidar inertial positioning system based on key feature extraction, characterized by comprising:
the module 1 is used for respectively obtaining multi-frame inertial data and multi-frame laser point clouds of a moving platform through an inertial measurement unit and a laser radar, and pre-integrating the inertial data to obtain an inertial residual error of the inertial measurement unit and a first covariance matrix corresponding to the inertial residual error;
the module 2 is used for compensating the motion distortion of the laser point cloud corresponding to the time frame by using the motion increment obtained in the pre-integration process of the inertial data, and calibrating the laser point cloud of each frame to obtain a calibrated point cloud;
the module 3 is used for obtaining a plurality of laser radar feature constraints and corresponding laser residual errors and a second covariance matrix by extracting line and surface features in the calibration point clouds and carrying out inter-frame feature matching on all the calibration point clouds;
the module 4 is used for quantitatively analyzing the influence of each laser radar characteristic constraint on the positioning result of the motion platform to obtain an analysis result of each laser radar characteristic constraint;
the module 5 is used for selecting the characteristic constraint which improves the precision of the positioning result according to the analysis result as the efficient characteristic constraint of the laser radar;
The module 6 is used for constraining the corresponding laser residual error and the second covariance matrix as well as the inertia residual error and the corresponding first covariance matrix according to the high-efficiency characteristic to obtain a positioning result of the motion platform, wherein the positioning result comprises the direction, the position, the speed and the zero offset of the inertia measurement unit;
the module 4 comprises:
t is obtained by j Estimation error δp of time and position state j ：
Wherein the method comprises the steps ofIncluding all laser residuals->l E laser feature set ∈ ->J epsilon set of multi-frame laser point clouds>J is->Relative to p j Jacobian matrix of (a); sigma represents the weight, p j For time t j Is (are) located>Is a state vector δp j The error covariance matrix of (a) is:
here cov (. Cndot.) represents the covariance function, (J) T J) Is a 3*3 symmetric matrix, which representsRelative to p j The transpose of jacobian matrix J multiplied by itself, (·) -1 For matrix inversion calculation, each variance is set to +.>
As the analysis result E, the following formula is given:
where tr (·) represents the operation of matrix tracing;
the module 5 comprises:
and removing low-efficiency characteristic constraints from the multiple laser radar characteristic constraints according to the analysis result E, a preset precision expansion factor lambda and an upper limit mu of an evaluation standard, and selecting the characteristic constraints with improved positioning result precision as the high-efficiency characteristic constraints of the laser radar.
6. The inertial positioning system of lightweight lidar based on key feature extraction of claim 5, wherein the specific process of calibrating the laser point cloud per frame using the intermediate amount of inertial pre-integration in the module 2 comprises:
when compensating the motion distortion of the laser spot at the time t, the time t is determined by the following method k Laser radar coordinates to time tThe system conversion method is as follows:
wherein the rotation vector incrementComprising rotation angle and rotation axis, equivalent to the time t k By time t k+1 Is rotated by an increment Δr in the coordinate system of (2) k,k+1 Two variables obtained on the left side of the above are +.>And Δp kt Respectively indicate time t k Rotation vector increment and position increment by time t, +.>And Δp kj Respectively correspond to the slave time t k By time t j Rotation vector increment and position increment, Δp of (a) k,k+1 For time t k By time t k+1 Position increment in;
for laser point cloudsEvery specific point observed at time t will first be +.>And Δp kt And->And Δp kj Respectively combining to obtain the time t from the time t to the time t j Motion delta information of (a); then the rotation quantity is multiplied by the left and then the position increment is added to map the point of the motion distortion to be corrected to t j And (5) a laser radar coordinate system at the moment to finish the calibration of the laser point cloud.
7. The inertial positioning system of claim 5, wherein the module 6 comprises obtaining the positioning result of the motion platform by
Wherein the residual error r 0 And its covariance matrix Σ 0 In order to set the value of the preset value,is the inertial residual, Σ I For the first covariance matrix,>sigma is the laser inertia residual error L For the second covariance matrix,>for time t j Laser feature set of frame,/>For a set of multi-frame laser point clouds, the state vector to be estimated is +.> x i Represents t i State vector to be estimated at moment, at t i Direction of time R i Position p i Velocity v i Zero offset of inertial measurement unit>
8. The lightweight lidar inertial positioning system based on key feature extraction of claim 5, wherein the module 4 further comprises:
determining loop detection timing by:
where tr (·) represents the operation of matrix tracing and J is the residual functionRelative to p j Is a jacobian matrix of (c),including all laser residuals->And inertial residual->Sigma is the covariance matrix Sigma of the features of the laser L And an inertial covariance matrix Σ I A block diagonal matrix is formed;
if the current moment is judged to be the proper loop detection time, judging whether the current position is overlapped with the historical track or not through the displacement track recorded by the inertia measurement unit, and if so, continuing to perform the inter-frame feature matching in the module 3 to obtain the available laser feature constraint.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110590374.3A CN113466890B (en) | 2021-05-28 | 2021-05-28 | Light laser radar inertial combination positioning method and system based on key feature extraction |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110590374.3A CN113466890B (en) | 2021-05-28 | 2021-05-28 | Light laser radar inertial combination positioning method and system based on key feature extraction |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113466890A CN113466890A (en) | 2021-10-01 |
| CN113466890B true CN113466890B (en) | 2024-04-09 |
Family
ID=77871655
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110590374.3A Active CN113466890B (en) | 2021-05-28 | 2021-05-28 | Light laser radar inertial combination positioning method and system based on key feature extraction |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113466890B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116964599A (en) * | 2022-02-25 | 2023-10-27 | 厦门大学 | Three-dimensional human body motion capturing method, training method and system based on laser radar |
| CN115236714A (en) * | 2022-05-24 | 2022-10-25 | 芯跳科技(广州)有限公司 | Multi-source data fusion positioning method, device and equipment and computer storage medium |
| CN115356740A (en) * | 2022-08-09 | 2022-11-18 | 群周科技(上海)有限公司 | Landing positioning method for landing area in airborne environment |
| CN115164887A (en) * | 2022-08-30 | 2022-10-11 | 中国人民解放军国防科技大学 | Pedestrian navigation positioning method and device based on laser radar and inertia combination |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110261870A (en) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | It is a kind of to synchronize positioning for vision-inertia-laser fusion and build drawing method |
| CN111060115A (en) * | 2019-11-29 | 2020-04-24 | 中国科学院计算技术研究所 | Visual SLAM method and system based on image edge features |
| CN111929699A (en) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | Laser radar inertial navigation odometer considering dynamic obstacles and mapping method and system |
| CN112484746A (en) * | 2020-11-26 | 2021-03-12 | 上海电力大学 | Monocular vision-assisted laser radar odometer method based on ground plane |
| US11002859B1 (en) * | 2020-02-27 | 2021-05-11 | Tsinghua University | Intelligent vehicle positioning method based on feature point calibration |
-
2021
- 2021-05-28 CN CN202110590374.3A patent/CN113466890B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110261870A (en) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | It is a kind of to synchronize positioning for vision-inertia-laser fusion and build drawing method |
| CN111060115A (en) * | 2019-11-29 | 2020-04-24 | 中国科学院计算技术研究所 | Visual SLAM method and system based on image edge features |
| US11002859B1 (en) * | 2020-02-27 | 2021-05-11 | Tsinghua University | Intelligent vehicle positioning method based on feature point calibration |
| CN111929699A (en) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | Laser radar inertial navigation odometer considering dynamic obstacles and mapping method and system |
| CN112484746A (en) * | 2020-11-26 | 2021-03-12 | 上海电力大学 | Monocular vision-assisted laser radar odometer method based on ground plane |
Non-Patent Citations (1)
| Title |
|---|
| 可变高度激光里程计在室内不平整地面环境下的地图构建;刘子明;陈庆盈;李阳;彭文飞;;宁波大学学报(理工版);33(04);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113466890A (en) | 2021-10-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113466890B (en) | Light laser radar inertial combination positioning method and system based on key feature extraction | |
| CN111795686B (en) | Mobile robot positioning and mapping method | |
| CN109059907B (en) | Trajectory data processing method and device, computer equipment and storage medium | |
| EP2133662A2 (en) | Methods and system of navigation using terrain features | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN105352509A (en) | Unmanned aerial vehicle motion target tracking and positioning method under geographic information space-time constraint | |
| CN114526745A (en) | Drawing establishing method and system for tightly-coupled laser radar and inertial odometer | |
| CN110763239A (en) | Filtering combined laser SLAM mapping method and device | |
| CN112346104A (en) | Unmanned aerial vehicle information fusion positioning method | |
| CN115143954B (en) | Unmanned vehicle navigation method based on multi-source information fusion | |
| CN115451948A (en) | Agricultural unmanned vehicle positioning odometer method and system based on multi-sensor fusion | |
| CN115127543A (en) | Method and system for eliminating abnormal edges in laser mapping optimization | |
| CN117367412A (en) | Tightly-coupled laser inertial navigation odometer integrating bundle set adjustment and map building method | |
| US20030118213A1 (en) | Height measurement apparatus | |
| CN116929338A (en) | Map construction method, device and storage medium | |
| CN116429090A (en) | Synchronous positioning and mapping method and device based on line laser and mobile robot | |
| CN115164887A (en) | Pedestrian navigation positioning method and device based on laser radar and inertia combination | |
| CN114705223A (en) | Inertial navigation error compensation method and system for multiple mobile intelligent bodies in target tracking | |
| CN112285697A (en) | Multi-sensor multi-target space-time deviation calibration and fusion method | |
| CN107796417B (en) | Method for adaptively estimating scene matching and inertial navigation installation error | |
| CN116256781B (en) | Combined characteristic multi-source satellite information track association method and system | |
| CN112797977B (en) | Inertia/gravity matching combination method based on robust point group filtering | |
| CN117433511B (en) | Multi-sensor fusion positioning method | |
| CN113450411B (en) | Real-time self-pose calculation method based on variance component estimation theory | |
| CN118031951A (en) | Multisource fusion positioning method, multisource fusion positioning device, electronic equipment and storage medium |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |