Map construction method, device and medium

Abstract

The present application provides a map construction method, device, and medium, which relate to the field of robot technology. Among them, the method includes: when the closed-loop detection condition is met, obtaining the image feature points of multiple key frames; according to the image feature points, taking the estimated pose of each key frame queried in the visual dictionary as the pose to be optimized, so as to obtain multiple poses to be optimized; determining multiple target poses from the target local map according to the multiple poses to be optimized, and taking the multiple target poses as multiple pre-closed-loop points, and updating the pose graph according to the pre-closed-loop points; finally, constructing a global map according to the updated pose graph. Among them, the visual dictionary includes the image feature points and estimated poses of each historical key frame, the pose graph includes the lidar point cloud data and estimated poses of historical key frames, and the target local map is generated according to the lidar point cloud data of each historical key frame. The technical solution provided by the present application can improve the accuracy of the constructed map.

Description

Technical Field

[0001] This application relates to the field of robot technology, and in particular, to a map construction method, device, and medium. Background Art

[0002] With the continuous progress of artificial intelligence technology, the functions of robots have become diversified, enabling various robots to perform tasks well in specific environments, and thus being more and more popular among people.

[0003] In most cases, the working environment of robots is unknown or uncertain. The autonomous movement and positioning of robots need to be realized with the help of environmental maps. Currently, most robots use sensors such as lidar or cameras to construct maps. This method is prone to the inability to close the loop in map construction in sites with insufficient feature points or similar scenes, resulting in a low accuracy of the constructed map. Summary of the Invention

[0004] In view of this, this application provides a map construction method, device, and equipment to improve the robustness of the robot when closing the loop in map construction in sites with insufficient feature points or similar scenes, and further improve the accuracy of the constructed map.

[0005] To achieve the above object, in a first aspect, an embodiment of this application provides a map construction method, including:

[0006] When performing loop closure detection, obtaining image feature points of multiple key frames;

[0007] According to the image feature points, taking the estimated pose of each key frame queried in the visual dictionary as the pose to be optimized, and obtaining multiple poses to be optimized, where the visual dictionary includes the image feature points and estimated poses of each historical key frame;

[0008] Determining multiple target poses from the target local map according to the multiple poses to be optimized, where the target local map is generated according to the lidar point cloud data of each historical key frame;

[0009] Taking the multiple target poses as multiple pre-loop closure points, and updating the pose graph according to the multiple pre-loop closure points, where the pose graph includes the lidar point cloud data and estimated poses of each historical key frame;

[0010] Constructing a global map according to the updated pose graph.

[0011] As an optional implementation manner of an embodiment of this application, before the step of obtaining image feature points of multiple key frames when performing loop closure detection, the method further includes:

[0012] Obtain the mapping information of the robot, where the mapping information includes the lidar point cloud data and the initial pose of the current key frame;

[0013] Construct the current local map according to the lidar point cloud data of the current key frame, and optimize the initial pose according to the current local map to obtain the estimated pose of the current key frame;

[0014] Update the pose graph according to the lidar point cloud data of the current key frame and the estimated pose of the current key frame.

[0015] As an optional implementation manner of an embodiment of the present application, the determining multiple target poses from the target local map according to the pose to be optimized includes:

[0016] Determine the target pose in the target local map by using the branch and bound method according to the multiple poses to be optimized.

[0017] As an optional implementation manner of an embodiment of the present application, the updating the pose graph according to the pre-closed-loop point includes:

[0018] Determine the closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point;

[0019] If the closed-loop constraint factor is less than the preset closed-loop error threshold, then construct an error equation according to the closed-loop constraint factor to optimize the estimated pose in the pose graph to obtain the updated pose graph.

[0020] As an optional implementation manner of an embodiment of the present application, the determining the closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point includes:

[0021] If the coordinate system of the target local map where the pre-closed-loop point is located is different from that of the current local map, then determine the target estimated pose of the estimated pose in the target local map according to the coordinate mapping relationship between the target local map and the current local map;

[0022] Determine the closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the target estimated pose, and the estimated pose corresponding to the previous pre-closed-loop point;

[0023] If the coordinate system of the target local map where the pre-closed-loop point is located is the same as that of the current local map, then determine the closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point.

[0024] As an optional implementation manner of an embodiment of the present application, after constructing the current local map according to the lidar point cloud data of the current key frame and before optimizing the initial pose according to the current local map, the method further includes:

[0025] Project the lidar point cloud data of the current key frame into the current local map;

[0026] Calculate a projection score according to the projection position of the lidar point cloud data of the current key frame in the current local map;

[0027] If the projection score is less than a set score, or the number of feature points in the current key frame is less than a set number of feature points, then construct a coordinate system of a new local map according to the lidar point cloud data of the current key frame and the pose of the robot in the current key frame, and record the coordinate mapping relationship between the new local map and the current local map.

[0028] As an optional implementation manner of an embodiment of the present application, the obtaining, according to the image feature points, the estimated pose of each key frame queried in the visual dictionary as the pose to be optimized to obtain a plurality of poses to be optimized includes:

[0029] In the visual dictionary, query the image feature points of each historical key frame whose matching degree with the image feature points of the plurality of key frames is higher than a matching degree threshold;

[0030] Obtain the image feature points of the target historical key frame with the highest matching degree with the image feature points of the plurality of key frames among the image feature points of the queried historical key frames;

[0031] Determine the estimated pose of the target historical key frame according to the corresponding relationship between the image feature points of the target historical key frame and the estimated pose;

[0032] Determine the estimated pose of the target historical key frame as the pose to be optimized.

[0033] As an optional implementation manner of an embodiment of the present application, the updating the pose graph according to the lidar point cloud data of the current key frame and the estimated pose of the current key frame includes:

[0034] If the parallax between the estimated pose and the previous estimated pose of the estimated pose is greater than a preset parallax threshold, then add the lidar point cloud data of the current key frame and the estimated pose of the current key frame to the pose graph to obtain an updated pose graph.

[0035] In a second aspect, an embodiment of the present application provides a map construction device, including:

[0036] Acquisition module: configured to acquire the image feature points of multiple key frames when performing closed-loop detection;

[0037] Query module: configured to, according to the image feature points, query in the visual dictionary to obtain the estimated pose of each key frame as the pose to be optimized, so as to obtain a plurality of poses to be optimized, wherein the visual dictionary includes the image feature points and estimated poses of each historical key frame;

[0038] Determination module: configured to determine a plurality of target poses from the target local map according to the plurality of poses to be optimized, where the target local map is generated according to the lidar point cloud data of each historical key frame;

[0039] Update module: configured to use the plurality of target poses as a plurality of pre-closed-loop points, and update the pose graph according to the plurality of pre-closed-loop points, where the pose graph includes the lidar point cloud data and estimated poses of each historical key frame;

[0040] Construction module: configured to construct a global map according to the updated pose graph.

[0041] In a third aspect, an embodiment of the present application provides an electronic device, including: a memory and a processor, where the memory is used to store a computer program; the processor is configured to execute the method described in the first aspect or any implementation manner of the first aspect when calling the computer program.

[0042] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the method described in the first aspect or any implementation manner of the first aspect is implemented.

[0043] In a fifth aspect, an embodiment of the present application provides a computer program product, when the computer program product runs on the electronic device, the electronic device is enabled to execute the path tracking method described in any item of the first aspect.

[0044] The map construction solution provided by the embodiments of the present application, when the closed-loop detection condition is met, obtains the image feature points of multiple key frames. According to the image feature points, the estimated pose of each key frame queried in the visual dictionary is used as the pose to be optimized, so as to obtain multiple poses to be optimized; multiple target poses are determined from the target local map according to the multiple poses to be optimized, and the multiple target poses are used as multiple pre-closed-loop points, and the pose graph is updated according to the pre-closed-loop points. Finally, a global map is constructed according to the updated pose graph. Among them, the visual dictionary includes the image feature points and estimated poses of each historical key frame, the pose graph includes the lidar point cloud data and estimated poses of historical key frames, and the target local map is generated according to the lidar point cloud data of each historical key frame. In the above solution, during closed-loop detection, first, according to the image feature points in the visual image information, the pose to be optimized is queried in the visual dictionary, and then in the historically generated target local map, the target pose is determined according to the pose to be optimized to obtain the pre-closed-loop points. Since there are more poses available for matching in the historically generated target local map, the number of poses that can be matched during closed-loop detection can be increased, making it easier to match the corresponding pose during closed-loop detection, improving the robustness of the closed-loop detection in map construction, and thus improving the accuracy of the map constructed by the robot in a venue with sparse feature points or similar scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a schematic flowchart of the pose graph update provided by the embodiments of the present application;

[0046] Figure 2 It is a schematic flowchart of the method for constructing a new local map provided by the embodiments of the present application;

[0047] Figure 3 It is a schematic flowchart of the map construction method provided by the embodiments of the present application;

[0048] Figure 4 It is a schematic diagram of the positional relationship between multiple pre-closed-loop points and the corresponding estimated poses provided by the embodiments of the present application;

[0049] Figure 5 It is a schematic structural diagram of the map construction device provided by the embodiments of the present application;

[0050] Figure 6 It is a schematic structural diagram of the electronic device provided by the embodiments of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0051] The embodiments of the present application will be described below with reference to the accompanying drawings in the embodiments of the present application. The terms used in the implementation part of the embodiments of the present application are only used to explain the specific embodiments of the present application, and are not intended to limit the present application. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments.

[0052] The map construction method provided by the embodiments of the present application can be implemented by a map construction device. The map construction device can be a self-mobile device, such as a robot, or a chip or circuit applied to a robot. Alternatively, the map construction device can also be an electronic device or a chip or circuit applied to an electronic device. For example, the map construction method can be used to construct a map on a computer. In the following embodiments, the map construction method is described by taking the application to a robot as an example. When the map construction device is an electronic device, the map construction device can interact with the robot. For example, the robot can report various sensor data of the robot to the electronic device. The embodiments of the present application do not limit this.

[0053] Among them, the robot can be a lawn mowing robot, a floor sweeping robot, a mine clearing robot, a cruise robot, etc. The embodiments of the present application do not make special limitations on this.

[0054] Closed-loop detection, also known as loop detection, refers to the ability of a robot to recognize that it has reached a certain scene, enabling the map to be closed. Since closed-loop detection provides the constraint relationship information between the lidar point cloud data and the estimated pose of the current key frame and the lidar point cloud data and the estimated pose of the historical key frames in the pose graph, before performing closed-loop detection, it is necessary to update the pose graph in real time. Figure 1 It is a schematic flow chart of the pose graph update provided by the embodiments of the present application, as Figure 1 shown, the method may include the following steps:

[0055] S110. Obtain the mapping information of the robot.

[0056] The robot can be configured with multiple different types of sensors and cameras. The sensors include but are not limited to encoders, inertial sensors, lidar, and GPS (Global Positioning System) sensors.

[0057] Among them, the encoder can be an absolute encoder or an incremental encoder, etc.

[0058] The inertial sensor can be a micro-electro-mechanical system (MEMS) or an inertial measurement unit (IMU), etc.

[0059] The camera can be a monocular camera, a binocular camera, an RGB-D camera, an event camera, etc.

[0060] The mapping information of the robot can include the lidar point cloud data of the current key frame and the initial pose, etc.

[0061] The robot can obtain the lidar point cloud data of the current key frame through the lidar sensor.

[0062] The initial pose of the robot can include the position (x, y) and the orientation angle theta of the robot. The position and the first orientation angle theta1 of the robot can be obtained from the robot's track acquired by the encoder. By integrating the angular velocity of the data collected by the inertial sensor, the second orientation angle theta2 of the robot is obtained. Then, the weighted average of theta1 and theta2 is calculated using the observation equation of the Extended Kalman Filter (EKF) to obtain the orientation angle theta of the robot. Then, the prediction equation, which can be a constant velocity model, is used to predict based on the current position and the orientation angle theta of the robot to obtain the initial pose of the robot.

[0063] The robot can collect visual images within the visual range through the camera, and then obtain the visual image information in the visual images.

[0064] In the following embodiments, an absolute encoder is used as the encoder, an IMU is used as the inertial sensor, and an RGB-D camera is used as the camera for exemplary illustration.

[0065] It can be understood that the initial pose can also be obtained through the data of other sensors, and the embodiments of the present application do not limit this.

[0066] S120. Construct the current local map according to the lidar point cloud data of the current key frame, and optimize the initial pose according to the current local map to obtain the estimated pose of the current key frame.

[0067] Every time the robot walks a certain distance, such as 0.5 m, or rotates a certain angle, such as 30 degrees, the initial pose corresponding to the current key frame and the lidar point cloud data of the current key frame are recorded, and the current local map is formed by splicing the lidar point cloud data corresponding to a preset number of key frames after the current key frame of the robot. The preset number can be based on the number of key frames collected within a certain range from the current position of the robot.

[0068] Furthermore, the Gauss-Newton iteration method can be used to optimize the initial pose of the current key frame of the robot according to the current local map to obtain the estimated pose after optimizing the initial pose of the current key frame.

[0069] It can be understood that when there are few features in the environment, it may cause the robot to fail in the closed-loop detection when performing closed-loop detection based on the current local map. Therefore, when the robot detects that it has entered an area with few features, it re-acquires the lidar point cloud data and the initial pose corresponding to the current key frame, and constructs a new local map based on the re-acquired lidar point cloud data and the initial pose corresponding to the current key frame for closed-loop detection matching.

[0070] Figure 2 It is a schematic flowchart of the method for constructing a new local map provided by an embodiment of the present application. As Figure 2 shown, the method may include the following steps:

[0071] S121. Project the lidar point cloud data of the current key frame into the current local map.

[0072] Specifically, according to the coordinates of each point in the lidar point cloud data of the current key frame, each point in the lidar point cloud data can be projected onto the current local map. The current local map is composed of small grids, and each point in the lidar point cloud data finally falls into different grids.

[0073] S122. Calculate the projection score according to the projection position of the lidar point cloud data of the current key frame in the current local map.

[0074] Each point in the lidar point cloud data has a corresponding grid position in the current local map. Taking point A in the lidar point cloud data as an example, the grid corresponding to point A in the current local map is A'. After projection, if point A falls into A', the projection score of point A can be 10 points; if point A falls into the grid in the 8-neighborhood of A', the projection score of point A can be 8 points; and so on. The farther the projection position of point A in the current local map is from A', the lower the projection score.

[0075] Add up the projection scores of each point in the lidar point cloud data to obtain the projection score of the lidar point cloud data of the current key frame in the current local map.

[0076] S123. If the projection score is less than the set score, or the number of image feature points in the current key frame is less than the set number of image feature points, then construct the coordinate system of the new local map according to the lidar point cloud data of the current key frame and the pose of the robot in the current key frame, and record the coordinate mapping relationship between the new local map and the current local map.

[0077] Specifically, the robot can also extract the image feature points in the current key frame. If the projection score is less than the set score, or the number of image feature points in the current key frame is less than the set number of image feature points, it indicates that there are fewer features in the area where the robot is currently located. When performing closed-loop detection and matching based on the current local map, it is easy to fail to match the corresponding image feature points, resulting in closed-loop failure. At this time, a coordinate system of a new local map can be constructed according to the lidar point cloud data and the pose of the robot in the current key frame for closed-loop detection and matching, where the origin of the coordinate system of the new local map is the current position of the robot.

[0078] The robot can also record the coordinate mapping relationship between the new local map and the current local map to facilitate subsequent closed-loop detection.

[0079] In addition, if the closed-loop detection and matching of the newly constructed local map still fails due to insufficient lidar point cloud data, the data collected by the encoder and the IMU can also be used to determine the current position of the robot to make up for the lack of laser information.

[0080] S130. Update the pose graph according to the lidar point cloud data of the current key frame and the estimated pose of the current key frame.

[0081] The pose graph includes the lidar point cloud data, the estimated pose of historical key frames, and the constraint relationship information between historical key frames. Among them, the constraint relationship information is the constraint relationship formed according to the relative poses between any two adjacent historical key frames in the pose graph.

[0082] If the parallax between the estimated pose and the previous estimated pose of the estimated pose is greater than the preset parallax threshold, the lidar point cloud data and the estimated pose of the current key frame are added to the pose graph.

[0083] Specifically, the initial pose in the current key frame is locally optimized to become the estimated pose 1, and the initial pose in the previous key frame is locally optimized to become the estimated pose 2. If the parallax between the estimated pose 1 and the estimated pose 2 is greater than the preset parallax threshold, the lidar point cloud data and the estimated pose 1 of the current key frame can be added to the pose graph to obtain the updated pose graph, that is, the updated pose graph also includes the lidar point cloud data and the estimated pose of the current key frame. It can be understood that when the interval duration between the generation time of the estimated pose of the current key frame and the generation time of the estimated pose of the previous key frame is greater than the set duration, the current key frame can also be added to the pose graph.

[0084] Figure 3 It is a schematic flowchart of the map construction method provided by the embodiment of the present application. As Figure 3 shown, the method may include the following steps:

[0085] S140. When performing closed-loop detection, obtain the image feature points of multiple key frames.

[0086] Specifically, obtain the images of multiple key frames, and extract the image feature points of each key frame through a feature extraction algorithm. Among them, the feature extraction algorithm can include but is not limited to traditional feature extraction methods such as Scale-invariant feature transform (SIFT) algorithm, Histogram of Oriented Gridients (HOG) algorithm, or deep learning network, which is not limited here.

[0087] S150. According to the image feature points, take the estimated pose of each key frame queried in the visual dictionary as the pose to be optimized, and obtain multiple poses to be optimized.

[0088] Specifically, in addition to storing historical key frames, the image feature points of historical key frames, and estimated poses in the visual dictionary, the robot can store the image feature points in the currently extracted key frame, the current key frame, and the corresponding estimated pose in the visual dictionary to construct a dictionary model for convenient matching when closed-loop detection is required in the future. Generally, the image feature points in each key frame can be extracted according to a set fixed object. For example, the fixed object can be an object such as a building, a flower bed, or a tree, which is not limited here.

[0089] Furthermore, closed-loop detection can be performed at intervals of a set number of key frames, that is, when the current key frame reaches the set number, enter the closed-loop detection step. For example, the set closed-loop detection condition is to perform closed-loop detection every 10 key frames, that is, when the current key frame reaches the tenth key frame, extract the image feature points of the current key frame.

[0090] When closed-loop detection is required, first query in the visual dictionary for the image feature points of historical key frames whose matching degree with the image feature points of the current key frame is higher than the matching degree threshold.

[0091] Specifically, the robot can select one or more image feature points from the image feature points of the current key frame, and then query for the image feature points of historical key frames whose matching degree with each selected image feature point is higher than the matching degree threshold.

[0092] Then determine the image feature points of the target historical key frame with the highest matching degree with the image feature points of the current key frame among the image feature points of the queried historical key frames.

[0093] Specifically, the robot can calculate the sum of the matching degrees of each image feature point in each queried historical key frame respectively, and take the historical key frame with the highest sum of matching degrees as the target historical key frame.

[0094] Further, the estimated pose of the target historical key frame can be determined according to the correspondence between the image feature points of the target historical key frame and the estimated pose.

[0095] Specifically, the robot can determine the estimated pose of the target historical key frame according to the correspondence between each image feature point and the estimated pose in the constructed dictionary model.

[0096] Finally, the estimated pose of the target historical key frame can be determined as the pose to be optimized.

[0097] Meanwhile, the current pose cur1 of the robot can also be recorded. Since when the robot creates a new local map, the robot is on the new local map, and later when performing loop closure detection and matching, the new local map is also used for matching. It is not until the corresponding old local map is found during loop closure detection that the robot will switch to the old local map. Therefore, if cur1 is on the newly established local map, the pose cur1_ of cur1 on the old map also needs to be obtained according to the correspondence between the new and old local maps. If cur1 is on the old local map, then cur1 is equal to cur1_.

[0098] S160. Determine the target pose from the target local map according to multiple poses to be optimized.

[0099] The target local map is generated according to the lidar point cloud data corresponding to each historical key frame.

[0100] Specifically, the pose to be optimized can be used as the input of the branch and bound algorithm, search for the estimated poses of each historical key frame in each target local map, respectively match the searched estimated poses of each historical key frame with the pose to be optimized, and use the historical estimated pose pose1 with the highest matching degree as the target pose. The matching efficiency between the estimated pose of the historical key frame and the pose to be optimized is greatly improved by the branch and bound algorithm.

[0101] S170. Use multiple target poses as multiple pre-loop closure points and update the pose graph according to the multiple pre-loop closure points.

[0102] The robot can first determine the loop closure constraint factor according to the pre-loop closure point pose1, the previous pre-loop closure point pose2 of the pre-loop closure point, the estimated pose cur1 corresponding to the pre-loop closure point, and the estimated pose cur2 corresponding to the previous pre-loop closure point.

[0103] Specifically, if the coordinate systems of the target local map where the pre-closed-loop point is located and the current local map are different, the robot can first determine the target estimated pose cur1_ of the estimated pose in the target local map according to the coordinate mapping relationship between the target local map and the current local map; then determine the closed-loop constraint factor according to the pre-closed-loop point pose1, the previous pre-closed-loop point pose2 of the pre-closed-loop point, the target estimated pose cur1_, and the estimated pose cur2 corresponding to the previous pre-closed-loop point.

[0104] If the coordinate systems of the target local map where the pre-closed-loop point is located and the current local map are the same, the robot can determine the closed-loop constraint factor according to the pre-closed-loop point pose1, the previous pre-closed-loop point pose2 of the pre-closed-loop point, the estimated pose cur1 corresponding to the pre-closed-loop point, and the estimated pose cur2 corresponding to the previous pre-closed-loop point.

[0105] Specifically, the closed-loop constraint factor can be calculated according to the following formula (1):

[0106] ΔT = T1 * T2 * T3 * T4............................... Formula (1)

[0107] Where ΔT is the closed-loop constraint factor, T1 is the relative pose between pose1 and pose2, T2 is the relative pose between pose2 and cur2, T3 is the relative pose between cur2 and cur1, and T4 is the relative pose between cur1 and pose1.

[0108] Figure 4 It is a schematic diagram of the positional relationship between multiple pre-closed-loop points and the corresponding estimated poses provided by the embodiments of the present application. As Figure 4 shown, the robot can also determine the closed-loop constraint factor according to multiple adjacent pre-closed-loop points and the estimated poses corresponding to the multiple pre-closed-loop points respectively.

[0109] Specifically, the closed-loop constraint factor can be calculated according to the following formula (2):

[0110]

[0111] Where represents the relative pose between cur1 and pose1, represents the relative pose between pose1 and pose2, represents the relative pose between pose2 and pose3, and so on, represents pose n and cur n between the relative pose, represents cur n and cur n-1The relative pose between represents the relative pose between cur2 and cur1, where 2 ≤ n ≤ 10.

[0112] Furthermore, the relative pose between any two poses can be calculated according to the following formula (3):

[0113]

[0114] where that is, i and j respectively represent any two corresponding poses, represents the inverse matrix corresponding to one of the poses, T j represents the matrix corresponding to the other pose relative to this pose. For example

[0115] The closed-loop constraint factor ΔT calculated through formula (2) and formula (3) can finally be output as: ΔT = (Δx, Δy, Δz, Δroll, Δpitch, Δyaw), where Δx, Δy, and Δz respectively represent the spatial coordinate errors after closed-loop screening, and Δroll, Δpitch, and Δyaw respectively represent the attitude errors after closed-loop screening, that is, the roll angle error, the pitch angle error, and the yaw angle error.

[0116] Compare the closed-loop constraint factor ΔT with the preset closed-loop error threshold thresh. If ΔT < thresh, then the poses in the pose graph can be optimized according to the closed-loop constraint factor to construct a global error equation. Among them, thresh = (thresh_x, thresh_y, thresh_z, thresh_roll, thresh_pitch, thresh_yaw), thresh_x, thresh_y, thresh_z represent the spatial coordinate error thresholds; thresh_roll, thresh_pitch, thresh_yaw represent the attitude error thresholds.

[0117] S180. Construct a global map according to the updated pose graph.

[0118] Specifically, the global map can be re - stitched according to the optimized estimated poses in the pose graph, the lidar point cloud data corresponding to the optimized estimated poses, and the constraint relationship information in the pose graph, so that the re - stitched map is closer to the real situation.

[0119] Those skilled in the art can understand that the above embodiments are exemplary and are not used to limit the present application. Where possible, the execution order of one or several steps in the above steps can be adjusted, or they can be selectively combined to obtain one or more first embodiments. Those skilled in the art can arbitrarily select and combine from the above steps. All those that do not depart from the essence of the present application's solution fall within the protection scope of the present application.

[0120] In the map construction solution provided by the embodiment of the present application, when the closed-loop detection condition is met, image feature points of multiple key frames are obtained. According to the image feature points, the estimated pose of each key frame queried in the visual dictionary is used as the pose to be optimized, so as to obtain multiple poses to be optimized; multiple target poses are determined from the target local map according to the multiple poses to be optimized, and the multiple target poses are used as multiple pre-closed-loop points, and the pose graph is updated according to the pre-closed-loop points. Finally, a global map is constructed according to the updated pose graph. Among them, the visual dictionary includes the image feature points and estimated poses of each historical key frame, the pose graph includes the lidar point cloud data and estimated poses of historical key frames, and the target local map is generated according to the lidar point cloud data of each historical key frame. In the above solution, during closed-loop detection, first, according to the image feature points in the visual image information, the pose to be optimized is queried in the visual dictionary, and then in the historically generated target local map, the target pose is determined according to the pose to be optimized to obtain the pre-closed-loop points. Since there are more poses available for matching in the historically generated target local map, the number of poses that can be matched during closed-loop detection can be increased, making it easier to match the corresponding pose during closed-loop detection, improving the robustness of the closed-loop detection in map construction, and thus improving the accuracy of the map constructed by the robot in a site with sparse feature points or similar scenes.

[0121] Based on the same inventive concept, as an implementation of the above method, the embodiment of the present application provides a map construction device. The device embodiment corresponds to the foregoing method embodiment. For the convenience of reading, the details in the foregoing method embodiment will not be described one by one in this device embodiment. However, it should be clear that the device in this embodiment can correspondingly implement all the contents in the foregoing method embodiment.

[0122] Figure 5 For the structural schematic diagram of the map construction device provided by the embodiment of the present application, as Figure 5 shown, the map construction device provided in this embodiment may include: an acquisition module 11, a query module 12, a determination module 13, an update module 14, and a construction module 15, where:

[0123] The acquisition module 11: is used to obtain image feature points of multiple key frames when performing closed-loop detection;

[0124] Query module 12: configured to use the image feature points to query the estimated poses of each of the key frames obtained from the visual dictionary as the poses to be optimized, so as to obtain a plurality of the poses to be optimized, where the visual dictionary includes the image feature points and estimated poses of each historical key frame;

[0125] Determination module 13: configured to determine a plurality of target poses from the target local map according to the plurality of poses to be optimized, where the target local map is generated according to the lidar point cloud data of each historical key frame;

[0126] Update module 14: configured to use the plurality of target poses as a plurality of pre-closed-loop points, and update the pose graph according to the plurality of pre-closed-loop points, where the pose graph includes the lidar point cloud data and estimated poses of each historical key frame;

[0127] Construction module 15: configured to construct a global map according to the updated pose graph.

[0128] As an alternative implementation, the map construction device is further configured to:

[0129] Obtain the mapping information of the robot, where the mapping information includes the lidar point cloud data and initial pose of the current key frame;

[0130] Construct a current local map according to the lidar point cloud data of the current key frame, and optimize the initial pose according to the current local map to obtain the estimated pose of the current key frame;

[0131] Update the pose graph according to the lidar point cloud data of the current key frame and the estimated pose of the current key frame.

[0132] As an alternative implementation, the determination module 13 is specifically configured to:

[0133] Determine the target pose in the target local map in a branch and bound manner according to the plurality of poses to be optimized.

[0134] As an alternative implementation, the update module 14 is specifically configured to:

[0135] Determine a closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point;

[0136] If the closed-loop constraint factor is less than a preset closed-loop error threshold, construct an error equation according to the closed-loop constraint factor to optimize the estimated pose in the pose graph to obtain an updated pose graph.

[0137] As an alternative implementation, the determination module 13 is specifically configured to:

[0138] If the coordinate systems of the target local map where the pre-closed-loop point is located and the current local map are different, then according to the coordinate mapping relationship between the target local map and the current local map, determine the target estimated pose of the estimated pose in the target local map;

[0139] Determine the closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the target estimated pose, and the estimated pose corresponding to the previous pre-closed-loop point;

[0140] If the coordinate systems of the target local map where the pre-closed-loop point is located and the current local map are the same, then determine the closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point.

[0141] As an alternative implementation, the map construction device is further configured to:

[0142] Project the lidar point cloud data of the current key frame into the current local map;

[0143] Calculate a projection score according to the projection position of the lidar point cloud data of the current key frame in the current local map;

[0144] If the projection score is less than a set score, or the number of feature points in the current key frame is less than the set number of feature points, then construct a coordinate system of a new local map according to the lidar point cloud data of the current key frame and the pose of the robot in the current key frame, and record the coordinate mapping relationship between the new local map and the current local map.

[0145] As an alternative implementation, the query module 12 is specifically configured to:

[0146] In the visual dictionary, query for the feature points of historical key frames whose matching degree with the image feature points of the multiple key frames is higher than the matching degree threshold;

[0147] Obtain the image feature points of the target historical key frame with the highest matching degree with the image feature points of the multiple key frames among the image feature points of each queried historical key frame;

[0148] Determine the estimated pose of the target historical key frame according to the correspondence between the image feature points of the target historical key frame and the estimated pose;

[0149] Determine the estimated pose of the target historical key frame as the pose to be optimized.

[0150] As an alternative embodiment, the map construction device is further configured to:

[0151] If the parallax between the estimated pose and the previous estimated pose of the estimated pose is greater than a preset parallax threshold, the lidar point cloud data of the current key frame and the estimated pose of the current key frame are added to the pose graph to obtain an updated pose graph.

[0152] The map construction device provided in this embodiment can execute the above method embodiment, and its implementation principle and technical effect are similar, which will not be elaborated here.

[0153] Based on the same inventive concept, an embodiment of the present application further provides an electronic device. Figure 6 Shown in the following is the structural schematic diagram of the electronic device provided in the embodiment of the present application. Figure 6 As shown, the electronic device provided in this embodiment includes: a memory 210 and a processor 220. The memory 210 is used to store a computer program; the processor 220 is configured to execute the method described in the above method embodiment when calling the computer program.

[0154] The electronic device provided in this embodiment can execute the above method embodiment, and its implementation principle and technical effect are similar, which will not be elaborated here.

[0155] An embodiment of the present application further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the method described in the above method embodiment is implemented.

[0156] An embodiment of the present application further provides a computer program product. When the computer program product runs on an electronic device, the electronic device is caused to execute the method described in the above method embodiment.

[0157] Those skilled in the art can clearly understand that for the convenience and brevity of description, only the above division of each functional unit and module is used as an example. In actual applications, the above functions can be allocated to different functional units and modules according to needs, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. Each functional unit and module in the embodiment can be integrated into a processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above integrated unit can be implemented in the form of hardware or in the form of a software functional unit. In addition, the specific names of each functional unit and module are only for the convenience of mutual distinction and do not limit the protection scope of the present application. The specific working processes of the units and modules in the above system can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated here.

[0158] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions described in the embodiments of the present application are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium or transmitted through the computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center by wire (such as coaxial cable, optical fiber, digital subscriber line) or wirelessly (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (such as a floppy disk, hard disk, or magnetic tape), an optical medium (such as a DVD), or a semiconductor medium (such as a solid state disk (SSD)), etc.

[0159] Those of ordinary skill in the art can understand that all or part of the processes of implementing the methods in the above embodiments can be completed by a computer program instructing relevant hardware. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above method embodiments. The foregoing storage medium can include: various media that can store program codes such as ROM or random access memory RAM, magnetic disks, or optical discs.

[0160] In the present application, the naming or numbering of steps does not mean that the steps in the method process must be executed in the time / logical sequence indicated by the naming or numbering. The named or numbered process steps can be changed in the order of execution according to the technical purpose to be achieved, as long as the same or similar technical effects can be achieved.

[0161] In the above embodiments, the descriptions of each embodiment have their own emphases. For the parts not detailed or recorded in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0162] In the embodiments provided in the present application, it should be understood that the disclosed apparatus / devices and methods can be implemented in other ways. For example, the apparatus / device embodiments described above are merely illustrative. For example, the division of the modules or units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some feature points can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection to each other can be through some interfaces. The indirect coupling or communication connection of the apparatus or unit can be in electrical, mechanical or other forms.

[0163] It should be understood that when used in the specification of the present application and the appended claims, the term "comprising" indicates the presence of the described feature points, wholes, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other feature points, wholes, steps, operations, elements, components and / or their combinations.

[0164] In the description of the present application, unless otherwise specified, " / " means that the associated objects before and after are in an "or" relationship. For example, A / B can represent A or B; the "and / or" in the present application is only a description of the association relationship of the associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist simultaneously, and B exists alone. These three situations, where A and B can be singular or plural.

[0165] Also, in the description of the present application, unless otherwise specified, "a plurality of" means two or more than two. "At least one of the following" or its similar expressions refer to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b, or c can represent: a, b, c, a - b, a - c, b - c, or a - b - c, where a, b, c can be single or multiple.

[0166] As used in the specification of the present application and the appended claims, the term "if" can be interpreted as "when...", "once", "in response to determining", or "in response to detecting" according to the context. Similarly, the phrase "if determined" or "if [the described condition or event] is detected" can be interpreted as meaning "once determined", "in response to determining", "once [the described condition or event] is detected", or "in response to detecting [the described condition or event]" according to the context.

[0167] In addition, in the description of the specification and the appended claims of this application, the terms "first", "second", "third", etc. are used to distinguish similar objects and do not necessarily have to be used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances so that the embodiments described herein can be implemented in an order other than that shown or described herein.

[0168] Reference to "an embodiment" or "some embodiments" or the like described in the specification of this application means that a specific feature, structure, or characteristic described in connection with the embodiment is included in one or more embodiments of this application. Thus, the statements "in one embodiment", "in some embodiments", "in other some embodiments", "in still other embodiments", etc. that appear in different places in this specification do not necessarily all refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways.

[0169] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application and are not intended to limit them; although this application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of this application.

Claims

1. A method for map construction, characterized in that, Including: When performing closed-loop detection, obtaining image feature points of multiple key frames; According to the image feature points, taking the estimated pose of each key frame obtained by querying in the visual dictionary as the pose to be optimized, obtaining multiple poses to be optimized, where the visual dictionary includes image feature points and estimated poses of each historical key frame; Determining multiple target poses from the target local map according to the multiple poses to be optimized, where the target local map is generated according to the lidar point cloud data of each historical key frame; Taking the multiple target poses as multiple pre-closed-loop points, and updating the pose graph according to the multiple pre-closed-loop points, where the pose graph includes lidar point cloud data and estimated poses of each historical key frame; Constructing a global map according to the updated pose graph.

2. The method according to claim 1, wherein Before obtaining the image feature points of multiple key frames when performing closed-loop detection, the method further includes: Obtaining mapping information of the robot, where the mapping information includes lidar point cloud data and an initial pose of the current key frame; Constructing a current local map according to the lidar point cloud data of the current key frame, and optimizing the initial pose according to the current local map to obtain the estimated pose of the current key frame; Updating the pose graph according to the lidar point cloud data of the current key frame and the estimated pose of the current key frame.

3. The method according to claim 1, characterized in that, The determining multiple target poses from the target local map according to the multiple poses to be optimized includes: Determining the target pose in the target local map in a branch and bound manner according to the multiple poses to be optimized.

4. The method according to claim 2, characterized in that, The updating the pose graph according to the multiple pre-closed-loop points includes: Determining a closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point; If the closed-loop constraint factor is less than a preset closed-loop error threshold, then constructing an error equation according to the closed-loop constraint factor to optimize the estimated pose in the pose graph to obtain an updated pose graph.

5. The method according to claim 4, characterized in that The determining a closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point includes: If the coordinate systems of the target local map where the pre-closed-loop point is located and the current local map are different, then determining the target estimated pose of the estimated pose in the target local map according to the coordinate mapping relationship between the target local map and the current local map; Determining a closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the target estimated pose, and the estimated pose corresponding to the previous pre-closed-loop point; If the coordinate systems of the target local map where the pre-closed-loop point is located and the current local map are the same, then determining a closed-loop constraint factor according to the pre-closed-loop point, the previous pre-closed-loop point of the pre-closed-loop point, the estimated pose corresponding to the pre-closed-loop point, and the estimated pose corresponding to the previous pre-closed-loop point.

6. The method according to claim 2, characterized in that, After constructing the current local map based on the lidar point cloud data of the current key frame and before optimizing the initial pose according to the current local map, the method further includes: Projecting the lidar point cloud data of the current key frame into the current local map; Calculating a projection score according to the projection position of the lidar point cloud data of the current key frame in the current local map; If the projection score is less than a set score, or the number of feature points in the current key frame is less than the set number of feature points, a coordinate system of a new local map is constructed according to the lidar point cloud data of the current key frame and the pose of the robot in the current key frame, and the coordinate mapping relationship between the new local map and the current local map is recorded.

7. The method according to claim 1, wherein The obtaining, according to the image feature points, the estimated poses of each of the key frames queried in the visual dictionary as the poses to be optimized, to obtain a plurality of the poses to be optimized, includes: In the visual dictionary, querying the image feature points of each historical key frame whose matching degree with the image feature points of the plurality of key frames is higher than the matching degree threshold; Obtaining the image feature points of the target historical key frame with the highest matching degree with the image feature points of the plurality of key frames among the queried image feature points of each historical key frame; Determining the estimated pose of the target historical key frame according to the corresponding relationship between the image feature points of the target historical key frame and the estimated pose; Determining the estimated pose of the target historical key frame as the pose to be optimized.

8. The method according to claim 2, wherein The updating the pose graph according to the lidar point cloud data of the current key frame and the estimated pose of the current key frame includes: If the parallax between the estimated pose and the previous estimated pose of the estimated pose is greater than a preset parallax threshold, the lidar point cloud data of the current key frame and the estimated pose of the current key frame are added to the pose graph to obtain an updated pose graph.

9. An electronic device, characterized in that, Including: A memory and a processor, the memory is used to store a computer program; the processor is used to execute the method according to any one of claims 1-8 when calling the computer program.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The computer program, when executed by the processor, implements the method according to any one of claims 1-8.

Images