CN110887493B - Track calculation method, medium, terminal and device based on local map matching - Google Patents
Track calculation method, medium, terminal and device based on local map matching Download PDFInfo
- Publication number
- CN110887493B CN110887493B CN201911205872.0A CN201911205872A CN110887493B CN 110887493 B CN110887493 B CN 110887493B CN 201911205872 A CN201911205872 A CN 201911205872A CN 110887493 B CN110887493 B CN 110887493B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- frame point
- pose
- frame
- relative
- 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
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a track calculation method, medium, terminal and device based on local map matching, which are used for obtaining a relative pose through matching calculation of adjacent point clouds, taking the relative pose as initial transformation to transform the current point clouds into a local map, and then obtaining the absolute pose through correction of a map matching algorithm. The method only uses one laser sensor, so that the difficulty of multi-sensor correction and fusion is avoided, the implementation is simpler, meanwhile, the track obtained by the method is improved by at least 10 times of precision compared with the track obtained by the prior inertial navigation or wheel type odometer, the pose error can be controlled within 0.1%, and meanwhile, the time for calculating absolute poses in a single time is greatly reduced and can be controlled within 20ms, so that the method is suitable for being deployed on a robot main body for real-time estimation.
Description
[ field of technology ]
The present invention relates to the field of navigation positioning, and in particular, to a track estimation method, medium, terminal and device based on local map matching.
[ background Art ]
With the progress of electronic hardware and the improvement of the computational power of processors, mobile robots capable of responding in real time are gradually moving into the daily life of human beings. Wherein, positioning capability is the key of the robot to realize autonomous movement. The positioning is a process that the robot calculates the pose of the robot by sensing the information of the robot and the surrounding environment and carrying out certain data fusion processing. At present, robot positioning is generally divided into relative positioning and absolute positioning. Common absolute positioning includes Global Positioning System (GPS), ultrasonic positioning system, and the like. Since the working area of a robot is usually defined and limited, it is generally not considered the position of the robot in the earth, but only the absolute position thereof in the working area. The commonly used relative positioning is referred to as inertial navigation or odometry. Inertial navigation mainly uses a three-axis acceleration, a three-axis gyroscope or a three-axis magnetometer provided by an Inertial Measurement Unit (IMU). In a static state, the self direction gesture is acquired through the magnetometer and the accelerometer. In a motion state, the position is acquired by an accelerometer, and the posture is corrected by a gyroscope. The mileage calculation method mainly uses an on-wheel encoder, calculates the relative pose of the robot according to the rotation pulse count of the on-wheel encoder in a motion state, and can accumulate the current absolute pose of the robot in a working area according to the initial pose. In recent years, relative positioning has progressed considerably. The pose of the robot obtained by the mileage calculation method is not limited to the use of an on-wheel encoder. Different from the self pose according to self information, the robot can also calculate the self relative pose information according to the environment information. For example, the relative displacement of the robot is back-deduced using the relative displacement of adjacent laser point clouds or visual images. The relative displacement information of the adjacent point cloud or the image can be obtained through matching calculation. According to the initial pose, the absolute pose of the robot in the working area can be obtained through accumulation calculation.
In absolute positioning, the Global Positioning System (GPS) cannot be used indoors and update frequency is low; although the ultrasonic positioning system has the characteristics of low cost, miniaturization and easy connection, the ultrasonic positioning system cannot be used under long distance, and meanwhile, corresponding devices are required to be arranged in a large area in a scene, and the problem of signal interference is solved, so that the ultrasonic positioning system cannot meet the positioning requirement of a robot. In the relative positioning, the pose calculation needs to perform primary or secondary integration on the data of an Inertial Measurement Unit (IMU), and the nonlinear temperature drift of the Inertial Measurement Unit (IMU) is added, so that the estimated value lacks stability and accuracy. The mileage calculation method based on the wheel encoder is limited by the slip caused by the vibration of the robot or ground factors, and the dead reckoning has accumulated errors, so that the method is also inaccurate. The accuracy of pose estimation is limited by the matching effect and the inherent deviation of laser or vision to the environmental perception, and the matching is limited by a certain overlapping range, so that the estimation result has larger error.
[ invention ]
The invention provides a track calculation method, medium, terminal and device based on local map matching, which solve the technical problems.
The technical scheme for solving the technical problems is as follows: a track reckoning method based on local map matching comprises the following steps:
step 1, taking an initial frame point cloud acquired at a starting position as a reference point cloud;
step 2, transforming the reference point cloud into a grid map;
step 3, acquiring a second frame point cloud adjacent to the reference point cloud and taking the second frame point cloud as a current point cloud;
step 4, matching the current point cloud with the reference point cloud to generate a first relative pose of the current point cloud relative to the reference point cloud;
step 5, accumulating the first relative pose to generate an absolute pose of a second frame point cloud, and adding the second frame point cloud to a grid map in the absolute pose of the second frame point cloud;
step 6, acquiring a third frame point cloud adjacent to the second frame point cloud, matching the third frame point cloud with the second frame point cloud to generate a second relative pose of the third frame point cloud relative to the second frame point cloud, and adding the third frame point cloud to the current grid map in the absolute pose of the second frame point cloud;
step 7, extracting the angle of the second relative pose, and generating the absolute pose of a third frame point cloud based on a map matching algorithm and a current grid map;
step 8, judging whether the distance between the absolute pose of the third frame point cloud and the initial position is larger than a preset distance, if not, updating the third frame point cloud into the current point cloud, continuously acquiring point cloud data, and repeating the steps 4-8 to calculate the robot track; if yes, deleting the current grid map, taking the absolute pose of the third frame point cloud as a starting position, taking the third frame point cloud as a reference point cloud, repeating the steps 2-8, reestablishing the grid map and calculating the robot track.
In a preferred embodiment, a first relative pose of the current point cloud with respect to the reference point cloud and a second relative pose of the third frame point cloud with respect to the second frame point cloud are generated using an iterative closest point algorithm ICP, the number of iterations of which is set to 2-5, and the initial transformation is set to (0, 0).
In a preferred embodiment, the predetermined distance is 10-20 meters.
In a preferred embodiment, accumulating the first relative pose using a wheeled differential drive model generates an absolute pose of the second frame point cloud.
A second aspect of the embodiments of the present invention provides a computer-readable storage medium storing a computer program which, when executed by a processor, implements the above-described trajectory estimation method based on local map matching.
A third aspect of the embodiments of the present invention provides a track estimation terminal based on local map matching, including the computer readable storage medium and a processor, where the processor implements the steps of the track estimation method based on local map matching when executing a computer program on the computer readable storage medium.
A fourth aspect of the embodiment of the present invention provides a track estimation device based on local map matching, which includes a point cloud acquisition module, a first grid map transformation module, a relative pose generation module, a first absolute pose generation module, a second grid map transformation module, a second absolute pose generation module and a judgment module,
the point cloud acquisition module is used for acquiring point cloud data;
the first grid map transformation module is used for taking the initial frame point cloud as a reference point cloud and transforming the reference point cloud into a grid map;
the relative pose generation module is used for matching the current point cloud with the reference point cloud to generate a first relative pose of the current point cloud relative to the reference point cloud; and the second relative pose of the third frame point cloud relative to the second frame point cloud is generated by matching the third frame point cloud with the second frame point cloud;
the first absolute pose generation module is used for accumulating the first relative pose to generate the absolute pose of the second frame point cloud;
the second grid map transformation module is used for adding the second frame point cloud and the third frame point cloud to the grid map in the absolute pose of the second frame point cloud;
the second absolute pose generation module is used for extracting the angle of the second relative pose and generating the absolute pose of a third frame point cloud based on a map matching algorithm and a current grid map;
the judging module is used for judging whether the distance between the absolute pose of the third frame point cloud and the initial position is larger than a preset distance, if not, updating the third frame point cloud into the current point cloud, continuously acquiring point cloud data, and repeating the steps 4-8 to calculate the robot track; if yes, deleting the current grid map, taking the absolute pose of the third frame point cloud as a starting position, taking the third frame point cloud as a reference point cloud, repeating the steps 2-8, reestablishing the grid map and calculating the robot track.
In a preferred embodiment, a first relative pose of the current point cloud with respect to the reference point cloud and a second relative pose of the third frame point cloud with respect to the second frame point cloud are generated using an iterative closest point algorithm ICP, the number of iterations of which is set to 2-5, and the initial transformation is set to (0, 0).
In a preferred embodiment, the predetermined distance is 10-20 meters.
In a preferred embodiment, the first absolute pose generation module is specifically configured to accumulate the first relative pose by using a wheeled differential driving model to generate the absolute pose of the second frame point cloud.
The invention provides a simple, effective, stable and accurate method for calculating relative pose and absolute pose, which is used for obtaining the relative pose through matching calculation of adjacent point clouds, transforming the current point clouds into a local map by taking the relative pose as initial transformation, and correcting the absolute pose through a map matching algorithm. The method only uses one laser sensor, so that the difficulty of multi-sensor correction and fusion is avoided, the implementation is simpler, meanwhile, the track obtained by the method is improved by at least 10 times of precision compared with the track obtained by the prior inertial navigation or wheel type odometer, the pose error can be controlled within 0.1%, and meanwhile, the time for calculating absolute poses in a single time is greatly reduced and can be controlled within 20ms, so that the method is suitable for being deployed on a robot main body for real-time estimation.
In order to make the above objects, features and advantages of the present invention more comprehensible, preferred embodiments accompanied with figures are described in detail below.
[ description of the drawings ]
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flow chart of a track estimation method based on local map matching provided in embodiment 1;
fig. 2 is a schematic structural diagram of a track estimation device based on local map matching provided in embodiment 2;
fig. 3 is a schematic structural diagram of a track estimation terminal based on local map matching provided in embodiment 3;
fig. 4 is a movement trajectory of a mobile robot in a 90 x 60 square meter scene;
fig. 5 is a grid map created by a mobile robot in a 90 x 60 square meter scene.
[ detailed description ] of the invention
In order to make the objects, technical solutions and advantageous technical effects of the present invention more apparent, the present invention will be further described in detail with reference to the accompanying drawings and detailed description. It should be understood that the detailed description is intended to illustrate the invention, and not to limit the invention.
Fig. 1 is a flow chart of a track estimation method based on local map matching provided in embodiment 1 of the present invention, as shown in fig. 1, the method includes the following steps:
step 1, taking an initial frame point cloud acquired at a starting position as a reference point cloud;
and step 2, acquiring absolute pose of the point cloud and preset size of the grid map, and converting the reference point cloud into the grid map.
And step 3, acquiring a second frame point cloud adjacent to the initial frame point cloud as a current point cloud.
And 4, matching the current point cloud with the reference point cloud by using an iterative closest point algorithm ICP, and generating a first relative pose of the current point cloud relative to the reference point cloud, wherein an initial transformation is required to be given, and the initial transformation given by the embodiment is (0, 0). Meanwhile, the generated first relative pose is only used as initial transformation in the map matching algorithm, so that only a relatively rough result of the iterative closest point algorithm is needed, and the iteration times are set to be 2-5 times, preferably 3 times, in order to improve the calculation efficiency. Of course, in other embodiments, other iterative algorithms may be used to generate the first relative pose of the current point cloud with respect to the reference point cloud, and the specific calculation process is not described in detail herein, but is within the scope of the present application.
And 5, accumulating the first relative pose by adopting a wheel type differential driving model to generate the absolute pose of the second frame point cloud, and adding the second frame point cloud to the grid map at the absolute pose of the second frame point cloud. Other robot kinematic models may also be employed in other embodiments to accumulate the first relative pose to generate an absolute pose of a second frame point cloud. In a preferred embodiment, the current point cloud is added to the grid map at the current absolute pose according to a grid map update law, such as a grid occupancy probability model.
And 6, acquiring a third frame point cloud adjacent to the second frame point cloud, matching the third frame point cloud with the second frame point cloud to generate a second relative pose of the third frame point cloud relative to the second frame point cloud, and adding the third frame point cloud to the current grid map in the absolute pose of the second frame point cloud.
And 7, extracting the angle of the second relative pose, and generating the absolute pose of the third frame point cloud based on a map matching algorithm and the current grid map. In the map matching algorithm, the occupied grid search is mainly performed based on the rasterized point cloud data and the current grid map, the score is calculated, and the pose is adjusted step by step according to the score. In this embodiment, the occupied grid search range is set to be within 5 layers of grids around the current rasterized point cloud, so as to ensure the calculation efficiency and accuracy.
Step 8, judging whether the distance between the absolute pose of the third frame point cloud and the initial position is larger than a preset distance, if not, updating the third frame point cloud to be the current point cloud (namely a new second frame point cloud) and continuously acquiring point cloud data as a new third frame point cloud, and then repeating the steps 4-8 to calculate the robot track; if yes, deleting the current grid map, taking the absolute pose of the third frame point cloud as a starting position, taking the third frame point cloud as a reference point cloud, repeating the steps 2-8, reestablishing the grid map and calculating the robot track. In a preferred embodiment, the predetermined distance is 10-20 meters, such as 15 meters. As shown in fig. 4 and 5, in the above preferred embodiment, the moving track of the mobile robot in the 90×60 square meter scene and the created grid map are shown.
The above preferred embodiment provides a simple, effective, stable and accurate method for calculating the relative pose and the absolute pose, which obtains the relative pose through matching calculation of adjacent point clouds, transforms the current point clouds into a local map as initial transformation, and then corrects the absolute pose through a map matching algorithm. The method only uses one laser sensor, so that the difficulty of multi-sensor correction and fusion is avoided, the implementation is simpler, meanwhile, the track obtained by the method is improved by at least 10 times of precision compared with the track obtained by the prior inertial navigation or wheel type odometer, the pose error can be controlled within 0.1%, and meanwhile, the time for calculating absolute poses in a single time is greatly reduced and can be controlled within 20ms, so that the method is suitable for being deployed on a robot main body for real-time estimation.
It should be understood that the sequence number of each step in the foregoing embodiment does not mean that the execution sequence of each process should be determined by the function and the internal logic, and should not limit the implementation process of the embodiment of the present invention.
The embodiment of the invention also provides a computer readable storage medium which stores a computer program, wherein the computer program realizes the track estimation method based on the local map matching when being executed by a processor.
Fig. 2 is a schematic structural diagram of a track estimation device based on local map matching according to embodiment 2 of the present invention, as shown in fig. 2, including a point cloud acquisition module 100, a first grid map transformation module 200, a relative pose generation module 300, a first absolute pose generation module 400, a second grid map transformation module 500, a second absolute pose generation module 600 and a determination module 700,
the point cloud acquisition module 100 is configured to acquire point cloud data;
the first grid map transformation module 200 is configured to take an initial frame point cloud as a reference point cloud, and transform the reference point cloud into a grid map;
the relative pose generating module 300 is configured to match the current point cloud with the reference point cloud to generate a first relative pose of the current point cloud relative to the reference point cloud; and the second relative pose of the third frame point cloud relative to the second frame point cloud is generated by matching the third frame point cloud with the second frame point cloud;
the first absolute pose generation module 400 is configured to accumulate the first relative pose to generate an absolute pose of the second frame point cloud;
the second grid map transformation module 500 is configured to add the second frame point cloud and the third frame point cloud to the grid map in the absolute pose of the second frame point cloud;
the second absolute pose generation module 600 is configured to extract an angle of the second relative pose, and generate an absolute pose of a third frame point cloud based on a map matching algorithm and a current grid map;
the judging module 700 is configured to judge whether the distance between the absolute pose of the third frame point cloud and the starting position is greater than a preset distance, if not, update the third frame point cloud to be the current point cloud, and continue to obtain the point cloud data and repeat the steps 4-8 to calculate the robot track; if yes, deleting the current grid map, taking the absolute pose of the third frame point cloud as a starting position, taking the third frame point cloud as a reference point cloud, repeating the steps 2-8, reestablishing the grid map and calculating the robot track.
In a preferred embodiment, the relative pose generation module 300 generates a first relative pose of the current point cloud with respect to the reference point cloud and a second relative pose of the third frame point cloud with respect to the second frame point cloud using an iterative closest point algorithm ICP, where the iterative closest point algorithm ICP is set to 2-5 times and the initial transformation is set to (0, 0).
In a preferred embodiment, the predetermined distance is 10-20 meters.
In a preferred embodiment, the first absolute pose generation module 400 is specifically configured to accumulate the first relative pose using a wheeled differential driving model to generate the absolute pose of the second frame point cloud.
The embodiment of the invention also provides a track reckoning terminal based on local map matching, which comprises the computer readable storage medium and a processor, wherein the processor realizes the steps of the track reckoning method based on local map matching when executing the computer program on the computer readable storage medium. Fig. 3 is a schematic structural diagram of a track estimation terminal based on local map matching provided in embodiment 3 of the present invention, and as shown in fig. 3, a track estimation terminal 8 based on local map matching in this embodiment includes: a processor 80, a readable storage medium 81, and a computer program 82 stored in the readable storage medium 81 and executable on the processor 80. The steps of the various method embodiments described above, such as steps 1 through 8 shown in fig. 1, are implemented when the processor 80 executes the computer program 82. Alternatively, the processor 80, when executing the computer program 82, performs the functions of the modules of the apparatus embodiments described above, such as the functions of the modules 100 through 700 shown in fig. 2.
By way of example, the computer program 82 may be partitioned into one or more modules that are stored in the readable storage medium 81 and executed by the processor 80 to perform the present invention. The one or more modules may be a series of computer program instruction segments capable of performing a specific function for describing the execution of the computer program 82 in the local map matching-based trajectory computation terminal 8.
The track estimation terminal 8 based on local map matching may include, but is not limited to, a processor 80, a readable storage medium 81. It will be understood by those skilled in the art that fig. 3 is merely an example of the track estimation terminal 8 based on the local map matching, and does not constitute a limitation of the track estimation terminal 8 based on the local map matching, and may include more or less components than those illustrated, or may combine some components, or different components, for example, the track estimation terminal based on the local map matching may further include a power management module, an operation processing module, an input/output device, a network access device, a bus, and the like.
The processor 80 may be a central processing unit (Central Processing Unit, CPU), other general purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), off-the-shelf programmable gate arrays (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The readable storage medium 81 may be an internal storage unit of the track estimation terminal 8 based on local map matching, for example, a hard disk or a memory of the track estimation terminal 8 based on local map matching. The readable storage medium 81 may be an external storage device of the track estimation terminal 8 based on local map matching, for example, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash Card (Flash Card) or the like provided on the track estimation terminal 8 based on local map matching. Further, the readable storage medium 81 may further include both an internal storage unit and an external storage device of the track estimation terminal 8 based on local map matching. The readable storage medium 81 is used for storing the computer program and other programs and data required by the track estimation terminal based on local map matching. The readable storage medium 81 may also be used to temporarily store data that has been output or is to be output.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-described division of the functional units and modules is illustrated, and in practical application, the above-described functional distribution may be performed by different functional units and modules according to needs, i.e. the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-described functions. The functional units and modules in the embodiment may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit, where the integrated units may be implemented in a form of hardware or a form of a software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working process of the units and modules in the above system may refer to the corresponding process in the foregoing method embodiment, which is not described herein again.
In the foregoing embodiments, the descriptions of the embodiments are emphasized, and in part, not described or illustrated in any particular embodiment, reference is made to the related descriptions of other embodiments.
Those of ordinary skill in the art will appreciate that the elements and method steps of the examples described in connection with the embodiments disclosed herein can be implemented as electronic hardware, or as a combination of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other manners. For example, the apparatus/terminal device embodiments described above are merely illustrative, e.g., the division of the modules or units is merely a logical function division, and there may be additional divisions in actual implementation, e.g., multiple units or components may be combined or integrated into another system, or some features may be omitted or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection via interfaces, devices or units, which may be in electrical, mechanical or other forms.
The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
In addition, each functional unit in the embodiments of the present invention may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The present invention is not limited to the details and embodiments described herein, and thus additional advantages and modifications may readily be made by those skilled in the art, without departing from the spirit and scope of the general concepts defined in the claims and the equivalents thereof, and the invention is not limited to the specific details, representative apparatus and illustrative examples shown and described herein.
Claims (10)
1. The track reckoning method based on local map matching is characterized by comprising the following steps of:
step 1, taking an initial frame point cloud acquired at a starting position as a reference point cloud;
step 2, transforming the reference point cloud into a grid map;
step 3, acquiring a second frame point cloud adjacent to the reference point cloud and taking the second frame point cloud as a current point cloud;
step 4, matching the current point cloud with the reference point cloud to generate a first relative pose of the current point cloud relative to the reference point cloud;
step 5, accumulating the first relative pose to generate an absolute pose of a second frame point cloud, and adding the second frame point cloud to a grid map in the absolute pose of the second frame point cloud;
step 6, acquiring a third frame point cloud adjacent to the second frame point cloud, matching the third frame point cloud with the second frame point cloud to generate a second relative pose of the third frame point cloud relative to the second frame point cloud, and adding the third frame point cloud to the current grid map in the absolute pose of the second frame point cloud;
step 7, extracting the angle of the second relative pose, and generating the absolute pose of a third frame point cloud based on a map matching algorithm and a current grid map;
step 8, judging whether the distance between the absolute pose of the third frame point cloud and the initial position is larger than a preset distance, if not, updating the third frame point cloud into the current point cloud, continuously acquiring point cloud data, and repeating the steps 4-8 to calculate the robot track; if yes, deleting the current grid map, taking the absolute pose of the third frame point cloud as a starting position, taking the third frame point cloud as a reference point cloud, repeating the steps 2-8, reestablishing the grid map and calculating the robot track.
2. The track estimation method based on local map matching according to claim 1, wherein the iterative closest point algorithm ICP is used to generate a first relative pose of the current point cloud with respect to the reference point cloud and a second relative pose of the third frame point cloud with respect to the second frame point cloud, the iterative number of iterations of the iterative closest point algorithm ICP is set to 2-5 times, and the initial transformation is set to (0, 0).
3. The trajectory estimation method based on local map matching according to claim 1 or 2, wherein the preset distance is 10-20 meters.
4. The local map matching-based trajectory estimation method of claim 3, wherein accumulating the first relative pose using a wheeled differential drive model generates an absolute pose of the second frame point cloud.
5. A computer-readable storage medium, characterized in that a computer program is stored, which, when being executed by a processor, implements the local map matching-based trajectory estimation method of any one of claims 1 to 4.
6. A local map matching-based trajectory estimation terminal comprising the computer-readable storage medium of claim 5 and a processor implementing the steps of the local map matching-based trajectory estimation method of any one of claims 1 to 4 when executing a computer program on the computer-readable storage medium.
7. The track reckoning device based on local map matching realizes the robot track reckoning method according to any one of claims 1-4, and is characterized by comprising a point cloud acquisition module, a first grid map transformation module, a relative pose generation module, a first absolute pose generation module, a second grid map transformation module, a second absolute pose generation module and a judgment module,
the point cloud acquisition module is used for acquiring point cloud data;
the first grid map transformation module is used for taking the initial frame point cloud as a reference point cloud and transforming the reference point cloud into a grid map;
the relative pose generation module is used for matching the current point cloud with the reference point cloud to generate a first relative pose of the current point cloud relative to the reference point cloud; and the second relative pose of the third frame point cloud relative to the second frame point cloud is generated by matching the third frame point cloud with the second frame point cloud;
the first absolute pose generation module is used for accumulating the first relative pose to generate the absolute pose of the second frame point cloud;
the second grid map transformation module is used for adding the second frame point cloud and the third frame point cloud to the grid map in the absolute pose of the second frame point cloud;
the second absolute pose generation module is used for extracting the angle of the second relative pose and generating the absolute pose of a third frame point cloud based on a map matching algorithm and a current grid map;
the judging module is used for judging whether the distance between the absolute pose of the third frame point cloud and the initial position is larger than a preset distance, if not, updating the third frame point cloud into the current point cloud, continuously acquiring point cloud data, and repeating the steps 4-8 to calculate the robot track; if yes, deleting the current grid map, taking the absolute pose of the third frame point cloud as a starting position, taking the third frame point cloud as a reference point cloud, repeating the steps 2-8, reestablishing the grid map and calculating the robot track.
8. The trajectory estimation device based on local map matching according to claim 7, wherein a first relative pose of a current point cloud with respect to a reference point cloud and a second relative pose of a third frame point cloud with respect to a second frame point cloud are generated by using an iterative closest point algorithm ICP, the iterative closest point algorithm ICP is set to 2-5 times, and the initial transformation is set to (0, 0).
9. The trajectory estimation device based on local map matching according to claim 7 or 8, wherein the preset distance is 10-20 meters.
10. The track estimation device based on local map matching according to claim 9, wherein the first absolute pose generation module is specifically configured to accumulate the first relative pose by using a wheeled differential driving model to generate the absolute pose of the second frame point cloud.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911205872.0A CN110887493B (en) | 2019-11-29 | 2019-11-29 | Track calculation method, medium, terminal and device based on local map matching |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911205872.0A CN110887493B (en) | 2019-11-29 | 2019-11-29 | Track calculation method, medium, terminal and device based on local map matching |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110887493A CN110887493A (en) | 2020-03-17 |
| CN110887493B true CN110887493B (en) | 2023-05-05 |
Family
ID=69749672
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911205872.0A Active CN110887493B (en) | 2019-11-29 | 2019-11-29 | Track calculation method, medium, terminal and device based on local map matching |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110887493B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111398984B (en) * | 2020-03-22 | 2022-03-29 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
| CN111461982B (en) * | 2020-03-30 | 2023-09-22 | 北京百度网讯科技有限公司 | Method and apparatus for splice point cloud |
| CN111881233B (en) * | 2020-06-28 | 2022-01-18 | 广州文远知行科技有限公司 | Distributed point cloud map construction method and device, server and computer readable storage medium |
| CN113094462B (en) * | 2021-04-30 | 2023-10-24 | 腾讯科技（深圳）有限公司 | Data processing method and device and storage medium |
| CN114019977B (en) * | 2021-11-03 | 2024-06-04 | 诺力智能装备股份有限公司 | Path control method and device for mobile robot, storage medium and electronic equipment |
| CN118067114B (en) * | 2024-04-24 | 2024-07-26 | 成都赛力斯科技有限公司 | Map construction method and device, electronic equipment and storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107144292A (en) * | 2017-06-08 | 2017-09-08 | 杭州南江机器人股份有限公司 | The odometer method and mileage counter device of a kind of sports equipment |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
| WO2019169540A1 (en) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | Method for tightly-coupling visual slam, terminal and computer readable storage medium |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109064506B (en) * | 2018-07-04 | 2020-03-13 | 百度在线网络技术（北京）有限公司 | High-precision map generation method and device and storage medium |
-
2019
- 2019-11-29 CN CN201911205872.0A patent/CN110887493B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107144292A (en) * | 2017-06-08 | 2017-09-08 | 杭州南江机器人股份有限公司 | The odometer method and mileage counter device of a kind of sports equipment |
| WO2019169540A1 (en) * | 2018-03-06 | 2019-09-12 | 斯坦德机器人（深圳）有限公司 | Method for tightly-coupling visual slam, terminal and computer readable storage medium |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
Non-Patent Citations (1)
| Title |
|---|
| 纪嘉文 ; 杨明欣 ; .一种基于多传感融合的室内建图和定位算法.成都信息工程大学学报.2018,(04),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110887493A (en) | 2020-03-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110887493B (en) | Track calculation method, medium, terminal and device based on local map matching | |
| US10852139B2 (en) | Positioning method, positioning device, and robot | |
| JP7482978B2 (en) | Vehicle Sensor Calibration and Localization | |
| CN110160542B (en) | Method and device for positioning lane line, storage medium and electronic device | |
| CN109211251B (en) | Instant positioning and map construction method based on laser and two-dimensional code fusion | |
| CN109506642B (en) | Robot multi-camera visual inertia real-time positioning method and device | |
| CN110986988B (en) | Track calculation method, medium, terminal and device integrating multi-sensor data | |
| CN107167148A (en) | Synchronous superposition method and apparatus | |
| EP3852065A1 (en) | Data processing method and apparatus | |
| CN111380514A (en) | Robot position and posture estimation method and device, terminal and computer storage medium | |
| CN110969649A (en) | Matching evaluation method, medium, terminal and device of laser point cloud and map | |
| CN113933818A (en) | Method, device, storage medium and program product for calibrating laser radar external parameter | |
| KR101985344B1 (en) | Sliding windows based structure-less localization method using inertial and single optical sensor, recording medium and device for performing the method | |
| CN112835064B (en) | Mapping positioning method, system, terminal and medium | |
| CN114689047B (en) | Deep learning-based integrated navigation method, device, system and storage medium | |
| WO2020135183A1 (en) | Method and apparatus for constructing point cloud map, computer device, and storage medium | |
| CN112362054A (en) | Calibration method, calibration device, electronic equipment and storage medium | |
| CN113960614A (en) | Elevation map construction method based on frame-map matching | |
| CN113554712B (en) | Registration method and device of automatic driving vehicle, electronic equipment and vehicle | |
| CN113218389B (en) | Vehicle positioning method, device, storage medium and computer program product | |
| KR102130687B1 (en) | System for information fusion among multiple sensor platforms | |
| CN110989619A (en) | Method, apparatus, device and storage medium for locating object | |
| CN113252023A (en) | Positioning method, device and equipment based on odometer | |
| CN115984463A (en) | Three-dimensional reconstruction method and system suitable for narrow roadway | |
| CN115560744A (en) | Robot, multi-sensor-based three-dimensional mapping method and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |