CN114440855A - Method and system for positioning and map updating in dynamic scene - Google Patents
Method and system for positioning and map updating in dynamic scene Download PDFInfo
- Publication number
- CN114440855A CN114440855A CN202210062357.7A CN202210062357A CN114440855A CN 114440855 A CN114440855 A CN 114440855A CN 202210062357 A CN202210062357 A CN 202210062357A CN 114440855 A CN114440855 A CN 114440855A
- Authority
- CN
- China
- Prior art keywords
- sub
- result
- feature matching
- positioning
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3837—Data obtained from a single source
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
Abstract
The invention discloses a method and a system for positioning and map updating in a dynamic scene, wherein the method comprises the following steps: obtaining a first initial map through the laser radar, wherein the first initial map comprises a first mapping track; obtaining a first node set and a first sub-graph set; obtaining a first positioning track of a first robot, wherein the first positioning track comprises a second node set and a second sub-graph set; performing feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and performing feature matching on the first sub-graph set and the second node set to obtain a second feature matching result; creating a first pose constraint; and obtaining a first optimization result by using a least square method, and positioning the first robot. The technical problem that in the prior art, due to scene change, the matching degree of characteristic data of real-time laser scanning of the robot and map characteristic data is poor, so that the positioning effect of the robot is influenced, and even the robot cannot execute tasks is solved.
Description
Technical Field
The invention relates to the technical field of computers, in particular to a method and a system for positioning and map updating in a dynamic scene.
Background
When a robot is used for map construction, errors in the sensing data are usually caused by noise, and thus the accuracy of map construction is affected. Before the robot performs navigation on a task, a mapping operation needs to be performed on a use scene, and the mapping method mainly comprises the steps of scanning a current scene through a laser radar to generate a grid map and storing the grid map into a map database. After the map is built, the robot executes a task to perform navigation and needs to perform path planning and positioning on the built map, the positioning method in the task executed by the robot mainly performs feature matching on the point cloud data acquired by scanning the current environment through a laser radar on the robot and the map stored before, and if the score of the feature matching exceeds a set threshold, the robot is judged to be positioned correctly. However, as demand increases, robots are beginning to be used in changing scenes. How to improve the positioning accuracy of the robot in a dynamic change scene and ensure the smooth execution of the robot task is of great significance. In addition, the map under the dynamic scene is updated in time by utilizing the computer technology, and the method has important significance for improving the positioning accuracy of the robot.
However, in the prior art, due to scene change, the matching degree of the feature data of the real-time laser scanning of the robot and the map feature data is poor, so that the positioning effect of the robot is affected, and even the robot cannot execute tasks.
Disclosure of Invention
The invention aims to provide a method and a system for positioning and map updating in a dynamic scene, which are used for solving the technical problems that the matching degree of the characteristic data of real-time laser scanning of a robot and the characteristic data of a map is poor due to scene change in the prior art, so that the positioning effect of the robot is influenced, and even the robot cannot execute tasks.
In view of the above problems, the present invention provides a method and system for positioning and map updating in a dynamic scene.
In a first aspect, the present invention provides a method for positioning and map updating in a dynamic scene, where the method is implemented by a system for positioning and map updating in a dynamic scene, and the method includes: obtaining a first initial map through the laser radar, wherein the first initial map comprises a first mapping track; obtaining a first node set and a first sub-graph set according to the first mapping track; obtaining a first positioning track of a first robot, wherein the first positioning track comprises a second node set and a second sub-graph set; performing feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and performing feature matching on the first sub-graph set and the second node set to obtain a second feature matching result; creating a first pose constraint according to the first feature matching result and the second feature matching result; optimizing the first attitude constraint by using a least square method to obtain a first optimization result; and positioning the first robot according to the first optimization result.
In another aspect, the present invention further provides a system for positioning and map updating in a dynamic scene, configured to perform the method for positioning and map updating in a dynamic scene according to the first aspect, where the system includes: a first obtaining unit: the first obtaining unit is used for obtaining a first initial map through a laser radar, wherein the first initial map comprises a first mapping track; a second obtaining unit: the second obtaining unit is used for obtaining a first node set and a first sub-graph set according to the first mapping track; a third obtaining unit: the third obtaining unit is configured to obtain a first positioning track of the first robot, where the first positioning track includes a second node set and a second sub-graph set; a fourth obtaining unit: the fourth obtaining unit is configured to perform feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and perform feature matching on the first sub-graph set and the second node set to obtain a second feature matching result; a first creation unit: the first creating unit is used for creating a first pose constraint according to the first feature matching result and the second feature matching result; a fifth obtaining unit: the fifth obtaining unit is used for optimizing the first attitude constraint by using a least square method to obtain a first optimization result; a first execution unit: the first execution unit is used for positioning the first robot according to the first optimization result.
In a third aspect, the present invention further provides a system for positioning and map updating in a dynamic scene, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method according to the first aspect when executing the program.
In a fourth aspect, an electronic device, comprising a processor and a memory;
the memory is used for storing;
the processor is configured to execute the method according to any one of the first aspect above by calling.
In a fifth aspect, a computer program product comprises a computer program and/or instructions which, when executed by a processor, performs the steps of the method of any of the first aspect described above.
One or more technical schemes provided by the invention at least have the following technical effects or advantages:
1. the method comprises the steps that firstly, a laser radar is arranged on a robot, when the robot travels along a certain path, the laser radar scans surrounding environment in real time, and then a first initial map is obtained based on scanning data analysis. Wherein, the path that the robot travelled is the first mapping track. And then loading a first initial map, wherein the track of the robot executing the task in the first initial map is the first positioning track. Further, the environmental feature data scanned when the robot performs the task is matched with the environmental feature data in the first initial map, so that the position of the robot in the first initial map, namely the first attitude constraint, is determined. And finally, optimizing the first position and posture constraint by using a least square method so as to determine the accurate position of the robot. And establishing an initial map through the laser radar, and further matching the ambient environment characteristic data scanned by the laser radar in real time when the robot executes a task with the initial map characteristic data, so that the relative position of the robot in the map is obtained, and further optimization is carried out and the specific position of the robot is determined. The technical effects of improving the positioning stability and the positioning accuracy of the robot are achieved.
2. By means of the preset cutting scheme, the subgraph and the nodes corresponding to the positioning track when the robot executes the task are selectively cut, so that key subgraph and node data are reserved for map updating, and the technical effects of ensuring accurate positioning of the robot and reducing system feature matching calculation amount and map data redundancy are achieved.
The foregoing description is only an overview of the technical solutions of the present invention, and the embodiments of the present invention are described below in order to make the technical means of the present invention more clearly understood and to make the above and other objects, features, and advantages of the present invention more clearly understandable.
Drawings
In order to more clearly illustrate the present invention or the technical solutions in the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only exemplary, and for those skilled in the art, other drawings can be obtained according to the provided drawings without inventive effort.
FIG. 1 is a schematic flow chart illustrating a method for positioning and map updating in a dynamic scene according to the present invention;
fig. 2 is a schematic flow chart illustrating the updating of the first initial map according to the first clipping result in the method for positioning and updating a map in a dynamic scene according to the present invention;
fig. 3 is a schematic flow chart illustrating the updating of the first initial map according to the first addition result in the method for positioning and map updating in a dynamic scene according to the present invention;
fig. 4 is a schematic flow chart illustrating the updating of the first initial map according to the second addition result in the method for positioning and map updating in a dynamic scene according to the present invention;
FIG. 5 is a schematic structural diagram of a system for positioning and map updating in a dynamic scene according to the present invention;
fig. 6 is a schematic structural diagram of an exemplary electronic device of the present invention.
Description of reference numerals:
a first obtaining unit 11, a second obtaining unit 12, a third obtaining unit 13, a fourth obtaining unit 14, a first creating unit 15, a fifth obtaining unit 16, a first executing unit 17, a bus 300, a receiver 301, a processor 302, a transmitter 303, a memory 304, and a bus interface 305.
Detailed Description
The invention provides a method and a system for positioning and map updating in a dynamic scene, and solves the technical problems that the matching degree of the characteristic data of real-time laser scanning of a robot and the characteristic data of a map is poor due to scene change in the prior art, so that the positioning effect of the robot is influenced, and even the robot cannot execute tasks. And establishing an initial map through the laser radar, and further matching the ambient environment characteristic data scanned by the laser radar in real time when the robot executes a task with the initial map characteristic data, so that the relative position of the robot in the map is obtained, and further optimization is carried out and the specific position of the robot is determined. The technical effects of improving the positioning stability and the positioning accuracy of the robot are achieved.
In the technical scheme of the invention, the data acquisition, storage, use, processing and the like all conform to relevant regulations of national laws and regulations.
In the following, the technical solutions in the present invention will be clearly and completely described with reference to the accompanying drawings, and it should be understood that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments of the present invention, and it should be understood that the present invention is not limited by the example embodiments described herein. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without making any creative effort, shall fall within the protection scope of the present invention. It should be further noted that, for the convenience of description, only some but not all of the elements associated with the present invention are shown in the drawings.
The invention provides a method for positioning and map updating in a dynamic scene, which is applied to a system for positioning and map updating in the dynamic scene, wherein the method comprises the following steps: obtaining a first initial map through the laser radar, wherein the first initial map comprises a first mapping track; obtaining a first node set and a first sub-graph set according to the first mapping track; obtaining a first positioning track of a first robot, wherein the first positioning track comprises a second node set and a second sub-graph set; performing feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and performing feature matching on the first sub-graph set and the second node set to obtain a second feature matching result; creating a first pose constraint according to the first feature matching result and the second feature matching result; optimizing the first attitude constraint by using a least square method to obtain a first optimization result; and positioning the first robot according to the first optimization result.
Having described the general principles of the invention, reference will now be made in detail to various non-limiting embodiments of the invention, examples of which are illustrated in the accompanying drawings.
Example one
Referring to fig. 1, the present invention provides a method for positioning and map updating in a dynamic scene, wherein the method is applied to a system for positioning and map updating in a dynamic scene, and the method specifically includes the following steps:
step S100: obtaining a first initial map through the laser radar, wherein the first initial map comprises a first mapping track;
specifically, the method for positioning and map updating in the dynamic scene is applied to the system for positioning and map updating in the dynamic scene, an initial map can be established through the laser radar, and then the ambient environment characteristic data scanned by the laser radar in real time when the robot executes a task is matched with the initial map characteristic data, so that the relative position of the robot in the map is obtained, and further optimization is carried out and the specific position of the robot is determined. The laser radar is arranged on the robot, and emits laser in real time to scan the surrounding environment in the moving process of the robot, and in addition, the environment data scanned in real time is transmitted to the system for analysis through the communication connection with the system for positioning and map updating in the dynamic scene. The system for positioning and map updating in the dynamic scene generates an initial map of a corresponding environment, namely the first initial map, by analyzing data scanned by the laser radar. And the track traveled by the robot based on the creation of the first initial map is the first mapping track. That is, in the process of map building, the environmental data scanned by the lidar is the environmental data around the first mapping track. By obtaining the first initial map, a basic environment is provided for the follow-up robot to execute tasks, and a basic technical effect is achieved for the follow-up real-time map updating based on the environment change.
Step S200: obtaining a first node set and a first sub-graph set according to the first mapping track;
specifically, when the robot manually constructs the first initial map and travels, the generated travel track is the first mapping track. In the first mapping trajectory, a plurality of nodes exist. Furthermore, a certain number of consecutive nodes constitutes a subgraph. Therefore, based on the first mapping track, a plurality of corresponding nodes and a plurality of sub-graphs can be obtained, namely the first node set and the first sub-graph set are formed. Wherein, a node represents a frame of data collected by the laser radar. For example, a frame of laser radar data extracted every time the robot moves by 0.2m or 5 ° is recorded as a node, and a subgraph is formed by 30 continuous node areas. Analyzing may obtain a first node set and a first sub-graph set in the first initial map by the first mapping trajectory based on the first initial map.
By obtaining the first node set and the first sub-map set, the goal of datamation and visualization of the first initial map information is achieved, and the technical effect of providing basic feature reference for subsequent robot positioning is achieved.
Step S300: obtaining a first positioning track of a first robot, wherein the first positioning track comprises a second node set and a second sub-graph set;
specifically, the first robot is any robot that performs a task in the first initial map. After the first robot obtains the corresponding task, the system for positioning and map updating in the dynamic scene automatically loads the first initial map. Further, the first robot travels in an area corresponding to the first initial map, so that the first positioning track is obtained. That is, when the first robot executes a task, the system is adjusted to the positioning mode, and at this time, the system records the position of the first robot in real time, so as to generate the first positioning track. Similarly, the first positioning track includes a plurality of nodes and a plurality of subgraphs, which respectively form the second node set and the second subgraph set. By obtaining the node set and the sub-graph set in the robot positioning mode, the technical effect of providing a data basis for subsequently judging the position of the robot in the first initial map is achieved.
Step S400: performing feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and performing feature matching on the first sub-graph set and the second node set to obtain a second feature matching result;
specifically, the nodes and the subgraph of the first robot in task execution are subjected to feature matching with the nodes and the subgraph in the first initial map construction, and therefore corresponding feature matching results are obtained. Performing feature matching on the first node set and the second sub-graph set, namely performing feature matching on a node during map construction and a sub-graph during task execution, wherein an obtained matching result is the first feature matching result; and performing feature matching on the first sub-graph set and the second node set, namely the sub-graphs during map construction and the nodes during task execution, wherein the obtained matching result is the second feature matching result. The matching results are obtained through respective matching, and the technical effect of providing guidance and reference for subsequently determining the relative position relationship of the first robot in the first initial map is achieved.
Step S500: creating a first pose constraint according to the first feature matching result and the second feature matching result;
specifically, when one of the first feature matching result and the second feature matching result meets a preset matching score threshold, it is indicated that a closed loop is formed between the first positioning track of the first robot executing the task and the first mapping track of the created map, and therefore, a relative pose relationship between the first positioning track and the first mapping track is created as a constraint, that is, the first pose constraint. The obtaining of the first pose constraint includes three conditions, the first condition is that the first feature matching result meets a preset matching score threshold, the second condition is that the second feature matching result meets a preset matching score threshold, and the third condition is that the first feature matching result and the second feature matching result both meet a preset matching score threshold. The preset matching scoring threshold is a feature matching scoring range determined by the system for positioning and map updating in the dynamic scene based on comprehensive analysis of actual task execution conditions of the robot, corresponding environment complex conditions, related positioning precision requirements and the like. By creating the first pose constraint, the preliminary judgment of the first robot positioning is realized, and preparation is made for accurately positioning the first robot position subsequently.
Step S600: optimizing the first attitude constraint by using a least square method to obtain a first optimization result;
specifically, when the nodes and the subgraphs are actually matched, the actual relative poses of the nodes and the subgraphs are not the relative poses when the matching is accurate, that is, each node, subgraph and pose constraint has a position deviation and does not completely conform to the first pose constraint. Therefore, when the first robot executes a task, each time a new sub-graph is generated, all the previously established pose constraints are optimally adjusted by using the least square method, and the current real-time pose of the first robot is included. The least square method is a method for performing comprehensive optimization based on error estimation and uncertainty conditions among all constraints. Through optimization and adjustment, each constraint is more accurate, and therefore the technical effect of improving the positioning accuracy of the first robot is achieved.
Step S700: and positioning the first robot according to the first optimization result.
Specifically, the first robot is positioned according to each constraint optimized by the least square method. And establishing an initial map through the laser radar, and further matching the ambient environment characteristic data scanned by the laser radar in real time when the robot executes a task with the initial map characteristic data, so that the relative position of the robot in the map is obtained, and further optimization is carried out and the specific position of the robot is determined. The technical effects of improving the positioning stability and the positioning accuracy of the robot are achieved.
Further, as shown in fig. 2, the present invention further includes step S800:
step S810: if the subgraph in the second subgraph set exceeds a preset subgraph number, a first cutting instruction is obtained;
step S820: according to the first clipping instruction, clipping the second node set and the second sub-set according to a preset clipping scheme to obtain a first clipping result;
step S830: and updating the first initial map according to the first cutting result.
Specifically, in the process that the first robot executes the task, when a new sub-graph corresponding to each preset sub-graph number is generated in the first positioning track, the system automatically sends out a first cutting instruction. For example, when the first positioning track, that is, the robot is executing a task, every 4 new sub-graphs are generated, that is, the first cutting instruction is issued. The preset subgraph is determined by the system for positioning and map updating in the dynamic scene based on the comprehensive analysis of the related precision requirement, the system performance and the like. The first cutting instruction is used for cutting partial subgraphs and nodes thereof in the first positioning track according to a preset cutting scheme, namely cutting the second node set and the second subgraph set, only partial subgraphs and nodes are reserved in the first cutting result obtained after cutting, and finally, the reserved subgraphs, nodes and other related information are stored in a database, so that the first initial map is updated. The preset clipping scheme refers to clipping of different rules on the second node set and the second sub-set corresponding to the first positioning track based on actual conditions.
By cutting the robot positioning track, reserving part of nodes and subgraphs and storing the corresponding nodes and subgraph related data, the technical effect of dynamically updating the first initial map is achieved.
Further, as shown in fig. 3, step S830 of the present invention further includes:
step S831: obtaining a third node set and a third sub-graph set according to the first cutting result;
step S832: performing feature matching on the third node set and the first sub-set to obtain a third feature matching result, and performing feature matching on the third sub-set and the first node set to obtain a fourth feature matching result;
step S833: creating a second pose constraint according to the third feature matching result and the fourth feature matching result;
step S834: collecting a first constraint quantity of the second attitude constraint;
step S835: if the first constraint quantity meets a preset constraint value, adding the third node set and the third sub-graph set to the first node set and the first sub-graph set respectively to obtain a first addition result;
step S836: and updating the first initial map according to the first adding result.
Specifically, the third node set and the third sub-set refer to sets formed by node portions and sub-set portions left after the second node set and the second sub-set are cut. And respectively performing feature matching on the third node set and the first sub-graph set, the third sub-graph set and the first node set to obtain corresponding matching results, namely the third feature matching result and the fourth feature matching result. That is, after the first robot executes the task to generate the corresponding first positioning track, the system automatically performs feature matching on the newly added sub-graph and the nodes thereof with the nodes and the sub-graphs in the first initial map every time a certain number of sub-graphs are added, so as to create all pose constraints. The system automatically matches the newly generated subgraph and the nodes thereof with the nodes and the subgraph in the first graph establishing track respectively to obtain all pose constraints. When the number of the obtained pose constraints accords with a preset constraint value, it is indicated that the position of the first robot can be accurately positioned by the currently added subgraph and the nodes thereof, at this time, the third node set and the third subgraph set which are left after cutting are respectively added to the first node set and the first sub-graph set, that is, the added subgraph and the nodes thereof are added to map data for storage, finally, the dynamic update of the first initial map is realized according to the adding result, and the updated map is directly loaded when the next task is executed.
By cutting out redundant subgraph and node data, only the latest subgraph and node data which can accurately constrain the robot are reserved and stored, the first initial map is updated, and the technical effects of reducing system matching, calculated amount and map redundancy while ensuring the robot to be accurately positioned are achieved.
Further, as shown in fig. 4, the present invention further includes a step S837:
step S8371: if the first constraint quantity does not accord with a preset constraint value, adding the second node set and the second sub-graph set to the first node set and the first sub-graph set respectively to obtain a second addition result;
step S8372: and updating the first initial map according to the second adding result.
Specifically, when the matching results of the newly added sub-graph and nodes thereof with the first node set and the first sub-graph set corresponding to the first initial map are poor, that is, the number of the correspondingly created pose constraints does not reach the preset constraint value, the sub-graph and nodes thereof before the first positioning track are not cut, but the second node set and the second sub-graph set are respectively added to the first node set and the first sub-graph set, that is, all sub-graphs and nodes thereof generated by the robot executing the task are stored and updated in the first initial map. The preset constraint value refers to the minimum pose constraint quantity which is determined by the system after comprehensive analysis and based on the actual positioning precision requirement and the condition that the robot executes tasks and ensures that the robot is accurately positioned.
In addition, when the constraint quantity of all poses created in the task executed by the robot does not accord with the preset constraint value, it can be judged that the matching effect of the robot at the current pose is not good, which may be caused by the inconsistency between the current actual environment and the previously stored environment of the first initial map, in order to keep the current updated actual environment map data, the sub-graphs and nodes in the first positioning track in the task executed by the robot are not cut, the number of the sub-graphs when the sub-graphs are not cut is recorded as n, and the number of the n is increased by 1 when every sub-graph is added in the subsequent first positioning track. And when the total number of the latest four sub-images and the nodes thereof in the first positioning track matched with the first mapping track meets a preset constraint value, the first positioning track has enough pose constraints to carry out closed loop with the first initial map. And when the subgraph and the nodes of the first positioning track are cut based on the first cutting instruction, the latest subgraph and the nodes corresponding to n recorded subgraphs and nodes thereof and the preset subgraph number are reserved, and the redundant subgraphs and the nodes thereof in the middle are cut.
By means of the preset cutting scheme, the sub-graphs and the nodes corresponding to the positioning tracks when the robot executes tasks are selectively cut, so that key sub-graph and node data are reserved for map updating, and the technical effect of dynamically updating the first initial map is achieved.
Further, step S100 of the present invention further includes:
step S110: obtaining a first mapping track of the first robot;
step S120: scanning peripheral obstacles of the first mapping track through the laser radar to obtain a first scanning result;
step S130: and obtaining the first initial map according to the first scanning result.
Specifically, the laser radar is arranged on the first robot, in the moving process of the first robot, the laser radar sends out laser in real time to scan the surrounding environment, and in addition, the system is in communication connection with the positioning and map updating system in the dynamic scene, and the environmental data scanned in real time are transmitted to the system to be analyzed. And the moving track of the first robot during mapping is the first mapping track. The scanning data received by the system is a scanning result obtained by scanning the peripheral obstacles of the first mapping track by the laser radar. And finally, analyzing and obtaining information such as obstacles in the corresponding environment according to the first scanning result, namely realizing the construction of the first initial map. The laser radar is used for realizing map construction, and the technical effect of providing a basic environment carrier for the subsequent robot to execute tasks is achieved.
Further, step S130 of the present invention further includes:
step S131: extracting the first scanning result by utilizing a motion filtering principle to obtain a first key frame set, wherein the first key frame set comprises a first key frame and a second key frame;
step S132: respectively obtaining first point cloud data of the first key frame and second point cloud data of the second key frame;
step S133: constructing a first occupation grid map according to the first point cloud data and the second point cloud data;
step S134: obtaining the first initial map according to the first occupancy grid map.
Specifically, in the first scanning result obtained by the laser radar scanning, the scanning data amount is large, and therefore the first scanning result is subjected to the moving filter principle by using the moving filter principle to extract data from the first scanning result, so that the first keyframe set is obtained. For example, when the robot moves 0.2m or 5 degrees each time, a frame of laser radar data is extracted and recorded as a node, and the corresponding frame is a key frame. And constructing a first occupation grid map based on the point cloud data corresponding to each key frame in the first key frame set, thereby realizing the construction of a first initial map. Generally, 15 frames of point cloud data of the current environment can be acquired in the laser radar 1s, the range of the grid map is expanded by matching and splicing the point cloud data of the current frame with the existing grid map, and the range of the grid map is gradually expanded when the robot moves, so that the current obstacle environment is stored in the form of the grid map in a laser radar scanning mode. The grid map is therefore composed of many frames of lidar data. The occupied grid map is a grid map indicating the probability that each key frame has an obstacle at a corresponding node, that is, an image discretized in space and brightness. By extracting key frame data in the scanning result, the calculation complexity of the system during map building is reduced, the map building cost is saved, and the technical effect of improving the map building efficiency is achieved.
Further, step S200 of the present invention further includes:
step S210: recording the first point cloud data as a first node, and recording the second point cloud data as a second node;
step S220: combining the first node and the second node to obtain the first node set;
step S230: and recording a preset number of continuous nodes as subgraphs, wherein all the subgraphs form the first subgraph set.
Specifically, after the laser radar scans the surrounding environment, the system automatically extracts point cloud data corresponding to key frames for analysis, the point cloud data corresponding to each key frame is a first node, and a preset number of continuous nodes form a subgraph. And in the process of executing the task by the first robot, all the generated nodes form the first node set, and all the generated subgraphs form the first sub-graph set. The preset quantity refers to a quantity value determined by comprehensive analysis of the system based on actual environment, robot task execution and other conditions. For example, every 30 consecutive nodes form a subgraph. By determining the subgraph and the node information thereof generated in the robot graph building or positioning process, the technical effect of providing a foundation for the sequential characteristic matching is achieved.
In summary, the method for positioning and map updating in a dynamic scene provided by the present invention has the following technical effects:
1. the method comprises the steps that firstly, a laser radar is arranged on a robot, when the robot travels along a certain path, the laser radar scans surrounding environment in real time, and then a first initial map is obtained based on scanning data analysis. Wherein, the path that the robot travelled is the first mapping track. And then loading a first initial map, wherein the track of the robot executing the task in the first initial map is the first positioning track. Further, the environmental feature data scanned when the robot performs the task is matched with the environmental feature data in the first initial map, so that the position of the robot in the first initial map, namely the first attitude constraint, is determined. And finally, optimizing the first position and posture constraint by using a least square method so as to determine the accurate position of the robot. And establishing an initial map through the laser radar, and further matching the ambient environment characteristic data scanned by the laser radar in real time when the robot executes a task with the initial map characteristic data, so that the relative position of the robot in the map is obtained, and further optimization is carried out and the specific position of the robot is determined. The technical effects of improving the positioning stability and the positioning accuracy of the robot are achieved.
2. By means of the preset cutting scheme, the subgraph and the nodes corresponding to the positioning track when the robot executes the task are selectively cut, so that key subgraph and node data are reserved for map updating, and the technical effects of ensuring accurate positioning of the robot and reducing system feature matching calculation amount and map data redundancy are achieved.
Example two
Based on the same inventive concept as the method for positioning and map updating in a dynamic scene in the foregoing embodiment, the present invention further provides a system for positioning and map updating in a dynamic scene, please refer to fig. 5, where the system includes:
a first obtaining unit 11, where the first obtaining unit 11 is configured to obtain a first initial map through a laser radar, where the first initial map includes a first mapping track;
a second obtaining unit 12, where the second obtaining unit 12 is configured to obtain a first node set and a first sub-graph set according to the first mapping track;
a third obtaining unit 13, where the third obtaining unit 13 is configured to obtain a first positioning trajectory of the first robot, where the first positioning trajectory includes a second node set and a second sub-graph set;
a fourth obtaining unit 14, where the fourth obtaining unit 14 is configured to perform feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and perform feature matching on the first sub-graph set and the second node set to obtain a second feature matching result;
a first creating unit 15, where the first creating unit 15 is configured to create a first pose constraint according to the first feature matching result and the second feature matching result;
a fifth obtaining unit 16, where the fifth obtaining unit 16 is configured to optimize the first attitude constraint by using a least square method, and obtain a first optimization result;
a first execution unit 17, wherein the first execution unit 17 is configured to locate the first robot according to the first optimization result.
Further, the system further comprises:
a sixth obtaining unit, configured to obtain a first clipping instruction if a subgraph in the second subgraph set exceeds a preset subgraph number;
a seventh obtaining unit, configured to cut the second node set and the second sub-set according to a preset cutting scheme according to the first cutting instruction, and obtain a first cutting result;
a second execution unit, configured to update the first initial map according to the first cropping result.
Further, the system further comprises:
an eighth obtaining unit, configured to obtain a third node set and a third sub-graph set according to the first clipping result;
a ninth obtaining unit, configured to perform feature matching on the third node set and the first sub-set to obtain a third feature matching result, and perform feature matching on the third sub-set and the first node set to obtain a fourth feature matching result;
a second creating unit, configured to create a second pose constraint according to the third feature matching result and the fourth feature matching result;
the first acquisition unit is used for acquiring a first constraint quantity of the second posture constraint;
a tenth obtaining unit, configured to add the third node set and the third sub-graph set to the first node set and the first sub-graph set, respectively, if the first constraint quantity meets a preset constraint value, and obtain a first addition result;
a second execution unit, configured to update the first initial map according to the first addition result.
Further, the system further comprises:
an eleventh obtaining unit, configured to add the second node set and the second sub-graph set to the first node set and the first sub-graph set, respectively, if the first constraint quantity does not meet a preset constraint value, and obtain a second addition result;
a third execution unit, configured to update the first initial map according to the second addition result.
Further, the system further comprises:
a twelfth obtaining unit, configured to obtain a first mapping trajectory of the first robot;
a thirteenth obtaining unit, configured to scan a peripheral obstacle of the first mapping trajectory by the laser radar, and obtain a first scanning result;
a fourteenth obtaining unit, configured to obtain the first initial map according to the first scanning result.
Further, the system further comprises:
a fifteenth obtaining unit, configured to extract the first scanning result by using a motion filtering principle, and obtain a first key frame set, where the first key frame set includes a first key frame and a second key frame;
a sixteenth obtaining unit, configured to obtain first point cloud data of the first key frame and second point cloud data of the second key frame respectively;
a first construction unit for constructing a first occupancy grid map from the first point cloud data and the second point cloud data;
a seventeenth obtaining unit configured to obtain the first initial map according to the first occupancy grid map.
Further, the system further comprises:
the first setting unit is used for recording the first point cloud data as a first node and recording the second point cloud data as a second node;
an eighteenth obtaining unit, configured to combine the first node and the second node to obtain the first node set;
a first composition unit for recording a preset number of consecutive nodes as subgraphs, all subgraphs constituting the first set of subgraphs.
In this specification, the embodiments are described in a progressive manner, and each embodiment focuses on a difference from other embodiments, the method and specific example for positioning and map updating in a dynamic scene in the first embodiment of fig. 1 are also applicable to the system for positioning and map updating in a dynamic scene in this embodiment, and through the foregoing detailed description of the method for positioning and map updating in a dynamic scene, a person skilled in the art can clearly know the system for positioning and map updating in a dynamic scene in this embodiment, so for the brevity of the description, detailed description is not repeated here. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Exemplary electronic device
The electronic device of the present invention is described below with reference to fig. 6.
Fig. 6 illustrates a schematic structural diagram of an electronic device according to the present invention.
Based on the inventive concept of a method for positioning and map updating in a dynamic scene as in the previous embodiments, the present invention further provides a system for positioning and map updating in a dynamic scene, on which a computer program is stored, which when executed by a processor, implements the steps of any one of the methods for positioning and map updating in a dynamic scene as described above.
Where in fig. 6 a bus architecture (represented by bus 300), bus 300 may include any number of interconnected buses and bridges, bus 300 linking together various circuits including one or more processors, represented by processor 302, and memory, represented by memory 304. The bus 300 may also link together various other circuits such as peripherals, voltage regulators, power management circuits, and the like, which are well known in the art, and therefore, will not be described any further herein. A bus interface 305 provides an interface between the bus 300 and the receiver 301 and transmitter 303. The receiver 301 and the transmitter 303 may be the same element, i.e., a transceiver, providing a means for communicating with various other apparatus over a transmission medium.
The processor 302 is responsible for managing the bus 300 and general processing, and the memory 304 may be used for storing data used by the processor 302 in performing operations.
The invention provides a method for positioning and map updating in a dynamic scene, which is applied to a system for positioning and map updating in the dynamic scene, wherein the method comprises the following steps: obtaining a first initial map through the laser radar, wherein the first initial map comprises a first mapping track; obtaining a first node set and a first sub-graph set according to the first mapping track; obtaining a first positioning track of a first robot, wherein the first positioning track comprises a second node set and a second sub-graph set; performing feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and performing feature matching on the first sub-graph set and the second node set to obtain a second feature matching result; creating a first pose constraint according to the first feature matching result and the second feature matching result; optimizing the first attitude constraint by using a least square method to obtain a first optimization result; and positioning the first robot according to the first optimization result. The technical problem that due to scene change, the matching degree of the feature data of the real-time laser scanning of the robot and the map feature data is poor, the positioning effect of the robot is affected, and even the robot cannot execute tasks in the prior art is solved. And establishing an initial map through the laser radar, and further matching the ambient environment characteristic data scanned by the laser radar in real time when the robot executes a task with the initial map characteristic data, so that the relative position of the robot in the map is obtained, and further optimization is carried out and the specific position of the robot is determined. The technical effects of improving the positioning stability and the positioning accuracy of the robot are achieved.
The invention also provides an electronic device, which comprises a processor and a memory;
the memory is used for storing;
the processor is configured to execute the method according to any one of the first embodiment through calling.
The invention also provides a computer program product comprising a computer program and/or instructions which, when executed by a processor, performs the steps of the method of any of the above embodiments.
As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, apparatus, or computer program product. Accordingly, the present invention may take the form of an entirely software embodiment, an entirely hardware embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention is in the form of a computer program product that may be embodied on one or more computer-usable storage media having computer-usable program code embodied therewith. And such computer-usable storage media include, but are not limited to: various media capable of storing program codes, such as a usb disk, a portable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk Memory, a Compact Disc Read-Only Memory (CD-ROM), and an optical Memory.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products of the invention. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a system for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including an instruction system which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks. While preferred embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts.
It will be apparent to those skilled in the art that various changes and modifications may be made in the present invention without departing from the spirit and scope of the invention. Thus, if such modifications and variations of the present invention fall within the scope of the present invention and its equivalent technology, it is intended that the present invention also include such modifications and variations.
Claims (10)
1. A method for positioning and map updating in a dynamic scene is applied to a system for positioning and map updating in a dynamic scene, wherein the system is connected with a laser radar in a communication mode, and the method comprises the following steps:
obtaining a first initial map through the laser radar, wherein the first initial map comprises a first mapping track;
obtaining a first node set and a first sub-graph set according to the first mapping track;
obtaining a first positioning track of a first robot, wherein the first positioning track comprises a second node set and a second sub-graph set;
performing feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and performing feature matching on the first sub-graph set and the second node set to obtain a second feature matching result;
creating a first pose constraint according to the first feature matching result and the second feature matching result;
optimizing the first attitude constraint by using a least square method to obtain a first optimization result;
and positioning the first robot according to the first optimization result.
2. The method of claim 1, wherein the method further comprises:
if the subgraph in the second subgraph set exceeds a preset subgraph number, a first cutting instruction is obtained;
according to the first clipping instruction, clipping the second node set and the second sub-set according to a preset clipping scheme to obtain a first clipping result;
and updating the first initial map according to the first cutting result.
3. The method of claim 2, wherein the updating the first initial map according to the first cropping result comprises:
obtaining a third node set and a third sub-graph set according to the first cutting result;
performing feature matching on the third node set and the first sub-set to obtain a third feature matching result, and performing feature matching on the third sub-set and the first node set to obtain a fourth feature matching result;
creating a second pose constraint according to the third feature matching result and the fourth feature matching result;
collecting a first constraint quantity of the second attitude constraint;
if the first constraint quantity meets a preset constraint value, adding the third node set and the third sub-graph set to the first node set and the first sub-graph set respectively to obtain a first addition result;
and updating the first initial map according to the first adding result.
4. The method of claim 3, wherein the method further comprises:
if the first constraint quantity does not accord with a preset constraint value, adding the second node set and the second sub-graph set to the first node set and the first sub-graph set respectively to obtain a second addition result;
and updating the first initial map according to the second adding result.
5. The method of claim 1, wherein said obtaining a first initial map comprises:
obtaining a first mapping track of the first robot;
scanning peripheral obstacles of the first mapping track through the laser radar to obtain a first scanning result;
and obtaining the first initial map according to the first scanning result.
6. The method of claim 5, wherein obtaining the first initial map according to the first scanning result comprises:
extracting the first scanning result by utilizing a motion filtering principle to obtain a first key frame set, wherein the first key frame set comprises a first key frame and a second key frame;
respectively obtaining first point cloud data of the first key frame and second point cloud data of the second key frame;
constructing a first occupation grid map according to the first point cloud data and the second point cloud data;
obtaining the first initial map according to the first occupancy grid map.
7. The method of claim 6, wherein obtaining the first set of nodes and the first set of sub-graphs comprises:
recording the first point cloud data as a first node, and recording the second point cloud data as a second node;
combining the first node and the second node to obtain the first node set;
and recording a preset number of continuous nodes as subgraphs, wherein all the subgraphs form the first subgraph set.
8. A system for positioning and map updating in a dynamic scene, the system comprising:
a first obtaining unit: the first obtaining unit is used for obtaining a first initial map through a laser radar, wherein the first initial map comprises a first mapping track;
a second obtaining unit: the second obtaining unit is used for obtaining a first node set and a first sub-graph set according to the first mapping track;
a third obtaining unit: the third obtaining unit is configured to obtain a first positioning track of the first robot, where the first positioning track includes a second node set and a second sub-graph set;
a fourth obtaining unit: the fourth obtaining unit is configured to perform feature matching on the first node set and the second sub-graph set to obtain a first feature matching result, and perform feature matching on the first sub-graph set and the second node set to obtain a second feature matching result;
a first creation unit: the first creating unit is used for creating a first pose constraint according to the first feature matching result and the second feature matching result;
a fifth obtaining unit: the fifth obtaining unit is used for optimizing the first attitude constraint by using a least square method to obtain a first optimization result;
a first execution unit: the first execution unit is used for positioning the first robot according to the first optimization result.
9. An electronic device comprising a processor and a memory;
the memory is used for storing;
the processor is used for executing the method of any one of claims 1-7 through calling.
10. A computer program product comprising a computer program and/or instructions, characterized in that the computer program and/or instructions, when executed by a processor, implement the steps of the method according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210062357.7A CN114440855B (en) | 2022-01-19 | 2022-01-19 | Method and system for positioning and map updating in dynamic scene |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210062357.7A CN114440855B (en) | 2022-01-19 | 2022-01-19 | Method and system for positioning and map updating in dynamic scene |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114440855A true CN114440855A (en) | 2022-05-06 |
| CN114440855B CN114440855B (en) | 2022-08-23 |
Family
ID=81367211
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210062357.7A Active CN114440855B (en) | 2022-01-19 | 2022-01-19 | Method and system for positioning and map updating in dynamic scene |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114440855B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111380510A (en) * | 2018-12-29 | 2020-07-07 | 深圳市优必选科技有限公司 | Repositioning method and device and robot |
| US20210063577A1 (en) * | 2019-08-26 | 2021-03-04 | Ubtech Robotics Corp Ltd | Robot relocalization method and apparatus and robot using the same |
| CN112579835A (en) * | 2021-02-25 | 2021-03-30 | 成都数联铭品科技有限公司 | Sub-graph matching method and system, electronic device and storage medium |
| CN113514843A (en) * | 2021-07-09 | 2021-10-19 | 深圳华芯信息技术股份有限公司 | Multi-subgraph laser radar positioning method and system and terminal |
| CN113591922A (en) * | 2021-06-30 | 2021-11-02 | 五八有限公司 | Behavior recognition method and device, electronic equipment and storage medium |
| CN113946717A (en) * | 2021-10-18 | 2022-01-18 | 海南普适智能科技有限公司 | Sub-map index feature obtaining method, device, equipment and storage medium |
-
2022
- 2022-01-19 CN CN202210062357.7A patent/CN114440855B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111380510A (en) * | 2018-12-29 | 2020-07-07 | 深圳市优必选科技有限公司 | Repositioning method and device and robot |
| US20210063577A1 (en) * | 2019-08-26 | 2021-03-04 | Ubtech Robotics Corp Ltd | Robot relocalization method and apparatus and robot using the same |
| CN112579835A (en) * | 2021-02-25 | 2021-03-30 | 成都数联铭品科技有限公司 | Sub-graph matching method and system, electronic device and storage medium |
| CN113591922A (en) * | 2021-06-30 | 2021-11-02 | 五八有限公司 | Behavior recognition method and device, electronic equipment and storage medium |
| CN113514843A (en) * | 2021-07-09 | 2021-10-19 | 深圳华芯信息技术股份有限公司 | Multi-subgraph laser radar positioning method and system and terminal |
| CN113946717A (en) * | 2021-10-18 | 2022-01-18 | 海南普适智能科技有限公司 | Sub-map index feature obtaining method, device, equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114440855B (en) | 2022-08-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111024100B (en) | Navigation map updating method and device, readable storage medium and robot | |
| CN111536964B (en) | Robot positioning method and device, and storage medium | |
| CN109540142B (en) | Robot positioning navigation method and device, and computing equipment | |
| US8359156B2 (en) | Map generation system and map generation method by using GPS tracks | |
| CN111123949B (en) | Obstacle avoidance method and device for robot, robot and storage medium | |
| CN111694356B (en) | Driving control method and device, electronic equipment and storage medium | |
| CN112595337B (en) | Obstacle avoidance path planning method and device, electronic device, vehicle and storage medium | |
| CN110900602B (en) | Positioning recovery method and device, robot and storage medium | |
| CN113341397A (en) | Reflection value map construction method and device | |
| CN104819726A (en) | Navigation data processing method, navigation data processing device and navigation terminal | |
| CN112284393A (en) | Global path planning method and system for intelligent mobile robot | |
| JP2019191498A (en) | Map creation device | |
| CN115326051A (en) | Positioning method and device based on dynamic scene, robot and medium | |
| CN112540607A (en) | Path planning method and device, electronic equipment and storage medium | |
| CN117141520B (en) | Real-time track planning method, device and equipment | |
| CN111951552B (en) | Method and related device for risk management in automatic driving | |
| CN114440855B (en) | Method and system for positioning and map updating in dynamic scene | |
| CN112767545A (en) | Point cloud map construction method, device, equipment and computer storage medium | |
| CN112578798B (en) | Robot map acquisition method and device, processor and electronic device | |
| CN115752474A (en) | Robot navigation planning method and device under non-flat ground environment and robot | |
| CN114419877A (en) | Vehicle track prediction data processing method and device based on road characteristics | |
| CN116592869B (en) | Map updating method and device, electronic equipment and storage medium | |
| CN116385825B (en) | Model joint training method and device and vehicle | |
| Shin | Fault Tolerant Control and Localization for Autonomous Driving: Systems and Architecture | |
| CN117629225A (en) | Global repositioning method and device for mobile vehicle, mobile vehicle and equipment |
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 |