Local map construction method and device, storage medium and robot

Abstract

The application relates to a local map construction method and device, a storage medium and a robot. The pose data of the robot is determined according to sensor data of the robot during movement. Cloud data, odometer data, position data and pose data at the same time are encapsulated into a frame of data. Key frames are selected from the frame of data. The pose data of each key frame is optimized by constructing loop constraint relationships, inter-frame constraint relationships and position constraint relationships of the key frames, so that the precision of the constructed map is improved. The optimized pose data of the key frames, the point cloud data, the position data and the odometer data are stored in a preset database. When a local map construction instruction carrying a target frame identifier is received, the corresponding key frame identifier can be acquired according to the target frame identifier. The optimized pose data and the point cloud data of the corresponding key frame are read from the database to quickly generate a local map of an environment where the robot is located, so that the construction efficiency of the local map is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robots, and in particular to a local map construction method and device, a storage medium and a robot. BACKGROUND

[0002] With the continuous expansion of the application field of robots, robots can realize positioning and navigation in unknown environments by identifying and intelligently analyzing the external environment during work. Specifically, the robot starts moving from an unknown position in an unknown environment, and performs self-positioning according to position estimation and data collected by sensors and other devices arranged in the body during movement, while constructing a map to realize navigation.

[0003] In the prior art, when a local map is obtained, the local map is usually obtained by directly cutting from a complete map, and the precision of the obtained local map is low and the obtaining is inconvenient. SUMMARY

[0004] The embodiments of the present application provide a local map construction method, device, storage medium and robot, which can improve the construction efficiency and precision of the local map.

[0005] In a first aspect, the embodiments of the present application provide a local map construction method, comprising the following steps:

[0006] Obtain sensor data of the robot during movement, determine pose data of the robot according to the sensor data, and encapsulate point cloud data, odometer data, position data and pose data at the same time into a frame of data; wherein the sensor data comprises point cloud data, odometer data and position data; each frame of data is provided with a corresponding frame identifier;

[0007] Determine a plurality of key frames according to a preset key frame determination strategy, and obtain pose data of each key frame;

[0008] Obtain historical key frame data of the robot, and construct a loop constraint relationship of the plurality of key frames based on the similarity of the key frame data and the historical key frame data;

[0009] Obtain an inter-frame constraint relationship of adjacent key frames in the plurality of key frames and a position constraint relationship of the plurality of key frames based on the position data;

[0010] Optimize the pose data of the plurality of key frames based on the loop constraint relationship of the plurality of key frames, the inter-frame constraint relationship of adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames, and obtain optimized pose data of the plurality of key frames;

[0011] store the optimized pose data, the point cloud data, the position data, and the wheel speed meter data of the plurality of key frames to a preset database; each key frame is provided with a corresponding key frame identifier;

[0012] In response to a local map construction instruction carrying a target frame identifier, acquire the key frame identifier corresponding to the target frame identifier, acquire the optimized pose data and the point cloud data corresponding to the key frame identifier from the database, and generate a local map based on the optimized pose data and the point cloud data of the key frame.

[0013] In a second aspect, an embodiment of the present application provides a local map construction device, comprising:

[0014] A data acquisition module is configured to acquire sensor data of a robot during movement, determine pose data of the robot according to the sensor data, and encapsulate point cloud data, wheel speed meter data, position data, and pose data at the same time into one frame of data; the sensor data includes point cloud data, wheel speed meter data, and position data; each frame of data is provided with a corresponding frame identifier.

[0015] A key frame determination module is configured to determine a plurality of key frames according to a preset key frame determination strategy, and acquire pose data of each key frame.

[0016] A loop constraint construction module is configured to acquire historical key frame data of the robot, and construct a loop constraint relationship of the plurality of key frames based on a similarity of the key frame data and the historical key frame data.

[0017] A constraint relationship acquisition module is configured to acquire an inter-frame constraint relationship of adjacent key frames in the plurality of key frames and a position constraint relationship of the plurality of key frames based on the position data.

[0018] A pose optimization module is configured to optimize pose data of the plurality of key frames based on a loop constraint relationship of the plurality of key frames, an inter-frame constraint relationship of adjacent key frames in the plurality of key frames, and a position constraint relationship of the plurality of key frames, and obtain optimized pose data of the plurality of key frames.

[0019] A storage module is configured to store the optimized pose data, the point cloud data, the position data, and the wheel speed meter data of the plurality of key frames to a preset database; each key frame is provided with a corresponding key frame identifier.

[0020] A map generation module is configured to, in response to a local map construction instruction carrying a target frame identifier, acquire the key frame identifier corresponding to the target frame identifier, acquire the optimized pose data and the point cloud data corresponding to the key frame identifier from the database, and generate a local map based on the optimized pose data and the point cloud data of the key frame.

[0021] In a third aspect, an embodiment of the present application provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the steps of the local map construction method according to any one of the preceding aspects.

[0022] In a fourth aspect, an embodiment of the present application provides a robot, which comprises a laser radar, a wheel speed meter, a positioning module, a memory, a processor and a computer program stored in the memory and executable by the processor.

[0023] The laser radar is configured to acquire point cloud data of the robot during movement.

[0024] The wheel speed meter is configured to acquire wheel speed meter data of the robot during movement.

[0025] The positioning module is configured to acquire position data of the robot during movement.

[0026] The processor implements the steps of the local map construction method according to any one of the preceding aspects when executing the computer program.

[0027] In the embodiment of the present application, the pose data of the robot is determined according to the sensor data of the robot during movement, the cloud data, the wheel speed meter data, the position data and the pose data at the same time are encapsulated as a frame of data, the key frames are selected from the frame of data, the loop constraint relationship, the inter-frame constraint relationship and the position constraint relationship of the key frames are constructed, the pose data of each key frame is optimized, and the accuracy of the constructed map is improved; the optimized pose data of the key frames, the point cloud data, the position data and the wheel speed meter data are stored in a preset database, when a local map construction instruction carrying a target frame identifier is received, the corresponding key frame identifier can be acquired according to the target frame identifier, the optimized pose data and the point cloud data of the corresponding key frame are read from the database to quickly generate a local map of the environment where the robot is located, and the construction efficiency of the local map is improved.

[0028] For better understanding and implementation, the present application is described in detail below with reference to the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0029] Figure 1 FIG. 1 is a structural schematic diagram of a robot according to an embodiment of the present application;

[0030] Figure 2 FIG. 2 is a flowchart of a local map construction method according to an embodiment of the present application;

[0031] Figure 3 FIG. 3 is a flowchart of a local map construction method according to another embodiment of the present application;

[0032] Figure 4A flow chart of a local map construction method in another embodiment of the present application;

[0033] Figure 5 A flow chart of a local map construction method in another embodiment of the present application;

[0034] Figure 6 A flow chart of a local map construction method in another embodiment of the present application;

[0035] Figure 7 A flow chart of a local map construction method in another embodiment of the present application;

[0036] Figure 8 A flow chart of a local map construction method in another embodiment of the present application;

[0037] Figure 9 A structural schematic diagram of a local map construction device in an embodiment of the present application;

[0038] Figure 10 A structural schematic diagram of a robot in an embodiment of the present application. DETAILED DESCRIPTION

[0039] In order to make the purposes, technical solutions and advantages of the present application clearer, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0040] It should be clear that the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0041] The terms used in the present application are merely for the purpose of describing the specific embodiments, and are not intended to limit the present application. The singular forms "a", "an" and "the" used in the present application and the appended claims are also intended to include the plural forms, unless the context clearly indicates otherwise. It should also be understood that the term "and / or" used herein means and includes any or all possible combinations of one or more associated listed items.

[0042] The following description refers to the accompanying drawings. Unless otherwise noted, like elements in different drawings have the same or similar notation, and the same or similar elements in different drawings are represented with the same or similar references. The following description of exemplary embodiments is not representative of all possible embodiments consistent with the present application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of the present application as detailed in the appended claims. In the description of the present application, it should be understood that the terms "first", "second", "third", etc., are used merely as identifiers to distinguish between similar objects, and are not necessarily used to describe a particular order or sequence, nor are they to be interpreted as indicating or implying relative importance. The specific meaning of the above terms in the present application can be understood according to the specific circumstances by those of ordinary skill in the art.

[0043] In addition, in the description of the present application, "several" means two or more, unless otherwise specified. The association between the associated objects described by "and / or" indicates that there can be three relationships, for example, A and / or B can represent the three cases of A alone, A and B together, and B alone. The character " / " generally represents an "or" relationship between the associated objects before and after it.

[0044] The partial map construction method of the present application can be applied to a map construction device 101, which can be applied to the fields of robots, self-driving cars, etc., for constructing maps for robots, self-driving cars, and realizing the automatic driving of robots and self-driving cars.

[0045] Please refer to Figure 1 , which is a schematic diagram of the application scenario of the map construction device 101 of the present application applied in a robot 100, as Figure 1 shown, a robot 100 of the present application includes a map construction device 101, a laser radar 102, a wheel speed meter 103, and a positioning module 104.

[0046] The laser radar 102 is used to collect laser point cloud data; preferably, the laser radar 102 of the present application can use a multi-line laser radar, which is a laser rotating ranging radar that simultaneously transmits and receives multiple beams of laser. The multi-line laser radar can capture more object details and form a clearer point cloud graph, which can effectively improve the accuracy of robot positioning and mapping.

[0047] The wheel speed meter 103 is a rotary sensor used to measure the rotation angle, rotation speed, and rotation direction. The wheel speed meter 103 calculates the distance and angle traveled by the robot through the number of revolutions of the motor.

[0048] The positioning module 104 is used to obtain the position data of the robot. In the embodiments of the present application, the positioning module 104 can be a GPS positioning module.

[0049] The map construction device 101 is configured to perform the steps of the local map construction method of the present application, so as to quickly construct a local map of an environment where the robot is located, and improve the local map construction efficiency.

[0050] Please refer to Figure 2 The embodiment of the present application provides a local map construction method, and the method comprises the following steps:

[0051] S101: acquiring sensor data of a robot in a moving process, determining pose data of the robot according to the sensor data, and encapsulating point cloud data, odometer data, position data and pose data at the same time into a frame of data; wherein the sensor data comprises point cloud data, odometer data and position data;

[0052] The odometer data is data collected by an odometer of the robot. The odometer is a rotary sensor for measuring the rotation angle, rotation speed and rotation direction, and the distance and angle walked by the robot are calculated by the number of rotations of the motor. The odometer data can include the rotation angle, rotation speed and rotation direction.

[0053] The position data can be data collected by a positioning module of the robot, and the positioning module can be a GPS positioning module. The position data can be GPS data obtained by the GPS positioning module.

[0054] When the pose data of the robot is acquired, the pose data of the robot can be acquired according to the sensor data of each sensor. Specifically, in the embodiment of the present application, the initial pose of the robot can be estimated according to the odometer data, and in the subsequent moving process, the pose of the robot can be calculated according to the following formula: T_1=T_0*delta_odom; wherein T_1 is the pose of the robot, T_0 is the initial pose of the robot, and delta_odom is the sensor data.

[0055] The point cloud data, odometer data, position data and pose data of the present application can be different in time due to different sampling frequencies of the equipment. Therefore, after the point cloud data, odometer data, position data and pose data are acquired, the point cloud data, odometer data, position data and pose data can be time-synchronized, so that the time of the point cloud data, odometer data, position data and pose data is consistent, thereby facilitating the encapsulation of the point cloud data, odometer data, position data and pose data at the same time into a frame of data.

[0056] S102: determining a plurality of key frames according to a preset key frame determination strategy, and acquiring pose data of each key frame;

[0057] During the movement of the robot, the scanning frequency of the laser radar is usually 10 hz, if the data of each frame (10 frames of radar per second at 10 hz) is saved, the memory consumption and the operation amount of the algorithm will be greatly increased when constructing the map, which affects the operation efficiency of the algorithm, therefore, in the embodiment of the application, a plurality of key frames are selected from the plurality of frames of data according to the key frame strategy, so that a part of the key frames participate in the operation, and the local map construction efficiency is improved. The key frame determination strategy in the application can be set according to the actual needs of the user.

[0058] The pose data of each key frame can be directly obtained according to the pose data of each frame, or preferably, the pose data of each key frame can also be obtained after laser matching of the point cloud data and the local point cloud map.

[0059] S103: Obtain historical key frame data of the robot, and construct a loop constraint relationship of the plurality of key frames based on the similarity of the key frame data and the historical key frame data.

[0060] The pose of the robot is often a recursive process, that is, the pose of the current frame is calculated from the pose of the previous frame; because the error of the pose is actually the cumulative error of the plurality of frames of pose, if the pose of the fifth key frame is calculated, the error of the pose of the fifth key frame has been accumulated in the poses of the previous four key frames. However, if the pose of the fifth key frame can be obtained by calculating the pose of the first key frame or the pose of the second key frame, the error of the pose of the fifth key frame obtained in this way will be smaller, and the detection of the pose constraint relationship between the current frame and a certain frame before the current frame is called loop detection. In the embodiment of the application, the loop constraint relationship of the plurality of key frames is constructed based on the relative pose transformation relationship between the current key frame and the corresponding historical key frame.

[0061] When constructing the loop constraint relationship of the plurality of key frames, the loop constraint relationship can include the identifier of the current key frame, the identifier of the historical key frame, and the relative pose transformation relationship.

[0062] S104: Obtain the inter-frame constraint relationship of adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames based on the position data.

[0063] The inter-frame constraint relationship can be constructed based on the pose transformation relationship between the current key frame and the adjacent key frame, and specifically, the inter-frame constraint relationship can be the difference value of the poses of the adjacent key frames.

[0064] The positioning module is used to collect position data of an absolute position of the robot, and the position data can be used to add position constraints to pose data of the robot. Specifically, a conversion relationship between position data in the pose data and position data of the positioning module is determined based on a comparison between the position data in the pose data and the position data of the positioning module, and the conversion relationship may, for example, be a difference between the two pieces of position data.

[0065] S105: based on the loop constraint relationship of the plurality of key frames, the inter-frame constraint relationship of adjacent key frames in the plurality of key frames, and the position constraint relationship of the plurality of key frames, optimizing the pose data of the plurality of key frames to obtain optimized pose data of the plurality of key frames;

[0066] Specifically, the pose data of the plurality of key frames can be optimized by a predetermined optimizer. The predetermined optimizer is a tool that guides each parameter of an objective function to update to a suitable size in a correct direction during a deep learning back propagation process, so that each updated parameter causes the value of the objective function to continuously approach a global minimum. Since the object of optimization is only the pose corresponding to the key frame when the initial poses corresponding to the plurality of key frames are optimized, and the optimization of the pose corresponding to the key frame is limited by the loop constraint relationship of the key frame, the inter-frame constraint relationship of adjacent key frames, and the position constraint relationship, the state dimension of the optimization is low, and the constraint relationship is strong. Therefore, the pose data of the plurality of key frames can be better optimized, and the problem of limited optimization and poor optimization effect in the prior art can be avoided.

[0067] By optimizing the pose data of the key frames using the loop constraint relationship, the inter-frame constraint relationship, and the position constraint relationship, the optimized pose data can better reflect the actual motion of the robot, and the map constructed based on the pose data of the key frames has higher quality and precision.

[0068] S106: storing the optimized pose data of the plurality of key frames, the point cloud data, the position data, and the odometer data in a predetermined database; wherein each key frame is provided with a corresponding key frame identifier;

[0069] The key frame identifier can correspond to the key frame one by one, and a unique key frame can be determined according to the key frame identifier. The key frame identifier can be a number or a string.

[0070] S107: in response to a local map construction instruction carrying a target frame identifier, obtaining a key frame identifier corresponding to the target frame identifier, obtaining optimized pose data and point cloud data corresponding to the key frame identifier from the database, and generating a local map based on the optimized pose data of the key frame and the point cloud data.

[0071] Specifically, the step of obtaining the key frame identifier corresponding to the target frame identifier specifically comprises:

[0072] In response to a local map loading command carrying a frame identifier, a key frame identifier for at least one key frame corresponding to the frame identifier is determined based on a preset search range.

[0073] In this embodiment, the robot's pose data is determined based on sensor data during its movement. Cloud data, wheel velocity data, position data, and pose data at the same moment are encapsulated into a single frame. Keyframes are selected from this frame, and the pose data of each keyframe is optimized by constructing closure constraints, inter-frame constraints, and position constraints to improve the accuracy of the constructed map. By storing the optimized pose data, point cloud data, position data, and wheel velocity data of the keyframes into a preset database, when a local map construction instruction carrying a target frame identifier is received, the corresponding keyframe identifier can be obtained based on the target frame identifier. The optimized pose data and point cloud data of the corresponding keyframes can be read from the database to quickly generate a local map of the robot's environment, thereby improving the efficiency of local map construction.

[0074] In step S101, since the point cloud data collected by the LiDAR during the robot's movement has certain distortions, if the map is directly constructed based on the collected point cloud data, the accuracy of the constructed map will be affected.

[0075] Therefore, as Figure 3 As shown, in a preferred embodiment, before encapsulating the point cloud data, wheel velocity data, position data, and pose data at the same moment into a single frame of data, the following steps are further included:

[0076] S201: Obtain the relative motion information between the current frame and adjacent frames, the time interval between the current frame and the starting frame, and the scanning cycle of the lidar;

[0077] S202: Based on the relative motion information, the time interval between the current frame and the starting frame, and the scanning cycle of the lidar, obtain the distortion parameters of the current frame point cloud data;

[0078] In this application, the distortion parameter is obtained by multiplying the relative motion information by the time ratio. The time ratio is calculated based on the time interval between the current frame and the starting frame and the scanning cycle of the lidar. Specifically, the time ratio is obtained in the following manner:

[0079] α=t_k / T

[0080] Where α is the time ratio, t_k is the time interval between the current frame and the starting frame, and T is the scanning cycle of the lidar.

[0081] S203: subtract the distortion parameters of the previous frame point cloud data from the current frame point cloud data to obtain the de-distorted point cloud data;

[0082] Specifically, the de-distorted point cloud data is obtained in the following manner:

[0083] P_k_new=P_k-α*ΔT

[0084] Wherein, P_k_new is the de-distorted point cloud data of the current frame, P_k is the point cloud data of the current frame.

[0085] The de-distorted point cloud data, the wheel speed meter data and the position data at the same time are encapsulated into a frame of data.

[0086] In the present application, the point cloud data of the current frame is de-distorted by combining the motion information of adjacent frames, the scanning period of the laser radar and the current frame time information, so that the obtained point cloud data can more truly reflect the actual motion of the robot, and the accuracy of the constructed map is improved.

[0087] The position data obtained in step S101 includes x1, y1 and z1, wherein x1, y1 and z1 are position data in x, y and z directions. The position data collected by the prior art usually has certain error. When the error of the position data is large, if the data is still used for mapping and positioning, the accuracy of mapping and positioning will be affected.

[0088] Therefore, as Figure 4 shown, in a preferred embodiment, before the point cloud data, the wheel speed meter data, the position data and the pose data at the same time are encapsulated into a frame of data, the following steps are further included:

[0089] S301: obtaining the position increment value between adjacent frames according to the position data of the current frame and the adjacent frame adjacent to the current frame;

[0090] If the position data is GPS data, before obtaining the position increment value between adjacent frames, the position data of the current frame and the adjacent frame can be converted to WSG84 coordinate system first, and then the position increment value is calculated according to the position data of WSG84 coordinate system.

[0091] S302: if the position increment value is greater than the preset position increment threshold, discarding the position data of the current frame, and encapsulating the point cloud data and the wheel speed meter data of the current frame into a frame of data.

[0092] When the position increment value of the two adjacent frames is greater than the preset position increment threshold, it is determined that the current position data is unstable, at this time, it is discarded to avoid affecting the subsequent mapping positioning. The position increment threshold can be set according to actual needs, and in the embodiment of the application, the position increment threshold is twice the last position increment corresponding to the adjacent frame of the current frame.

[0093] As shown in Figure 5 in an embodiment, after obtaining the position increment value between the adjacent frames, the following steps can also be included:

[0094] S303: If the position increment value is less than or equal to the preset position increment threshold, the wheel speedometer increment value is obtained according to the wheel speedometer data of the current frame and the adjacent frame adjacent to the current frame;

[0095] S304: If the difference between the position increment value and the wheel speedometer increment value is less than the preset increment threshold, the covariance value of x1 and y1 is less than the preset covariance, and x1 and y1 are greater than the preset threshold, the point cloud data, the wheel speedometer data and the position data of the current frame are encapsulated as a frame of data;

[0096] The increment threshold can be set according to user needs, for example, it can be set to 0.2 meters.

[0097] The covariance threshold can be set according to user needs, for example, it can be set to 2.

[0098] The preset threshold can be set according to user needs, for example, it can be set to 1e-6.

[0099] S305: Otherwise, discard the position data of the current frame, and encapsulate the point cloud data and the wheel speedometer data of the current frame as a frame of data.

[0100] In the present application, only when the position increment value of the current frame is less than or equal to the preset position increment threshold, the difference between the position increment value and the wheel speedometer increment value of the current frame is less than the preset increment threshold, the covariance value of x1 and y1 of the current frame is less than the preset covariance, and x1 and y1 of the current frame are greater than the preset threshold, it is determined that the position data of the current frame is reliable, at this time, the point cloud data, the wheel speedometer data and the position data of the current frame are encapsulated as a frame of data, otherwise the position data of the current frame is discarded, and the point cloud data and the wheel speedometer data of the current frame are encapsulated as a frame of data, thereby reducing the negative impact of unstable position data on building a map and improving the accuracy of the built map.

[0101] The pose data in step S101 can be the initial pose data of the robot estimated according to the wheel speedometer data, or, in a preferred embodiment, the pose data is the updated pose data after laser matching of the point cloud data and the local map.

[0102] Specifically, as shown in Figure 6 After determining the pose data of the robot according to the sensor data, the method further comprises the following steps:

[0103] S401: Based on the position data of the current frame, point cloud data of key frames within a preset distance range is obtained;

[0104] Specifically, based on the preset distance range, the point cloud data of the key frames within the distance range is obtained with the position of the current frame as the center. The distance range can be set according to requirements, for example, it can be set to within 10 meters.

[0105] S402: Based on the point cloud data of the key frames within the preset distance range, a local point cloud map is constructed;

[0106] When constructing the local point cloud map, the point cloud data of the key frames within the preset distance range can be spliced to obtain the local point cloud map.

[0107] S403: The point cloud data of the current frame is matched with the local point cloud map, and the pose data of the current frame is updated.

[0108] Specifically, a laser matching algorithm such as LOAM, LEGO-LOAM, A-LOAM, V-LOAM, etc. can be used to match the point cloud data with the local point cloud map.

[0109] In the embodiments of the present application, the point cloud data of the current frame is matched with the local point cloud map using the LOAM algorithm. Specifically,

[0110] The curvature of each point cloud in the point cloud data is calculated in the following manner:

[0111]

[0112] wherein, is the curvature of the point cloud k, P k is the point cloud data of the point cloud k, P k+i is the point cloud data of the point cloud k+, i∈[-5,5], the curvature of the current point cloud is calculated according to the point cloud data of the 5 points before and after each point cloud, reducing the computational load of the curvature.

[0113] According to the curvature of each point cloud, the point cloud is determined to be a line point or a surface point, the line point and the surface point are matched with the local map respectively, the pose transformation relationship between the line point and the local map and the pose transformation relationship between the surface point and the local map are obtained, and the pose data of the current frame is updated according to the pose transformation relationship.

[0114] In step S102, the step of determining multiple keyframes according to the preset keyframe determination strategy specifically includes:

[0115] If the current frame is the first frame, then the current frame is used as the keyframe;

[0116] Based on the difference between the pose data of the current frame and the pose data of the previous keyframe adjacent to the current frame, obtain the pose increment value between the current frame and the previous keyframe.

[0117] If the pose increment value is greater than the pose increment threshold, the current frame is taken as the key frame.

[0118] Specifically, the timestamp of the current frame can be used to determine whether the current frame is the first frame.

[0119] The pose increment value can be obtained from the difference between the pose data of the current frame and the adjacent previous keyframe. The pose data can include position and angle, and the position increment value can include the position increment value and the angle increment value. The position increment value can be obtained from the position difference between the current frame and the adjacent previous keyframe, and the angle increment value can be obtained from the angle difference between the current frame and the adjacent previous keyframe.

[0120] The pose increment threshold may include a position increment threshold and an angle increment threshold. Specifically, when the position increment value is greater than the position increment threshold and the angle increment value is greater than the angle increment threshold, it is determined that the pose increment value is greater than the pose increment threshold, and the current frame is then used as a key frame. Alternatively, in another embodiment, when the position increment value is greater than the position increment threshold, or when the angle increment value is greater than the angle increment threshold, it is determined that the pose increment value is greater than the pose increment threshold, and the current frame is then used as a key frame.

[0121] In this embodiment, the pose data of each frame is updated by laser matching point cloud data with local point cloud map, and multiple key frames are selected from them according to the preset key frame determination strategy. This reduces the amount of data computation and improves the efficiency of local map construction, while also improving the accuracy of local map construction.

[0122] In step S103, before constructing the loop closure constraint relationship of multiple key frames, it is necessary to perform loop closure detection on the key frame data and the historical key frame data. Only when the loop closure detection is successful can the loop closure constraint relationship of multiple key frames be constructed.

[0123] like Figure 7 As shown, specifically, the steps for constructing the closure constraint relationship of the multiple keyframes include:

[0124] S501: Based on the descriptor algorithm, generate point cloud descriptors for point cloud data of each key frame and multiple historical key frames;

[0125] The point cloud descriptor algorithm can employ a scan-context, a SHOT, or the like point cloud descriptor algorithm to generate a point cloud descriptor of the point cloud data. Specifically, in the embodiments of the present application, a scan-context algorithm is used to generate a point cloud descriptor of the point cloud data of each key frame and a plurality of historical key frames.

[0126] The scan-context algorithm divides the 3D physical space into a 2D "block" space by a Ring and a Sector, each block being uniquely determined by the Ring and the Sector, and the height, number, and the like of the point cloud in the block being stored in each block. The Ring is similar to the angle in polar coordinates, and the Sector is similar to the length in polar coordinates.

[0127] Preferably, a point cloud Ring-Key descriptor of each key frame and a plurality of historical key frames can also be generated. The point cloud Ring-Key is a rotation-invariant descriptor, and is specifically represented as a one-bit array, each element in the array being an encoding value of a corresponding Ring. Compared with the Ring-Sector matrix, the Ring-Key descriptor compresses and reduces the dimension of the data, and is more efficient in matching.

[0128] S502: Calculate the similarity of the point cloud descriptors of each key frame and a plurality of historical key frames. When the similarity of the key frame and the historical key frame is greater than a similarity threshold, and the time interval of the key frame and the historical frame is greater than a time threshold, the relative pose transformation relationship between the key frame and the historical key frame is obtained.

[0129] When the scan-context algorithm is used to generate the point cloud descriptor of the point cloud data of each key frame and a plurality of historical key frames, the similarity can be calculated according to the Ring-Key descriptor of the key frame and the Ring-Key descriptor of the historical key frame.

[0130] The similarity can be calculated by using an existing similarity calculation formula. In the present application, the cosine distance can be used to calculate the similarity of the Ring-Key descriptor of the key frame and the Ring-Key descriptor of the historical key frame.

[0131] The time interval of the key frame and the historical frame can be determined according to the time stamp of the key frame and the historical frame. When the similarity of the key frame and the historical key frame is greater than a similarity threshold, and the time interval of the key frame and the historical frame is greater than a time threshold, it is determined that the loop detection is successful, otherwise, it is determined that the loop detection fails.

[0132] S503: Construct a loop constraint relationship of the plurality of key frames according to the relative pose transformation relationship.

[0133] By constructing a point cloud descriptor of the key frame and the historical key frame, similarity of the key frame and the historical key frame is calculated based on the point cloud descriptor, whether the loop detection is successful is determined according to the similarity of the key frame and the historical key frame and the interval time, when the loop detection is successful, the loop constraint relationship of the key frame is constructed according to the relative pose transformation relationship between the key frame and the historical key frame, thereby optimizing the pose data of the key frame, and improving the accuracy of the pose data of the key frame.

[0134] When the loop detection fails, the loop constraint relationship of the plurality of key frames can not be constructed, and only the inter-frame constraint relationship of adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames are used to optimize the pose data, or in another embodiment, the pose data can not be optimized.

[0135] As shown in Figure 8 In the embodiment of the present application, before the loop constraint relationship of the plurality of key frames is constructed based on the similarity of the key frame data and the historical key frame data, the following steps are further included:

[0136] S601: determining whether the loop detection is successful based on the similarity of the key frame data and the historical key frame data;

[0137] The step of loop detection can refer to the steps S501-S503 of the above-mentioned embodiment, which will not be repeated here. Figure 7

[0138] S602: if the loop detection is successful, constructing the loop constraint relationship of the plurality of key frames based on the similarity of the key frame data and the historical key frame data;

[0139] S603: if the loop detection fails and the number of position data reaches a preset position number threshold, obtaining the inter-frame constraint relationship of adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames based on the position data, and optimizing the pose data of the plurality of key frames based on the inter-frame constraint relationship of adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames, to obtain the optimized pose data of the plurality of key frames.

[0140] The position number threshold can be set according to the actual running condition of the robot, for example, it can be set to 100.

[0141] ​In the application, when loop detection is successful, the loop constraint relationship of multiple key frames, the inter-frame constraint relationship of adjacent key frames in the multiple key frames and the position constraint relationship of the multiple key frames are used to optimize the pose data. When loop detection fails, if the number of position data reaches a position number threshold, it indicates that the robot has traveled a distance. At this time, the pose data needs to be optimized in time, so as to reduce the cumulative error of the pose data. When the number of position data does not reach the position number threshold, it is considered that the cumulative error is small in a short time. At this time, the pose data can not be optimized, so as to reduce the data processing amount and resource occupation.

[0142] Please refer to Figure 9 The application also provides a local map construction device, comprising:

[0143] The data acquisition module 201 is configured to acquire sensor data of a robot in a moving process, determine pose data of the robot according to the sensor data, and encapsulate point cloud data, odometer data, position data and the pose data at the same time into one frame of data; wherein the sensor data comprises the point cloud data, the odometer data and the position data; each frame of data is provided with a corresponding frame identifier.

[0144] The key frame determination module 202 is configured to determine multiple key frames according to a preset key frame determination strategy, and acquire pose data of each key frame.

[0145] The loop constraint construction module 203 is configured to acquire historical key frame data of the robot, and construct a loop constraint relationship of the multiple key frames based on the similarity of the key frame data and the historical key frame data.

[0146] The constraint relationship acquisition module 204 is configured to acquire an inter-frame constraint relationship of adjacent key frames in the multiple key frames and a position constraint relationship of the multiple key frames based on the position data.

[0147] The pose optimization module 205 is configured to optimize the pose data of the multiple key frames based on the loop constraint relationship of the multiple key frames, the inter-frame constraint relationship of the adjacent key frames in the multiple key frames and the position constraint relationship of the multiple key frames, and obtain optimized pose data of the multiple key frames.

[0148] The storage module 206 is configured to store the optimized pose data of the multiple key frames, the point cloud data, the position data and the odometer data into a preset database; wherein each key frame is provided with a corresponding key frame identifier.

[0149] The map generation module 207 is configured to, in response to a local map construction instruction carrying a target frame identifier, acquire a key frame identifier corresponding to the target frame identifier, acquire optimized pose data and point cloud data corresponding to the key frame identifier from the database, and generate a local map based on the optimized pose data of the key frame and the point cloud data.

[0150] It should be noted that the local map construction device provided in the above embodiment is only used as an example to illustrate the division of the above functional modules when the local map construction method is performed. In actual application, the above functions can be completed by different functional modules according to needs, that is, the internal structure of the device is divided into different functional modules to complete all or part of the above-described functions. In addition, the local map construction device provided in the above embodiment and the local map construction method of the second embodiment belong to the same concept, and the implementation process is described in detail in the second embodiment, which will not be described here.

[0151] The application also provides a computer readable storage medium having a computer program stored thereon, the computer program being executed by a processor to implement the steps of the local map construction method according to any one of the above.

[0152] The application can adopt the form of a computer program product implemented on one or more storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing program code. The computer readable storage medium includes permanent and non-permanent, removable and non-removable media, and can be implemented by any method or technology. Information storage. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to: phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassette, magnetic tape disk storage or other magnetic storage devices or any other non-transmission medium that can be used to store information that can be accessed by a computing device.

[0153] Please refer to Figure 10 The application also provides a robot 300, which comprises a laser radar 301, a wheel speed meter 302, a positioning module 303, a memory 304, a processor 305, and a computer program stored in the memory 304 and executable by the processor 305.

[0154] The laser radar 301 is configured to acquire point cloud data of the robot during movement.

[0155] Preferably, the laser radar 301 of the present application can adopt a multi-line laser radar, which refers to a laser rotating ranging radar that simultaneously emits and receives multiple laser beams. The multi-line laser radar can capture more details of objects, the formed point cloud map is clearer, and the accuracy of robot positioning and mapping can be effectively improved.

[0156] The wheel speed meter 302 is configured to acquire wheel speed meter data of the robot during movement.

[0157] The positioning module 303 is configured to acquire position data of the robot during movement.

[0158] In the embodiment of the present application, the positioning module 304 can be a GPS positioning module.

[0159] The processor 305 implements the steps of the local map construction method according to any one of the above when executing the computer program.

[0160] The present application is not limited to the above-described embodiments, and various modifications or changes can be made to the present application without departing from the spirit and scope of the present application, and the present application is intended to include such modifications and changes if they fall within the scope of the claims of the present application and the equivalent technical scope.

Claims

1. A local map construction method characterized by, The method comprises the following steps: obtaining sensor data of the robot during movement, determining pose data of the robot according to the sensor data, and encapsulating point cloud data, wheel speed meter data, position data and pose data at the same time into a frame of data; wherein the sensor data comprises point cloud data, wheel speed meter data and position data; each frame of data is provided with a corresponding frame identifier; determining a plurality of key frames according to a preset key frame determination strategy, and obtaining pose data of each key frame; obtaining historical key frame data of the robot, and constructing a loop constraint relationship of the plurality of key frames based on the similarity of the key frame data and the historical key frame data; obtaining an inter-frame constraint relationship of adjacent key frames in the plurality of key frames and a position constraint relationship of the plurality of key frames based on the position data; optimizing the pose data of the plurality of key frames based on the loop constraint relationship of the plurality of key frames, the inter-frame constraint relationship of adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames, and obtaining optimized pose data of the plurality of key frames; storing the optimized pose data of the plurality of key frames, the point cloud data, the position data and the wheel speed meter data into a preset database; wherein each key frame is provided with a corresponding key frame identifier; in response to a local map construction instruction carrying a target frame identifier, obtaining a key frame identifier corresponding to the target frame identifier, obtaining optimized pose data and point cloud data corresponding to the key frame identifier from the database, and generating a local map based on the optimized pose data of the key frame and the point cloud data.

2. The local map building method according to claim 1, characterized by, Before encapsulating the point cloud data, the wheel speed meter data, the position data and the pose data at the same time into a frame of data, the method further comprises the following steps: obtaining relative motion information between a current frame and an adjacent frame, a time interval of the current frame relative to a starting frame and a scanning period of a laser radar; obtaining distortion parameters of current frame point cloud data according to the relative motion information, the time interval of the current frame relative to the starting frame and the scanning period of the laser radar; subtracting the distortion parameters of the previous frame point cloud data from the point cloud data of the current frame to obtain de-distorted point cloud data; encapsulating the de-distorted point cloud data, the wheel speed meter data and the position data at the same time into a frame of data.

3. The local map building method according to claim 1, wherein, Before encapsulating the point cloud data, the wheel speed meter data, the position data and the pose data at the same time into a frame of data, the method further comprises the following steps: obtaining a position increment value between adjacent frames according to the position data of the current frame and the adjacent frame adjacent to the current frame; if the position increment value is greater than a preset position increment threshold, discarding the position data of the current frame and encapsulating the point cloud data and the wheel speed meter data of the current frame into a frame of data.

4. The local map building method according to claim 3, wherein, The position data comprises x1, y1 and z1, wherein x1, y1 and z1 are position data in three different directions of x, y and z; after obtaining the position increment value between adjacent frames, the method further comprises the following steps: if the position increment value is less than or equal to the preset position increment threshold, obtaining a wheel speed increment value according to the wheel speed meter data of the current frame and the adjacent frame adjacent to the current frame; If a difference between the position increment value and the wheel speed increment value is less than a preset increment threshold, a covariance value of x1 and y1 is less than a preset covariance, and both x1 and y1 are greater than a preset threshold, the point cloud data, the wheel speed data and the position data of the current frame are encapsulated as a frame of data; Otherwise, the position data of the current frame is discarded, and the point cloud data and the wheel speed data of the current frame are encapsulated as a frame of data.

5. The local map building method according to claim 1, wherein, The step of determining the plurality of key frames according to the preset key frame determination strategy specifically comprises If the current frame is the first frame, the current frame is taken as a key frame; According to a difference between the pose data of the current frame and the pose data of a last key frame adjacent to the current frame, a pose increment value between the current frame and the last key frame is obtained; If the pose increment value is greater than a pose increment threshold, the current frame is taken as a key frame.

6. The local map building method according to claim 1, wherein, The step of constructing the loop constraint relationship of the plurality of key frames specifically comprises Based on a descriptor algorithm, a point cloud descriptor of the point cloud data of each key frame and a plurality of historical key frames is generated; Similarities of the point cloud descriptors of each key frame and the plurality of historical key frames are calculated, if a similarity of a key frame and a historical key frame is greater than a similarity threshold, and a time interval between the key frame and the historical frame is greater than a time threshold, a relative pose transformation relationship between the key frame and the historical key frame is obtained; According to the relative pose transformation relationship, a loop constraint relationship of the plurality of key frames is constructed.

7. The local map construction method according to claim 6, wherein, Before the loop constraint relationship of the plurality of key frames is constructed based on the similarity between the key frame data and the historical key frame data, the following steps are further included Based on the similarity between the key frame data and the historical key frame data, it is determined whether loop detection is successful; If the loop detection is successful, the loop constraint relationship of the plurality of key frames is constructed based on the similarity between the key frame data and the historical key frame data; If the loop detection fails, and a quantity of the position data reaches a preset position quantity threshold, an inter-frame constraint relationship of adjacent key frames in the plurality of key frames and a position constraint relationship of the plurality of key frames based on the position data are obtained, the pose data of the plurality of key frames is optimized based on the inter-frame constraint relationship of the adjacent key frames in the plurality of key frames and the position constraint relationship of the plurality of key frames, and optimized pose data of the plurality of key frames is obtained.

8. A local map construction device characterized by comprising: Comprise A data acquisition module is configured to acquire sensor data of a robot in a moving process, determine pose data of the robot according to the sensor data, and encapsulate point cloud data, wheel speed data, position data and pose data at the same time into a frame of data; wherein the sensor data comprises the point cloud data, the wheel speed data and the position data; each frame of data is provided with a corresponding frame identifier; A key frame determination module is configured to determine a plurality of key frames according to a preset key frame determination strategy, and obtain pose data of each key frame; A loop constraint construction module is configured to obtain historical key frame data of the robot, and construct a loop constraint relationship of the plurality of key frames based on the similarity between the key frame data and the historical key frame data. The constraint relationship acquisition module is configured to acquire interframe constraint relationships of adjacent key frames in the plurality of key frames and position constraint relationships of the plurality of key frames based on the position data; The pose optimization module is configured to optimize pose data of the plurality of key frames based on the loop constraint relationships of the plurality of key frames, the interframe constraint relationships of adjacent key frames in the plurality of key frames, and the position constraint relationships of the plurality of key frames, to obtain optimized pose data of the plurality of key frames; The storage module is configured to store the optimized pose data of the plurality of key frames, the point cloud data, the position data, and the odometry data in a preset database; each key frame is provided with a corresponding key frame identifier; The map generation module is configured to, in response to a local map construction instruction carrying a target frame identifier, acquire a key frame identifier corresponding to the target frame identifier, acquire optimized pose data and point cloud data corresponding to the key frame identifier from the database, and generate a local map based on the optimized pose data of the key frame and the point cloud data.

9. A computer readable storage medium having stored thereon a computer program, characterized in that: The computer program is executed by the processor to implement the steps of the local map construction method according to any one of claims 1-7.

10. A robot, characterized in that The computer program is executed by the processor to implement the steps of the local map construction method according to any one of claims 1-7. The computer program is executed by the processor to implement the steps of the local map construction method according to any one of claims 1-7. The computer program is executed by the processor to implement the steps of the local map construction method according to any one of claims 1-7. The computer program is executed by the processor to implement the steps of the local map construction method according to any one of claims 1-7. ​

Images