Fusion map construction method, device, robot and storage medium

Abstract

The present application relates to a method, device, robot and storage medium for building a fusion map. The method comprises: interpolating the poses of each image frame from the image frame set collected by the image acquisition device based on the laser pose data set obtained by laser radar mapping; selecting a key frame set from the image frame set based on the pose of each image frame and the feature matching relationship between each adjacent image frame, and constructing a correspondence set between each key frame in the key frame set and each initial map point in the initial map point set; updating the initial map point set and the correspondence set based on the feature matching relationship between the current key frame selected from the key frame set and the related key frame set corresponding to the current key frame, to obtain an updated map point set and an updated correspondence set; constructing a target fusion map based on the three-dimensional map point set converted from the updated map point set, the updated correspondence set and the poses of each key frame in the updated correspondence set. The use of this method can expand the environmental range of normal positioning and operation of the robot.

Description

Technical Field

[0001] The present application relates to the field of computer technology, and in particular to a method, device, robot and storage medium for constructing a fusion map. Background Art

[0002] The popularity of robots has led to their use in more and more complex scenarios, such as supermarkets, underground parking lots, restaurants, factories, and office buildings, which include dynamic and dark scenes. In the prior art, robots are either positioned and operated only by laser or by visual SLAM, which makes it impossible for robots to operate normally in multiple scenarios (such as dynamic and dark scenes), resulting in a smaller range of environments in which robots can operate normally. Summary of the invention

[0003] Based on this, it is necessary to provide a fused map construction method, device, robot and storage medium that can realize the fusion construction of laser maps and visual maps to control the robot to efficiently and accurately locate and operate normally in multiple scenarios, in order to address the above-mentioned technical problems, and to expand the environmental range of the robot's normal positioning and operation.

[0004] A fusion map construction method is applied to a robot, wherein the robot is equipped with a laser radar and an image acquisition device; the method comprises:

[0005] Acquire an image frame set acquired by an image acquisition device and a laser pose data set obtained by laser radar mapping, and perform an interpolation operation on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame;

[0006] Acquire a feature matching relationship between adjacent image frames, select a key frame set from the image frame set based on the pose corresponding to each image frame and the feature matching relationship, acquire an initial map point set corresponding to the key frame set, and construct a corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set to obtain a corresponding relationship set;

[0007] Selecting the current key frame and the related key frame set corresponding to the current key frame from the key frame set in sequence, and updating the initial map point set and the corresponding relationship set based on the feature matching relationship between the current key frame and the related key frame set to obtain an updated map point set and an updated corresponding relationship set;

[0008] The updated map point set is converted into a three-dimensional map point set, and a target fusion map is constructed based on the three-dimensional map point set, the updated correspondence set, and the position and posture of each key frame in the updated correspondence set.

[0009] In one embodiment, interpolation operation is performed on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame, including:

[0010] Select a current image frame from the image frame set in sequence, and based on a current timestamp corresponding to the current image frame, obtain a forward laser pose corresponding to the current timestamp, a forward timestamp corresponding to the forward laser pose, a backward laser pose, and a backward timestamp corresponding to the backward laser pose from the laser pose data set;

[0011] The current timestamp, the forward timestamp, the backward timestamp, the forward laser pose and the backward laser pose are interpolated to obtain the pose corresponding to the current image frame.

[0012] In one embodiment, obtaining the feature matching relationship between adjacent image frames includes:

[0013] Performing a feature point extraction operation on each image frame in the image frame set to obtain feature points corresponding to each image frame, wherein the feature points include descriptors;

[0014] Selecting in sequence from the image frame set a first image frame and a second image frame adjacent to each of the image frames, and obtaining a first descriptor corresponding to the first image frame and a second descriptor corresponding to the second image frame;

[0015] Traversing the first descriptors corresponding to the first image frame, taking the descriptor in the second descriptors that is most similar to the first descriptor as the first target descriptor, and establishing a first corresponding relationship between the first descriptor and the first target descriptor;

[0016] Traversing the second descriptors corresponding to the second image frame, taking the descriptor in the first descriptors that is most similar to the second descriptor as the second target descriptor, and constructing a second corresponding relationship between the second descriptor and the second target descriptor;

[0017] Feature points corresponding to the descriptors having consistent relationships in the first corresponding relationship and the second corresponding relationship are matched to obtain feature matching relationships corresponding to each of the first image frames and each of the second image frames in the image frame set.

[0018] In one embodiment, based on the pose corresponding to each image frame and the feature matching relationship, filtering out a key frame set from the image frame set includes:

[0019] Based on the posture corresponding to the first image frame and the posture corresponding to the second image frame adjacent to each of the image frames in the image frame set, the change distance corresponding to each of the first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the change distance greater than the preset change distance is used as the key frame in the key frame set;

[0020] or,

[0021] Based on the feature matching relationship between the first image frame and the second image frame adjacent to each image frame in the image frame set, the feature matching rate corresponding to each first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the feature matching rate less than the preset matching rate is used as the key frame in the key frame set.

[0022] In one embodiment, a current key frame and a related key frame set corresponding to the current key frame are sequentially selected from the key frame set, and based on the feature matching relationship between the current key frame and the related key frame set, the initial map point set and the corresponding relationship set are updated to obtain the updated map point set and the updated corresponding relationship set, including:

[0023] Obtaining a preset distance, selecting a current key frame from the key frame set in turn, and selecting a related key frame set corresponding to the current key frame from the key frame set based on a posture corresponding to the current key frame and the preset distance;

[0024] Performing feature matching on the current key frame and each related key frame in the related key frame set to obtain a feature matching relationship between the current key frame and each related key frame;

[0025] When the initial map point in the initial map point set corresponding to the current matching feature point of the current key frame is inconsistent with the initial map point corresponding to the current relevant matching feature point of the relevant key frame, the initial map point corresponding to the current matching feature point is merged with the initial map point corresponding to the current relevant matching feature point to obtain an updated map point;

[0026] adding the updated map point to the initial map point set, deleting the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point from the initial map point set, and changing the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point in the correspondence relationship set to the updated map point, thereby obtaining an updated correspondence relationship and an updated map point;

[0027] The key frames in the key frame set are traversed to obtain the updated map point set and the updated corresponding relationship set.

[0028] In one embodiment, the updated map point set includes pixel coordinates of each updated map point on a corresponding key frame, and converting the updated map point set into a three-dimensional map point set includes:

[0029] The update map point set is converted into a three-dimensional map point set based on the position and posture of the key frame corresponding to each update map point in the update map point set and the pixel coordinates of each update map point on the corresponding key frame.

[0030] In one embodiment, constructing a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the pose of each key frame in the updated correspondence set includes:

[0031] Calculating each total reprojection error of each 3D map point in the 3D map point set on each corresponding key frame, and when the total reprojection error is greater than a preset number of pixels, deleting the correspondence between the 3D map point and the corresponding key frame from the updated correspondence set to obtain an intermediate correspondence set;

[0032] Selecting a current 3D map point from the 3D map point set in sequence, calculating the number of corresponding key frames corresponding to the current 3D map point, deleting the current 3D map point from the 3D map point set when the number of corresponding key frames is less than a preset number, deleting the corresponding relationship between the current 3D map point and the key frame corresponding to the current 3D map point from the intermediate corresponding relationship set, and obtaining a target 3D map point set and a target corresponding relationship set;

[0033] The target fusion map is constructed based on the target three-dimensional map point set, the target correspondence set and the position and posture of each key frame in the target correspondence set.

[0034] A fusion map construction device, the device comprising:

[0035] An interpolation module is used to obtain an image frame set collected by an image acquisition device and a laser pose data set obtained by laser radar mapping, and perform an interpolation operation on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame;

[0036] A matching module is used to obtain a feature matching relationship between adjacent image frames, select a key frame set from the image frame set based on the posture corresponding to each image frame and the feature matching relationship, obtain an initial map point set corresponding to the key frame set, and construct a corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set to obtain a corresponding relationship set;

[0037] An updating module, used to select the current key frame and the related key frame set corresponding to the current key frame from the key frame set in sequence, and update the initial map point set and the corresponding relationship set based on the feature matching relationship between the current key frame and the related key frame set to obtain an updated map point set and an updated corresponding relationship set;

[0038] A construction module is used to convert the updated map point set into a three-dimensional map point set, and to construct a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and posture of each key frame in the updated correspondence set.

[0039] In one of the embodiments, the interpolation module is also used to select the current image frame from the image frame set in sequence, and based on the current timestamp corresponding to the current image frame, obtain the forward laser pose corresponding to the current timestamp, the forward timestamp corresponding to the forward laser pose, the backward laser pose and the backward timestamp corresponding to the backward laser pose from the laser pose data set; interpolate the current timestamp, the forward timestamp, the backward timestamp, the forward laser pose and the backward laser pose to obtain the pose corresponding to the current image frame.

[0040] In one of the embodiments, the matching module is further used to perform a feature point extraction operation on each image frame in the image frame set to obtain each feature point corresponding to each image frame, wherein the feature point includes a descriptor; sequentially select a first image frame and a second image frame adjacent to each of the image frames from the image frame set to obtain a first descriptor corresponding to the first image frame and a second descriptor corresponding to the second image frame; traverse the first descriptor corresponding to the first image frame, take the descriptor in the second descriptor that is most similar to the first descriptor as the first target descriptor, and establish a first correspondence between the first descriptor and the first target descriptor; traverse the second descriptor corresponding to the second image frame, take the descriptor in the first descriptor that is most similar to the second descriptor as the second target descriptor, and establish a second correspondence between the second descriptor and the second target descriptor; match the feature points corresponding to the descriptors that have the same relationship in the first correspondence and the second correspondence to obtain a feature matching relationship between each of the first image frames and each of the second image frames in the image frame set.

[0041] In one of the embodiments, the matching module is further used to calculate the change distance corresponding to each first image frame and the second image frame in the image frame set based on the posture corresponding to the first image frame and the posture corresponding to the second image frame adjacent to each image frame in the image frame set, and use the second image frame corresponding to the change distance greater than the preset change distance as the key frame in the key frame set;

[0042] or,

[0043] Based on the feature matching relationship between the first image frame and the second image frame adjacent to each image frame in the image frame set, the feature matching rate corresponding to each first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the feature matching rate less than the preset matching rate is used as the key frame in the key frame set.

[0044] In one of the embodiments, the updating module is further used to obtain a preset distance, select the current key frame from the key frame set in turn, and select the related key frame set corresponding to the current key frame from the key frame set based on the posture corresponding to the current key frame and the preset distance; perform feature matching on the current key frame and each related key frame in the related key frame set to obtain a feature matching relationship between the current key frame and each related key frame; when the initial map point in the initial map point set corresponding to the current matching feature point of the current key frame is inconsistent with the initial map point corresponding to the current related matching feature point of the related key frame, merge the initial map point corresponding to the current matching feature point with the initial map point corresponding to the current related matching feature point to obtain an updated map point; add the updated map point to the initial map point set, delete the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point from the initial map point set, change the initial map point corresponding to the current matching feature point in the correspondence relationship set and the initial map point corresponding to the current related matching feature point into an updated map point, and obtain an updated correspondence relationship and an updated map point; traverse the key frames in the key frame set to obtain the updated map point set and the updated correspondence relationship set.

[0045] In one of the embodiments, the construction module is further used to convert the updated map point set into a three-dimensional map point set based on the pose of the key frame corresponding to each updated map point in the updated map point set and the pixel coordinates of each updated map point on the corresponding key frame.

[0046] In one of the embodiments, the construction module is further used to calculate the total reprojection errors of each 3D map point in the 3D map point set on the corresponding key frames. When the total reprojection error is greater than a preset number of pixels, the correspondence between the 3D map point and the corresponding key frame is deleted from the updated correspondence set to obtain an intermediate correspondence set; the current 3D map point is selected from the 3D map point set in sequence, and the number of corresponding key frames corresponding to the current 3D map point is calculated. When the number of corresponding key frames is less than a preset number, the current 3D map point is deleted from the 3D map point set, and the correspondence between the current 3D map point and the key frame corresponding to the current 3D map point is deleted from the intermediate correspondence set to obtain a target 3D map point set and a target correspondence set; the target fusion map is constructed based on the target 3D map point set, the target correspondence set and the poses of each key frame in the target correspondence set.

[0047] A robot is provided, wherein the robot is equipped with a laser radar and an image acquisition device, and further comprises a memory and a processor, wherein the memory stores computer-readable instructions, and the processor is used to implement the steps of the above-mentioned fusion map construction method when executing the computer-readable instructions.

[0048] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the steps of the above-mentioned fusion map construction method.

[0049] The above-mentioned fusion map construction method, device, robot and storage medium obtain an image frame set collected by an image acquisition device and a laser pose data set obtained by laser radar mapping, and interpolate each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame; obtain the feature matching relationship between each adjacent image frame, and screen out a key frame set from the image frame set based on the pose corresponding to each image frame and the feature matching relationship, obtain the initial map point set corresponding to the key frame set, and construct the correspondence between each key frame in the key frame set and each initial map point in the initial map point set to obtain a correspondence set; select the current key frame and the related key frame set corresponding to the current key frame from the key frame set in turn, and update the initial map point set and the correspondence set based on the feature matching relationship between the current key frame and the related key frame set to obtain an updated map point set and an updated correspondence set; convert the updated map point set into a three-dimensional map point set, and construct a target fusion map based on the three-dimensional map point set, the updated correspondence set and the pose of each key frame in the updated correspondence set. The fusion of laser pose data and visual maps is realized, which effectively combines the characteristics of laser maps and visual maps. This enables the robot to efficiently and correctly locate and operate in multiple scenarios based on the fused map, thereby expanding the environmental range of the robot's normal positioning and operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A schematic diagram of a flow chart of a method for constructing a fusion map in one embodiment;

[0051] Figure 2 A schematic diagram of a process of posture interpolation in one embodiment;

[0052] Figure 3 A schematic diagram of a feature matching process in one embodiment;

[0053] Figure 4 A schematic diagram of a process of creating a key frame in one embodiment;

[0054] Figure 5 A schematic diagram of a data update process in one embodiment;

[0055] Figure 6 A schematic diagram of a process of data optimization in one embodiment;

[0056] Figure 7 It is a structural block diagram of a fusion map construction device in one embodiment;

[0057] Figure 8 1 is a diagram showing the internal structure of a robot in one embodiment. DETAILED DESCRIPTION

[0058] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0059] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with embodiments and drawings.

[0060] The fusion map construction method provided in the present application is applied to a robot, the robot is used to obtain an image frame set collected by an image acquisition device and a laser pose data set obtained by laser radar mapping, and interpolate each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame; obtain the feature matching relationship between each adjacent image frame, and select a key frame set from the image frame set based on the pose and feature matching relationship corresponding to each image frame, obtain the initial map point set corresponding to the key frame set, and construct the corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set to obtain a corresponding relationship set; select the current key frame and the related key frame set corresponding to the current key frame from the key frame set in turn, and update the initial map point set and the corresponding relationship set based on the feature matching relationship between the current key frame and the related key frame set to obtain an updated map point set and an updated corresponding relationship set; convert the updated map point set into a three-dimensional map point set, and construct a target fusion map based on the three-dimensional map point set, the updated corresponding relationship set, and the pose of each key frame in the updated corresponding relationship set. Wherein, the robot can be a robot with a running positioning function type, including but not limited to a delivery robot, a cleaning robot, a guide robot, etc.

[0061] In one embodiment, Figure 1 As shown, a fusion map construction method is provided, the method is applied to a robot, the robot is equipped with a laser radar and an image acquisition device; the method comprises:

[0062] Step S100, obtaining an image frame set acquired by an image acquisition device and a laser pose data set obtained by laser radar mapping, and performing an interpolation operation on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame.

[0063] Among them, the image acquisition device refers to a device installed on the robot itself for taking images, which can be installed on the robot according to the camera field of view, and the direction of the camera installation can be vertically upward or in any direction. Lidar is a sensor used to obtain accurate position information. The laser radar carried by the robot itself can be a 2D laser radar or a 3D laser radar, which can be mounted on the bottom of the robot or in any other position. This application is explained with the camera installed vertically upward, the laser radar is a 2D laser radar, and is installed at the bottom of the robot. An image frame set refers to a set of image frames collected by the robot's image acquisition device, and these image frames have their corresponding timestamps. A laser pose data set refers to the laser pose recorded when a laser radar is used to build a map, which contains the timestamps corresponding to each laser pose, that is, the pose obtained by the robot through laser radar mapping at a certain time point; it can be obtained by building a laser map through the open source cartographer laser mapping program and keeping the laser pose corresponding to each frame timestamp. Pose represents position and attitude. Any rigid body can be accurately and uniquely represented by position and attitude in the spatial coordinate system (OXYZ), where position refers to the x, y, and z coordinates, and attitude refers to the angle between the rigid body and the OX axis, the OY axis, and the OZ axis. Interpolation refers to the operation of calculating the pose corresponding to the image frame through the timestamp corresponding to the laser pose data set and the image frame.

[0064] Specifically, to achieve vision and laser fusion mapping, we can obtain the image frame set obtained by the camera carried by the robot itself, and obtain the laser pose corresponding to each frame timestamp recorded when the laser map is established using the open source cartographer laser mapping program, that is, obtain the laser pose data set. In the laser mapping process, the corresponding trajectory will be generated. According to these trajectories and the laser pose and timestamp corresponding to each adjacent frame in the corresponding laser pose data, as well as the timestamp corresponding to each frame in the image frame set, the pose corresponding to each image frame in the image frame set is calculated, which provides data support for the realization of laser and vision fusion mapping. In actual scenarios, 2D laser radars are usually installed at the bottom of the robot, which is extremely susceptible to interference from dynamic objects. It is very difficult to use 2D lasers for positioning in underground parking lots or crowded supermarkets. Using visual SLAM for positioning allows the field of view of the camera installed on the robot itself to be vertically upward, which can avoid the interference of dynamic objects and enable the robot to be positioned in a highly dynamic environment. Furthermore, in low-light environments, such as in a restaurant with the lights off, the accuracy of positioning using vision is low, while laser positioning can be used in such dark environments. Therefore, the concept of integrating laser and visual positioning can greatly expand the range of environments in which the robot can normally locate and operate. In addition, the interpolation operation of the laser pose on each image frame in the image frame set provides an important data foundation for the ultimate construction of the laser and visual fusion map.

[0065] Step S102, obtaining the feature matching relationship between each adjacent image frame, filtering out a key frame set from the image frame set based on the posture and feature matching relationship corresponding to each image frame, obtaining an initial map point set corresponding to the key frame set, constructing a correspondence between each key frame in the key frame set and each initial map point in the initial map point set, and obtaining a correspondence set.

[0066] Among them, the feature matching relationship refers to the matching relationship between feature points. Key frame refers to an image frame that can better reflect the characteristics of the image frame and has more representative features. Key frame set refers to a collection of key frames. The initial map point set refers to a set of two-dimensional map points created based on the matched feature points in adjacent image frames, which contains the key frame ID corresponding to each initial map point and the pixel coordinates of each initial map point projected on the corresponding key frame. The correspondence relationship set refers to a set used to store the correspondence between initial map points and key frames, where the initial map point stores the position of the corresponding feature point in each corresponding key frame, and each key frame also stores the position of the initial map point corresponding to each feature point in the key frame. In other words, there is a many-to-one relationship between feature points and initial map points, and a many-to-many relationship between key frames and initial map points.

[0067] Specifically, in order to better obtain image frames that are significantly different from other image frames, the feature matching relationships between adjacent image frames can be obtained, and the poses corresponding to each image frame and the feature matching relationships between adjacent image frames are used as the calculation basis in the key frame screening process. The pose data of the image frames can calculate the change distance between different image frames. When the change distance reaches a certain threshold, it indicates that the image information between the two image frames is likely to change significantly, and at this time, there will also be more characteristic information in the image frames. The feature matching relationship between image frames can be used to calculate the feature matching rate between feature points in the image frames. The lower the feature matching rate, the greater the difference in image information between the two images, and at this time, there will also be more characteristic information in the image frames. Moreover, the key frames contain information with relatively large differences from the features of other image frames, which can better reflect the characteristics of the environment in the image and play a good role in the construction of the final fused map. The image frame corresponding to the latest timestamp when the distance difference between two adjacent image frames in the image frame set is relatively large can be selected as the key frame. Among them, the image frame corresponding to the latest timestamp can be understood according to the following example: for example, two adjacent image frames are a and b, the timestamp corresponding to a is t1, the timestamp corresponding to b is t2, and t1 < t2, that is, t2 is closer to the current time, then the image frame b corresponding to t2 is used as the image frame corresponding to the latest timestamp. The image frame corresponding to the latest timestamp with the feature matching rate between two adjacent image frames less than the preset feature matching rate can also be selected from the image frame set as the key frame, so that multiple key frame sets can be screened from the image frame set to form a key frame set.

[0068] In one embodiment, to construct the fused map, the corresponding initial map point set needs to be obtained. The initial map points can be created according to the feature points that are matched in the matching relationship corresponding to the adjacent image frames, and the corresponding relationship between each initial map point in the initial map point set and each key frame in the key frame set is established (actually, the corresponding relationship between the initial map points and the feature points in the key frames is constructed). That is, each initial map point stores the positions of the corresponding feature points in each key frame corresponding to it, and each key frame stores the positions of the initial map points corresponding to each feature point in the key frame, which further prepares data for the construction of the final fused map.

[0069] Step S104: Select the current key frame and the corresponding relevant key frame set corresponding to the current key frame from the key frame set in sequence. Based on the feature matching relationship between the current key frame and the relevant key frame set, update the initial map point set and the corresponding relationship set to obtain an updated map point set and an updated corresponding relationship set.

[0070] Among them, the current key frame refers to the key frame selected during the current execution process. The related key frame set refers to the set of key frames with a preset distance from the current key frame. The updated map point set refers to the set of map points obtained after adding new map points and deleting the original map points in the initial map point set. The updated correspondence set refers to the correspondence set after updating the correspondence between the initial map points and the key frames in the initial map point set.

[0071] Specifically, when selecting the relevant keyframe set corresponding to the current keyframe, a preset distance can be customized, and then all keyframes far from the current keyframe preset distance can be obtained according to the position of the keyframe. The set of these keyframes is the relevant keyframe. For example, if the preset distance is 3m, the relevant keyframe set with a distance of 3m around it can be obtained according to the position of the current keyframe. In order to obtain more effective map point information, the initial map point set and the information in the corresponding relationship set can be updated according to the feature matching relationship between the current keyframe and the relevant keyframe. That is, the feature points in each keyframe can be matched with the feature points corresponding to each relevant keyframe in its corresponding relevant keyframe set, so as to determine the feature matching relationship between each current keyframe and the corresponding relevant keyframe, and the feature matching relationship can be used as the basis for judging the update of the initial map point set and the corresponding relationship set.

[0072] The understanding of feature matching between the current key frame and the related key frames in the related key frame set can be understood by the following example: for example, the current key frame is K, and a related key frame in the related key frame set is P. The feature points corresponding to the current key frame K are matched with the feature points corresponding to the related key frame P to obtain the feature matching relationship between the two key frames; if the feature point k1 in the current key frame K matches the feature point p1 in the related key frame P, then it is determined whether the initial map point Q1 corresponding to the feature point k1 is consistent with the initial map point Q2 corresponding to the feature point p1; if not, If they are consistent, the initial map point Q1 and the initial map point Q2 are merged to generate an updated map point Q3 and a new correspondence between Q3 and the feature points k1 and p1 (that is, the correspondence between the updated map point Q3 and the current key frame K and the related key frame P), and the initial map points Q1 and Q2 are deleted from the initial map point set and the correspondence set, and the updated map point Q3 is added to the initial map point set and the new correspondence between the updated map point Q3 and the feature points k1 and p1 is added to the correspondence set, thereby obtaining an updated map point set and an updated correspondence set.

[0073] Step S106, converting the updated map point set into a three-dimensional map point set, and constructing a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and posture of each key frame in the updated correspondence set.

[0074] Among them, the three-dimensional map point set refers to the map points with three-dimensional positions, which can be calculated by triangulation technology according to the pose of the key frame corresponding to each map point in the updated map point set and the pixel position of each map point in the corresponding key frame. The target fusion map refers to the visual map obtained after fusing the laser pose data, which is used to realize the positioning operation of the robot in various scenarios.

[0075] Specifically, the corresponding three-dimensional information of each updated map point in the updated map point set has not been calculated, and the creation of the visual map after the fusion of laser posture data has not been completed. The triangulation technology can be used to generate a corresponding three-dimensional map point set according to the posture of the key frames corresponding to each updated map point in the updated map point set and the imaging position (pixel position) of each updated map point on the corresponding key frames, and the three-dimensional map points in the three-dimensional map point set are optimized and deleted. In the process of optimizing and deleting the three-dimensional map point set, the correspondence between the corresponding map points in the correspondence relationship set (the map points are also converted into three-dimensional map points at this time) and the key frames is also updated to obtain more accurate three-dimensional map point positions and three-dimensional map points with corresponding relationship information with the key frames, and then the target fusion map is obtained, realizing the construction of a fused visual map by the laser posture fusion visual map, and the target fusion map can be used to control the positioning operation of the robot to realize the positioning operation of the robot in multiple scenes.

[0076] The above-mentioned fusion map construction method interpolates each image frame in the image frame set collected by the image acquisition device through the laser pose data set obtained by laser radar mapping to obtain each pose corresponding to each image frame, and then selects a key frame set from the image frame set based on the pose corresponding to each image frame and the feature matching relationship between each adjacent image frame obtained, and constructs a correspondence relationship set between each key frame and each initial map point based on the key frame set and the obtained initial map point set, and updates the initial map point set and the correspondence relationship set based on the current key frame in the key frame set and the related key frame set corresponding to the current key frame in turn to obtain an updated map point set and an updated correspondence relationship set, and converts the updated map point set into a three-dimensional map point set. Finally, based on the three-dimensional map point set, the updated correspondence relationship set and the pose of each key frame in the updated correspondence relationship set, a target fusion map that can be used to control the positioning operation of the robot is constructed, which realizes the fusion mapping of laser pose data and visual maps, and effectively combines the characteristics of laser maps and visual maps, so that the robot can efficiently and correctly locate and operate in multiple scenarios based on the fusion map, thereby expanding the environmental range of the robot's normal positioning operation.

[0077] In one embodiment, Figure 2 As shown, step S100 includes:

[0078] Step S200: Sequentially select the current image frame from the image frame set. Based on the current timestamp corresponding to the current image frame, obtain the forward laser pose corresponding to the current timestamp, the forward timestamp corresponding to the forward laser pose, the backward laser pose, and the backward timestamp corresponding to the backward laser pose from the laser pose dataset.

[0079] Among them, the current image frame refers to the initial image frame corresponding to the current interpolation operation process. A timestamp refers to data that can represent that a piece of data has existed before a specific time, is complete, and verifiable. Usually, it is a character sequence that uniquely identifies a moment in time. The current timestamp refers to the timestamp corresponding to the current image frame. The forward laser pose refers to the laser pose corresponding to the previous timestamp relative to the current timestamp. The forward timestamp refers to the previous timestamp relative to the current timestamp. The backward laser pose refers to the laser pose corresponding to the next timestamp relative to the current timestamp. The backward timestamp refers to the next timestamp relative to the current timestamp.

[0080] Specifically, each image frame in the image frame set records a corresponding timestamp, and each laser pose in the laser pose dataset also records a corresponding timestamp. The pose information corresponding to the same timestamp in the laser pose dataset can be determined according to the timestamp corresponding to the current image frame, that is, the forward laser pose, the forward timestamp corresponding to the forward laser pose, the backward laser pose, and the backward timestamp corresponding to the backward laser pose. The acquisition of these data prepares the data for calculating the poses corresponding to each image frame in the subsequent process.

[0081] Step S202: Perform interpolation operations on the current timestamp, forward timestamp, backward timestamp, forward laser pose, and backward laser pose to obtain the pose corresponding to the current image frame.

[0082] Specifically, the poses corresponding to each image frame can be calculated according to the current timestamp, forward laser pose, forward timestamp corresponding to the forward laser pose, backward laser pose, and backward timestamp corresponding to the backward laser pose corresponding to each image frame. Further, the pose corresponding to the image frame can be regarded as T, the corresponding current timestamp as t, the forward laser pose as T1, the forward timestamp corresponding to the forward laser pose as t1, the backward laser pose as T2, and the backward timestamp corresponding to the backward laser pose as t2, where t1 < t < t2. Through the uniform motion model, we can get (T - T1) / (t - t1) = (T2 - T1) / (t2 - t1), that is, the pose T corresponding to the image frame = T1 + (T2 - T1)(t - t1) / (t2 - t1), thus completing the interpolation operations on the current timestamp t, forward timestamp t1, backward timestamp t2, forward laser pose T1, and backward laser pose T2.

[0083] In the above embodiment, the current image frame is selected from the image frame set in sequence, and based on the current timestamp corresponding to the current image frame, the forward laser pose corresponding to the current timestamp, the forward timestamp corresponding to the forward laser pose, the backward laser pose and the backward timestamp corresponding to the backward laser pose are obtained from the laser pose data set, and the current timestamp, the forward timestamp, the backward timestamp, the forward laser pose and the backward laser pose are interpolated to obtain the pose corresponding to the current image frame, completing the operation of interpolating the laser pose into the image frame, and realizing the fusion of laser pose data and visual images, which serves as an important data foundation for the subsequent construction of a fused visual map, is facilitating the positioning and operation of the robot application in multiple scenarios using the map finally fused, and further facilitating the expansion of the environmental range of the robot's normal positioning and operation.

[0084] In one embodiment, Figure 3 As shown, step S102 includes:

[0085] Step S300 , performing a feature point extraction operation on each image frame in the image frame set to obtain each feature point corresponding to each image frame, wherein the feature point includes a descriptor.

[0086] Feature point extraction refers to the operation of extracting feature points corresponding to each image frame in an image frame set, which can be extracting ORB (Oriented FAST and Rotated BRIEF) feature points, SIFT (Scale-Invariant Feature Transform) feature points, or other deep learning feature points. A descriptor is a data structure that describes features. The dimension of a descriptor can be multi-dimensional, which can represent the orientation of feature points and surrounding pixel information.

[0087] Step S302 : selecting the first image frame and the second image frame adjacent to each image frame in sequence from the image frame set, and obtaining a first descriptor corresponding to the first image frame and a second descriptor corresponding to the second image frame.

[0088] The first image frame refers to the image frame with the earlier corresponding timestamp among the two adjacent image frames. The second image frame refers to the image frame with the later corresponding timestamp among the two adjacent image frames. The first descriptor refers to the descriptor corresponding to each feature point in the first image frame. The second descriptor refers to the descriptor corresponding to each feature point in the second image frame.

[0089] Specifically, the construction of the fusion map requires the feature matching relationship of each feature point in the image frame as the data basis. The ORB feature points (or SIFT feature points, etc.) corresponding to each image frame can be extracted first to obtain the feature point information in each image frame. In order to select the key frame containing more effective image information from the image frame, a corresponding judgment can be made based on the feature matching relationship between adjacent image frames to decide which image frames to use as key frames. In the calculation process of the feature matching relationship between adjacent image frames, the descriptors corresponding to the two adjacent image frames are needed, that is, the first descriptor corresponding to each feature point in the first image frame and the second descriptor corresponding to each feature point in the second image frame, so as to prepare data for the subsequent matching of feature points.

[0090] Step S304, traverse the first descriptors corresponding to the first image frame, take the descriptor in the second descriptors that is most similar to the first descriptor as the first target descriptor, and establish a first corresponding relationship between the first descriptor and the first target descriptor.

[0091] The first object descriptor refers to the second descriptor that is most similar to the first descriptor corresponding to the first image frame. The first corresponding relationship refers to the corresponding relationship between the first descriptor in the first image frame and the first object descriptor in the second image frame.

[0092] Specifically, for the feature matching relationship corresponding to the feature points, it can be constructed based on the corresponding relationship between the first descriptor corresponding to the first image frame and the second descriptor corresponding to the second image frame, and whether the corresponding relationship between the second descriptor corresponding to the second image frame and the first descriptor corresponding to the first image frame is consistent. Prior to this, each first descriptor in the first image frame can be traversed and searched, and at the same time, the second descriptor that is most similar to each first descriptor can be found in the second image frame, and each most similar second descriptor is used as the first target descriptor corresponding to each first descriptor, and then the first corresponding relationship between each first descriptor and each corresponding first target descriptor is constructed, so as to provide comparison data for whether the feature points in the subsequent first image frame match the feature points in the second image frame.

[0093] Step S306, traverse the second descriptors corresponding to the second image frame, take the descriptor in the first descriptors that is most similar to the second descriptor as the second target descriptor, and construct a second corresponding relationship between the second descriptor and the second target descriptor.

[0094] The second target descriptor refers to the first descriptor that is most similar to the second descriptor in the current traversal step. The second corresponding relationship refers to the corresponding relationship between the second descriptor in the second image frame and the corresponding second target descriptor in the first image frame.

[0095] Specifically, in order to detect the correspondence between each first descriptor in the first image frame and each second descriptor in the second image frame, and whether the correspondence between each second descriptor in the second image frame and each first descriptor in the first image frame is the same, it is possible to traverse and search for each second descriptor in the second image frame, and at the same time find the first descriptor that is most similar to each second descriptor in the first image frame, and use each most similar first descriptor as the second target descriptor corresponding to each second descriptor, and then construct a second correspondence between each second descriptor in the second image frame and the corresponding second target descriptors, so as to provide a comparison basis for whether the feature points in the subsequent first image frame match the feature points in the second image frame.

[0096] Step S308 , matching feature points corresponding to descriptors having consistent relationships in the first correspondence and the second correspondence, to obtain feature matching relationships corresponding to each of the first image frames and each of the second image frames in the image frame set.

[0097] Specifically, when the relationship between the first correspondence and the second correspondence is consistent, it means that the feature points corresponding to the corresponding descriptors are matched, and the feature points corresponding to each descriptor with consistent correspondence are matched to obtain the feature matching relationship between each first image frame and the second image frame in the image frame set. Furthermore, the adjacent first image frame can be regarded as P1, the second image frame can be regarded as P2, the first descriptor corresponding to the first image frame P1 can be regarded as d1, the second descriptor corresponding to the second image frame P2 can be regarded as d2, the second descriptor d2 which is most similar to the first descriptor d1 can be found in the second image frame P2, and the first correspondence relationship between the first descriptor d1 in the first image frame P1 and the second descriptor d2 which is most similar to the second image frame P2 can be constructed. For example, the first correspondence relationship includes "d1_1—d2_1, d1_3—d2_3" (d1_1 and d1_3 are the first descriptors in the first image frame; d2_1 and d2_3 are the second descriptors in the second image frame. Similarly, as long as there is d1_, they are all the first descriptors in the first image frame, and as long as there is d2_, they are all the second descriptors in the second image frame); conversely, in the first The first descriptor d1 that is most similar to the second descriptor d2 is found in the image frame P1, and a second correspondence relationship between the second descriptor d2 in the second image frame P2 and the first descriptor d2 that is most similar to the first image frame P1 is constructed. For example, the second correspondence relationship includes "d2_1-d1_5, d2_3-d1_3". From the example of the correspondence relationship, it can be seen that the first correspondence relationship "d1_1-d2_1" is inconsistent with the second correspondence relationship "d2_1-d1_5", and the feature matching relationship between the feature points corresponding to the first descriptor d1_1 and the feature points corresponding to the second descriptor d2_1 is not constructed. However, the first correspondence relationship "d1_3-d2_3" is consistent with the second correspondence relationship "d2_3-d1_3", and the feature matching relationship between the feature points corresponding to the first descriptor d1_3 and the feature points corresponding to the second descriptor d2_3 is constructed.

[0098] In the above embodiment, feature points of each image frame in the image frame set are extracted to obtain feature points including descriptions corresponding to each image frame, and each descriptor in the adjacent first image frame and second image frame is traversed in turn to construct a first correspondence between each first descriptor in the first image frame and each corresponding first target descriptor in the second image frame, and a second correspondence between each second descriptor in the second image frame and each corresponding second target descriptor in the first image frame. Based on the comparison between the first correspondence and the second correspondence, each feature point in the first image frame is matched with each feature point in the second image frame to obtain a corresponding feature matching relationship between each first image frame and each second image frame in the image frame set, thereby achieving feature point matching of adjacent image frames and constructing a feature matching relationship between feature points corresponding to adjacent image frames. This provides a data basis for the creation of key frames and map points in subsequent processes, and improves the execution efficiency of creating a visual map that incorporates laser pose data to a certain extent.

[0099] In one embodiment, Figure 4 As shown, step S102 also includes:

[0100] Step S400, based on the posture corresponding to the first image frame and the posture corresponding to the second image frame adjacent to each image frame in the image frame set, calculate the change distance corresponding to each first image frame and the second image frame in the image frame set, and take the second image frame corresponding to the change distance greater than the preset change distance as the key frame in the key frame set.

[0101] The change distance refers to the distance between adjacent image frames, and the preset change distance refers to the value set when defining key frames.

[0102] Specifically, the selection of key frames mainly considers whether there is a large feature information gap between adjacent image frames. The change distance between adjacent image frames can be calculated according to the corresponding postures of the adjacent image frames. The larger the change distance, the more feature points with differences in the image, and the more changes in the corresponding individual characteristics and special features between the image frames. At this time, a corresponding preset change distance can be set. When the change distance between adjacent image frames exceeds the preset change distance, the second image frame can be used as a key frame in the key frame set. For example, the change distance between the adjacent first image frame P1 and the second image frame P2 is 1m, and the preset change distance is 0.5m. The change distance 1m is greater than the preset change distance 0.5m, so the second image frame P2 can be used as a key frame.

[0103] Step S402, based on the feature matching relationship between the first image frame and the second image frame adjacent to each image frame in the image frame set, calculate the feature matching rate corresponding to each first image frame and the second image frame in the image frame set, and take the second image frame with a feature matching rate less than a preset matching rate as a key frame in the key frame set.

[0104] The feature matching rate refers to the probability of feature points of two adjacent frames matching, which can be obtained by dividing the number of matching feature points by the total number of feature points corresponding to the first image frame. The preset matching rate refers to the probability value set for determining the key frame, which can be set according to actual conditions.

[0105] Specifically, for the creation of key frames, the feature matching relationship corresponding to each first image frame and the second image frame in the image frame set can be used to count the number of matching feature points corresponding to adjacent image frames, and divide it by the total number of feature points corresponding to the corresponding first image frame in the adjacent image frames to obtain the feature matching rate corresponding to each first image frame and the second image frame in the image frame set, and then the corresponding second image frame when the feature matching rate is less than the preset feature matching rate is used as the key frame in the key frame set.

[0106] In the above embodiment, the change distance corresponding to each first image frame and the second image frame in the image frame set is calculated based on the posture corresponding to the first image frame adjacent to each image frame in the image frame set and the posture corresponding to the second image frame, and the second image frame corresponding to the change distance greater than the preset change distance is used as the key frame in the key frame set, or, based on the feature matching relationship corresponding to the first image frame and the second image frame adjacent to each image frame in the image frame set, the feature matching rate corresponding to each first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the feature matching rate less than the preset matching rate is used as the key frame in the key frame set, thereby realizing the creation of key frames. The two-dimensional consideration of key frame creation can better obtain more useful information in the key frames, which is conducive to creating a target fusion map that can locate the actual environment more accurately.

[0107] In one embodiment, Figure 5 As shown, step S104 includes:

[0108] Step S500, obtaining a preset distance, selecting a current key frame from a key frame set in turn, and selecting a related key frame set corresponding to the current key frame from the key frame set based on a posture corresponding to the current key frame and the preset distance.

[0109] The preset distance refers to the distance set for selecting key frames around the current key frame.

[0110] Step S502: performing feature matching on the current key frame and each related key frame in the related key frame set to obtain a feature matching relationship between the current key frame and each related key frame.

[0111] Specifically, the keyframe contains more useful information about environmental changes. The corresponding preset distance can be determined according to the actual situation. According to the posture corresponding to the current keyframe, the related keyframes that are far from the preset distance around the current keyframe are found. There may be large differences in image information between different keyframes. By comparing the related keyframes that are far from the preset distance with the current keyframe, it is helpful to optimize the image after the laser posture is fused, thereby improving the effectiveness of the fused map information in the final target fused map.

[0112] Step S504, when the initial map point in the initial map point set corresponding to the current matching feature point of the current key frame is inconsistent with the initial map point corresponding to the current relevant matching feature point of the relevant key frame, the initial map point corresponding to the current matching feature point is merged with the initial map point corresponding to the current relevant matching feature point to obtain an updated map point.

[0113] The current matching feature point refers to the feature point in the current related key frame being traversed in the current traversal step. The current related matching feature point refers to the feature point in the current related key frame being traversed.

[0114] Specifically, when the initial map point corresponding to the current matching feature point is inconsistent with the initial map point corresponding to the current related matching feature point, the two inconsistent initial map points can be merged to generate the corresponding updated map point. For example, the current key frame is K, the related key frame in the related key frame set is P, and the initial map point corresponding to the current matching feature point k1 in the current key frame K is inconsistent with the initial map point corresponding to the current related matching feature point p2 in the related key frame P. At this time, the initial map point corresponding to the current matching feature point k1 is Q1, and the initial map point corresponding to the current related matching feature point p2 is Q2. The corresponding relationship between the initial map point Q1 and the initial map point Q2 and the current matching feature point k1 and the current related matching feature point p2 is Q1<—>k1, Q2<—>p2. The initial map point Q1 and the initial map point Q2 can be merged to generate the corresponding updated map point Q3, where "<—>" refers to a bidirectional corresponding relationship.

[0115] Step S506, adding the updated map point to the initial map point set, deleting the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point from the initial map point set, changing the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point in the correspondence set to the updated map point, and obtaining the updated correspondence and the updated map point.

[0116] Specifically, the newly generated updated map points can be added to the initial map point set, and the initial map points whose current matching feature points are inconsistent with the current related matching feature points can be removed from the initial map point set to update the initial map point set and obtain the updated map point set. In addition, the correspondence between the updated map points and the corresponding key frames and the feature points in the key frames can be changed in the correspondence set to update the correspondence between the updated map points and the corresponding key frames. According to the example described in step S504, it can be further understood that the updated map point Q3 is added to the initial map point set, and the initial map point Q1 and the initial map point Q2 are deleted from the initial map point set. At this time, the correspondence between the map points (including the initial map points Q1, Q2, and the updated map point Q3) and the feature points (including the current matching feature point k1 and the current related matching feature point p2) also needs to be changed, from the original "Q1<—>k1, Q2<—>p2" to "Q3<—>k1 and p2". The change in the correspondence between the feature points and the map points also represents the change in the correspondence between the map points and the corresponding key frames, thereby realizing the update of the initial map points and the corresponding relationship.

[0117] Step S508, traverse the key frames in the key frame set to obtain an updated map point set and an updated corresponding relationship set.

[0118] In the above embodiment, by selecting a relevant key frame set corresponding to the current key frame based on a preset distance and a posture corresponding to the current key frame, and performing feature matching on the current key frame and each relevant key frame in the relevant key frame set, a feature matching relationship that can be used as a basis for optimizing the key frame and the initial map point is obtained. When the initial map point corresponding to the current matching feature point corresponding to the current key frame is inconsistent with the initial map point corresponding to the current relevant matching feature point corresponding to the relevant key frame, the initial map point corresponding to the previous matching feature point is merged with the initial map point corresponding to the current relevant matching feature point, and the updated map point is added to the initial map point set, and the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current relevant matching feature point are deleted from the initial map point set, and the initial map point corresponding to the current matching feature point is deleted. The initial map point corresponding to the current matching feature point in the relationship set and the initial map point corresponding to the current relevant matching feature point are changed to updated map points to obtain updated correspondences and updated map points, and the key frames in the key frame set are repeatedly traversed until the updated map point set and the updated correspondence set are obtained, thereby realizing the update of the initial map point set and the correspondence set, making the data information in the updated map point set and the updated correspondence set more accurate, and more conducive to improving the accuracy of constructing a fused map by fusing the laser pose and the visual map, so that the fused map can better integrate the corresponding functionality of the laser map and the visual map, so that the robot can be positioned in the corresponding scene environment during laser positioning and visual positioning according to the fused map, thereby being beneficial to expanding the environmental range of the robot during normal operation to a certain extent.

[0119] In one embodiment, step S106 includes: based on the pose of the key frame corresponding to each updated map point in the updated map point set and the pixel coordinates of each updated map point in the corresponding key frame, converting the updated map point set into a three-dimensional map point set.

[0120] The pixel position refers to the pixel coordinates of the updated map point on the corresponding key frame.

[0121] Specifically, the map points in the updated map point set are still map points in two-dimensional positions. Through triangulation technology, according to the posture of the key frames corresponding to each updated map point and the pixel coordinates of each updated map point on the corresponding key frames, the three-dimensional map points corresponding to each updated map point can be calculated and generated, thereby providing a data basis for the subsequent generation of the target fusion map.

[0122] In the above embodiment, the updated map point set is converted into a three-dimensional map point set based on the posture of the key frames corresponding to each updated map point in the updated map point set and the pixel coordinates of each updated map point on the corresponding key frames, thereby realizing the creation of a three-dimensional map point set, which provides an important data basis for the subsequent generation of the final target fusion map.

[0123] In one embodiment, Figure 6 As shown, step S106 includes:

[0124] Step S600, calculating the total reprojection errors of each 3D map point in the 3D map point set on each corresponding key frame, and when the total reprojection error is greater than a preset number of pixels, deleting the correspondence between the 3D map point and the corresponding key frame from the updated correspondence set to obtain an intermediate correspondence set.

[0125] The total reprojection error refers to the error between the reprojection of the 3D map point in the corresponding key frame and the first projection, which can be obtained by reprojecting the 3D map point to the corresponding key frames and calculating the sum of the reprojection errors of the 3D map point on each corresponding key frame. The preset number of pixels refers to the value defined for judging the accuracy of the 3D map point. The intermediate correspondence set refers to the correspondence set obtained after deleting the correspondence between the 3D map point and the key frame in the updated correspondence set.

[0126] Specifically, in order to obtain more accurate three-dimensional map point location information, the position corresponding to each three-dimensional map point can be reprojected onto the corresponding key frames, and the corresponding reprojection points can be recorded. These reprojection points are compared with the feature points corresponding to the three-dimensional map points. If the reprojection points are consistent with the feature points, it means that there is almost no error between the reprojection points and the feature points, and then the total reprojection error of each three-dimensional map point on each corresponding key frame is calculated. To obtain more accurate three-dimensional map point location information, the corresponding preset number of pixels can be set according to specific needs. When the total reprojection error is less than or equal to the preset number of pixels, it means that the three-dimensional location information corresponding to the three-dimensional map point is more accurate; when the total reprojection error is greater than the preset number of pixels, it means that the three-dimensional location information corresponding to the three-dimensional map point is too different from the actual one. If the three-dimensional map point whose location information is greatly different from the actual one is used, it will have an adverse effect on the positioning operation of the robot. Therefore, the correspondence between the corresponding three-dimensional map point and the corresponding key frame when the total reprojection error is greater than the preset number of pixels can be deleted from the updated correspondence set, that is, the correspondence between the three-dimensional map point and the corresponding feature point in the key frame is deleted, and the data information of these correspondences is no longer used in the control of the robot positioning operation, thereby avoiding the interference of invalid data.

[0127] Step S602, selecting the current 3D map point from the 3D map point set in turn, calculating the number of corresponding key frames corresponding to the current 3D map point, and when the number of corresponding key frames is less than a preset number, deleting the current 3D map point from the 3D map point set, deleting the correspondence between the current 3D map point and the key frame corresponding to the current 3D map point from the intermediate correspondence set, and obtaining a target 3D map point set and a target correspondence set.

[0128] Among them, the current 3D map point refers to the 3D map point selected from the 3D map point set during the current execution process. The number of corresponding key frames refers to the number of key frames corresponding to the 3D map point. The preset number refers to the value used to define the deletion of the 3D map point and its corresponding key frame. The target correspondence set refers to the data information of the correspondence between the 3D map point and the corresponding key frame required to form the final target fusion map.

[0129] Specifically, when the number of key frames corresponding to a three-dimensional map point is small, the accuracy of the key information that can be obtained is not guaranteed. At this time, the map data information obtained may cause the robot to have low accuracy in positioning and operation in multiple scenes. Therefore, the number of key frames corresponding to each three-dimensional map point (i.e., the number of corresponding key frames) can be calculated, and the preset number determined according to actual needs can be compared with the corresponding number of key frames, and the current three-dimensional map point whose number of corresponding key frames is less than the preset number can be deleted from the three-dimensional map point set, and the correspondence between the current three-dimensional map point and the key frame corresponding to the current three-dimensional map point can be deleted from the intermediate correspondence set, thereby appropriately deleting some data information that affects the accuracy of the information corresponding to the final generated target fusion map, thereby obtaining a target three-dimensional map point set and a target correspondence set with higher accuracy.

[0130] Step S604: construct a target fusion map based on the target three-dimensional map point set, the target correspondence set, and the position and posture of each key frame in the target correspondence set.

[0131] Specifically, after layers of information optimization, the accuracy of the target three-dimensional map point set, the target correspondence set, and the data information of the position and posture of each key frame in the target correspondence set are all high. The combination of these data can make the constructed target fusion map closer to the positioning environment of the actual scene, so that the robot using the target fusion map can accurately and efficiently locate and operate in multiple scenes.

[0132] In the above embodiment, by comparing the calculated total projection error corresponding to each three-dimensional map point with a preset number of pixels, the correspondence between the three-dimensional map point and the corresponding key frame is optimized, thereby obtaining an intermediate correspondence set with more accurate data information; then, the number of corresponding key frames corresponding to each three-dimensional map point is compared with the preset number, and the three-dimensional map points associated with fewer key frames and the correspondence between the key frames corresponding to them are deleted from the three-dimensional map points and the intermediate correspondence set, respectively, thereby achieving further optimization of the data information and obtaining a target three-dimensional map point set and a target correspondence set with higher accuracy; a target three-dimensional map point set with higher data information accuracy, a target correspondence set, and the position and posture of each key frame in the target correspondence set are used to construct a target fusion map, so that the data information corresponding to the constructed target fusion map is closer to the real scene, and the accuracy of its data information positioning is higher, thereby ensuring that the robot accurately and efficiently locates and operates in multiple scenes when applying the target fusion map, thereby greatly expanding the environmental range of normal operation of the robot.

[0133] In one embodiment, the laser pose data obtained by the robot through 2D laser radar in a dark scene can be used as a laser pose data set, and visual SLAM is used for positioning in a high dynamic environment, and the image data obtained by collecting the camera is used as an image frame set. Through the obtained image frame set and laser pose data set, an interpolation operation is performed on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame; the feature matching relationship between each adjacent image frame is obtained, and based on the pose and feature matching relationship corresponding to each image frame, a key frame set is screened out from the image frame set, and an initial map point set corresponding to the key frame set is obtained, and a corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set is constructed to obtain a corresponding relationship set; the current key frame and the related key frames corresponding to the current key frame are sequentially selected from the key frame set. Key frame set, based on the feature matching relationship between the current key frame and the related key frame set, the initial map point set and the correspondence set are updated to obtain the updated map point set and the updated correspondence set; based on the triangulation technology, the updated map point set is converted into a three-dimensional map point set, and then a target fusion map that integrates laser pose data and visual map data is constructed according to the three-dimensional map point set, the updated correspondence set and the pose of each key frame in the updated correspondence set, so that the robot can use the target fusion map to efficiently and accurately locate and operate in dark environments and high dynamic environments, thereby effectively avoiding the problem that the robot is disturbed by dynamic objects when only laser positioning is used, and cannot be correctly located and operated in dark environments when only visual positioning is used, thereby greatly expanding the robot's operating environment and expanding the environmental range of the robot's normal positioning and operation.

[0134] In one embodiment, Figure 7 As shown, a fusion map construction device is provided, which can be a part of the robot using a software module or a hardware module, or a combination of the two. The device specifically includes: an interpolation module 700, a matching module 702, an update module 704 and a construction module 706, wherein:

[0135] The interpolation module 700 is used to obtain an image frame set captured by an image acquisition device and a laser pose data set obtained by laser radar mapping, and perform an interpolation operation on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame.

[0136] The matching module 702 is used to obtain the feature matching relationship between each adjacent image frame, filter out a key frame set from the image frame set based on the posture corresponding to each image frame and the feature matching relationship, obtain the initial map point set corresponding to the key frame set, and construct the corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set to obtain a corresponding relationship set.

[0137] The updating module 704 is used to select the current key frame and the related key frame set corresponding to the current key frame from the key frame set in sequence, and update the initial map point set and the corresponding relationship set based on the feature matching relationship between the current key frame and the related key frame set to obtain an updated map point set and an updated corresponding relationship set.

[0138] The construction module 706 is used to convert the updated map point set into a three-dimensional map point set, and construct a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and posture of each key frame in the updated correspondence set.

[0139] In the above embodiment, each image frame in the image frame set collected by the image acquisition device is interpolated by the laser pose data set obtained by laser radar mapping to obtain each pose corresponding to each image frame, and then a key frame set is screened from the image frame set based on the pose corresponding to each image frame and the feature matching relationship between each adjacent image frame obtained, and a correspondence set between each key frame and each initial map point is constructed based on the key frame set and the obtained initial map point set, and the initial map point set and the correspondence set are updated in turn based on the current key frame in the key frame set and the related key frame set corresponding to the current key frame to obtain an updated map point set and an updated correspondence set, and the updated map point set is converted into a three-dimensional map point set, and finally, a target fusion map that can be used to control the positioning operation of the robot is constructed based on the three-dimensional map point set, the updated correspondence set, and the pose of each key frame in the updated correspondence set, thereby realizing the fusion mapping of laser pose data and visual maps, and effectively combining the characteristics of laser maps and visual maps, so that the robot can efficiently and correctly locate and operate in multiple scenarios based on the fusion map, thereby expanding the environmental range of the normal positioning operation of the robot.

[0140] In one embodiment, the interpolation module 700 is also used to select the current image frame from the image frame set in sequence, and based on the current timestamp corresponding to the current image frame, obtain the forward laser pose corresponding to the current timestamp, the forward timestamp corresponding to the forward laser pose, the backward laser pose and the backward timestamp corresponding to the backward laser pose from the laser pose data set; interpolate the current timestamp, the forward timestamp, the backward timestamp, the forward laser pose and the backward laser pose to obtain the pose corresponding to the current image frame.

[0141] In one embodiment, the matching module 702 is further used to perform a feature point extraction operation on each image frame in the image frame set to obtain each feature point corresponding to each image frame, wherein the feature point includes a descriptor; sequentially select a first image frame and a second image frame adjacent to each of the image frames from the image frame set to obtain a first descriptor corresponding to the first image frame and a second descriptor corresponding to the second image frame; traverse the first descriptor corresponding to the first image frame, take the descriptor in the second descriptor that is most similar to the first descriptor as the first target descriptor, and establish a first correspondence between the first descriptor and the first target descriptor; traverse the second descriptor corresponding to the second image frame, take the descriptor in the first descriptor that is most similar to the second descriptor as the second target descriptor, and establish a second correspondence between the second descriptor and the second target descriptor; match the feature points corresponding to the descriptors that have the same relationship in the first correspondence and the second correspondence to obtain a feature matching relationship between each of the first image frames and each of the second image frames in the image frame set.

[0142] In one embodiment, the matching module 702 is further configured to

[0143] Based on the posture corresponding to the first image frame and the posture corresponding to the second image frame adjacent to each of the image frames in the image frame set, the change distance corresponding to each of the first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the change distance greater than the preset change distance is used as the key frame in the key frame set;

[0144] or,

[0145] Based on the feature matching relationship between the first image frame and the second image frame adjacent to each image frame in the image frame set, the feature matching rate corresponding to each first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the feature matching rate less than the preset matching rate is used as the key frame in the key frame set.

[0146] In one embodiment, the updating module 704 is further used to obtain a preset distance, select a current key frame from the key frame set in turn, and select a related key frame set corresponding to the current key frame from the key frame set based on the posture corresponding to the current key frame and the preset distance; perform feature matching on the current key frame and each related key frame in the related key frame set to obtain a feature matching relationship between the current key frame and each related key frame; when an initial map point in the initial map point set corresponding to the current matching feature point of the current key frame is inconsistent with an initial map point corresponding to the current related matching feature point of the related key frame, merge the initial map point corresponding to the current matching feature point with the initial map point corresponding to the current related matching feature point to obtain an updated map point; add the updated map point to the initial map point set, delete the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point from the initial map point set, change the initial map point corresponding to the current matching feature point in the correspondence relationship set and the initial map point corresponding to the current related matching feature point into an updated map point, and obtain an updated correspondence relationship and an updated map point; traverse the key frames in the key frame set to obtain the updated map point set and the updated correspondence relationship set.

[0147] In one embodiment, the construction module 706 is further configured to convert the updated map point set into a three-dimensional map point set based on the pose of the key frame corresponding to each updated map point in the updated map point set and the pixel coordinates of each updated map point in the corresponding key frame.

[0148] In one embodiment, the construction module 706 is further configured to convert the updated map point set into a three-dimensional map point set based on the pose of the key frame corresponding to each updated map point in the updated map point set and the pixel coordinates of each updated map point in the corresponding key frame.

[0149] For the specific definition of the fusion map construction device, please refer to the definition of the fusion map construction method above, which will not be repeated here. Each module in the above-mentioned fusion map construction device can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the robot in the form of hardware, or can be stored in the memory of the robot in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0150] In one embodiment, a robot is provided, whose internal structure diagram can be shown as follows: Figure 8As shown. The robot includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the robot is used to provide computing and control capabilities. The memory of the robot includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the robot is used to communicate with an external robot in a wired or wireless manner, and the wireless manner can be achieved through WIFI, an operator network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a method for building a fusion map is implemented. The display screen of the robot can be a liquid crystal display screen or an electronic ink display screen, and the input device of the robot can be a touch layer covering the display screen, or a button, trackball or touchpad set on the robot shell, or an external keyboard, touchpad or mouse, etc.

[0151] Those skilled in the art will understand that Figure 8 The structure shown in the figure is merely a block diagram of a partial structure related to the scheme of the present application, and does not constitute a limitation on the robot to which the scheme of the present application is applied. The specific robot may include more or fewer components than shown in the figure, or combine certain components, or have a different arrangement of components.

[0152] In one embodiment, a robot is also provided, comprising a memory, a processor, and computer-readable instructions stored in the memory and executable on the processor, wherein the processor implements the steps in the above-mentioned method embodiments when executing the computer instructions.

[0153] In one embodiment, a computer-readable storage medium is provided, storing a computer program, which implements the steps in the above method embodiments when executed by a processor.

[0154] In one embodiment, a computer program product or computer program is provided, the computer program product or computer program comprising computer instructions, the computer instructions being stored in a computer-readable storage medium. A processor of a robot reads the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the robot performs the steps in the above-mentioned method embodiments.

[0155] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory or optical memory, etc. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0156] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0157] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A method for constructing a fusion map, characterized in that: The method is applied to a robot, which is equipped with a laser radar and an image acquisition device; the method comprises: Acquire an image frame set acquired by an image acquisition device and a laser pose data set obtained by laser radar mapping, and perform an interpolation operation on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame; Acquire a feature matching relationship between adjacent image frames, select a key frame set from the image frame set based on the pose corresponding to each image frame and the feature matching relationship, acquire an initial map point set corresponding to the key frame set, and construct a corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set to obtain a corresponding relationship set; Obtaining a preset distance, selecting a current key frame from the key frame set in turn, and selecting a related key frame set corresponding to the current key frame from the key frame set based on a posture corresponding to the current key frame and the preset distance; Performing feature matching on the current key frame and each related key frame in the related key frame set to obtain a feature matching relationship between the current key frame and each related key frame; When the initial map point in the initial map point set corresponding to the current matching feature point of the current key frame is inconsistent with the initial map point corresponding to the current relevant matching feature point of the relevant key frame, the initial map point corresponding to the current matching feature point is merged with the initial map point corresponding to the current relevant matching feature point to obtain an updated map point; adding the updated map point to the initial map point set, deleting the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point from the initial map point set, and changing the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point in the correspondence relationship set to the updated map point, thereby obtaining an updated correspondence relationship and an updated map point; Traversing the key frames in the key frame set to obtain an updated map point set and an updated corresponding relationship set; The updated map point set is converted into a three-dimensional map point set, and a target fusion map is constructed based on the three-dimensional map point set, the updated correspondence set, and the position and posture of each key frame in the updated correspondence set.

2. The method according to claim 1, characterized in that The interpolation operation is performed on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame, including: Select a current image frame from the image frame set in sequence, and based on a current timestamp corresponding to the current image frame, obtain a forward laser pose corresponding to the current timestamp, a forward timestamp corresponding to the forward laser pose, a backward laser pose, and a backward timestamp corresponding to the backward laser pose from the laser pose data set; The current timestamp, the forward timestamp, the backward timestamp, the forward laser pose and the backward laser pose are interpolated to obtain the pose corresponding to the current image frame.

3. The method according to claim 1, characterized in that The acquiring of the feature matching relationship between adjacent image frames comprises: Performing a feature point extraction operation on each image frame in the image frame set to obtain feature points corresponding to each image frame, wherein the feature points include descriptors; Selecting in sequence from the image frame set a first image frame and a second image frame adjacent to each of the image frames, and obtaining a first descriptor corresponding to the first image frame and a second descriptor corresponding to the second image frame; Traversing the first descriptors corresponding to the first image frame, taking the descriptor in the second descriptors that is most similar to the first descriptor as the first target descriptor, and establishing a first corresponding relationship between the first descriptor and the first target descriptor; Traversing the second descriptors corresponding to the second image frame, taking the descriptor in the first descriptors that is most similar to the second descriptor as the second target descriptor, and constructing a second corresponding relationship between the second descriptor and the second target descriptor; Feature points corresponding to the descriptors having consistent relationships in the first corresponding relationship and the second corresponding relationship are matched to obtain feature matching relationships corresponding to each of the first image frames and each of the second image frames in the image frame set.

4. The method according to any one of claims 1 to 3, characterized in that The filtering out a key frame set from the image frame set based on the posture corresponding to each image frame and the feature matching relationship comprises: Based on the posture corresponding to the first image frame and the posture corresponding to the second image frame adjacent to each of the image frames in the image frame set, the change distance corresponding to each of the first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the change distance greater than the preset change distance is used as the key frame in the key frame set; or, Based on the feature matching relationship between the first image frame and the second image frame adjacent to each image frame in the image frame set, the feature matching rate corresponding to each first image frame and the second image frame in the image frame set is calculated, and the second image frame corresponding to the feature matching rate less than the preset matching rate is used as the key frame in the key frame set.

5. The method according to claim 4, characterized in that The calculating, based on the feature matching relationship between the first image frame and the second image frame adjacent to each of the image frames in the image frame set, the feature matching rate corresponding to each of the first image frame and the second image frame in the image frame set comprises: According to the feature matching relationship between the first image frame and the second image frame adjacent to each of the image frames in the image frame set, the number of matched feature points corresponding to the adjacent image frames is counted and divided by the total number of feature points corresponding to the corresponding first image frame in the adjacent image frames to obtain the feature matching rate corresponding to each of the first image frame and the second image frame in the image frame set.

6. The method according to claim 1, characterized in that The updated map point set includes pixel coordinates of each updated map point on a corresponding key frame, and converting the updated map point set into a three-dimensional map point set includes: The update map point set is converted into a three-dimensional map point set based on the position and posture of the key frame corresponding to each update map point in the update map point set and the pixel coordinates of each update map point on the corresponding key frame.

7. The method according to claim 1, characterized in that The step of constructing a target fusion map based on the three-dimensional map point set, the updated correspondence relationship set, and the updated posture of each key frame in the correspondence relationship set includes: Calculating each total reprojection error of each 3D map point in the 3D map point set on each corresponding key frame, and when the total reprojection error is greater than a preset number of pixels, deleting the correspondence between the 3D map point and the corresponding key frame from the updated correspondence set to obtain an intermediate correspondence set; Selecting a current 3D map point from the 3D map point set in sequence, calculating the number of corresponding key frames corresponding to the current 3D map point, deleting the current 3D map point from the 3D map point set when the number of corresponding key frames is less than a preset number, deleting the corresponding relationship between the current 3D map point and the key frame corresponding to the current 3D map point from the intermediate corresponding relationship set, and obtaining a target 3D map point set and a target corresponding relationship set; The target fusion map is constructed based on the target three-dimensional map point set, the target correspondence set and the position and posture of each key frame in the target correspondence set.

8. A fusion map construction device, characterized in that: The device comprises: An interpolation module is used to obtain an image frame set collected by an image acquisition device and a laser pose data set obtained by laser radar mapping, and perform an interpolation operation on each image frame in the image frame set based on the laser pose data set to obtain each pose corresponding to each image frame; A matching module is used to obtain a feature matching relationship between adjacent image frames, select a key frame set from the image frame set based on the posture corresponding to each image frame and the feature matching relationship, obtain an initial map point set corresponding to the key frame set, and construct a corresponding relationship between each key frame in the key frame set and each initial map point in the initial map point set to obtain a corresponding relationship set; An updating module is used to obtain a preset distance, select a current key frame from the key frame set in turn, and select a related key frame set corresponding to the current key frame from the key frame set based on the posture and preset distance corresponding to the current key frame; perform feature matching on the current key frame and each related key frame in the related key frame set to obtain a feature matching relationship between the current key frame and each related key frame; when an initial map point in the initial map point set corresponding to a current matching feature point of the current key frame is inconsistent with an initial map point corresponding to a current related matching feature point of the related key frame, merge the initial map point corresponding to the current matching feature point with the initial map point corresponding to the current related matching feature point to obtain an updated map point; add the updated map point to the initial map point set, delete the initial map point corresponding to the current matching feature point and the initial map point corresponding to the current related matching feature point from the initial map point set, change the initial map point corresponding to the current matching feature point in the correspondence relationship set and the initial map point corresponding to the current related matching feature point to an updated map point, and obtain an updated correspondence relationship and an updated map point; traverse the key frames in the key frame set to obtain an updated map point set and an updated correspondence relationship set; A construction module is used to convert the updated map point set into a three-dimensional map point set, and to construct a target fusion map based on the three-dimensional map point set, the updated correspondence set, and the position and posture of each key frame in the updated correspondence set.

9. A robot, characterized in that: The robot is equipped with a laser radar and an image acquisition device, and also includes a memory and a processor. The memory stores computer-readable instructions, and the processor is used to implement the steps of the method described in any one of claims 1 to 7 when executing the computer-readable instructions.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images