CN112781594B - Laser radar iteration closest point improvement algorithm based on IMU coupling - Google Patents
Laser radar iteration closest point improvement algorithm based on IMU coupling Download PDFInfo
- Publication number
- CN112781594B CN112781594B CN202110032151.5A CN202110032151A CN112781594B CN 112781594 B CN112781594 B CN 112781594B CN 202110032151 A CN202110032151 A CN 202110032151A CN 112781594 B CN112781594 B CN 112781594B
- Authority
- CN
- China
- Prior art keywords
- imu
- laser radar
- original
- point cloud
- coordinate 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.)
- Active
Links
- 230000008878 coupling Effects 0.000 title claims abstract description 21
- 238000010168 coupling process Methods 0.000 title claims abstract description 21
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 21
- 230000006872 improvement Effects 0.000 title claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims abstract description 45
- 230000009466 transformation Effects 0.000 claims abstract description 22
- 238000006243 chemical reaction Methods 0.000 claims abstract description 13
- 230000010354 integration Effects 0.000 claims abstract description 10
- 238000000034 method Methods 0.000 claims abstract description 10
- 238000007781 pre-processing Methods 0.000 claims abstract description 7
- 238000006073 displacement reaction Methods 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000000717 retained effect Effects 0.000 claims description 2
- 238000013507 mapping Methods 0.000 abstract description 5
- 230000008859 change Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
Images
Classifications
-
- 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/20—Instruments for performing navigational calculations
-
- 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
-
- 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
Abstract
A laser radar iteration closest point improvement algorithm based on IMU coupling belongs to the field of laser radar mapping and positioning. The method aims at the problems that the existing iterative closest point algorithm is low in operation speed and sensitive to an initial value and is easily limited to a local optimal solution. The method comprises the following steps: synchronizing the lidar and IMU clock sources; preprocessing the laser radar original point cloud, and then extracting a characteristic point set to obtain a laser radar characteristic point set; obtaining a pre-integration result and a conversion initial matrix of two adjacent frames of laser radar original point clouds according to the IMU original poses corresponding to the two adjacent frames of laser radar original point clouds; constructing an original error equation matched with the point cloud of the laser radar, constructing an observation error equation of the IMU, and constructing a combined error equation of the IMU and the laser radar; and carrying out ICP algorithm iterative computation on the joint error equation to obtain an optimal transformation matrix. The invention improves the running speed and the matching precision of the ICP algorithm.
Description
Technical Field
The invention relates to a laser radar iteration closest point improvement algorithm based on IMU coupling, and belongs to the field of laser radar mapping and positioning.
Background
The laser radar can help the unmanned vehicle to establish a surrounding map in an unknown environment, and meanwhile, the laser radar can estimate the self-movement, and is one of important modes for the unmanned vehicle to carry out accurate navigation.
The laser radar mapping and positioning can be divided into four modules of front-end scanning matching, rear-end optimization, closed-loop detection and mapping construction. The front end scanning matching is a core module, a pose change matrix between two adjacent frames of point clouds is estimated through point cloud matching, and the two adjacent frames of point clouds are sequentially spliced into a global map according to a pose change relation, so that the map building is realized; and then, calculating state information between two adjacent frames according to the pose transformation matrix, namely realizing positioning.
In the front-end scanning matching module, an Iterative Closest Point (ICP) algorithm developed by Besl and McKay is the most commonly used method for realizing mapping and positioning, the algorithm is a registration algorithm based on corresponding points, two points with the minimum Euclidean distance are used as the corresponding points, and a transformation matrix between two frames of point cloud data is obtained by minimizing the sum of squared distances between the corresponding points in two point sets. The algorithm is simple and convenient to operate, high in matching accuracy, low in running speed, sensitive to an initial value and easy to limit to a local optimal solution.
Disclosure of Invention
Aiming at the problems that the existing iterative closest point algorithm is low in operation speed and sensitive to an initial value and is easily limited to a local optimal solution, the invention provides an improved iterative closest point algorithm of a laser radar based on IMU coupling.
The invention relates to a lidar iteration closest point improvement algorithm based on IMU coupling, which comprises,
synchronizing the lidar and IMU clock sources;
preprocessing the laser radar original point cloud, and then extracting a characteristic point set to obtain a laser radar characteristic point set; obtaining a pre-integration result and a conversion initial matrix of two adjacent frames of laser radar original point clouds according to the IMU original poses corresponding to the two adjacent frames of laser radar original point clouds;
constructing an original error equation of laser radar point cloud matching according to the laser radar feature point set; constructing an observation error equation of the IMU according to the pre-integration result; then constructing a joint error equation of the IMU and the laser radar based on the original error equation and the observation error equation;
and performing ICP algorithm iterative computation on a joint error equation by taking the conversion initial matrix as an initial value based on the laser radar feature point sets corresponding to the two adjacent frames of laser radar original point clouds to obtain an optimal transformation matrix.
According to the IMU coupling based lidar iterative closest point refinement algorithm of the present invention,
and the laser radar and the IMU realize clock source synchronization through a time synchronizer.
According to the iterative closest point improvement algorithm of the laser radar based on IMU coupling,
after preprocessing the laser radar original point cloud, extracting a characteristic point set, wherein the process of obtaining the laser radar characteristic point set comprises the following steps:
collecting laser radar original point cloud, and removing outliers by using a filter; sequentially connecting all the reserved original point clouds into a line, and calculating the cosine absolute value of an included angle formed by the connection line of each original point cloud and the two adjacent original point clouds in front and back; if the cosine absolute value is smaller than a preset minimum threshold, the current original point cloud is an angular point; if the cosine absolute value is greater than or equal to a preset minimum threshold and less than a preset maximum threshold, the current original point cloud is a non-feature point; if the cosine absolute value is larger than a preset maximum threshold value, the current original point cloud is a plane point;
the laser radar characteristic point set comprises an angular point set formed by all angular points and a plane point set formed by all plane points.
According to the IMU coupling-based laser radar iterative closest point improvement algorithm, the specific process of constructing the observation error equation of the IMU according to the pre-integration result comprises the following steps:
establishing a laser radar coordinate system L and an IMU coordinate system B, and converting the IMU coordinate system B into the laser radar coordinate system L;
setting IMU original pose information as follows:
wherein
The IMU state of the laser radar under a laser radar coordinate system L corresponding to the k frame original point cloud of the laser radar, including the IMU position
IMU speed
IMU quaternion
IMU speed drift
And IMU acceleration drift
Pre-integrating IMU state data corresponding to the k frame of original point cloud of the laser radar and the k +1 frame of original point cloud to obtain state information containing noise in a laser radar coordinate system L relative to the IMU original poses corresponding to the k frame of original point cloud of the laser radar and the k +1 frame of original point cloud:
wherein
The position of the IMU at the k +1 th frame time of the laser radar coordinate system L,
The position of the IMU at the kth frame time of the laser radar coordinate system L,
Is the velocity, Δ t, of the IMU at the kth frame time of the lidar coordinate system L k The time variation, g, corresponding to the IMU from the kth frame to the (k + 1) th frame of the laser radar coordinate system L L Is the gravity acceleration of the IMU under the laser radar coordinate system L,
The displacement of the IMU from the kth frame to the (k + 1) th frame in a laser radar coordinate system L;
the velocity of the IMU at the kth +1 frame time of the lidar coordinate system L
Comprises the following steps:
in the formula
The velocity of the IMU from the kth frame to the (k + 1) th frame in a laser radar coordinate system L;
further obtain
Wherein
Is the (k + 1) th frame IMU quaternion,
the rotation of the IMU from the kth frame to the (k + 1) th frame under a laser radar coordinate system L;
constructing a state error equation of displacement, speed and angle of the IMU:
wherein
Representing the displacement error from the k frame to the k +1 frame of the IMU under the laser radar coordinate system L,
Representing the speed error of the IMU from the kth frame to the k +1 frame under the laser radar coordinate system L,
Representing the rotation error of the IMU from the k frame to the k +1 frame under a laser radar coordinate system L;
constructing an observation error equation of the IMU:
E B representing the observed error of the IMU.
According to the laser radar iterative closest point improvement algorithm based on IMU coupling, the original error equation of laser radar point cloud matching is constructed according to the laser radar feature point set and is as follows:
in the formula E L Representing the original error of the laser radar point cloud matching, R representing the rotation matrix, A i Representing source point clouds, B i Representing the target point cloud and t representing the translation matrix.
According to the laser radar iterative closest point improvement algorithm based on IMU coupling, the joint error equation is
Where E represents the joint error.
According to the laser radar iterative closest point improvement algorithm based on IMU coupling, the ICP algorithm iterative computation of the joint error equation comprises the following steps:
recording the original point clouds of two adjacent frames of laser radars as source point cloud A i ＝(x i y i z i ) And a target point cloud B j ＝(x j y j z j ) Wherein i belongs to (1,2, 3., n), j belongs to (1,2, 3.., n, m), in the formula, (x y z) the partial table is a coordinate value of the point cloud under a laser radar coordinate system L, n represents the cloud quantity of source points, and m represents the cloud quantity of target points;
determining an angular point set and a plane point set corresponding to two adjacent frames of laser radar original point clouds according to a Euclidean distance minimum principle between two points; and performing ICP algorithm iterative computation on the joint error equation by taking the conversion initial matrix as an initial value to obtain an optimal transformation matrix, wherein the optimal transformation matrix enables the joint error equation value to be minimum.
According to the laser radar iterative closest point improvement algorithm based on IMU coupling, the ICP algorithm iterative computation of the joint error equation further comprises the following steps:
transforming the target point cloud by adopting the optimal transformation matrix to obtain a new target point cloud;
calculating the average distance d between the new target point cloud and the source point cloud:
b 'in the formula' i Representing a new target point cloud;
and when the average distance d is smaller than a set distance threshold lambda or the iteration times reach a set iteration time N, stopping iterative computation, and taking the obtained optimal transformation matrix as a final optimal transformation matrix.
The invention has the beneficial effects that: in order to improve the running speed of the ICP algorithm and ensure the matching precision, the method firstly extracts a laser radar feature point set, and reduces the matching calculation amount and the corresponding points of error matching; and secondly, solving a conversion initial matrix of the ICP algorithm by using the IMU original pose of the inertial measurement unit, constructing a joint error equation of the IMU and the laser radar in order to reduce errors caused by IMU drift, and finally iteratively calculating an optimal transformation matrix.
The invention improves the running speed and the matching precision of the ICP algorithm.
Drawings
FIG. 1 is a flowchart of an embodiment of an IMU coupling-based lidar iterative closest point improvement algorithm according to the present invention;
FIG. 2 is a flow chart of obtaining a set of lidar feature points;
FIG. 3 is a flow chart for constructing an observation error equation for an IMU;
fig. 4 is a flow chart of iterative computation of the ICP algorithm for the joint error equation.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It should be noted that the embodiments and features of the embodiments may be combined with each other without conflict.
The invention is further described with reference to the following drawings and specific examples, which are not intended to be limiting.
First embodiment, referring to fig. 1, the present invention provides an iterative closest point improvement algorithm for lidar based on IMU coupling, which includes,
synchronizing the lidar and the IMU clock sources;
preprocessing the laser radar original point cloud, and then extracting a characteristic point set to obtain a laser radar characteristic point set; obtaining a pre-integration result and a conversion initial matrix of two adjacent frames of laser radar original point clouds according to IMU original poses corresponding to the two adjacent frames of laser radar original point clouds;
constructing an original error equation of laser radar point cloud matching according to the laser radar feature point set; constructing an observation error equation of the IMU according to the pre-integration result; then constructing a joint error equation of the IMU and the laser radar based on the original error equation and the observation error equation;
and performing ICP algorithm iterative computation on a joint error equation by taking the conversion initial matrix as an initial value based on the laser radar feature point sets corresponding to the two adjacent frames of laser radar original point clouds to obtain an optimal transformation matrix.
As an example, the lidar and the IMU implement clock source synchronization through a time synchronizer.
Further, with reference to fig. 2, the method for extracting the feature point set after preprocessing the laser radar original point cloud to obtain the laser radar feature point set includes:
collecting original point clouds of the laser radar, and removing outliers by using a filter; sequentially connecting all the retained original point clouds to form a line, and calculating the cosine absolute value of an included angle formed by connecting lines of each original point cloud and the two adjacent original point clouds in front and back; if the cosine absolute value is smaller than a preset minimum threshold, the current original point cloud is an angular point; if the cosine absolute value is greater than or equal to a preset minimum threshold and less than a preset maximum threshold, the current original point cloud is a non-characteristic point; if the cosine absolute value is larger than a preset maximum threshold value, the current original point cloud is a plane point;
the laser radar characteristic point set comprises an angular point set formed by all angular points and a plane point set formed by all plane points.
Still further, with reference to fig. 3, a specific process of constructing the observation error equation of the IMU according to the pre-integration result includes:
establishing a laser radar coordinate system L and an IMU coordinate system B, and converting the IMU coordinate system B into the laser radar coordinate system L;
setting IMU original pose information as follows:
wherein
The IMU state of the laser radar under a laser radar coordinate system L corresponding to the k frame original point cloud of the laser radar, including the IMU position
IMU speed
IMU quaternion
IMU speed drift
And IMU acceleration drift
Pre-integrating IMU state data corresponding to the kth frame of original point cloud of the laser radar and the (k + 1) th frame of original point cloud to obtain state information containing noise in a laser radar coordinate system L relative to the IMU original pose corresponding to the kth frame of original point cloud of the laser radar and the (k + 1) th frame of original point cloud:
wherein
The position of the IMU at the k +1 th frame time of the laser radar coordinate system L,
The position of the IMU at the kth frame time of the laser radar coordinate system L,
Is the velocity, Δ t, of the IMU at the kth frame time of the lidar coordinate system L k The time variation, g, corresponding to the IMU from the kth frame to the (k + 1) th frame of the laser radar coordinate system L L For IMU under lidar coordinate system LThe acceleration of gravity of,
The displacement of the IMU from the kth frame to the (k + 1) th frame in a laser radar coordinate system L;
the velocity of the IMU at the moment of the (k + 1) th frame of the lidar coordinate system L
Comprises the following steps:
in the formula
The velocity of the IMU from the kth frame to the (k + 1) th frame in a laser radar coordinate system L;
further obtain
Wherein
Is the (k + 1) th frame IMU quaternion,
the rotation of the IMU from the kth frame to the (k + 1) th frame under a laser radar coordinate system L;
constructing a state error equation of displacement, speed and angle of the IMU:
wherein
Representing the displacement error from the k frame to the k +1 frame of the IMU under the laser radar coordinate system L,
Representing the speed error from the k frame to the k +1 frame of the IMU under the laser radar coordinate system L,
Representing the rotation error of the IMU from the k frame to the k +1 frame under a laser radar coordinate system L;
constructing an observation error equation of the IMU:
E B indicating the observed error of the IMU.
The main drift amount of the IMU is that the displacement is obtained by twice integrating the acceleration, and in order to reduce the calculated amount, the observation error equation of the IMU can be simplified into E B 。
In this embodiment, the IMU original pose information includes angular velocity, acceleration, quaternion, velocity, and displacement, and the poses of these original information may be represented by a matrix
To indicate, wherein:
r is a rotation matrix expressed in terms of quaternions,
t＝[S x S y S z ] T and is a translation matrix representing displacements in the x, y, z directions of the IMU coordinate system B.
q 0 To representDimension, q 1 Expression of a vector representing rotation about the z-axis, q 2 Representing a vector representation of a rotation around the y-axis, q 3 Representing a vector representation of a rotation about the x-axis.
Constructing an original error equation of laser radar point cloud matching according to the laser radar feature point set as follows:
r, A therein i 、B i And t represents a rotation matrix, a source point cloud ith point, a target point cloud ith point and a translation matrix respectively.
The initial conversion matrix of the two adjacent frames of laser radar original point clouds
Comprises the following steps:
wherein
Represents the inverse of IMU pose corresponding to the kth frame point cloud, T k+1 And representing the IMU pose corresponding to the point cloud of the (k + 1) th frame.
The joint error equation is:
still further, as shown in fig. 4, the iterative computation of the ICP algorithm on the joint error equation includes:
recording the original point clouds of two adjacent frames of laser radars as source point cloud A i ＝(x i y i z i ) And a target point cloud B j ＝(x j y j z j ) Wherein i belongs to (1,2, 3., n), j belongs to (1,2, 3.., m), and in the formula, (x y z) is expressed as the seating of the point cloud under the laser radar coordinate system LScaling, wherein n represents the number of source point clouds, and m represents the number of target point clouds;
determining an angular point set and a plane point set corresponding to two adjacent frames of laser radar original point clouds according to a Euclidean distance minimum principle between two points; and performing ICP algorithm iterative computation on the joint error equation by taking the conversion initial matrix as an initial value to obtain an optimal transformation matrix, wherein the optimal transformation matrix enables the joint error equation value to be minimum.
Still further, as shown in fig. 4, the iterative computation of the ICP algorithm on the joint error equation further includes:
transforming the target point cloud by adopting the optimal transformation matrix to obtain a new target point cloud;
calculating the average distance d between the new target point cloud and the source point cloud:
b 'in the formula' i Representing a new target point cloud;
and when the average distance d is smaller than a set distance threshold lambda or the iteration times reach a set iteration time N, stopping iterative computation, and taking the obtained optimal transformation matrix as a final optimal transformation matrix.
In this embodiment, after the average distance d is obtained each time, if any one of the distance threshold λ and the set iteration number N is satisfied, the iterative computation may be stopped, and the optimal transformation matrix is output.
Claims (6)
1. An iterative closest point improvement algorithm of laser radar based on IMU coupling is characterized by comprising the following steps,
synchronizing the lidar and IMU clock sources;
preprocessing the laser radar original point cloud, and then extracting a characteristic point set to obtain a laser radar characteristic point set; obtaining a pre-integration result and a conversion initial matrix of two adjacent frames of laser radar original point clouds according to the IMU original poses corresponding to the two adjacent frames of laser radar original point clouds;
constructing an original error equation of laser radar point cloud matching according to the laser radar feature point set; constructing an observation error equation of the IMU according to the pre-integration result; then constructing a joint error equation of the IMU and the laser radar based on the original error equation and the observation error equation;
taking the initial conversion matrix as an initial value, and carrying out ICP algorithm iterative computation on a joint error equation based on a laser radar feature point set corresponding to two adjacent frames of laser radar original point clouds to obtain an optimal conversion matrix;
after preprocessing the laser radar original point cloud, extracting a characteristic point set, wherein the process of obtaining the laser radar characteristic point set comprises the following steps:
collecting original point clouds of the laser radar, and removing outliers by using a filter; sequentially connecting all the retained original point clouds to form a line, and calculating the cosine absolute value of an included angle formed by connecting lines of each original point cloud and the two adjacent original point clouds in front and back; if the cosine absolute value is smaller than a preset minimum threshold, the current original point cloud is an angular point; if the cosine absolute value is greater than or equal to a preset minimum threshold and less than a preset maximum threshold, the current original point cloud is a non-feature point; if the cosine absolute value is larger than a preset maximum threshold value, the current original point cloud is a plane point;
the laser radar characteristic point set comprises an angular point set formed by all angular points and a plane point set formed by all plane points;
the specific process of constructing the observation error equation of the IMU according to the pre-integration result comprises the following steps:
establishing a laser radar coordinate system L and an IMU coordinate system B, and converting the IMU coordinate system B into the laser radar coordinate system L;
setting IMU original pose information as follows:
wherein
The IMU state under the laser radar coordinate system L corresponding to the k frame original point cloud of the laser radar comprises IMU position
IMU speed
IMU quaternion
IMU speed drift
And IMU acceleration drift
Pre-integrating IMU state data corresponding to the kth frame of original point cloud of the laser radar and the (k + 1) th frame of original point cloud to obtain state information containing noise in a laser radar coordinate system L relative to the IMU original pose corresponding to the kth frame of original point cloud of the laser radar and the (k + 1) th frame of original point cloud:
wherein
The position of the IMU at the k +1 th frame time of the laser radar coordinate system L,
The position of the IMU at the kth frame time of the laser radar coordinate system L,
Velocity, Δ t, of the IMU at the kth frame time of the lidar coordinate system L k The time variation, g, corresponding to the IMU from the kth frame to the (k + 1) th frame of the laser radar coordinate system L L For IMU in laser mineThe gravity acceleration under the coordinate system L is reached,
The displacement of the IMU from the kth frame to the (k + 1) th frame in a laser radar coordinate system L;
the velocity of the IMU at the moment of the (k + 1) th frame of the lidar coordinate system L
Comprises the following steps:
in the formula
The velocity of the IMU from the kth frame to the (k + 1) th frame in a laser radar coordinate system L;
further obtain
Wherein
Is the (k + 1) th frame IMU quaternion,
the rotation angle of the IMU from the kth frame to the (k + 1) th frame under a laser radar coordinate system L;
constructing a state error equation of displacement, speed and angle of the IMU:
wherein
The displacement error of the IMU from the kth frame to the k +1 frame under the laser radar coordinate system L is shown,
Representing the speed error from the k frame to the k +1 frame of the IMU under the laser radar coordinate system L,
Representing the rotation error of the IMU from the k frame to the k +1 frame under a laser radar coordinate system L;
constructing an observation error equation of the IMU:
E B representing the observed error of the IMU.
2. The IMU coupling based lidar iterative closest point refinement algorithm of claim 1, wherein,
and the laser radar and the IMU realize clock source synchronization through a time synchronizer.
3. The IMU coupling based lidar iterative closest point refinement algorithm of claim 1,
constructing an original error equation of laser radar point cloud matching according to the laser radar feature point set, wherein the original error equation comprises the following steps:
in the formula E L Representing the original error of the laser radar point cloud matching, R representing the rotation matrix, A i Representing source point clouds, B i Representing the target point cloud and t representing the translation matrix.
4. The IMU coupling based lidar iterative closest point refinement algorithm of claim 3, wherein,
the joint error equation is
Where E represents the joint error.
5. The IMU coupling based lidar iterative closest point refinement algorithm of claim 4, wherein,
the iterative calculation of the ICP algorithm on the joint error equation comprises the following steps:
recording the original point clouds of two adjacent frames of laser radar as source point cloud A i ＝(x i y i z i ) And a target point cloud B j ＝(x j y j z j ) Wherein i belongs to (1,2, 3., n), j belongs to (1,2, 3., m), in the formula, (x y z) is a coordinate value of the point cloud under a laser radar coordinate system L, n represents the quantity of source point clouds, and m represents the quantity of target point clouds;
determining an angular point set and a plane point set corresponding to two adjacent frames of laser radar original point clouds according to a Euclidean distance minimum principle between two points; and performing ICP algorithm iterative computation on the joint error equation by taking the conversion initial matrix as an initial value to obtain an optimal transformation matrix, wherein the optimal transformation matrix enables the joint error equation value to be minimum.
6. The IMU coupling based lidar iterative closest point refinement algorithm of claim 5,
performing ICP algorithm iterative calculations on the joint error equation further includes:
transforming the target point cloud by adopting the optimal transformation matrix to obtain a new target point cloud;
calculating the average distance d between the new target point cloud and the source point cloud:
b 'in the formula' i Representing a new target point cloud;
and when the average distance d is smaller than a set distance threshold lambda or the iteration times reach a set iteration time N, stopping iterative computation, and taking the obtained optimal transformation matrix as a final optimal transformation matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110032151.5A CN112781594B (en) | 2021-01-11 | 2021-01-11 | Laser radar iteration closest point improvement algorithm based on IMU coupling |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110032151.5A CN112781594B (en) | 2021-01-11 | 2021-01-11 | Laser radar iteration closest point improvement algorithm based on IMU coupling |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112781594A CN112781594A (en) | 2021-05-11 |
| CN112781594B true CN112781594B (en) | 2022-08-19 |
Family
ID=75756514
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110032151.5A Active CN112781594B (en) | 2021-01-11 | 2021-01-11 | Laser radar iteration closest point improvement algorithm based on IMU coupling |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112781594B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113391300B (en) * | 2021-05-21 | 2022-02-01 | 中国矿业大学 | Laser radar three-dimensional point cloud real-time motion compensation method based on IMU |
| CN113625302A (en) * | 2021-09-03 | 2021-11-09 | 国网山东省电力公司济宁供电公司 | Underground corridor dangerous area identification method and system based on handheld laser radar |
| CN113625301B (en) * | 2021-09-03 | 2024-01-19 | 国网山东省电力公司济宁供电公司 | Method and system for expanding view field of building map based on single-scanning-head laser radar |
| CN114660589B (en) * | 2022-03-25 | 2023-03-10 | 中国铁建重工集团股份有限公司 | Method, system and device for positioning underground tunnel |
| CN114723795B (en) * | 2022-04-18 | 2023-03-28 | 长春工业大学 | Bucket wheel machine unmanned operation positioning and mapping method based on improved nearest point registration |
| CN115683170B (en) * | 2023-01-04 | 2023-03-14 | 成都西物信安智能系统有限公司 | Calibration method based on radar point cloud data fusion error |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020168787A1 (en) * | 2019-02-20 | 2020-08-27 | 苏州风图智能科技有限公司 | Method and device for determining pose of vehicle body, and drafting method |
Family Cites Families (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10481267B2 (en) * | 2017-06-13 | 2019-11-19 | TuSimple | Undistorted raw LiDAR scans and static point extractions method for ground truth static scene sparse flow generation |
| CN108680185B (en) * | 2018-04-26 | 2020-09-22 | 广东宝乐机器人股份有限公司 | Mobile robot gyroscope data correction method, device and equipment |
| WO2020041668A1 (en) * | 2018-08-23 | 2020-02-27 | The Regents Of The University Of California | Signals of opportunity aided inertial navigation |
| CN110009739B (en) * | 2019-01-29 | 2023-03-24 | 浙江省北大信息技术高等研究院 | Method for extracting and coding motion characteristics of digital retina of mobile camera |
| CN110658530B (en) * | 2019-08-01 | 2024-02-23 | 北京联合大学 | Map construction method and system based on double-laser-radar data fusion and map |
| CN111045438B (en) * | 2019-10-21 | 2023-09-08 | 贵州省水利水电勘测设计研究院 | Shipborne self-stabilizing platform and control system and method thereof |
| CN110879400A (en) * | 2019-11-27 | 2020-03-13 | 炬星科技（深圳）有限公司 | Method, equipment and storage medium for fusion positioning of laser radar and IMU |
| CN111207774B (en) * | 2020-01-17 | 2021-12-03 | 山东大学 | Method and system for laser-IMU external reference calibration |
| CN111273269B (en) * | 2020-02-18 | 2022-06-03 | 桂林电子科技大学 | IPSO-BP-based radar target positioning method of frequency diversity array |
| CN111461981B (en) * | 2020-03-30 | 2023-09-01 | 北京百度网讯科技有限公司 | Error estimation method and device for point cloud stitching algorithm |
| CN111522043B (en) * | 2020-04-30 | 2023-07-25 | 北京联合大学 | Unmanned vehicle laser radar quick re-matching positioning method |
| CN111982091A (en) * | 2020-07-09 | 2020-11-24 | 安徽博龙动力科技股份有限公司 | Laser point cloud distortion correction method based on synchronous IMU |
| CN111812669B (en) * | 2020-07-16 | 2023-08-04 | 南京航空航天大学 | Winding machine inspection device, positioning method thereof and storage medium |
| CN111951341A (en) * | 2020-08-25 | 2020-11-17 | 桂林电子科技大学 | Closed loop detection improvement method based on RGB-D SLAM |
-
2021
- 2021-01-11 CN CN202110032151.5A patent/CN112781594B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020168787A1 (en) * | 2019-02-20 | 2020-08-27 | 苏州风图智能科技有限公司 | Method and device for determining pose of vehicle body, and drafting method |
Non-Patent Citations (1)
| Title |
|---|
| 浅析记载激光雷达测量系统的误差检验方法;束庆波等;《低碳世界》;20150309;第304-305页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112781594A (en) | 2021-05-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112781594B (en) | Laser radar iteration closest point improvement algorithm based on IMU coupling | |
| CN112347840B (en) | Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method | |
| CN109709801B (en) | Indoor unmanned aerial vehicle positioning system and method based on laser radar | |
| CN112649016B (en) | Visual inertial odometer method based on dotted line initialization | |
| CN112304307A (en) | Positioning method and device based on multi-sensor fusion and storage medium | |
| CN112268559B (en) | Mobile measurement method for fusing SLAM technology in complex environment | |
| CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
| CN115265523A (en) | Robot simultaneous positioning and mapping method, device and readable medium | |
| CN113311411A (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN114001733B (en) | Map-based consistent efficient visual inertial positioning algorithm | |
| CN107144278B (en) | Lander visual navigation method based on multi-source characteristics | |
| CN115407357A (en) | Low-beam laser radar-IMU-RTK positioning mapping algorithm based on large scene | |
| CN115272596A (en) | Multi-sensor fusion SLAM method oriented to monotonous texture-free large scene | |
| CN108613675B (en) | Low-cost unmanned aerial vehicle movement measurement method and system | |
| CN113763549A (en) | Method, device and storage medium for simultaneous positioning and mapping by fusing laser radar and IMU | |
| CN110751123A (en) | Monocular vision inertial odometer system and method | |
| CN116429116A (en) | Robot positioning method and equipment | |
| CN114754768A (en) | Visual inertial navigation method based on point-line fusion | |
| CN113959437A (en) | Measuring method and system for mobile measuring equipment | |
| CN114459474B (en) | Inertial/polarization/radar/optical-fluidic combined navigation method based on factor graph | |
| CN114543786B (en) | Wall climbing robot positioning method based on visual inertial odometer | |
| CN114993338B (en) | High-efficiency visual inertial mileage calculation method based on multi-section independent map sequence | |
| CN115218889A (en) | Multi-sensor indoor positioning method based on dotted line feature fusion | |
| CN114897942A (en) | Point cloud map generation method and device and related storage medium | |
| 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20210511 Assignee: Guilin Qunmei Technology Co.,Ltd. Assignor: GUILIN University OF ELECTRONIC TECHNOLOGY Contract record no.: X2023980044664 Denomination of invention: Improved Iterative Nearest Point Algorithm for LiDAR Based on IMU Coupling Granted publication date: 20220819 License type: Common License Record date: 20231031 |
|
| EE01 | Entry into force of recordation of patent licensing contract |