CN112711012B - Global position initialization method and system of laser radar positioning system - Google Patents
Global position initialization method and system of laser radar positioning system Download PDFInfo
- Publication number
- CN112711012B CN112711012B CN202011511527.2A CN202011511527A CN112711012B CN 112711012 B CN112711012 B CN 112711012B CN 202011511527 A CN202011511527 A CN 202011511527A CN 112711012 B CN112711012 B CN 112711012B
- Authority
- CN
- China
- Prior art keywords
- map
- global
- initial
- conversion matrix
- initialization
- 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
Images
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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/46—Indirect determination of position data
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Radar Systems Or Details Thereof (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
Abstract
The invention provides a global position initialization method and a system of a laser radar positioning system, which comprises the following steps: loading a global map and related parameters; according to the related parameters, a local grid sub map is segmented in the global map; generating n initial estimation positions which are randomly estimated in each segmented local grid sub-map; for each initial estimation position, generating a matching degree score and a position conversion matrix thereof at a possible initial position of the mobile chassis; determining global position initialization according to the matching degree score and the position conversion matrix; and judging the situation of triggering the global position initialization again, and re-executing the steps to perform relocation. A corresponding terminal and storage medium are also provided. The invention automatically initializes the global position, thereby avoiding the complicated operation of manually inputting the initial position; when the positioning is lost, the global position repositioning is automatically carried out, so that the manual intervention is avoided; the autonomous mobile chassis without human intervention is realized.
Description
Technical Field
The invention relates to the technical field of intelligent robots, in particular to a global position initialization method and a system of a laser radar positioning system, and provides a corresponding terminal and a storage medium.
Background
As autonomous mobile chassis technology evolves, it is now common to move an autonomous mobile chassis from a known location into a room where the environmental map is known.
A problem encountered in lidar-based positioning algorithms is how to relate the initial position of the autonomous mobile chassis to an environmental map. The current solution is to manually input an initial position estimate for the autonomous mobile chassis through human intervention, and then the autonomous mobile chassis can start automatic positioning and navigation activities.
In addition, if positioning drift occurs due to interference of other factors in the positioning process, manual secondary intervention is often required to initialize the global position of the system again, and the positioning and navigation functions of the mobile chassis are recovered.
Therefore, the current positioning navigation system has the problem that the global position initialization cannot be carried out, and the global position initialization process depends on the participation of operators seriously, so that the autonomous mobile chassis often needs manual intervention and debugging, and the use and maintenance cost of the autonomous mobile chassis is increased.
Through search, the following results are found:
the invention discloses a robot with global position rapid estimation capability and a positioning method thereof, wherein the granted announcement number is CN103412565B, and the granted announcement date is 2016, 01, month and 27, and the positioning process comprises the following steps: carrying out 3D scanning on an environment space in advance to obtain a complete global map, rasterizing the environment space, and calculating a distance value from each grid to a barrier point closest to the grid; and starting the 3D scanning device to finish a frame of 3D scanning frame, simultaneously simulating a certain amount of particles to move according to the dynamic model of the robot platform by adopting a particle filter frame, carrying out simulated scanning on the environment map by the 3D scanning frame according to the position and the posture of each particle to obtain a laser scanning point of each particle, calculating by using a raster image to obtain the particle closest to the laser scanning point of the robot, wherein the position of the particle is the final estimation result of the global position of the robot. The method has the following defects:
1. the grid map involved in the method is a two-dimensional map, and a three-dimensional sensor is not required to be used for scanning and matching;
2. the method utilizes a particle filter framework to calculate the global position of the robot, and a precondition is required, namely, a good estimation is carried out on the initial position of the robot when the system is initialized, and the part of work still needs manual input into the system.
In summary, the prior art including the above patent documents still cannot avoid human participation, and no explanation or report on the similar technology to the present invention is found, and no similar data is collected at home and abroad.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a global position initialization method and a system of a laser radar positioning system, and simultaneously provides a corresponding terminal and a storage medium.
According to an aspect of the present invention, there is provided a global position initialization method for a lidar positioning system, including:
loading a global map and related parameters;
according to the related parameters, a local grid sub map is segmented in the global map;
generating n initial estimation positions which are estimated randomly in each segmented local grid sub-map;
for each initial estimation position, generating a matching degree score and a position conversion matrix thereof at a possible initial position of the corresponding mobile chassis;
determining global position initialization according to the matching degree score and the position conversion matrix;
and judging the situation of triggering the global position initialization again, and re-executing the steps to perform relocation.
Preferably, the global map refers to: a known environment map comprising a global coordinate system.
Preferably, the map type of the global map includes: pcd, ply, obj, stl, and pgm.
Preferably, the relevant parameters include: the method comprises the steps of grid sub-map segmentation scale parameters, random position numbers generated in each sub-map, score threshold parameters and repositioning trigger related parameters.
Preferably, the segmenting the local grid sub-map in the global map according to the relevant parameters includes:
calculating the number of grid horizontal and vertical lines and the coordinates of the grid central points according to the grid sub-map segmentation scale parameters in the related parameters;
and dividing the global map into a plurality of local grid sub-maps according to the grid central point coordinates.
Preferably, the obtained plurality of the partial grid maps are stored in a container.
Preferably, the generating n initial estimated positions estimated randomly in each of the partial mesh sub-maps of the segmentation comprises:
traversing each local grid sub-map, taking the central point, the x axis and the y axis of the local grid sub-map as ranges, generating n initial estimation positions of system random estimation, and obtaining a corresponding set P guess {P guess_1 ,P guess_2 ,…,P guess_n }。
Preferably, the set P guess Each of the initial estimated positions includes an x-axis position, a y-axis position, and a yaw angle θ, denoted as P (x, y, θ).
Preferably, x, y ∈ submap, θ ∈ (3.14).
Preferably, for each of the initial estimated positions, generating a matching degree score and a position transformation matrix thereof at a possible initial position of the corresponding mobile chassis includes:
traversing all possible initial positions of the mobile chassis, taking the initial estimation position as a calculation initial value at each possible initial position, calculating a matching degree score and a position conversion matrix thereof at the possible initial position by using an iterative closest point algorithm, and respectively obtaining a corresponding set S { S } 1 ,S 2 ,…,S n And P correct {P correct_1 ,P correct_2 ,…,P correct_n }。
Preferably, the determining the global position initialization according to the matching degree score and the position conversion matrix includes:
traversing the matching degree score, obtaining a possible initial position in the position conversion matrix corresponding to the minimum score, and if the minimum score is smaller than a set threshold a, determining that the global position initialization is successful, and directly starting positioning by the radar positioning system.
Preferably, the threshold value a is set as: 0.01.
preferably, the determining triggers the global position initialization again, and re-executes the above steps to perform relocation, including:
and monitoring and positioning one thread or one process in real time, and judging that the global position initialization condition is triggered again to perform relocation when any one or any plurality of abnormal conditions occur:
-two positioning results jump abnormally;
the results of the two positioning operations differ too much from the odometer or inertial measurement unit data;
-the current position location match degree score is greater than a set threshold b.
Preferably, the set threshold b is: 1.0.
according to another aspect of the present invention, there is provided a global position initialization system for a lidar positioning system, comprising:
the loading module is used for loading the global map and the related parameters;
the grid sub map segmentation module is used for segmenting a local grid sub map in the global map loaded by the loading module according to the relevant parameters loaded by the loading module;
an initial estimated position generating module that generates n initial estimated positions that are randomly estimated in each of the partial grid sub-maps segmented by the grid sub-map segmenting module;
a matching score and position conversion matrix generation module: the module generates a matching degree score and a position conversion matrix thereof at the possible initial position of the corresponding mobile chassis for each initial estimation position generated by the initial estimation position generation module;
a global position initialization module for determining global position initialization according to the matching degree score and the position conversion matrix generated by the matching score and position conversion matrix generation module;
and the repositioning module is used for judging whether the global position initialization condition is triggered again to reposition.
According to a third aspect of the present invention, there is provided a terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor when executing the program being operable to perform any of the methods described above.
According to a fourth aspect of the invention, there is provided a computer readable storage medium having stored thereon a computer program which, when executed by a processor, is operable to perform the method of any of the above.
Due to the adoption of the technical scheme, compared with the prior art, the invention has the following beneficial effects:
1. the global position initialization method and system, the terminal and the storage medium of the laser radar positioning system automatically initialize the global position, and avoid the complicated operation of manually inputting the initial position.
2. According to the global position initialization method and system, the terminal and the storage medium of the laser radar positioning system, when positioning is lost, global position relocation is automatically carried out, and manual intervention is avoided.
3. The global position initialization method and system, the terminal and the storage medium of the laser radar positioning system provided by the invention realize the autonomous mobile chassis without human intervention so as to reduce the labor cost.
4. The global position initialization method and system, the terminal and the storage medium of the laser radar positioning system provided by the invention remove the link of manually estimating the initial position and setting the initial position according to the environment map before the mobile chassis starts to operate, and can bring more accurate positioning effect on the basis of simplifying the working process.
5. The global position initialization method and system of the laser radar positioning system, the terminal and the storage medium provided by the invention realize the unmanned operation self-starting of the mobile chassis.
6. The invention provides a global position initialization method and system of a laser radar positioning system, a terminal and a storage medium, and realizes automatic loss relocation of a mobile chassis.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a flowchart illustrating a method for initializing a global position of a lidar positioning system according to an embodiment of the invention;
FIG. 2 is a flowchart of a global position initialization method for a lidar positioning system in a preferred embodiment of the invention;
FIG. 3 is a schematic diagram of a global map in a preferred embodiment of the present invention;
FIG. 4 is a schematic diagram of a segmented partial grid map according to a preferred embodiment of the present invention;
FIG. 5 is a diagram illustrating a randomly generated initial estimated position (in a first grid for example) in a preferred embodiment of the present invention;
FIG. 6 is a schematic diagram of possible initial positions and initial estimated positions in a local grid map in accordance with a preferred embodiment of the present invention;
FIG. 7 is a diagram illustrating successful global position initialization in a preferred embodiment of the present invention;
FIG. 8 is a schematic diagram of an environment map (i.e., a global map) built by webots software according to an embodiment of the present invention;
FIG. 9 is a schematic view of a visualization interface in an embodiment of the present invention;
fig. 10 is a block diagram of a global position initialization system of a lidar positioning system according to an embodiment of the present invention.
Detailed Description
The following examples illustrate the invention in detail: the embodiment is implemented on the premise of the technical scheme of the invention, and a detailed implementation mode and a specific operation process are given. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention.
Fig. 1 is a flowchart of a global position initialization method of a lidar positioning system in an embodiment of the present invention.
As shown in fig. 1, the global position initialization method for a lidar positioning system provided in this embodiment may include the following steps:
s100, loading a global map and related parameters;
s200, segmenting a local grid sub-map in the global map according to the related parameters;
s300, generating n initial estimation positions which are randomly estimated in each segmented partial grid sub-map;
s400, for each initial estimation position, generating a matching degree score and a position conversion matrix of the possible initial position of the corresponding mobile chassis;
s500, determining global position initialization according to the matching degree score and the position conversion matrix;
s600, judging the situation of triggering the global position initialization again, and re-executing the steps to perform relocation.
In S100 of this embodiment, as a preferred embodiment, the global map may be: a known environment map comprising a global coordinate system.
In S100 of this embodiment, as a preferred embodiment, the map type of the global map may include: pcd, ply, obj, stl, and pgm.
In S100 of this embodiment, as a preferred embodiment, the relevant parameters may include: the method comprises the steps of grid sub-map segmentation scale parameters, random position numbers generated in each sub-map, score threshold parameters and repositioning trigger related parameters.
In S200 of this embodiment, as a preferred embodiment, the segmenting the local mesh sub-map in the global map according to the relevant parameters may include the following steps:
s201, calculating the grid horizontal and vertical numbers and grid central point coordinates according to grid sub-map segmentation scale parameters in related parameters;
and S202, dividing the global map into a plurality of local grid sub-maps according to the grid central point coordinates.
In S202 of this embodiment, as a preferred embodiment, the obtained plurality of local mesh sub-maps are stored in a container.
In S300 of this embodiment, as a preferred embodiment, generating n initial estimated positions randomly estimated in each of the sliced partial grid sub-maps may include the following steps:
traversing each local grid sub-map, taking the central point, the x axis and the y axis of the local grid sub-map as ranges, generating n initial estimation positions which are estimated by the system randomly, and obtaining a corresponding set P guess {P guess_1 ,P guess_2 ,…,P guess_n }。
In S300 of this embodiment, as a preferred embodiment, the set P guess Each initial estimated position may include an x-axis position, a y-axis position, and a yaw angle θ, denoted as P (x, y, θ).
In a specific example of this embodiment, x, y ∈ submap, θ ∈ (3.14).
In S400 of this embodiment, as a preferred embodiment, for each initial estimated position, generating a matching degree score at a possible initial position of the corresponding mobile chassis and a position transformation matrix thereof, may include the following steps:
traversing all possible initial positions of the mobile chassis, calculating a matching degree score and a position conversion matrix of the possible initial positions by using the initial estimation position as a calculation initial value and using an iterative closest point algorithm at each possible initial position to respectively obtain a corresponding set S { S } 1 ,S 2 ,…,S n And P correct {P correct_1 ,P correct_2 ,…,P correct_n }。
In S500 of this embodiment, as a preferred embodiment, determining the global position initialization according to the matching degree score and the position transformation matrix may include the following steps:
traversing the matching degree score to obtain a minimum score S correct_min Corresponding set P correct Possible initial position P in (1) correct_min If the minimum score is S correct_min If the global position is smaller than the set threshold a, the global position is considered to be successfully initialized, and the radar positioning system directly starts positioning.
In a specific application example of this embodiment, the setting threshold a may be: 0.01
In S600 of this embodiment, determining that the global position initialization is triggered again, re-executing the above steps, and performing relocation may include the following steps:
and monitoring and positioning one thread or one process in real time, and judging that the global position initialization condition is triggered again to perform relocation when any one or any plurality of abnormal conditions occur:
-two positioning results jump abnormally;
the results of the two positioning operations differ too much from the odometer or inertial measurement unit data;
-the current position location match degree score is greater than a set threshold b.
In a specific application example of this embodiment, the threshold b is set as: 1.0
Fig. 2 is a flowchart of a global position initialization method for a lidar positioning system according to a preferred embodiment of the present invention.
As shown in fig. 2, the global position initialization method for a lidar positioning system provided in the preferred embodiment may include the following steps:
step 1: loading a global map and related parameters;
step 2: based on the loaded global map and related parameters, segmenting the local grid sub-map;
and step 3: in each divided partial grid sub-map, generating a randomly estimated initial estimation position to obtain a set P of the positions guess {P guess_1 ,P guess_2 ,…,P guess_n }；
And 4, step 4: for each initial estimation position, generating a matching degree score and a position conversion matrix of the corresponding possible initial position of the mobile chassis to obtain corresponding matchingMatching degree score set S and converted initial estimation position set P correct ；
And 5: performing global position optimal estimation according to the matching degree score and the position conversion matrix;
step 6: and monitoring according to the positioning condition and automatically relocating.
As a preferred embodiment, step 1 comprises the steps of:
step 1.1: loading a global map, wherein the supported map types comprise pcd, ply, obj, stl, pgm and the like, and the map is a known environment map containing a global coordinate system, as shown in fig. 3;
step 1.2: loading relevant parameters, including: a grid sub-map segmentation scale parameter, the number of random positions generated in each sub-map, a score threshold parameter and a repositioning trigger related parameter.
As a preferred embodiment, as shown in fig. 4, step 2 includes the following steps:
step 2.1: and calculating the number of grids in the horizontal direction and the longitudinal direction and the coordinates of the grid center points according to the grid sub-map segmentation scale parameters.
Step 2.2: and dividing the global map into local grid maps according to the grid center coordinates, and storing the local grid maps in a container.
As a preferred embodiment, as shown in fig. 5, step 3 includes the following steps:
traversing each partial grid sub-map, taking the central point of the grid and the x and y axes of the grid as ranges, generating n initial estimation positions randomly estimated by the system, and obtaining a corresponding set P guess {P guess_1 ,P guess_2 ,…,P guess_n }。
As a preferred embodiment, for the set P guess Each initial estimated location in (b), consisting of three parts, comprising: the x-axis position, y-axis position, and yaw angle (yaw), θ, may be referred to as P (x, y, θ).
In fig. 5, the random initial position estimate is the initial estimated position of the random estimate.
Further, x, y ∈ submap, θ ∈ (3.14).
As a preferred embodiment, as shown in fig. 6, step 4 includes the following steps:
traversing all possible initial positions of the mobile chassis, calculating a matching degree score and a position conversion matrix of the possible initial positions by using the initial estimation position as a calculation initial value and using an iterative closest point algorithm at each possible initial position to respectively obtain a corresponding set S { S } 1 ,S 2 ,…,S n And P correct {P correct_1 ,P correct_2 ,…,P correct_n }。
In fig. 6, the random initial position estimate is the randomly estimated initial position.
As a preferred embodiment, as shown in fig. 7, step 5 includes the following steps:
traversing the set S, and searching the S with the maximum matching degree and the minimum score correct_min Corresponding initial estimated position P correct_min The score physical meaning is the sum of deviation values between matching point cloud data, the smaller the score is, the better the matching degree is, that is, the score represents the distance between nearest adjacent points, and the smaller the distance is, the larger the probability is; if S correct_min If the global position is successfully initialized when the value is less than the set threshold value a, the radar positioning system can directly start positioning.
Further, the threshold value a is set as: 0.01.
in fig. 7, the random initial position estimate is the initial estimated position of the random estimate.
As a preferred embodiment, step 6 comprises the steps of:
and ensuring that one thread or process monitors a positioning result in real time, and judging that the global position initialization is triggered again to perform system relocation when any one or any plurality of abnormal conditions occur.
The abnormal conditions include:
-two positioning results jump abnormally;
the results of the two positioning operations differ too much from the odometer or inertial measurement unit data;
the current position location match degree score (probability too low) is greater than the set threshold b.
Further, the threshold b is set as: 1.0.
the technical solutions provided by the above embodiments of the present invention are further described in detail below with reference to the accompanying drawings and a specific application example.
In the specific application example, a std standard container in C + + is used for storing data structures such as a local grid map generated according to parameter cutting, a random initial estimated position, a matching degree score and the like.
The method provided by the embodiment of the invention is verified by using robot simulation software such as webots and gazebo to simulate radar data and an environment map, and dynamic result display can be performed by using a visualization class carried by a pcl point cloud library, wherein an example of the method is shown in fig. 8.
Fig. 9 is a schematic view of a visualization interface in this specific application example, as shown in fig. 9, a visualization of key elements of the methods including radar raw data, an environment map, a current sub-map and a matching result is included.
Another embodiment of the present invention provides a global position initialization system for a lidar positioning system, as shown in fig. 10, which may include the following modules:
the system comprises a loading module, a grid sub-map segmentation module, an initial estimation position generation module, a matching score and position conversion matrix generation module, a global position initialization module and a relocation module.
Wherein:
the loading module is used for loading the global map and the related parameters;
the grid sub map segmentation module is used for segmenting a local grid sub map in the global map loaded by the loading module according to the relevant parameters loaded by the loading module;
an initial estimated position generating module which generates n initial estimated positions which are randomly estimated in each local grid sub-map which is segmented by the grid sub-map segmenting module;
a matching score and position conversion matrix generation module: the module generates a matching degree score and a position conversion matrix thereof at the possible initial position of the corresponding mobile chassis for each initial estimation position generated by the initial estimation position generation module;
a global position initialization module for determining global position initialization according to the matching score and the matching degree score and the position conversion matrix generated by the position conversion matrix generation module;
and the repositioning module is used for judging whether the global position initialization condition is triggered again to reposition.
A third embodiment of the present invention provides a terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor is operable to execute the method of any one of the above embodiments of the present invention when executing the program.
Optionally, a memory for storing a program; a Memory, which may include a volatile Memory (RAM), such as a Random Access Memory (SRAM), a Double Data Rate Synchronous Dynamic Random Access Memory (DDR SDRAM), and the like; the memory may also include a non-volatile memory, such as a flash memory. The memories are used to store computer programs (e.g., applications, functional modules, etc. that implement the above-described methods), computer instructions, etc., which may be stored in partition in the memory or memories. And the computer programs, computer instructions, data, etc. described above may be invoked by a processor.
The computer programs, computer instructions, etc. described above may be stored in one or more memories in a partitioned manner. And the computer programs, computer instructions, data, etc. described above may be invoked by a processor.
A processor for executing the computer program stored in the memory to implement the steps of the method according to the above embodiments. Reference may be made in particular to the description relating to the previous method embodiments.
The processor and the memory may be separate structures or may be an integrated structure integrated together. When the processor and the memory are separate structures, the memory, the processor may be coupled by a bus.
A fourth embodiment of the invention provides a computer-readable storage medium, on which a computer program is stored which, when executed by a processor, is operable to perform the method of any one of the above-described embodiments of the invention.
The global position initialization method and system, the terminal and the storage medium of the laser radar positioning system provided by the embodiment of the invention automatically initialize the global position, thereby avoiding the complicated operation of manually inputting the initial position; when the positioning is lost, the global position repositioning is automatically carried out, so that the manual intervention is avoided; the autonomous mobile chassis without human intervention is realized, so that the labor cost is reduced; the link that the initial position is manually presumed and set before the mobile chassis starts to operate according to the environment map is removed, and more accurate positioning effect can be brought on the basis of simplifying the working process; the unmanned operation self-starting of the mobile chassis is realized; and automatic lost relocation of the mobile chassis is realized.
It should be noted that, the steps in the method provided by the present invention may be implemented by using corresponding modules, devices, units, and the like in the system, and those skilled in the art may implement the composition of the system by referring to the technical solution of the method, that is, the embodiment in the method may be understood as a preferred example for constructing the system, and will not be described herein again.
Those skilled in the art will appreciate that, in addition to implementing the system and its various means provided by the present invention in purely computer readable program code means, the system and its various means provided by the present invention can be implemented with the same functionality in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like by entirely programming the method steps logically. Therefore, the system and various devices thereof provided by the present invention can be regarded as a hardware component, and the devices included in the system and various devices thereof for realizing various functions can also be regarded as structures in the hardware component; means for performing the various functions may also be conceived of as structures within both software modules and hardware components of the illustrated method.
The foregoing description has described specific embodiments of the present invention. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes and modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention.
Claims (7)
1. A global position initialization method of a laser radar positioning system is characterized by comprising the following steps:
loading a global point cloud map and related parameters;
according to the related parameters, a local grid sub-map is segmented in the global point cloud map;
generating n initial estimation positions which are estimated randomly in each segmented local grid sub-map;
for each initial estimation position, generating a matching degree score and a position conversion matrix thereof at a possible initial position of the corresponding mobile chassis;
determining global position initialization according to the matching degree score and the position conversion matrix;
judging the situation of triggering the global position initialization again, and re-executing the steps to perform relocation;
the judging triggers the global position initialization condition again, and re-executes the steps to perform relocation, and the method comprises the following steps:
and monitoring and positioning one thread or one process in real time, and judging that the global position initialization condition is triggered again to perform relocation when any one or any plurality of abnormal conditions occur:
-two positioning results jump abnormally;
the results of the two positioning operations differ too much from the odometer or inertial measurement unit data;
-the current position location match degree score is greater than a set threshold b;
in each of the sliced partial mesh sub-maps, n initial estimated positions which are randomly estimated are generated, including the steps of:
traversing each local grid sub-map, taking the central point, the x axis and the y axis of the local grid sub-map as ranges, generating n initial estimation positions of system random estimation, and obtaining a corresponding set P guess {P guess_1 ,P guess_2 ,…,P guess_n }; wherein:
set P guess Each of the initial estimated positions, including an x-axis position, a y-axis position, and a yaw angle θ, denoted as P (x, y, θ);
for each of the initial estimated positions, generating a matching degree score and a position conversion matrix thereof at a possible initial position of the corresponding mobile chassis, comprising the following steps:
traversing all possible initial positions of the mobile chassis, taking the corresponding initial estimation position as a calculation initial value at each possible initial position, calculating the matching degree score and the position conversion matrix thereof at the possible initial position by using an iterative closest point algorithm, and respectively obtaining the corresponding set S { S } 1 ,S 2 ,…,S n And P correct {P correct_1 ,P correct_2 ,…,P correct_n }。
2. The global position initialization method of the lidar positioning system according to claim 1, wherein the global point cloud map is: a known environment map comprising a global coordinate system; and/or
The relevant parameters comprise: a grid sub-map segmentation scale parameter, the number of random positions generated in each sub-map, a score threshold parameter and a relocation trigger related parameter.
3. The global position initialization method of the lidar positioning system according to claim 1, wherein the segmenting the local grid sub-map in the global point cloud map according to the related parameter comprises the following steps:
calculating the number of grid horizontal and vertical lines and the coordinates of the grid central points according to the grid sub-map segmentation scale parameters in the related parameters;
and dividing the global point cloud map into a plurality of local grid sub-maps according to the grid central point coordinates.
4. The global position initialization method of the lidar positioning system according to claim 1, wherein the determining the global position initialization based on the matching degree score and the position transformation matrix comprises:
traversing the matching degree score, obtaining a possible initial position in the position conversion matrix corresponding to the minimum score, and if the minimum score is smaller than a set threshold a, determining that the global position initialization is successful, and directly starting positioning by the radar positioning system.
5. A global position initialization system for a lidar positioning system, comprising:
the loading module is used for loading the global point cloud map and the related parameters;
the grid sub-map segmentation module is used for segmenting a local grid sub-map in the global point cloud map loaded by the loading module according to the relevant parameters loaded by the loading module;
an initial estimated position generating module which generates n initial estimated positions which are randomly estimated in each local grid sub-map segmented by the grid sub-map segmenting module;
a matching score and position conversion matrix generation module: the module generates a matching degree score and a position conversion matrix thereof at the possible initial position of the corresponding mobile chassis for each initial estimation position generated by the initial estimation position generation module;
a global position initialization module for determining global position initialization according to the matching degree score and the position conversion matrix generated by the matching score and position conversion matrix generation module;
the relocation module is used for judging the situation of triggering the global position initialization again and carrying out relocation; the method comprises the following steps of monitoring and positioning one thread or one process in real time, judging that the global position initialization condition is triggered again when any one or any multiple abnormal conditions occur, and repositioning:
-two positioning results jump abnormally;
the results of the two positioning operations differ too much from the odometer or inertial measurement unit data;
-the current position location match degree score is greater than a set threshold b;
wherein:
in each of the sliced partial mesh sub-maps, n initial estimated positions which are randomly estimated are generated, including the steps of:
traversing each local grid sub-map, taking the central point, the x axis and the y axis of the local grid sub-map as ranges, generating n initial estimation positions of system random estimation, and obtaining a corresponding set P guess {P guess_1 ,P guess_2 ,…,P guess_n }; wherein:
set P guess Each of the initial estimated positions, including an x-axis position, a y-axis position, and a yaw angle θ, denoted as P (x, y, θ);
for each initial estimation position, generating a matching degree score and a position conversion matrix thereof at the possible initial position of the corresponding mobile chassis, and comprising the following steps:
traversing all possible initial positions of the mobile chassis, taking the corresponding initial estimation position as a calculation initial value at each possible initial position, calculating a matching degree score and a position conversion matrix thereof at the possible initial position by using an iterative closest point algorithm, and respectively obtaining a corresponding set S { S } 1 ,S 2 ,…,S n And P correct {P correct_1 ,P correct_2 ,…,P correct_n }。
6. A terminal comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor, when executing the program, is operative to perform the method of any of claims 1-4.
7. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, is adapted to carry out the method of any one of claims 1 to 4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011511527.2A CN112711012B (en) | 2020-12-18 | 2020-12-18 | Global position initialization method and system of laser radar positioning system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011511527.2A CN112711012B (en) | 2020-12-18 | 2020-12-18 | Global position initialization method and system of laser radar positioning system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112711012A CN112711012A (en) | 2021-04-27 |
| CN112711012B true CN112711012B (en) | 2022-10-11 |
Family
ID=75544595
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011511527.2A Active CN112711012B (en) | 2020-12-18 | 2020-12-18 | Global position initialization method and system of laser radar positioning system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112711012B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116840820B (en) * | 2023-08-29 | 2023-11-24 | 上海仙工智能科技有限公司 | Method and system for detecting 2D laser positioning loss and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107395433A (en) * | 2017-08-18 | 2017-11-24 | 中南大学 | A kind of wireless sensor node dispositions method based on fireworks algorithm |
| WO2018006082A2 (en) * | 2016-07-01 | 2018-01-04 | Uber Technologies, Inc. | Autonomous vehicle control using submaps |
| CN107908185A (en) * | 2017-10-14 | 2018-04-13 | 北醒（北京）光子科技有限公司 | A kind of robot autonomous global method for relocating and robot |
| CN110686677A (en) * | 2019-10-10 | 2020-01-14 | 东北大学 | Global positioning method based on geometric information |
| CN111429520A (en) * | 2020-03-02 | 2020-07-17 | 广州视源电子科技股份有限公司 | Negative obstacle detection method and device, terminal device and storage medium |
| CN111538918A (en) * | 2020-04-23 | 2020-08-14 | 上海高仙自动化科技发展有限公司 | Recommendation method and device, electronic equipment and storage medium |
| CN111928862A (en) * | 2020-08-10 | 2020-11-13 | 廊坊和易生活网络科技股份有限公司 | Method for constructing semantic map on line by fusing laser radar and visual sensor |
| CN111947672A (en) * | 2020-03-02 | 2020-11-17 | 北京百度网讯科技有限公司 | Method, apparatus, device and medium for detecting environmental changes |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201116959D0 (en) * | 2011-09-30 | 2011-11-16 | Bae Systems Plc | Vehicle localisation with 2d laser scanner and 3d prior scans |
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN107167788B (en) * | 2017-03-21 | 2020-01-21 | 深圳市速腾聚创科技有限公司 | Method and system for obtaining laser radar calibration parameters and laser radar calibration |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
| US10891769B2 (en) * | 2019-02-14 | 2021-01-12 | Faro Technologies, Inc | System and method of scanning two dimensional floorplans using multiple scanners concurrently |
| CN110031009A (en) * | 2019-04-04 | 2019-07-19 | 肖卫国 | A kind of initial alignment method, computer readable storage medium and system |
| CN111060888B (en) * | 2019-12-31 | 2023-04-07 | 芜湖哈特机器人产业技术研究院有限公司 | Mobile robot repositioning method fusing ICP and likelihood domain model |
| CN111678516B (en) * | 2020-05-08 | 2021-11-23 | 中山大学 | Bounded region rapid global positioning method based on laser radar |
| CN111578946A (en) * | 2020-05-27 | 2020-08-25 | 杭州蓝芯科技有限公司 | Laser navigation AGV repositioning method and device based on deep learning |
| CN111737395B (en) * | 2020-08-19 | 2020-12-22 | 浙江欣奕华智能科技有限公司 | Method and device for generating occupancy grid map and robot system |
| CN112012450B (en) * | 2020-09-18 | 2022-04-12 | 上海蔚建科技有限公司 | Automatic plastering machine |
-
2020
- 2020-12-18 CN CN202011511527.2A patent/CN112711012B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018006082A2 (en) * | 2016-07-01 | 2018-01-04 | Uber Technologies, Inc. | Autonomous vehicle control using submaps |
| CN107395433A (en) * | 2017-08-18 | 2017-11-24 | 中南大学 | A kind of wireless sensor node dispositions method based on fireworks algorithm |
| CN107908185A (en) * | 2017-10-14 | 2018-04-13 | 北醒（北京）光子科技有限公司 | A kind of robot autonomous global method for relocating and robot |
| CN110686677A (en) * | 2019-10-10 | 2020-01-14 | 东北大学 | Global positioning method based on geometric information |
| CN111429520A (en) * | 2020-03-02 | 2020-07-17 | 广州视源电子科技股份有限公司 | Negative obstacle detection method and device, terminal device and storage medium |
| CN111947672A (en) * | 2020-03-02 | 2020-11-17 | 北京百度网讯科技有限公司 | Method, apparatus, device and medium for detecting environmental changes |
| CN111538918A (en) * | 2020-04-23 | 2020-08-14 | 上海高仙自动化科技发展有限公司 | Recommendation method and device, electronic equipment and storage medium |
| CN111928862A (en) * | 2020-08-10 | 2020-11-13 | 廊坊和易生活网络科技股份有限公司 | Method for constructing semantic map on line by fusing laser radar and visual sensor |
Non-Patent Citations (4)
| Title |
|---|
| New Monte Carlo Localization Using Deep Initialization: A Three-Dimensional LiDAR and a Camera Fusion Approach;HYUNGGI JO;《IEEE Access》;20200817;74485-74496 * |
| 基于典型栅格地图的轮式移动机器人路径规划与跟踪;王启宇等;《西南科技大学学报》;20171230(第04期);57-59、91 * |
| 移动机器人视觉-惯性组合定位及建图算法研究;张婉莹;《中国优秀硕士学位论文全文数据库 信息科技辑》;20200215;全文 * |
| 空间记忆与类脑导航研究进展;斯白露等;《人工智能》;20200210(第01期);17-32 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112711012A (en) | 2021-04-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112884894B (en) | Scene reconstruction data acquisition method and device, computer equipment and storage medium | |
| CN109931942B (en) | Robot path generation method, device, robot and storage medium | |
| CN111429574A (en) | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion | |
| US10303180B1 (en) | Generating and utilizing non-uniform volume measures for voxels in robotics applications | |
| US20140025203A1 (en) | Collision detection system, collision detection data generator, and robot | |
| CN106408604A (en) | Filtering method and device for point cloud data | |
| EP3974778B1 (en) | Method and apparatus for updating working map of mobile robot, and storage medium | |
| CN110986953B (en) | Path planning method, robot and computer readable storage medium | |
| CN107622530B (en) | Efficient and robust triangulation network cutting method | |
| CN112835064B (en) | Mapping positioning method, system, terminal and medium | |
| Moura et al. | Bim-based localization and mapping for mobile robots in construction | |
| CN112090078B (en) | Game character movement control method, device, equipment and medium | |
| CN113119112B (en) | Motion planning method and system suitable for vision measurement of six-degree-of-freedom robot | |
| US20230015214A1 (en) | Planar contour recognition method and apparatus, computer device, and storage medium | |
| CN111210475A (en) | Map updating method and device | |
| CN112711012B (en) | Global position initialization method and system of laser radar positioning system | |
| CN115326051A (en) | Positioning method and device based on dynamic scene, robot and medium | |
| CN115752474A (en) | Robot navigation planning method and device under non-flat ground environment and robot | |
| US20150103080A1 (en) | Computing device and method for simulating point clouds | |
| KR20220035879A (en) | Grid map generation method, apparatus and computer readable storage medium | |
| CN113671523A (en) | Robot positioning method, device, storage medium and robot | |
| CN111161424B (en) | Determination method and determination device for three-dimensional map | |
| JP5766936B2 (en) | 3D environment restoration device, 3D environment restoration method, and robot | |
| Neves et al. | An environment to support structural testing of autonomous vehicles | |
| Birr et al. | Oriented surface reachability maps for robot placement |
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 |