CN115855082A - Dual-mode quick repositioning method based on point cloud prior map - Google Patents
Dual-mode quick repositioning method based on point cloud prior map Download PDFInfo
- Publication number
- CN115855082A CN115855082A CN202211561710.2A CN202211561710A CN115855082A CN 115855082 A CN115855082 A CN 115855082A CN 202211561710 A CN202211561710 A CN 202211561710A CN 115855082 A CN115855082 A CN 115855082A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- registration
- ndt
- pose
- icp
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 claims abstract description 27
- 239000011159 matrix material Substances 0.000 claims abstract description 18
- 230000001133 acceleration Effects 0.000 claims abstract description 14
- 238000010606 normalization Methods 0.000 claims abstract description 3
- 238000012545 processing Methods 0.000 claims abstract description 3
- 230000003068 static effect Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 8
- 230000006870 function Effects 0.000 claims description 7
- 230000000007 visual effect Effects 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 239000013256 coordination polymer Substances 0.000 claims 1
- 238000013519 translation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
The invention relates to a dual-mode quick relocation method based on a point cloud prior map, which comprises the following steps: acquiring the actual three-axis acceleration of the normalized vehicle body through a laser radar and an inertia measurement unit IMU, recording a data packet of the inertia measurement unit IMU, calculating an average measurement result of the inertia measurement unit IMU based on the data packet, performing normalization processing on the measurement result, calculating a z-axis included angle theta of a coordinate system of the laser radar and a vehicle body coordinate system, and acquiring a rotation matrix; loading a prior map, a key frame point cloud and pose information, performing one-to-one correspondence by rotating a matrix and storing the key frame point cloud and the corresponding pose by using a hash table, entering an automatic registration mode, and outputting a repositioning pose if the positioning is successful; and if the execution times of the automatic registration mode is greater than a preset threshold, switching to a manual prompt registration mode to obtain a repositioning pose. The invention enhances the robustness and the practicability of the positioning system.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to a dual-mode quick relocation method based on a point cloud prior map.
Background
Positioning is one of the core problems in the field of automatic driving, and is a necessary condition for autonomous capabilities of unmanned vehicle motion planning, control and the like. The mainstream positioning scheme in the current engineering is as follows: firstly, a high-precision point cloud map is constructed by fusing various high-precision sensors, and then the initial position of the current vehicle is obtained by matching the real-time point cloud with the prior point cloud map, so that the problem of how to quickly and accurately obtain the initial pose of the vehicle is a positioning system. In the existing solutions, some auxiliary devices such as two-dimensional codes and markers are used for repositioning, so that the method increases the cost and has low applicability; some methods adopt point cloud matching based on an NDT algorithm, but when the prior map is large, the method is slow in convergence and time-consuming.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a dual-mode quick relocation method based on a point cloud prior map, the method provides two relocation modes of automatic registration and manual prompt registration, and can automatically switch according to a matching result, and realize quick and accurate relocation of an unmanned system under the condition of not needing an external auxiliary device.
In order to achieve the purpose, the invention provides the following scheme:
a dual-mode quick relocation method based on a point cloud prior map comprises the following steps:
acquiring a vehicle body standing triaxial acceleration and a data packet of an Inertial Measurement Unit (IMU), calculating an average measurement result of the IMU based on the data packet, and carrying out normalization processing on the measurement result;
calculating a z-axis included angle theta of a laser radar coordinate system and a vehicle body coordinate system by combining the vehicle body standing triaxial acceleration and the normalized measurement result to obtain a rotation matrix;
loading a prior map, a key frame point cloud and pose information, storing the point cloud of the key frame and the corresponding pose through the rotation matrix, entering an automatic registration mode, positioning based on the rotation matrix, and outputting a repositioning pose if the positioning is successful; and if the execution times of the automatic registration mode is greater than a preset threshold, switching to a manual prompt registration mode to obtain a repositioning pose.
Preferably, taking the vehicle body resting triaxial acceleration comprises:
fixing the laser radar and the inertial measurement unit IMU on a support at the top of the vehicle, fixedly connecting the laser radar and the inertial measurement unit IMU, aligning the z axis of the vehicle body with the gravity direction to obtain the actual three-axis acceleration of the vehicle body, normalizing the actual three-axis acceleration of the vehicle body to obtain the three-axis acceleration of the vehicle body in a standing mode.
Preferably, obtaining the normalized measurement result comprises:
and recording the data packet of the inertial measurement unit IMU through a rossbag tool of the ROS system, calculating the average measurement result of the IMU, and normalizing to obtain the normalized measurement result.
Preferably, obtaining the rotation matrix comprises:
acquiring a vector outer product n _ axi of a z axis of the laser radar coordinate system and a z axis of the vehicle body coordinate system through the included angle theta, and acquiring the rotation matrix through the vector outer product n _ axi:
wherein ,
is a rotation matrix.
Preferably, entering the automatic registration mode comprises:
s4.1, starting a laser radar driving program, reading point cloud data, rotating the point cloud according to the rotation matrix after receiving the real-time point cloud, and performing down-sampling on the aligned point cloud, and removing outliers to obtain a source point cloud;
s4.2, traversing the key frame point clouds, calculating a rotation invariance descriptor of each frame point cloud, comparing the descriptor with a descriptor of the source point cloud to obtain the score of each key frame, and regarding the key frame with the highest score and larger than a threshold value as a matched key frame;
s4.3, calculating an initial repositioning pose T by using the rotation invariance descriptor sc In terms of T sc Performing ICP algorithm matching on the matched key frame point cloud and the source point cloud to obtain an optimized pose T as an initial value icp ＝[R icp |t icp ]And then carrying out NDT algorithm registration to obtain a registration result, if the registration result is converged, the relocation is successful, and the output relocation pose is T ndt ＝[R ndt |t ndt ](ii) a Otherwise, the S4.1-S4.3 are repeatedly executed, meanwhile, the number of times of repeated execution is calculated by using a counter flag, when the flag is larger than a preset threshold value, the current real-time point cloud is considered to be insufficient for matching to obtain a repositioning pose, and the system is switched to a manual prompting registration mode.
Preferably, the process of performing the automatic registration includes:
s5.1, after correcting the original point cloud in the z-axis direction, performing down-sampling by using voxel filtering, then removing outliers by using a radius filter, and setting a search radius and the minimum number of adjacent points to obtain a source point cloud;
s5.2, recording the number of the received point cloud frames by using a static variable cloudo _ counter, updating the source point cloud to be the current real-time point cloud when the cloudo _ counter is larger than 20, resetting the cloudo _ counter to be 0 after the updating is finished, and starting the next round of counting;
s5.3, traversing all key frame point clouds, calculating a rotation invariance descriptor, comparing the rotation invariance descriptor with the descriptor of the source point cloud, calculating a similarity score, searching the key frame with the score which is the highest and is greater than a self-defined threshold value and corresponding pose T by utilizing a hash table sc ＝[R sc |t sc ]；
S5.4, constructing a Kdtree, inputting a global map, and inquiring t sc Position ofConstructing a subgraph by the point cloud in the radius;
s5.5, setting the maximum matching distance, the maximum iteration number, the difference between two transformation matrixes, the mean square error and the initial value of ICP registration as the maximum matching distance, the maximum iteration number, the difference between two transformation matrixes, the mean square error and the initial value, registering the source point cloud and the subgraph obtained in the S5.4 to obtain the pose T icp ＝[R icp |t icp ]；
S5.6, setting a difference value, a grid resolution and an initial value between NDT registration two transformation matrixes, registering the source point cloud and the subgraph, and obtaining a final repositioning pose T if convergence occurs ndt ＝[R ndt |t ndt ]。
Preferably, updating the source point cloud comprises:
creating an exclusive amount cloudmutex, when the source point cloud needs to be updated, subscribing a callback function thread of the point cloud to lock the cloudmutex, waiting for an automatic registration thread, unlocking the cloudmutex by the point cloud callback function thread after the update is completed, locking the cloudmutex by the automatic registration thread, obtaining the authority of accessing a memory of the source point cloud, declaring a new variable to store the updated source point cloud, unlocking the cloudmutex, and completing the update.
Preferably, switching to the manual prompt registration mode includes:
determining a static variable registration _ counter and initializing to 0, calculating the registration times in an automatic mode, and increasing the registration _ counter by 1 after each registration failure in the automatic registration mode;
when the registration _ counter is greater than the preset threshold, the system switches to the manual prompt registration mode.
Preferably, the manual prompt registration mode is performed, including:
step 6.1, printing information through a terminal, prompting a user to estimate the position of the vehicle through a visual interface Rviz of an ROS system, issuing a/initial topic, and after receiving the topic, constructing a sub-graph within the determined position radius for searching;
6.2, carrying out NDT matching on the real-time point cloud and the subgraph to obtain an optimized bitPosture T ndt ＝[R ndt |t ndt ]Then by T ndt Performing ICP algorithm registration for the initial value, and obtaining a repositioning pose T if the result is converged icp ＝[R icp |t icp ](ii) a Otherwise, the manual registration operation is repeatedly executed, and if the execution times are larger than a preset threshold value, the relocation is failed;
6.3, if the repositioning is successful, the system prints repositioning pose information at the terminal; otherwise, the system prompts that the relocation is failed, and prompts the user to execute the relocation program again after the user needs to move the machine body.
Preferably, the manual prompt registration process includes:
s7.1, the system prints prompt contents at the terminal to prompt a user to select 2D (two-dimensional) position Estimate on a visual interface, clicks the position of the vehicle on a map, and issues an initial position topic to obtain an initial guess position T guess ＝[I|t guess ]；
S7.2, creating Kdtree, inputting global map, and inquiring t guess Constructing subgraphs within the position radius;
s7.3, preprocessing the real-time point cloud to obtain a corrected source point cloud;
s7.4, registering the source point cloud and the subgraph obtained in the S7.2 by using NDT registration, configuring NDT parameters and obtaining an initial pose T ndt ＝[R ndt |t ndt ]；
S7.5, setting the ICP registration parameter to be consistent with the ICP parameter, and setting T ndt Registering the source point cloud and the subgraph as an initial value, and obtaining a repositioning pose T if convergence occurs icp ＝[R icp |t icp ]。
The invention has the beneficial effects that:
(1) The invention provides two different point cloud registration modes based on a laser radar point cloud prior map, and realizes the switching of the two modes by setting a reasonable threshold value, so that the system can still perform manual prompt under the condition of failure of automatic search registration, and the robustness and the practicability of the positioning system are enhanced;
(2) The invention also provides the characteristic of the initial registration value according to the two modes, adopts a fine registration method combining ICP and NDT, and adjusts the front and back sequence of the ICP algorithm and the NDT algorithm so as to achieve the effect of quick convergence, and improve the rapidity and the accuracy of the repositioning system.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a flow chart of mode switching according to an embodiment of the present invention;
FIG. 2 is a schematic view of alignment of a laser radar and a vehicle body coordinate system with respect to a z-axis according to an embodiment of the present invention;
FIG. 3 is a flow chart of an automatic model point cloud registration according to an embodiment of the present invention;
fig. 4 is a flow chart of manual mode point cloud registration according to an embodiment of the present invention.
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.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
The embodiment provides a dual-mode quick relocation method based on a point cloud prior map, which specifically comprises the following steps:
step 1, fixing the laser radar and the IMU on a support at the top of the vehicle, so that the visual field of the laser radar is wide, the laser radar is ensured to be fixedly connected with the IMU, and the z axis is aligned.
Step 2, standing the vehicle on the flat ground, and regarding the z axis of the vehicle body as being aligned with the gravity direction, wherein the actual three-axis acceleration of the vehicle body is normalized to be n 1 ＝[0，0，1] T Recording IMU data packets for about 30 seconds through a rossbag tool of the ROS system, calculating the average IMU measurement, and normalizing to obtain n 2 ＝[a x ，a y ，a z ] T 。
Step 3, n obtained in step 2 1 、n 2 Calculating a z-axis included angle theta of a laser radar coordinate system and a vehicle body coordinate system, obtaining n _ axis by the vector outer product of the two z-axis directions, and finally obtaining a rotation matrix which enables the laser radar coordinate system and the vehicle body coordinate system to be aligned to the z axis as shown in figure 2
And 4, loading the prior map, the point cloud of the key frame and the pose information, and storing the point cloud of the key frame and the corresponding pose by using a hash table to enable the point cloud of the key frame and the corresponding pose to be in one-to-one correspondence, so that the quick search is facilitated.
And 4.1, starting a laser radar driving program, reading point cloud data, and after receiving the real-time point cloud, pressing a rotation matrix
The point cloud is rotated to prevent the original point cloud from tilting up or lying down, the aligned point cloud is subjected to down sampling, outliers are removed and then the point cloud is used as the source point cloud, and in order to prevent the source point cloud matching from failing to cause program deadlock, source frames are required to be updated every 2 seconds.
The method comprises the steps of using multiple threads to realize the updating and automatic registration process of source point cloud, creating an exclusive amount cloudjmutex, when the source point cloud needs to be updated, subscribing a callback function thread of the point cloud to lock the cloudjmutex, waiting an automatic registration thread, unlocking the cloudjmutex by the point cloud callback function thread after the updating is completed, locking the cloudjmutex by the automatic registration thread, obtaining the authority of accessing the source point cloud memory, declaring a new variable to store the updated source point cloud, and then unlocking the cloudjmutex.
And 4.2, quickly traversing all key frame point clouds by using a hash table structure, calculating a 'rotation invariance' descriptor of each frame point cloud, comparing the descriptor with the descriptor of the source point cloud to obtain the score of each key frame, and regarding the key frame with the highest score and larger than a threshold as the matched key frame.
Step 4.3, the obtained prior pose is T obtained by using a rotation invariance descriptor sc The repositioning pose is estimated in the rotating direction, and the error in the translation direction is larger, so that firstly, the repositioning pose is used as an initial value, ICP algorithm matching is carried out on the matched key frame point cloud and source point cloud, and the optimized pose T is obtained icp ＝[R icp |t icp ]And then carrying out NDT algorithm registration, if the result is converged, the relocation is successful, and the output relocation pose is T ndt ＝[R ndt |t ndt ](ii) a Otherwise, the program repeatedly executes the steps 4.1 to 4.3, meanwhile, the number of times of repeated execution is calculated by using a counter flag, when the flag is greater than the threshold 3, the current real-time point cloud is considered to be insufficient for matching to obtain a repositioning pose, and the system is switched to a manual prompt registration mode, as shown in fig. 1.
And 4.4, when the system is switched to a manual prompting registration mode, the terminal prints information to prompt a user to estimate the approximate position of the vehicle on a visual interface Rviz of the ROS system, clicking and publishing the/initial _ position topic, and after the system receives the topic, constructing a sub-graph within the radius of 50m of the selected position to accelerate searching.
Step 4.5, "/initializose" issued by the user contains the initial estimation pose in the translation direction and lacks the estimation in the rotation direction, therefore, NDT matching is firstly carried out on the real-time point cloud and the sub-graph to obtain the optimized pose T ndt ＝[R ndt |t ndt ]Then by T ndt Performing ICP algorithm registration for the initial value, and obtaining a repositioning pose ofT icp ＝[R icp |t icp ](ii) a Otherwise, the manual registration operation is repeatedly executed until the execution times are larger than the threshold value 2, and the repositioning fails.
Step 5, if the repositioning is successful, the system prints repositioning pose information on the terminal; otherwise, the system prompts that the relocation is failed, and prompts the user to execute the relocation program again after the user needs to move the machine body.
The process of switching the automatic registration to the manual prompt registration in the step 4.3 comprises the following specific steps:
and 4.31, preprocessing the real-time point cloud and starting an automatic registration mode.
4.32, if the registration is successful in the automatic registration mode, the relocation is successful; otherwise, the static variable registration _ counter is declared and initialized to 0 for calculating the registration times in the automatic mode, and the registration _ counter is increased by 1 after each registration failure.
And 4.33, when the registration _ counter is larger than the maximum allowable automatic registration time, switching the system to a manual prompt registration mode.
The automatic registration process in the steps 4.1-4.3 comprises the following specific steps:
step 1, after correcting the original point cloud in the z-axis direction, using voxel filtering to perform down-sampling, setting the minimum voxel unit to be (0.2, 0.2 and 0.2), then using a radius filter to remove outliers, setting the search radius to be 5m, setting the number of minimum adjacent points to be 30, and finally obtaining the source point cloud.
And 2, in a callback function of the point cloud topic, recording the number of received point cloud frames by using a static variable cloudo _ counter, updating the source point cloud to be the current real-time point cloud when the cloudo _ counter is larger than 20, preventing the matching failure and program deadlock, resetting the cloudo _ counter to be 0 after the updating is finished, and starting the next round of counting.
Step 3, traversing all key frame point clouds, calculating a 'rotation invariance' descriptor, comparing the descriptor with the descriptor of the source point cloud, calculating a similarity score, wherein the key frame with the highest score and larger than a user-defined threshold value is a matched key frame, and quickly querying a corresponding pose T by utilizing a hash table sc ＝[R sc |t sc ]As shown in fig. 3.
Step 4, constructing a Kdtree, inputting a global map, and inquiring t sc And (4) constructing a sub-graph by using the point cloud within the position radius of 50m, and accelerating the search.
Step 5, setting the maximum matching distance of ICP registration to be 0.05m, the maximum iteration number to be 100, the difference value between two transformation matrixes to be 1e-6, the mean square error to be 1e-6 and the initial value to be T sc Registering the source point cloud with the subgraph obtained in the step 4 to obtain a pose T icp ＝[R icp |t icp ]。
Step 6, setting the difference value between two transformation matrixes of NDT registration to be 0.01, the grid resolution to be 1.0m and the initial value to be T icp Registering the source point cloud with the subgraph obtained in the step 4, and obtaining a final repositioning pose T if convergence occurs ndt ＝[R ndt |t ndt ]。
The manual prompt registration process of steps 4.4-4.5 includes the following specific steps:
step 1, the system prints prompt contents at a terminal, prompts a user to select 2D position Estimate on an Rviz visual interface, clicks the position of a vehicle on a map, and issues an initialpose topic to obtain an initial guess Pose T guess ＝[I|t guess ]。
Step 2, creating Kdtree, inputting global map, inquiring t guess And constructing a subgraph by the subgraphs within the position radius of 100 m.
And 3, preprocessing the real-time point cloud, wherein the method is consistent with the method in the step 1 of the claim 2, and obtaining the corrected source point cloud.
Step 4, because the initial position lacks the estimation in the rotation direction, firstly, the NDT registration source point cloud is used for registering with the subgraph obtained in the step 2, the parameter configuration is consistent with the NDT parameter in the claim 3, and the initial pose T is obtained ndt ＝[R ndt |t ndt ]As shown in fig. 4.
Step 5, setting ICP registration parameter consistent with ICP parameter in claim 3, setting T ndt As an initial value, the source point cloud and the subgraph obtained in the step 2 are processedRegistering, if converging, obtaining a repositioning pose T icp ＝[R icp |t icp ]。
If the repositioning is successful, the system prints repositioning pose information at the terminal; otherwise, the system prompts that the relocation is failed, and after the machine body needs to be moved, the relocation program is executed again.
The above-described embodiments are merely illustrative of the preferred embodiments of the present invention, and do not limit the scope of the present invention, and various modifications and improvements of the technical solutions of the present invention can be made by those skilled in the art without departing from the spirit of the present invention, and the technical solutions of the present invention are within the scope of the present invention defined by the claims.
Claims (10)
1. A dual-mode quick repositioning method based on a point cloud prior map is characterized by comprising the following steps:
acquiring a data packet of a vehicle body static triaxial acceleration and an inertial measurement unit IMU, calculating an average measurement result of the inertial measurement unit IMU based on the data packet, and carrying out normalization processing on the measurement result;
calculating a z-axis included angle theta of a laser radar coordinate system and a vehicle body coordinate system by combining the vehicle body standing triaxial acceleration and the normalized measurement result to obtain a rotation matrix;
loading a prior map, a key frame point cloud and pose information, storing the point cloud of the key frame and the corresponding pose through the rotation matrix, entering an automatic registration mode, positioning based on the rotation matrix, and outputting a repositioning pose if the positioning is successful; and if the execution times of the automatic registration mode is greater than a preset threshold, switching to a manual prompt registration mode to obtain a repositioning pose.
2. The point cloud prior map-based dual-mode quick repositioning method according to claim 1, wherein taking the three-axis acceleration of vehicle body standing comprises:
and fixing the laser radar and the inertia measurement unit IMU on a support at the top of the vehicle, fixedly connecting the laser radar and the inertia measurement unit IMU, aligning the z axis of the vehicle body with the gravity direction to obtain the actual three-axis acceleration of the vehicle body, and normalizing the actual three-axis acceleration of the vehicle body to obtain the static three-axis acceleration of the vehicle body.
3. The dual-mode fast relocation method based on point cloud a-priori map according to claim 1, wherein obtaining the normalized measurement result comprises:
and recording the data packet of the inertial measurement unit IMU through a rossbag tool of the ROS system, calculating the average measurement result of the IMU, and normalizing to obtain the normalized measurement result.
4. The point cloud a-priori map based dual-mode fast repositioning method of claim 1, wherein obtaining the rotation matrix comprises:
acquiring a vector outer product n _ axi of a z axis of the laser radar coordinate system and a z axis of the vehicle body coordinate system through the included angle theta, and acquiring the rotation matrix through the vector outer product n _ axi:
wherein ,
is a rotation matrix.
5. The dual-mode fast repositioning method based on point cloud a-priori map according to claim 4, wherein entering the automatic registration mode comprises:
s4.1, starting a laser radar driving program, reading point cloud data, rotating the point cloud according to the rotation matrix after receiving the real-time point cloud, and performing down-sampling on the aligned point cloud, and removing outliers to obtain a source point cloud;
s4.2, traversing the key frame point clouds, calculating a rotation invariance descriptor of each frame point cloud, comparing the descriptor with a descriptor of the source point cloud to obtain the score of each key frame, and regarding the key frame with the highest score and larger than a threshold value as a matched key frame;
s4.3, calculating an initial repositioning pose T by using the rotation invariance descriptor sc In terms of T sc Performing ICP algorithm matching on the matched key frame point cloud and the source point cloud to obtain an optimized pose T as an initial value icp ＝[R icp |t icp ]And then carrying out NDT algorithm registration to obtain a registration result, if the registration result is converged, the relocation is successful, and the output relocation pose is T ndt ＝[R ndt |t ndt ](ii) a Otherwise, the S4.1-S4.3 are repeatedly executed, meanwhile, the number of times of repeated execution is calculated by using a counter flag, when the flag is larger than a preset threshold value, the current real-time point cloud is considered to be insufficient for matching to obtain a repositioning pose, and the system is switched to a manual prompting registration mode.
6. The dual-mode fast repositioning method based on point cloud a-priori map according to claim 5, wherein the process of performing the automatic registration comprises:
s5.1, after correcting the original point cloud in the z-axis direction, performing down-sampling by using voxel filtering, then eliminating outliers by using a radius filter, and setting a search radius and the minimum number of neighbor points to obtain a source point cloud;
s5.2, recording the number of the received point cloud frames by using a static variable, updating the source point cloud into the current real-time point cloud when the group _ counter is more than 20, resetting the group _ counter to be 0 after the updating is finished, and starting the next round of counting;
s5.3, traversing all key frame point clouds, calculating a rotation invariance descriptor, comparing the rotation invariance descriptor with a descriptor of the source point cloud, calculating a similarity score, wherein the key frame with the highest score and larger than a user-defined threshold value is a matched key frame, and inquiring a corresponding pose T by utilizing a hash table sc ＝[R sc |t sc ]；
S5.4, constructing a Kdtree, inputting a global map, and inquiring t sc Constructing a subgraph by point cloud in the position radius;
s5.5, setting the maximum matching distance, the maximum iteration number, the difference between two transformation matrixes, the mean square error and the initial value of ICP registration as the maximum matching distance, the maximum iteration number, the difference between two transformation matrixes, the mean square error and the initial value, registering the source point cloud and the subgraph obtained in the S5.4 to obtain the pose T icp ＝[R icp |t icp ]；
S5.6, setting a difference value, a grid resolution and an initial value between NDT (normalized difference transform) registration two transformation matrixes, registering the source point cloud with the subgraph, and obtaining a final repositioning pose T if convergence is achieved ndt ＝[R ndt |t ndt ]。
7. The dual-mode fast relocation method based on point cloud a-priori map according to claim 6, wherein updating the source point cloud comprises:
creating an exclusive amount cloudmutex, when the source point cloud needs to be updated, subscribing a callback function thread of the point cloud to lock the cloudmutex, waiting for an automatic registration thread, unlocking the cloudmutex by the point cloud callback function thread after the update is completed, locking the cloudmutex by the automatic registration thread, obtaining the authority of accessing a memory of the source point cloud, declaring a new variable to store the updated source point cloud, unlocking the cloudmutex, and completing the update.
8. The dual-mode fast repositioning method based on point cloud prior map according to claim 1, wherein switching to the manual prompt registration mode comprises:
determining a static variable registration _ counter, initializing the static variable registration _ counter to 0, calculating the registration times in an automatic mode, and increasing the registration _ counter by 1 after each registration failure in the automatic registration mode;
when the registration _ counter is greater than the preset threshold, the system switches to the manual prompt registration mode.
9. The dual-mode fast repositioning method based on point cloud prior map according to claim 1, wherein the manual prompt registration mode is performed, comprising:
step 6.1, printing information through a terminal, prompting a user to estimate the position of the vehicle through a visual interface Rviz of an ROS system, issuing a/initial topic, and after receiving the topic, constructing a sub-graph within the determined position radius for searching;
6.2, carrying out NDT matching on the real-time point cloud and the subgraph to obtain an optimized pose T ndt ＝[R ndt |t ndt ]Then by T ndt Performing ICP algorithm registration for the initial value, and obtaining a repositioning pose T if the result is converged icp ＝[R icp |t icp ](ii) a Otherwise, the manual registration operation is repeatedly executed, and if the execution times are larger than a preset threshold value, the relocation fails;
6.3, if the repositioning is successful, the system prints repositioning pose information at the terminal; otherwise, the system prompts that the relocation is failed, and prompts the user to execute the relocation program again after the user needs to move the machine body.
10. The dual-mode fast repositioning method based on the point cloud prior map according to claim 9, wherein the manual prompting of the registration process comprises:
s7.1, the system prints prompt contents at the terminal to prompt a user to select 2D (two-dimensional) position Estimate on a visual interface, clicks the position of the vehicle on a map, and issues an initial position topic to obtain an initial guess position T guess ＝[I|t guess ]；
S7.2, creating Kdtree, inputting global map, and inquiring t guess Constructing subgraphs within the position radius;
s7.3, preprocessing the real-time point cloud to obtain a corrected source point cloud;
s7.4, registering the source point cloud and the subgraph obtained in the S7.2 by using NDT registration, configuring NDT parameters and obtaining an initial pose T ndt ＝[R ndt |t ndt ]；
S7.5, setting IThe CP registration parameter is consistent with the ICP parameter, and T is set ndt Registering the source point cloud and the subgraph as an initial value, and obtaining a repositioning pose T if convergence occurs icp ＝[R icp |t icp ]。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211561710.2A CN115855082B (en) | 2022-12-07 | 2022-12-07 | Dual-mode rapid repositioning method based on point cloud priori map |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211561710.2A CN115855082B (en) | 2022-12-07 | 2022-12-07 | Dual-mode rapid repositioning method based on point cloud priori map |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115855082A true CN115855082A (en) | 2023-03-28 |
| CN115855082B CN115855082B (en) | 2023-10-20 |
Family
ID=85670595
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211561710.2A Active CN115855082B (en) | 2022-12-07 | 2022-12-07 | Dual-mode rapid repositioning method based on point cloud priori map |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115855082B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116442286A (en) * | 2023-06-15 | 2023-07-18 | 国网瑞嘉(天津)智能机器人有限公司 | Robot work object positioning system, method, device, robot and medium |
| CN117589154A (en) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | Relocation method of self-mobile device, self-mobile device and readable storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190234753A1 (en) * | 2018-01-31 | 2019-08-01 | Hitachi, Ltd. | Information processing apparatus and automatic driving track management system |
| CN112444246A (en) * | 2020-11-06 | 2021-03-05 | 北京易达恩能科技有限公司 | Laser fusion positioning method in high-precision digital twin scene |
| CN113432600A (en) * | 2021-06-09 | 2021-09-24 | 北京科技大学 | Robot instant positioning and map construction method and system based on multiple information sources |
| CN114236552A (en) * | 2021-11-12 | 2022-03-25 | 苏州玖物互通智能科技有限公司 | Repositioning method and system based on laser radar |
| CN115127543A (en) * | 2022-07-22 | 2022-09-30 | 上海于万科技有限公司 | Method and system for eliminating abnormal edges in laser mapping optimization |
-
2022
- 2022-12-07 CN CN202211561710.2A patent/CN115855082B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190234753A1 (en) * | 2018-01-31 | 2019-08-01 | Hitachi, Ltd. | Information processing apparatus and automatic driving track management system |
| CN112444246A (en) * | 2020-11-06 | 2021-03-05 | 北京易达恩能科技有限公司 | Laser fusion positioning method in high-precision digital twin scene |
| CN113432600A (en) * | 2021-06-09 | 2021-09-24 | 北京科技大学 | Robot instant positioning and map construction method and system based on multiple information sources |
| CN114236552A (en) * | 2021-11-12 | 2022-03-25 | 苏州玖物互通智能科技有限公司 | Repositioning method and system based on laser radar |
| CN115127543A (en) * | 2022-07-22 | 2022-09-30 | 上海于万科技有限公司 | Method and system for eliminating abnormal edges in laser mapping optimization |
Non-Patent Citations (3)
| Title |
|---|
| FURONG PENG 等: "Street view cross-sourced point cloud matching and registration", 《2014 IEEE INTERNATIONAL CONFERENCE ON IMAGE PROCESSING (ICIP)》, pages 1 - 5 * |
| 李彦 等: "提高钢厂物料定位精度的改进配准算法研究及应用", 《冶金自动化》, pages 129 - 134 * |
| 王勇;唐靖;饶勤菲;袁巢燕;: "采用点云重心距离进行边界检测的点云数据配准", 小型微型计算机系统, no. 09, pages 2096 - 2101 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116442286A (en) * | 2023-06-15 | 2023-07-18 | 国网瑞嘉(天津)智能机器人有限公司 | Robot work object positioning system, method, device, robot and medium |
| CN116442286B (en) * | 2023-06-15 | 2023-10-20 | 国网瑞嘉(天津)智能机器人有限公司 | Robot work object positioning system, method, device, robot and medium |
| CN117589154A (en) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | Relocation method of self-mobile device, self-mobile device and readable storage medium |
| CN117589154B (en) * | 2024-01-19 | 2024-05-24 | 深圳竹芒科技有限公司 | Relocation method of self-mobile device, self-mobile device and readable storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115855082B (en) | 2023-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115855082A (en) | Dual-mode quick repositioning method based on point cloud prior map | |
| CN112179330B (en) | Pose determination method and device of mobile equipment | |
| Cai et al. | Practical optimal registration of terrestrial LiDAR scan pairs | |
| KR102068419B1 (en) | Method, apparatus and computer readable medium for adjusting point cloud data collection trajectory | |
| CN110930495A (en) | Multi-unmanned aerial vehicle cooperation-based ICP point cloud map fusion method, system, device and storage medium | |
| CN109141364A (en) | Obstacle detection method, system and robot | |
| CN115290097B (en) | BIM-based real-time accurate map construction method, terminal and storage medium | |
| US10540813B1 (en) | Three-dimensional point data alignment | |
| WO2014077272A1 (en) | Three-dimensional object recognition device and three-dimensional object recognition method | |
| CN113587933B (en) | Indoor mobile robot positioning method based on branch-and-bound algorithm | |
| CN109872350A (en) | A kind of new point cloud autoegistration method | |
| US20230114588A1 (en) | Warehouse robot control method and apparatus, robot, and warehouse system | |
| CN111400830B (en) | Machining calibration method and device for three-dimensional blank workpiece | |
| CN111754579A (en) | Method and device for determining external parameters of multi-view camera | |
| CN110097598B (en) | Three-dimensional object pose estimation method based on PVFH (geometric spatial gradient frequency) features | |
| Chen et al. | Determining pose of 3D objects with curved surfaces | |
| CN112484746A (en) | Monocular vision-assisted laser radar odometer method based on ground plane | |
| CN113034347B (en) | Oblique photography image processing method, device, processing equipment and storage medium | |
| CN116934851A (en) | Robot positioning method, device, equipment and storage medium | |
| WO2020014941A1 (en) | Map establishment method, positioning method and apparatus, terminal and storage medium | |
| Pradalier et al. | Concurrent matching, localization and map building using invariant features | |
| CN115131208A (en) | Structured light 3D scanning measurement method and system | |
| CN113687336A (en) | Radar calibration method and device, electronic equipment and medium | |
| CN115222787B (en) | Real-time point cloud registration method based on hybrid retrieval | |
| CN115953593B (en) | Contour recognition method, apparatus, device and computer storage medium for industrial parts |
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 |