CN115655291A - Laser SLAM closed-loop mapping method and device, mobile robot, equipment and medium - Google Patents
Laser SLAM closed-loop mapping method and device, mobile robot, equipment and medium Download PDFInfo
- Publication number
- CN115655291A CN115655291A CN202211192389.5A CN202211192389A CN115655291A CN 115655291 A CN115655291 A CN 115655291A CN 202211192389 A CN202211192389 A CN 202211192389A CN 115655291 A CN115655291 A CN 115655291A
- Authority
- CN
- China
- Prior art keywords
- frame
- laser
- loop
- grid map
- key frame
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 74
- 238000013507 mapping Methods 0.000 title claims abstract description 32
- 230000008569 process Effects 0.000 claims abstract description 29
- 238000012545 processing Methods 0.000 claims abstract description 21
- 238000013519 translation Methods 0.000 claims abstract description 19
- 238000005457 optimization Methods 0.000 claims abstract description 16
- 238000012544 monitoring process Methods 0.000 claims abstract description 11
- 238000004891 communication Methods 0.000 claims description 14
- 238000001514 detection method Methods 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 7
- 230000001953 sensory effect Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 10
- 230000007246 mechanism Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 230000000295 complement effect Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000009616 inductively coupled plasma Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000003780 insertion Methods 0.000 description 2
- 230000037431 insertion Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000010408 sweeping Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000002592 echocardiography Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000010511 looping mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000004806 packaging method and process Methods 0.000 description 1
- 239000000758 substrate Substances 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The disclosure relates to a method, a device, a mobile robot, equipment and a medium for laser SLAM closed-loop mapping, wherein the method comprises the following steps: acquiring laser sensing data of a moving object; initializing the grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame; determining a target key frame in the subsequent laser frames aiming at the subsequent laser frames positioned after the initial laser frame time sequence in the laser sensing data, wherein the difference of the target key frame and the last existing key frame in the grid map in at least one of translation and rotation is larger than a set threshold value; inserting the target key frame into the grid map; performing loop matching search within a preset distance threshold range of the target key frame; and/or monitoring whether a matching instruction for the marked loop back frame is received; and responding to the matching result existing in the loop matching search or receiving a matching instruction, performing closed-loop graph optimization processing, and constructing and obtaining a target map perceived by the moving object in the moving process.
Description
Technical Field
The disclosure relates to the technical field of mobile robot navigation, in particular to a method and a device for building a graph by laser SLAM closed loop, a mobile robot, equipment and a medium.
Background
The instant positioning and Mapping (SLAM) is a process that a mobile robot moves in an unknown environment, corresponding sensing data is obtained by adopting a laser radar or a visual camera, positioning is carried out according to the sensing data, and a map perceived in the moving process is incrementally constructed.
However, in SLAM mapping based on lidar, since the algorithm estimates errors and accumulates errors as image frames are generated, a loop-back mechanism is needed to reduce or even eliminate the accumulated errors.
Disclosure of Invention
To solve or at least partially solve the technical problems found in: the embodiment of the disclosure provides a method, a device, a mobile robot, equipment and a medium for laser SLAM closed-loop mapping.
In a first aspect, an embodiment of the present disclosure provides a method for laser SLAM closed-loop mapping. The method comprises the following steps: acquiring laser sensing data of a moving object; initializing the grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame; determining a target key frame in the subsequent laser frames according to the subsequent laser frames positioned after the initial laser frame time sequence in the laser sensing data, wherein the difference between the target key frame and the last existing key frame in the grid map in at least one of translation and rotation is larger than a set threshold value; inserting the target key frame into the grid map; performing loop matching search within a preset distance threshold range of the target key frame in the grid map; and/or monitoring whether a matching instruction for the marked loop back frame is received; and responding to a matching result existing in the loop matching search or receiving the matching instruction, performing closed-loop graph optimization processing, and constructing and obtaining a target map perceived by the moving object in the moving process.
According to an embodiment of the present disclosure, determining a target key frame in a subsequent laser frame after the initial laser frame timing in the laser sensing data includes: performing grid filtering processing on each laser frame in the subsequent laser frames to obtain point cloud data of the subsequent laser frames; carrying out position matching on the point cloud data of the subsequent laser frame and the grid map to obtain corresponding pose information of the point cloud data of the subsequent laser frame in a coordinate system of the grid map; and determining whether each laser frame is a target key frame according to the difference between the pose information of each laser frame in the subsequent laser frames and the pose information of the last laser frame.
According to an embodiment of the present disclosure, the pose information includes: position information and attitude angle information; the setting of the threshold value includes: a first set threshold for defining a distance gap and a second set threshold for defining an angle gap; determining whether the current laser frame is a target key frame according to the difference between the pose information of the current laser frame and the pose information of the previous laser frame, including: calculating the translation distance between the current laser frame and the previous laser frame according to the respective position information of the current laser frame and the previous laser frame; calculating a rotation angle between the current laser frame and the previous laser frame according to the respective attitude angle information of the current laser frame and the previous laser frame; determining the current laser frame as a target key frame under the condition that a preset condition is met, wherein the preset condition comprises at least one of the following conditions: the translation distance is greater than the first set threshold, or the rotation angle is greater than the second set threshold.
According to an embodiment of the present disclosure, inserting the target key frame into the grid map includes: and inserting the point cloud data of the target key frame into the grid map based on the corresponding target pose information.
According to the embodiment of the disclosure, the set threshold is determined according to the consumption of computing resources and the preset precision of mapping, and the preset distance threshold range is related to the error of the laser sensing data matching.
According to an embodiment of the present disclosure, in the grid map, performing a loop matching search within a preset distance threshold range of the target key frame includes: determining whether a loop frame of the target key frame exists in key frames within a preset distance range of the target key frame in the grid map, wherein the loop frame and the target key frame are used for representing the same place; determining that a matching result exists in the loop matching search under the condition that a loop frame exists; and under the condition that the loop frame does not exist, determining that no matching result exists in the loop matching search.
According to an embodiment of the present disclosure, initializing a grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame, including: initializing the origin of the coordinate system of the grid map under the condition of receiving the initial laser frame; performing grid filtering processing on the initial laser frame to obtain point cloud data of the initial laser frame; carrying out position matching on the point cloud data of the initial laser frame and the grid map to obtain corresponding pose information of the point cloud data of the initial laser frame in a coordinate system of the grid map; and inserting the initial laser frame into the grid map according to the corresponding pose information to obtain an initial key frame.
In a second aspect, an embodiment of the present disclosure provides an apparatus for laser SLAM closed-loop mapping. The above-mentioned device includes: the map updating system comprises a sensing data acquisition module, a map initialization module, a key frame determination module, a map updating module, a closed-loop condition detection module and a closed-loop processing module. The sensing data acquisition module is used for acquiring laser sensing data of a moving object. The map initialization module is used for initializing the grid map according to the initial laser frame in the laser sensing data to obtain an initial key frame. The key frame determining module is configured to determine, for a subsequent laser frame in the laser sensor data after the initial laser frame timing, a target key frame in the subsequent laser frame, where a difference between the target key frame and a previous key frame existing in the grid map in at least one of translation and rotation is greater than a set threshold. The map updating module is used for inserting the target key frame into the grid map. The closed-loop condition detection module is used for performing loop matching search in the grid map within a preset distance threshold range of the target key frame; and/or monitoring whether a matching instruction for the marked loop back frame is received. The closed-loop processing module is used for responding to a matching result existing in the loop matching search or receiving the matching instruction, performing closed-loop graph optimization processing, and constructing and obtaining a target map perceived by the moving object in the moving process.
In a third aspect, embodiments of the present disclosure provide a mobile robot. The mobile robot stores a group of instruction sets, and the instruction sets are executed by the mobile robot to realize the laser SLAM closed-loop mapping method; or the mobile robot comprises the laser SLAM closed-loop mapping device.
In a fourth aspect, embodiments of the present disclosure provide an electronic device. The electronic equipment comprises a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory are communicated with each other through the communication bus; a memory for storing a computer program; and the processor is used for realizing the laser SLAM closed-loop mapping method when executing the program stored in the memory.
In a fifth aspect, embodiments of the present disclosure provide a computer-readable storage medium. The computer readable storage medium stores thereon a computer program, which when executed by a processor implements the method for closed-loop mapping of the laser SLAM as described above.
The technical scheme provided by the embodiment of the disclosure at least has part or all of the following advantages:
in the provided method for constructing the map by the laser SLAM closed loop, the common frequency of the laser radar is considered to be higher, the target key frame inserted into the grid map is obtained by setting the threshold value on at least one of translation and rotation, compared with the method that each frame of point cloud is inserted into the map in the related art, the method can save the memory occupation and reduce the consumption of computing resources, and the value of the set threshold value can balance the comprehensive requirements of the computing resources and the computing precision; meanwhile, two complementary and consistent closed-loop condition detection strategies are provided, one is that in the grid map, loop matching search is carried out within a preset distance threshold range of the target key frame; the other method is to monitor whether a matching instruction for a marked loop back frame is received, under the condition that some loop back matching searches do not have matching results, for example, when a map is built in some large space environment, if the accumulated error is larger than the preset distance threshold range, the matching results cannot be searched, so that the loop closing cannot be performed, and the mode of receiving the matching instruction for the marked loop back frame can realize the manual control of the loop closing. The method meets the requirements of drawing construction in various scenes, does not need to assist a closed loop by an additional sensor, reduces the requirements on the performance of the processor, and reduces the consumption and cost of computing resources.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present disclosure and together with the description, serve to explain the principles of the disclosure.
In order to more clearly illustrate the embodiments of the present disclosure or the technical solutions in the prior art, the drawings needed to be used in the description of the embodiments or the related art will be briefly described below, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
Fig. 1 schematically illustrates a flow chart of a method of laser SLAM closed loop mapping according to an embodiment of the disclosure;
FIG. 2 schematically shows a schematic diagram of a grid map after an initialization process according to an embodiment of the disclosure;
FIG. 3 schematically illustrates a schematic diagram of target key frame insertion in a grid map according to an embodiment of the present disclosure;
fig. 4A schematically illustrates a scene diagram where there is no matching result in a loop-back matching search on a grid map according to an embodiment of the present disclosure;
FIG. 4B schematically shows a close-up view of the loop back portion of FIG. 4A;
FIG. 5A schematically illustrates a target map resulting from a closed-loop map optimization process according to an embodiment of the disclosure;
FIG. 5B schematically shows a close-up view of the loop back portion of FIG. 5A;
fig. 6 schematically shows a detailed implementation flowchart of step S120 according to an embodiment of the present disclosure;
fig. 7 schematically shows a detailed implementation flowchart of step S130 according to an embodiment of the present disclosure;
fig. 8 schematically shows a block diagram of a structure of an apparatus for laser SLAM closed loop mapping according to an embodiment of the present disclosure; and
fig. 9 schematically shows a block diagram of an electronic device provided by an embodiment of the present disclosure.
Detailed Description
In the SLAM mapping process, errors need to be reduced or even eliminated through a loop-back mechanism. The looping mechanism refers to: closed loop detection is formed by identifying whether the current position where the robot is located is a position that has been previously traversed, and accumulated errors are reduced by performing global or local graph optimization.
However, in development it was found that: some loop-back mechanisms have large calculation amount, low calculation efficiency and high performance requirement on a processor; some loop mechanisms cannot be suitable for scenes with large accumulated errors, and no loop can be searched under the condition that the accumulated errors exceed a limited range; some loop mechanisms need to assist in visual sensing to loop, not only need to add a new sensor, and have complex hardware setting, but also have large calculation amount related to vision, and are easily influenced by visual angles and illumination; some loop-back mechanisms are easy to fail in some scenes which cannot be closed.
In view of this, an embodiment of the present disclosure provides a method for creating a map of a laser SLAM closed loop, where the method includes: acquiring laser sensing data of a moving object; initializing the grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame; determining a target key frame in the subsequent laser frames according to the subsequent laser frames positioned after the initial laser frame time sequence in the laser sensing data, wherein the difference between the target key frame and the last existing key frame in the grid map in at least one of translation and rotation is larger than a set threshold value; inserting the target key frame into the grid map; performing loop matching search in the grid map within a preset distance threshold range of the target key frame; or, monitoring whether a matching instruction for the marked loop back frame is received; and responding to a matching result existing in the loop matching search or receiving the matching instruction, performing closed-loop graph optimization processing, and constructing and obtaining a target map perceived by the moving object in the moving process.
To make the objects, technical solutions and advantages of the embodiments of the present disclosure more clear, the technical solutions of the embodiments of the present disclosure will be described clearly and completely with reference to the drawings in the embodiments of the present disclosure, and it is obvious that the described embodiments are some embodiments of the present disclosure, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments disclosed herein without making any creative effort, shall fall within the protection scope of the present disclosure.
In the description of the embodiments of the present disclosure, in the same paragraph or sentence, "a and/or B" means the following three alternatives: a alone, B alone, or both A and B.
A first exemplary embodiment of the present disclosure provides a method for laser SLAM closed loop mapping.
Fig. 1 schematically shows a flowchart of a method of laser SLAM closed loop mapping according to an embodiment of the disclosure.
Referring to fig. 1, a method for creating a map of a laser SLAM closed loop provided in an embodiment of the present disclosure includes the following steps: s110, S120, S130, S140, S150, and S160. The steps S110 to S160 may be executed by an electronic device with computing capability, and the electronic device may be a mobile object for positioning and mapping, or an electronic device capable of performing data communication with the mobile object.
In step S110, laser sensing data of the moving object is acquired.
The moving object is, for example, a mobile robot, including but not limited to: the intelligent automatic driving vehicle, the intelligent sweeping robot, the intelligent search and rescue robot, the exploration robot, the patrol robot, the unmanned aerial vehicle and the like.
The laser radar is installed on the moving object, the laser radar is used for emitting in the surrounding environment at a preset frequency, and laser sensing data are obtained by detecting laser echoes.
The laser sensing data is time sequence laser frame data and is updated in real time along with the position movement of the moving object, and the laser frame data carries information of the surrounding environment. For example, the moving object is at the initial position L 0 Time, corresponding to time t 0 The collected laser sensing data is an initial laser frame J 0 . In the process that the moving object moves at a preset moving speed, the laser radar on the moving object emits at a preset frequency, and laser frames acquired after the initial position are collectively called as subsequent laser frames, namely the subsequent laser frames are positioned after the initial laser frame time sequence.
For example, the moving object is at time t 1 (t 1 Is t 0 Next sampling time) as a laser frame J 1 At time t 2 (t 2 Is t 1 Next sampling time) as a laser frame J 2 At time t, as time goes on k The collected laser sensing data is a laser frame J k K is the serial number of the laser frame and is a positive integer, and the laser frame J 0 、J 1 、J 2 And the carried environment information is changed along with the position of the moving object. Therefore, as the position of the moving object moves, a group of time series laser frame data which is continuously updated along with time and changes in real time along with the position of the moving object is obtained.
In step S120, a grid map is initialized according to the initial laser frame in the laser sensing data, so as to obtain an initial key frame.
And in the process of initializing the grid map, generating a corresponding initial key frame on the grid map according to the data of the initial laser frame. For example, in an exemplary embodiment, the grid map has a grid side length of 0.05m.
Fig. 2 schematically shows a schematic diagram of a grid map after an initialization process according to an embodiment of the present disclosure.
And the electronic equipment starts the SLAM mapping system, and when the SLAM mapping system can normally receive the laser sensing data acquired by the laser radar, mapping begins. The initial position of the moving object is used as the origin of the coordinate system (0,0), i.e. the starting point of the mapping. Generating an initial key frame (i.e., a first frame key frame) in a grid map according to data of an initial laser frame (i.e., the first frame laser frame), as shown in fig. 2, in the grid map after initialization processing, a grid map is indicated by using gray-filled grids, and an edge or an independent pattern (two separate line segments) of a light region on the grid map is an initial laser frame J 0 The laser point cloud 201 of (1), a rectangular frame 202 around the laser point cloud 201 is also illustrated.
In step S130, a target key frame in the subsequent laser frames after the initial laser frame timing in the laser sensor data is determined, where a difference between the target key frame and a previous key frame existing in the grid map in at least one of translation and rotation is greater than a set threshold.
According to the embodiment of the disclosure, the set threshold is determined according to the consumption of computing resources and the preset precision of drawing.
Because the general frequency of the laser radar is higher, a large amount of time sequence laser frame data can be generated in the moving process of the position of the moving object, and a target key frame for inserting into the grid map is obtained by setting a threshold value on at least one of translation and rotation.
For example, in an exemplary embodiment, a motion filter is used to determine the target keyframe, where the threshold for the translation gap is 0.2m and the threshold for the rotation angle gap is 3 °.
For example, for initial laser frame J 0 Each subsequent laser frame J thereafter 1 And J 2 And determining a specific subsequent laser frame as a target key frame by performing condition judgment according to a set threshold value of at least one of translation and rotation under the condition that the specific subsequent laser frame meets the condition that the specific subsequent laser frame is larger than the set threshold value compared with the last existing key frame in the grid map. As an example, the value of k is 50, and in the subsequent laser frames, it is determined that the obtained target key frames are respectively: j. the design is a square 5 、J 10 、J 14 、J 18 、J 26 、J 30 、J 35 、J 40 、J 46 、J 50 。
In step S140, the target key frame is inserted into the grid map.
In the process of inserting the target key frame into the grid map, the number of the target key frame (the number recorded according to the time sequence) and the corresponding pose information of the target key frame in the grid map coordinate system can be recorded. The above pose information includes: position information and attitude angle information. The grid map may be a two-dimensional map or a three-dimensional map. Accordingly, the pose information may be two-dimensional data or three-dimensional data.
According to an embodiment of the present disclosure, inserting the target key frame into the grid map includes: and inserting the point cloud data of the target key frame into the grid map based on the corresponding target pose information.
Fig. 3 schematically shows a schematic diagram of target key frame insertion in a grid map according to an embodiment of the present disclosure.
Referring to FIG. 3, in addition to including the existing initial laser frame J in the grid map 0 In addition to the laser point cloud 201, a newly inserted target key frame, such as laser frame J, is also included 5 Corresponding laser point cloud 301.
For laser frames that are not target keyframes, there is no need to insert into the grid map.
In step S150, performing a loop matching search within a preset distance threshold range of the target keyframe in the grid map; and/or monitoring whether a matching instruction for the marked loop back frame is received.
In some embodiments, the laser frames with obviously same characteristics are marked as loop frames in advance by manual marking, for example, in the process of moving object motion mapping, the moving object is observed to return to a position L already passed by artificially Old age At this time, the position L is marked manually Old age The corresponding laser frame is a loop frame according to the current laser frame and the position L Old age And constructing an error item corresponding to the laser frame, and performing closed-loop graph optimization processing.
The preset distance threshold range is associated with a fundamental error of the laser sensing data matching. For example, the preset distance threshold range may be set to be greater than the basic error of laser sensing data matching, and may be set to be 5 to 15 times, and multiple adjustment may be performed according to the scene size, which is not limited to the numerical example herein, and in addition, the value of the preset distance threshold range may be coordinated according to the calculation accuracy and the consumption of calculation resources.
In an exemplary embodiment, in the case where the error of the laser sensing data matching is 1m, the preset distance threshold range may be set to 5m. If the route is long, the accumulated error is large, for example, 6m, and the accumulated error exceeds the preset distance threshold range.
FIG. 4A is a schematic diagram illustrating a scenario in which there is no matching result in a loop back matching search on a grid map according to an embodiment of the present disclosure; fig. 4B schematically shows a partially enlarged view of the loop back portion in fig. 4A. The frame selection area S that needs to be enlarged is illustrated in fig. 4A and includes a looping portion, and the frame selection area S is illustrated in fig. 4B in an enlarged manner.
Considering that there is a scene in which closed-loop cannot be performed, for example, as shown in fig. 4A and 4B, when a graph is built in a large space environment, for example, when an accumulated error is greater than a preset distance threshold range, a matching result does not exist in the loop matching search, that is, a matching result cannot be obtained through search, so that closed-loop cannot be performed, for example, an initial key frame 201 (i.e., a first frame key frame), a second frame key frame 301, and subsequent key frames are illustrated in fig. 4A, and a loop frame 310B is illustrated by using the second key frame, in a moving process of a moving object, an accumulated error between a key frame 310a corresponding to a current time and a loop frame 310B corresponding to a same position is greater than 10m and exceeds a preset distance threshold range, and then a matching result does not exist in a manner of performing loop matching search within the preset distance threshold range of the target key frame.
In this scenario, the method of the present disclosure further provides a strategy for performing a manual closed loop compensation, that is, by monitoring whether a matching instruction for the marked loop back frame is received, if a scene requiring a manual closed loop is detected, a matching instruction for performing a closed loop on the marked loop back frame is manually sent to the electronic device. By receiving a matching instruction for the marked loop back frame, the closed loop can be artificially controlled.
According to the embodiment of the disclosure, two closed-loop detection conditions are provided, and the two detection conditions can be organically combined and used as a complementary scheme to realize the strategy of automatic closed-loop and manual closed-loop matching. In some embodiments, either of the two closed-loop detection conditions may also be employed alone to achieve closed-loop.
In some embodiments, a loop matching search is performed within a preset distance threshold range of the target keyframe; and/or, monitoring whether a matching instruction for a marked loop back frame is received, including the following embodiments: performing loop matching search within a preset distance threshold range of the target key frame; and in the case that no matching result exists in the loop matching search, monitoring whether a matching instruction for the marked loop frame is received.
That is, in this embodiment, the following steps may be preferentially performed: performing loop matching search within a preset distance threshold range of the target key frame; in case there is no matching result, then the following steps are performed: it is monitored whether a matching instruction for a marked loop back frame is received. There is complementary and sequential logic between these two closed loop detection conditions.
In other embodiments, the above two schemes can be executed synchronously, that is, whether a matching instruction for the marked loop back frame is received is synchronously monitored while the loop back matching search is automatically performed. As soon as one of them occurs, the subsequent step S160 is performed.
According to an embodiment of the present disclosure, in the grid map, performing a loop matching search within a preset distance threshold range of the target key frame includes: determining whether a loop frame of the target key frame exists in key frames within a preset distance range of the target key frame in the grid map, wherein the loop frame and the target key frame are used for representing the same place; determining that a matching result exists in the loop matching search under the condition that a loop frame exists; and in the case that the loop back frame does not exist, determining that the loop back matching search does not have a matching result.
In step S160, in response to the matching result existing in the loop matching search or receiving the matching instruction, performing closed-loop map optimization processing to construct and obtain a target map perceived by the moving object in the moving process.
And performing closed-loop graph optimization processing under the condition that a matching result exists in the loop matching search or the matching instruction is received. In the process of closed-loop graph optimization, the optimal pose information of each key frame in the global closed loop or the local closed loop when the error term is minimum is solved by constructing the error term and carrying out global or local graph optimization. All keyframes (including the initial keyframe and one or more target keyframes) form a target map according to the optimal pose information.
FIG. 5A schematically illustrates a target map resulting from a closed-loop map optimization process according to an embodiment of the disclosure; fig. 5B schematically shows a partially enlarged view of the loop back portion in fig. 5A. The frame selection area S that needs to be enlarged is illustrated in fig. 5A and includes a loop portion, and the frame selection area S is illustrated in fig. 5B in an enlarged manner.
For example, referring to fig. 5A and 5B, in the case of manual closed loop, the electronic device receives a matching instruction for a marked loop frame, performs closed loop map optimization processing, and optimizes pose information of each key frame in the loop, so that a key frame corresponding to a current position and a second frame key frame (loop frame) form a closed loop 410 (i.e., are located at the same position), and the obtained target map eliminates an accumulated error.
In the method including the steps S110 to S160, considering that the general frequency of the laser radar is high, the target key frame for inserting into the grid map is obtained by setting the threshold value on at least one of translation and rotation, and compared with the method in which each frame of point cloud is inserted into the map in the related art, the method can save the memory occupation and reduce the consumption of the calculation resources, and the value of the set threshold value can balance the comprehensive requirements of the calculation resources and the calculation accuracy; meanwhile, two complementary and consistent closed-loop condition detection strategies are provided, one is that in the grid map, loop matching search is carried out within a preset distance threshold range of the target key frame; the other method is to monitor whether a matching instruction for a marked loop back frame is received, under the condition that some loop back matching searches do not have matching results, for example, when a map is built in some large space environment, if the accumulated error is larger than the preset distance threshold range, the matching results cannot be searched, so that the loop closing cannot be performed, and the mode of receiving the matching instruction for the marked loop back frame can realize the manual control of the loop closing. The method meets the requirements of drawing construction in various scenes, does not need to assist a closed loop by an additional sensor, reduces the requirements on the performance of the processor, and reduces the consumption and cost of computing resources.
Fig. 6 schematically shows a detailed implementation flowchart of step S120 according to an embodiment of the present disclosure.
According to an embodiment of the present disclosure, as shown in fig. 6, in the step S120, initializing a grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame, including the following steps: s610, S620, S630 and S640.
In step S610, when the initial laser frame is received, the origin of the coordinate system of the grid map is initialized.
The initial position of the moving object is used as the origin of the coordinate system of the grid map (0,0), namely, the starting point of drawing.
In step S620, a grid filtering process is performed on the initial laser frame to obtain point cloud data of the initial laser frame.
The principle of grid filtering is to scratch the current point cloud into a voxel grid of fixed side length. All points falling into the same grid are represented by coordinates of the center point of the grid, so that the calculation amount during matching of laser frames can be reduced on one hand, and the weights of all points are balanced on the other hand.
In step S630, the point cloud data of the initial laser frame is position-matched with the grid map to obtain pose information corresponding to the point cloud data of the initial laser frame in the coordinate system of the grid map.
In some embodiments, the grid map is a two-dimensional map, and the pose information of the point cloud data on the two-dimensional grid map is (x, y, θ), where x and y represent x and y coordinates in the two-dimensional position information, and θ represents two-dimensional attitude angle information, specifically, corner information.
Computing the point cloud p after grid filtering current Relative pose T with current map ε The solving formula of (2) is:
k is the total number of the point clouds to be matched currently; m smoot h is a smooth function using a double-cube interpolation value pair to interpolate the grid map; p is a radical of current Point cloud data (which may also be described as a laser point cloud) that is a laser frame; t is ε Is to mix p current All the points are converted into pose information under the current map coordinate system.
The pose information of the current laser frame can be output in real time through the solving formula.
In some embodiments, the grid map is a three-dimensional map, and the pose information of the point cloud data on the three-dimensional grid map is (x, y, z, theta) P ，θ R ，θ Y ) Wherein (x, y, z) represents three-dimensional position informationX, y and z coordinates; theta.theta. P Representing pitch angle, theta R Representing roll angle, theta Y Indicating the yaw angle. The three-dimensional pose information may also be calculated by using an algorithm such as ICP (Iterative Closest Point, which is a Point cloud matching algorithm), NDT (Normal Distribution Transform), and the like.
In step S640, the initial laser frame is inserted into the grid map according to the corresponding pose information, so as to obtain an initial key frame.
And inserting the initial laser frame into the grid map with the corresponding pose information as an initial key frame of the grid map.
Based on the steps S601 to S640, by performing raster filtering in the process of initializing the raster map, the calculation amount of inter-frame matching can be reduced in the process of performing preliminary pose information calculation, and the weights of the points in the point cloud are equalized.
Fig. 7 schematically shows a detailed implementation flowchart of step S130 according to an embodiment of the present disclosure.
According to an embodiment of the present disclosure, referring to fig. 7, in the step S130, for a subsequent laser frame located after the initial laser frame timing in the laser sensing data, determining a target key frame in the subsequent laser frame includes the following steps: s710, S720 and S730.
In step S710, a grid filtering process is performed on each of the subsequent laser frames to obtain point cloud data of the subsequent laser frames.
The principle of grid filtering is to scratch the current point cloud into a voxel grid of fixed side length. All points falling into the same grid are represented by coordinates of the center point of the grid, so that the calculation amount during matching of laser frames can be reduced on one hand, and the weights of all points are balanced on the other hand.
In step S720, the point cloud data of the subsequent laser frame is position-matched with the grid map to obtain pose information corresponding to the point cloud data of the subsequent laser frame in the coordinate system of the grid map.
In this embodiment, for the two-dimensional grid map, the pose information may be calculated with reference to the foregoing formula (1). For the three-dimensional grid map, the pose information can be calculated by adopting ICP (inductively coupled plasma) and NDT (normalized difference transform) algorithms and the like.
In step S730, it is determined whether each laser frame is a target key frame according to a difference between the pose information of each laser frame in the subsequent laser frames and the pose information of the previous laser frame.
According to an embodiment of the present disclosure, the pose information includes: position information and attitude angle information.
The setting of the threshold value includes: a first set threshold for defining the distance gap and a second set threshold for defining the angle gap.
In step S730, determining whether the current laser frame is a target key frame according to a difference between the pose information of the current laser frame and the pose information of the previous laser frame includes: calculating the translation distance between the current laser frame and the previous laser frame according to the respective position information of the current laser frame and the previous laser frame; calculating a rotation angle between the current laser frame and the previous laser frame according to the respective attitude angle information of the current laser frame and the previous laser frame; determining the current laser frame as a target key frame under the condition that a preset condition is met, wherein the preset condition comprises at least one of the following conditions: the translation distance is greater than the first set threshold, or the rotation angle is greater than the second set threshold.
For a two-dimensional pose angle, the rotation angle is expressed as the absolute value of the difference between the rotation angle of the current laser frame and the rotation angle of the previous laser frame.
For the three-dimensional attitude angles, the rotation angles are three and are respectively expressed as the absolute value of the difference between the pitch angle, the absolute value of the difference between the roll angle and the yaw angle of the current laser frame and the previous laser frame.
A second exemplary embodiment of the present disclosure provides an apparatus for laser SLAM closed loop mapping.
Fig. 8 schematically shows a block diagram of a structure of an apparatus for laser SLAM closed loop mapping according to an embodiment of the present disclosure.
Referring to fig. 8, an apparatus 800 for creating a map of a laser SLAM closed loop according to an embodiment of the present disclosure includes: a sensing data acquisition module 801, a map initialization module 802, a key frame determination module 803, a map update module 804, a closed loop condition detection module 805, and a closed loop processing module 806.
The sensing data acquiring module 801 is configured to acquire laser sensing data of a moving object.
The map initialization module 802 is configured to initialize the grid map according to the initial laser frame in the laser sensing data to obtain an initial key frame.
The key frame determining module 803 is configured to determine, for a subsequent laser frame in the laser sensor data after the initial laser frame timing, a target key frame in the subsequent laser frame, where a difference between the target key frame and a previous key frame existing in the grid map in at least one of translation and rotation is greater than a set threshold.
The map update module 804 is configured to insert the target key frame into the grid map.
The closed-loop condition detection module 805 is configured to perform a loop matching search within a preset distance threshold range of the target keyframe in the grid map; and/or monitoring whether a matching instruction for the marked loop back frame is received.
The closed-loop processing module 806 is configured to perform closed-loop map optimization processing in response to a matching result existing in the loop matching search or receiving the matching instruction, and construct and obtain a target map perceived by the moving object in the moving process.
The details of the implementation of the functional modules included in the apparatus provided in the embodiment of the present disclosure may refer to the description of the first embodiment, and are not described herein again.
Any number of the functional modules included in the apparatus 800 may be combined into one module to be implemented, or any one of the functional modules may be split into a plurality of modules. Alternatively, at least part of the functionality of one or more of these modules may be combined with at least part of the functionality of the other modules and implemented in one module. At least one of the functional modules included in the apparatus 800 may be at least partially implemented as a hardware circuit, such as a Field Programmable Gate Array (FPGA), a Programmable Logic Array (PLA), a system on a chip, a system on a substrate, a system on a package, an Application Specific Integrated Circuit (ASIC), or may be implemented by hardware or firmware in any other reasonable manner of integrating or packaging a circuit, or implemented by any one of three implementations of software, hardware and firmware, or implemented by any suitable combination of any of the three. Alternatively, at least one of the functional modules comprised by the apparatus 800 described above may be at least partially implemented as a computer program module, which when executed may perform the corresponding function.
A third exemplary embodiment of the present disclosure provides a mobile robot.
The mobile robot stores a group of instruction sets, and the instruction sets are executed by the mobile robot to realize the laser SLAM closed-loop mapping method; or the mobile robot comprises the laser SLAM closed-loop mapping device.
The mobile robot includes but is not limited to: the intelligent automatic driving vehicle, the intelligent sweeping robot, the intelligent search and rescue robot, the exploration robot, the patrol robot, the unmanned aerial vehicle and the like.
A fourth exemplary embodiment of the present disclosure provides an electronic apparatus.
Fig. 9 schematically shows a block diagram of an electronic device provided in an embodiment of the present disclosure.
Referring to fig. 9, an electronic device 900 provided in the embodiment of the present disclosure includes a processor 901, a communication interface 902, a memory 903, and a communication bus 904, where the processor 901, the communication interface 902, and the memory 903 complete communication with each other through the communication bus 904; a memory 903 for storing computer programs; the processor 901 is configured to implement the method for creating the closed-loop map of the laser SLAM as described above when executing the program stored in the memory.
A fifth exemplary embodiment of the present disclosure also provides a computer-readable storage medium. The computer readable storage medium stores thereon a computer program, which when executed by a processor implements the method for closed-loop mapping of the laser SLAM as described above.
The computer-readable storage medium may be contained in the apparatus/device described in the above embodiments; or may be present alone without being assembled into the device/apparatus. The computer-readable storage medium carries one or more programs which, when executed, implement the method according to an embodiment of the disclosure.
According to embodiments of the present disclosure, the computer-readable storage medium may be a non-volatile computer-readable storage medium, which may include, for example but is not limited to: a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the present disclosure, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
It is noted that, in this document, relational terms such as "first" and "second," and the like, may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising a … …" does not exclude the presence of another identical element in a process, method, article, or apparatus that comprises the element.
The foregoing are merely exemplary embodiments of the present disclosure, which enable those skilled in the art to understand or practice the present disclosure. 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 disclosure. Thus, the present disclosure 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.
Claims (11)
1. A method for laser SLAM closed-loop mapping is characterized by comprising the following steps:
acquiring laser sensing data of a moving object;
initializing a grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame;
determining a target key frame in the subsequent laser frames according to the subsequent laser frames positioned after the initial laser frame time sequence in the laser sensing data, wherein the difference between the target key frame and the last key frame existing in the grid map in at least one of translation and rotation is larger than a set threshold value;
inserting the target keyframe into the grid map;
performing loop matching search in the grid map within a preset distance threshold range of the target key frame; and/or monitoring whether a matching instruction for the marked loop back frame is received;
and responding to a matching result existing in the loop matching search or receiving the matching instruction, performing closed-loop graph optimization processing, and constructing and obtaining a target map perceived by the moving object in the moving process.
2. The method of claim 1, wherein determining a target keyframe in a subsequent laser frame of the laser sensory data that follows the initial laser frame timing comprises:
performing grid filtering processing on each laser frame in the subsequent laser frames to obtain point cloud data of the subsequent laser frames;
carrying out position matching on the point cloud data of the subsequent laser frame and the grid map to obtain corresponding pose information of the point cloud data of the subsequent laser frame in a coordinate system of the grid map;
and determining whether each laser frame is a target key frame or not according to the difference between the pose information of each laser frame in the subsequent laser frames and the pose information of the last laser frame.
3. The method according to claim 2, characterized in that the pose information comprises: position information and attitude angle information; the setting of the threshold includes: a first set threshold for defining a distance gap and a second set threshold for defining an angle gap;
determining whether the current laser frame is a target key frame according to the difference between the pose information of the current laser frame and the pose information of the previous laser frame, including:
calculating the translation distance between the current laser frame and the last laser frame according to the respective position information of the current laser frame and the last laser frame;
calculating a rotation angle between the current laser frame and the previous laser frame according to the respective attitude angle information of the current laser frame and the previous laser frame;
determining that the current laser frame is a target key frame under the condition that a preset condition is met, wherein the preset condition comprises at least one of the following conditions: the translation distance is greater than the first set threshold, or the rotation angle is greater than the second set threshold.
4. The method of claim 2, wherein inserting the target keyframe into the grid map comprises:
and inserting the point cloud data of the target key frame into the grid map based on the corresponding target pose information.
5. The method according to any one of claims 1-4, wherein the set threshold is determined based on computational resource consumption and a pre-set accuracy of mapping, and the pre-set distance threshold range is associated with an error in the laser sensing data matching.
6. The method of claim 1, wherein performing a loop matching search in the grid map within a preset distance threshold of the target keyframe comprises:
determining whether a loop frame of the target key frame exists in key frames within a preset distance range of the target key frame in the grid map, wherein the loop frame and the target key frame are used for representing the same place;
determining that a matching result exists in the loop matching search under the condition that a loop frame exists;
and in the case that the loop back frame does not exist, determining that the loop back matching search does not have a matching result.
7. The method of claim 1, wherein initializing a grid map according to an initial laser frame in the laser sensing data to obtain an initial key frame comprises:
initializing a coordinate system origin of the grid map under the condition that the initial laser frame is received;
performing grid filtering processing on the initial laser frame to obtain point cloud data of the initial laser frame;
carrying out position matching on the point cloud data of the initial laser frame and the grid map to obtain position and pose information corresponding to the point cloud data of the initial laser frame in a coordinate system of the grid map;
and inserting the initial laser frame into the grid map according to the corresponding pose information to obtain an initial key frame.
8. An apparatus for closed-loop mapping of laser SLAM, comprising:
the sensing data acquisition module is used for acquiring laser sensing data of a moving object;
the map initialization module is used for initializing the grid map according to the initial laser frame in the laser sensing data to obtain an initial key frame;
a keyframe determination module, configured to determine, for a subsequent laser frame in the laser sensor data that is located after the initial laser frame timing, a target keyframe in the subsequent laser frame, where a difference between the target keyframe and a previous keyframe existing in the grid map at least one of translation and rotation is greater than a set threshold;
the map updating module is used for inserting the target key frame into the grid map;
the closed-loop condition detection module is used for performing loop matching search in the grid map within a preset distance threshold range of the target key frame; and/or monitoring whether a matching instruction for the marked loop back frame is received;
and the closed-loop processing module is used for responding to the matching result existing in the loop matching search or receiving the matching instruction, performing closed-loop graph optimization processing, and constructing and obtaining a target map perceived by the moving object in the moving process.
9. A mobile robot, characterized in that a set of instructions is stored, which are executed by the mobile robot to implement the method of any of claims 1-7, or which comprise the apparatus of claim 8.
10. An electronic device is characterized by comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory are communicated with each other through the communication bus;
a memory for storing a computer program;
a processor for implementing the method of any one of claims 1 to 7 when executing a program stored on a memory.
11. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out the method of any one of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211192389.5A CN115655291A (en) | 2022-09-28 | 2022-09-28 | Laser SLAM closed-loop mapping method and device, mobile robot, equipment and medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211192389.5A CN115655291A (en) | 2022-09-28 | 2022-09-28 | Laser SLAM closed-loop mapping method and device, mobile robot, equipment and medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115655291A true CN115655291A (en) | 2023-01-31 |
Family
ID=84984642
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211192389.5A Pending CN115655291A (en) | 2022-09-28 | 2022-09-28 | Laser SLAM closed-loop mapping method and device, mobile robot, equipment and medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115655291A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117451035A (en) * | 2023-12-25 | 2024-01-26 | 江苏中科重德智能科技有限公司 | Method and system for self-adaptively and automatically updating map by laser slam |
-
2022
- 2022-09-28 CN CN202211192389.5A patent/CN115655291A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117451035A (en) * | 2023-12-25 | 2024-01-26 | 江苏中科重德智能科技有限公司 | Method and system for self-adaptively and automatically updating map by laser slam |
| CN117451035B (en) * | 2023-12-25 | 2024-02-27 | 江苏中科重德智能科技有限公司 | Method and system for self-adaptively and automatically updating map by laser slam |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110335316B (en) | Depth information-based pose determination method, device, medium and electronic equipment | |
| CN110988912B (en) | Road target and distance detection method, system and device for automatic driving vehicle | |
| CN105667518B (en) | The method and device of lane detection | |
| KR101725060B1 (en) | Apparatus for recognizing location mobile robot using key point based on gradient and method thereof | |
| CN107329490B (en) | Unmanned aerial vehicle obstacle avoidance method and unmanned aerial vehicle | |
| CN108955718B (en) | Visual odometer and positioning method thereof, robot and storage medium | |
| US9071829B2 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
| CN102426019B (en) | Unmanned aerial vehicle scene matching auxiliary navigation method and system | |
| WO2022007776A1 (en) | Vehicle positioning method and apparatus for target scene region, device and storage medium | |
| CN107167826B (en) | Vehicle longitudinal positioning system and method based on variable grid image feature detection in automatic driving | |
| CN110807350A (en) | System and method for visual SLAM for scan matching | |
| WO2019196476A1 (en) | Laser sensor-based map generation | |
| CN113568435B (en) | Unmanned aerial vehicle autonomous flight situation perception trend based analysis method and system | |
| CN110992424B (en) | Positioning method and system based on binocular vision | |
| CN111161334B (en) | Semantic map construction method based on deep learning | |
| CN112379681A (en) | Unmanned aerial vehicle obstacle avoidance flight method and device and unmanned aerial vehicle | |
| CN114217665A (en) | Camera and laser radar time synchronization method, device and storage medium | |
| CN115655291A (en) | Laser SLAM closed-loop mapping method and device, mobile robot, equipment and medium | |
| CN114077249B (en) | Operation method, operation equipment, device and storage medium | |
| CN111812978A (en) | Cooperative SLAM method and system for multiple unmanned aerial vehicles | |
| CN110864670B (en) | Method and system for acquiring position of target obstacle | |
| CN112733971A (en) | Pose determination method, device and equipment of scanning equipment and storage medium | |
| CN112184906A (en) | Method and device for constructing three-dimensional model | |
| CN112380933A (en) | Method and device for identifying target by unmanned aerial vehicle and unmanned aerial vehicle | |
| Alliez et al. | Indoor localization and mapping: Towards tracking resilience through a multi-slam approach |
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 |