Map construction method, device and equipment for robot navigation and storage medium

Abstract

The application relates to an intelligent robot, and provides a map construction method, a device and equipment for robot navigation and a storage medium, wherein the method constructs a supplementary pose graph and a supplementary map according to a travel route of a robot and collected pictures; a constraint relationship between the supplementary pose graph and a known pose graph is determined through algorithm matching of the known pose graph and the supplementary pose graph; a position relationship between the supplementary map and the known map is mapped according to the constraint relationship; and the supplementary map is connected to the known map according to the position relationship, so as to generate an expanded map. The application adds the supplementary map to the known map based on matching constraint of the supplementary pose graph and the known pose graph and mapping relationship between the pose graph and the map, so that the robot map is expanded, the generation efficiency of the expanded map is improved, and the accuracy of the expanded map is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent robots, and particularly relates to a map construction method and device for robot navigation, equipment and a storage medium. BACKGROUND

[0002] With the development of society and the expansion of the application field of robots, people have higher and higher requirements for intelligent robots. SLAM means simultaneous localization and mapping, which is mainly used to solve the problems of localization and map construction of robots in the position environment. However, when the working site changes greatly or needs to be expanded far away from the working site, the existing mapping scheme generally reconstructs the map of the robot. In a large-scale scene application, the re-mapping of the known area by the robot will generate a huge workload and reduce the working efficiency of the robot. Therefore, how to solve the low efficiency of the existing robot map construction has become a technical problem to be solved at present. SUMMARY

[0003] The main purpose of the present application is to provide a map construction method and device for robot navigation, equipment and a storage medium, which aims to solve the technical problem of low efficiency of the existing robot map construction.

[0004] To achieve the above-mentioned purpose, the present application provides a map construction method for robot navigation, which comprises: constructing a supplementary constructed pose graph and a supplementary constructed map according to the travel route of the robot and the collected pictures; acquiring a known map and a known pose graph, determining the constraint relationship between the supplementary constructed pose graph and the known pose graph through algorithm matching of the known pose graph and the supplementary constructed pose graph; mapping the position relationship between the supplementary constructed map and the known map according to the constraint relationship; and connecting the supplementary constructed map to the known map according to the position relationship to generate an expanded map.

[0005] In addition, to achieve the above-mentioned purpose, the present application also provides a map construction device for robot navigation, which comprises: a pose graph and map construction module, which is used to construct a supplementary constructed pose graph and a supplementary constructed map according to the travel route of the robot and the collected pictures; a pose graph matching module, which is used to acquire a known map and a known pose graph, and determine the constraint relationship between the supplementary constructed pose graph and the known pose graph through algorithm matching of the known pose graph and the supplementary constructed pose graph; a map matching module, which is used to map the position relationship between the supplementary constructed map and the known map according to the constraint relationship; and an expanded map generation module, which is used to connect the supplementary constructed map to the known map according to the position relationship to generate an expanded map.

[0006] In addition, to achieve the above object, the present application also provides a map construction device for robot navigation, which comprises a processor, a memory, and a map construction program for robot navigation stored on the memory and executable by the processor, wherein the map construction program for robot navigation, when executed by the processor, implements the steps of the map construction method for robot navigation as described above.

[0007] In addition, to achieve the above object, the present application also provides a computer readable storage medium, which stores a map construction program for robot navigation, wherein the map construction program for robot navigation, when executed by a processor, implements the steps of the map construction method for robot navigation as described above.

[0008] The present application provides a map construction method for robot navigation, which constructs a reconstructed pose graph and a reconstructed map according to a travel route of a robot and collected pictures, acquires a known map and a known pose graph, determines a constraint relationship between the reconstructed pose graph and the known pose graph through algorithm matching of the known pose graph and the reconstructed pose graph, maps a position relationship between the reconstructed map and the known map according to the constraint relationship, and connects the reconstructed map to the known map according to the position relationship to generate an expanded map. In this way, the present application matches and constrains the reconstructed pose graph constructed by the robot in an unknown area and the known pose graph, maps the position relationship between the reconstructed map and the known map, adds the reconstructed map to the known map on the basis of the known map, completes expansion of the robot map, avoids re-construction of the known map, reduces the data calculation amount of the robot, improves the generation efficiency of the expanded map, improves the accuracy of the expanded map, and solves the technical problem of low efficiency of the robot map construction. BRIEF DESCRIPTION OF DRAWINGS

[0009] Figure 1 A hardware structure diagram of the map construction device for robot navigation involved in the embodiment of the present application;

[0010] Figure 2 A flowchart of the first embodiment of the map construction method for robot navigation of the present application;

[0011] Figure 3 A flowchart of the second embodiment of the map construction method for robot navigation of the present application;

[0012] Figure 4 A flowchart of the third embodiment of the map construction method for robot navigation of the present application;

[0013] Figure 5 This is a flowchart illustrating the fourth embodiment of the map construction method for robot navigation according to the present invention;

[0014] Figure 6 This is a schematic diagram of the functional modules of the first embodiment of the map building and generation device for robot navigation of the present invention.

[0015] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0016] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0017] The map building method for robot navigation involved in the embodiments of the present invention is mainly applied to map building equipment for robot navigation. The map building equipment for robot navigation can be a PC, a portable computer, a mobile terminal, or other devices with display and processing functions.

[0018] Reference Figure 1 , Figure 1 This is a schematic diagram of the hardware structure of a map-building device for robot navigation according to an embodiment of the present invention. In this embodiment, the map-building device for robot navigation may include a processor 1001 (e.g., a CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to implement communication between these components; the user interface 1003 may include a display screen or an input unit such as a keyboard; the network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface); the memory 1005 may be a high-speed RAM memory or a stable, non-volatile memory, such as a disk storage device, and optionally, the memory 1005 may also be a storage device independent of the aforementioned processor 1001.

[0019] Those skilled in the art will understand that Figure 1 The hardware structure shown does not constitute a limitation on the map-building device used for robot navigation, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0020] Continue to refer to Figure 1 , Figure 1 The memory 1005, which is a computer-readable storage medium, may include an operating system, a network communication module, and a map-building program for robot navigation.

[0021] existFigure 1 In the embodiment, the network communication module is mainly used for connecting the server and communicating data with the server, and the processor 1001 can call the map construction program for robot navigation stored in the memory 1005 and execute the map construction method for robot navigation provided by the embodiment of the application.

[0022] The embodiment of the application provides a map construction method for robot navigation.

[0023] With reference to Figure 2 , Figure 2 FIG. 1 is a flowchart of a first embodiment of the map construction method for robot navigation.

[0024] In the embodiment, the map construction method for robot navigation comprises the following steps:

[0025] In step S10, the robot platform constructs a reconstructed pose graph and a reconstructed map according to the travel route of the robot and the collected pictures.

[0026] In the embodiment, the robot collects pictures by using a camera during movement in a running site to obtain environmental information on the travel route. The robot platform extracts key frame pictures regularly, matches and calculates feature points in the key frame pictures, obtains pose transformation information of the robot during travel, continuously constructs pose nodes of the robot on the travel route, obtains pose edges between the pose nodes according to constraint relationships between the pose nodes, and further generates a reconstructed pose graph.

[0027] Further, the pose is uniquely determined according to a reference coordinate system, and when the pose of the same object is described in different reference coordinate systems, the pose representation also changes, so that a coordinate change is needed to associate the descriptions of the object in different reference coordinate systems, thereby forming a constraint relationship.

[0028] Further, the pose constraint is exemplified. When the key frame pictures collected by two pose nodes have the same feature points, the transformation information between the corresponding two pose nodes can be calculated according to the deformation amount, the pixel area size and other information of the same feature points between the two key frame pictures. The latter pose node is calculated based on the pose information of the former pose node, so that the former pose node has a constraint relationship with the latter pose node, and when the former pose node changes, the latter pose node also changes.

[0029] Further, the pose edge refers to a spatial constraint between different nodes. The edge between adjacent nodes is a mileage constraint, and the remaining edges represent closed loop constraints. It can be understood as a fitting trajectory between pose nodes with control constraints.

[0030] Further, the robot platform acquires map information on a robot travel route according to the reconstructed pose graph in combination with environment information contained in the collected pictures, and constructs a reconstructed map.

[0031] It can be understood that the pose node of the robot can be acquired in various forms, for example, the real-time position of the robot is acquired through GPS, the attitude of the robot is acquired through calculation of deformation amount and other information of feature points in the collected pictures of the robot, so as to obtain the pose information of the robot and establish the pose node.

[0032] In step S20, the known map and the known pose graph are acquired, and the constraint relationship between the reconstructed pose graph and the known pose graph is determined through algorithm matching of the known pose graph and the reconstructed pose graph.

[0033] In the embodiment, the robot platform can directly acquire the map, the grid map, the pose graph and the point cloud graph of the known area of the robot. If it is detected that the position of the robot movement is within the range of the known map, the reconstructed pose graph and the known pose graph need to be matched, the overlapping part of the pose graph is established to establish the constraint, so as to determine the relative position of the reconstructed pose graph and the known pose graph, and the reconstructed pose graph is corrected according to the known pose graph, the coordinate system of the reconstructed pose graph is converted into the coordinate system of the known pose graph, so as to add the reconstructed pose graph to the known pose graph.

[0034] In step S30, the position relationship between the reconstructed map and the known map is mapped according to the constraint relationship.

[0035] In the embodiment, the reconstructed map is constructed in combination with the reconstructed pose graph and the environment information contained in the collected pictures, the reconstructed pose graph and the known pose graph establish the constraint relationship, and correspondingly, the reconstructed map part corresponding to the pose node and the pose edge of the constraint also has the connection relationship with the known map part. The position relationship between the reconstructed map and the known map can be determined by matching the areas with the connection relationship.

[0036] In step S40, the reconstructed map is connected to the known map according to the position relationship, and an expanded map is generated.

[0037] In the embodiment, after the position relationship between the reconstructed map and the known map is determined, the coordinate system of the reconstructed map is converted into the coordinate system of the known map, the reconstructed map is added to the known map through matching and connection of the map areas with the connection relationship, and the expansion of the known map is completed, so as to obtain the expanded map.

[0038] In this embodiment, by collecting and extracting key frame pictures at different positions in the unknown area, the pose nodes and pose edges of the robot at different positions are established according to the feature point transformation information between the key frame pictures corresponding to adjacent positions, so as to construct the reconstructed pose graph. By the pose nodes constructed by the robot in the known area, the reconstructed pose graph is constrained and optimized with the known pose graph, so as to determine the positional relationship between the reconstructed pose graph and the known pose graph. According to the mapping relationship between the pose graph and the map, the positional relationship between the reconstructed map and the known map is obtained, and then the reconstructed map is added to the known map to generate the expanded map. Thus, by matching and constraining the reconstructed pose graph constructed by the robot in the unknown area with the known pose graph, the positional relationship between the reconstructed map and the known map is mapped, so that the reconstructed map is added to the known map on the basis of the known map, the expansion of the robot map is completed, the reconstruction of the known map is avoided, the data calculation amount of the robot is reduced, the generation efficiency of the expanded map is improved, the accuracy of the expanded map is improved, and the technical problem of low efficiency of the robot map construction is solved.

[0039] With reference to Figure 3 , Figure 3 FIG. 2 is a flowchart of a second embodiment of the map construction method for robot navigation according to the present application.

[0040] Based on the above Figure 2 embodiment, in this embodiment, step S10 comprises:

[0041] In step S11, the pose nodes of the robot on the travel route and the pose edges between the pose nodes are obtained by matching the feature points of the key frame pictures collected by the robot; wherein the feature points include lines, inflection points and easily distinguishable markers.

[0042] In this embodiment, according to the pictures collected by the robot on the travel route, the key frame pictures are extracted, the positions of the robot corresponding to the front and rear two pictures and the depth between the same feature points in the pictures are calculated, and then the triangular calculation is performed; by matching the same feature points of the front and rear two key frame pictures, and calculating the angle and depth of the feature points, the pose transformation information of the robot corresponding to the front and rear two key frame pictures collected by the camera is obtained.

[0043] Further, the feature points in the key frame pictures can include objects, lines, inflection points, protrusions, pits, angles, corners and other entities that are easy to identify and distinguish.

[0044] In step S12, the reconstructed pose graph is constructed according to the pose nodes and the pose edges between the pose nodes.

[0045] In the embodiment, the robot platform establishes pose nodes on the robot travel route in sequence according to the acquired robot pose transformation information, and establishes constraints on the pose nodes according to the pose transformation information, so as to generate the supplemented pose graph.

[0046] Further, each pose node is calculated according to a previous pose node, so that the previous pose node has a constraint on the next pose node, and the pose nodes are associated according to the constraint to generate the supplemented pose graph.

[0047] In step S13, environment information of the travel route is acquired according to the pictures collected by the robot, wherein the environment information includes obstacles, obstacle distances, corners, and road widths.

[0048] In the embodiment, the robot platform identifies and calculates the environment on the robot travel route through feature analysis and extraction of the pictures collected by the robot, such as identifying obstacles, calculating the equal scale size of the robot movable area, the equal scale size of the obstacle area, the depth information of the feature points, and the road conditions and road widths.

[0049] In step S14, the supplemented map is constructed according to the supplemented pose graph and the environment information.

[0050] In the embodiment, the robot platform can match the environment information and the pose information through a mapping algorithm according to the pose information in the supplemented pose graph and the environment information contained in the pictures, so as to facilitate the construction of the two-dimensional and / or three-dimensional map of the robot travel route.

[0051] In the embodiment, the robot platform records the travel route and the pose transformation of the robot through the pictures collected by the robot in the unknown area, and constructs the unknown environment of the robot through the environment information and the pose transformation information contained in the pictures, so as to obtain the supplemented pose graph and the supplemented map, which facilitates the subsequent matching and splicing of the supplemented map and the known map.

[0052] Reference Figure 4 , Figure 4 FIG. 3 is a flowchart of a third embodiment of the map construction method for robot navigation of the present application.

[0053] Based on the above Figure 3 embodiment, in the embodiment, the step S11 specifically includes:

[0054] In step S111, the key frame pictures are selected according to a preset coincidence feature ratio of the pictures collected by the robot.

[0055] In this embodiment, first, at least one camera is arranged on the robot to collect environment pictures of the running site; second, a plurality of easily identifiable and distinguishable markers can be arranged in the running site of the robot for identification and matching of the pictures collected by the camera; the features such as objects, lines, protrusions, pits, corners, etc. in the collected pictures can also be identified and matched.

[0056] In this embodiment, the camera shoots pictures according to a preset frame rate during the travel, but the pictures collected continuously have relatively high repetition, and if all the pictures are calculated and processed, a lot of unnecessary calculation will be increased, so it is necessary to extract key frame pictures according to certain rules and criteria, to obtain the required environment information and pose information by matching the information of a small number of key frame pictures under the premise that the picture information can be completely connected, to avoid the influence of redundant information, to reduce the data processing amount of the robot platform, and to improve the mapping efficiency.

[0057] Further, the robot continuously collects pictures during the movement, but in order to reduce the calculation amount and ensure that the trajectory is not lost, it is necessary to determine whether the collected pictures can be used as key frame pictures, at this time, the key frame pictures can be selected according to certain rules and criteria.

[0058] Further, the rules and criteria for selecting key frames can be to set a feature point threshold range, such as setting the feature point threshold range to 50-80, so that the picture with 50-80 same feature points with the previous key frame picture is selected as the current key frame picture; or a feature point ratio can be set, such as selecting a picture containing fifty percent of the feature point number of the previous key frame as the current key frame picture; or an overlap area range can be set, such as setting a picture with a thirty percent overlap area as the selection basis, so that the picture with a thirty percent overlap area with the previous key frame picture is selected as the next key frame picture, and all the same feature points in the overlap area are matched.

[0059] In step S112, the pose transformation information between the two positions of the robot when collecting the adjacent two key frame pictures is obtained by matching the feature points of the adjacent two key frame pictures.

[0060] In this embodiment, the robot platform extracts the feature points in the key frame picture collected at the current collection position, compares the feature points in the key frame picture collected at the previous position, calculates the curvature of the feature points in the current key frame picture, and thus calculates the pose transformation of the current pose of the robot relative to the pose at the previous position and the moving distance and other information.

[0061] In step S113, the pose node corresponding to the next picture is established according to the pose transformation information and the pose node corresponding to the previous picture as a constraint.

[0062] In this embodiment, the robot platform can calculate the pose of the current position of the robot according to the pose extracted by the robot at the previous position, and the pose transformation and the moving distance of the robot at the current position relative to the previous position.

[0063] In step S114, the pose edge between the pose nodes is obtained according to the constraint of the pose node.

[0064] In this embodiment, because the robot platform establishes the pose node according to the previous pose node, the previous pose node has a constraint on the next pose node, and the pose edge between the two pose nodes can be obtained through the constraint.

[0065] Further, the pose nodes established by the robot platform in the unknown area and the pose edges formed according to the constraint relationship between the pose nodes constitute the reconstructed pose graph.

[0066] Further, when the robot platform establishes a new pose node, loop detection needs to be performed on the pose node to determine whether the reconstructed pose graph has a loop.

[0067] Further, loop detection is performed on the picture corresponding to the new pose node according to all the collected key frame pictures, and if there is a key frame picture having the same feature point with the picture corresponding to the new pose node in addition to the previous key frame picture, it means that the loop exists, and the pose node corresponding to the key frame picture having the same feature point is constrained with the new pose node, thereby optimizing the reconstructed pose graph.

[0068] Further, if there is no key frame picture having the same feature point with the picture corresponding to the new pose node in addition to the previous key frame picture, it means that the loop does not exist, and the reconstructed pose graph does not need to be optimized.

[0069] In this embodiment, the robot platform drives the robot to move in the running site and collects pictures at a preset frame rate. The front end of the robot platform can extract a picture having feature points within a threshold range of the feature points of the previous key frame picture as the next key frame picture of the robot by matching the picture collected by the robot in a certain distance with the previous key frame picture. The front end can obtain the transformation information such as the distance and the angle of the next key frame picture relative to the previous key frame picture by calculating the same feature point information of the next key frame picture and the previous key frame picture, thereby determining the pose node corresponding to the next key frame picture according to the pose node corresponding to the previous key frame picture, and obtaining the pose edge between the two pose nodes according to the constraint relationship between the two pose nodes, thereby establishing the reconstructed pose graph.

[0070] In the embodiment, the pictures collected by the robot in the unknown area are used to obtain the environmental information and the pose transformation on the route of the robot, construct a reconstructed pose graph according to the pose transformation information, and construct a reconstructed map in combination with the reconstructed pose graph and the environmental information. The layout of the reconstructed map corresponds to the pose node and other information of the reconstructed pose graph, facilitating the matching and splicing of the reconstructed map and the known map.

[0071] Further, based on the above Figure 4 As shown in the embodiment, the step S111 specifically includes:

[0072] The easily recognizable features in the previous key frame picture are extracted as the feature points of the previous frame picture.

[0073] In the pictures collected after the previous key frame picture, a picture containing a certain number of feature points is selected as a next key frame picture.

[0074] The number threshold of the feature points is set, and the number of feature points contained in the next key frame picture and the threshold are used to determine the key frame picture inserted between the previous key frame picture and the next key frame picture.

[0075] In the embodiment, the robot platform takes the determined previous key frame picture as a reference picture, performs feature matching on all pictures collected after the front key frame picture, sets a screening threshold of feature points, such as 50-80, and then selects a picture with the same number of feature points within the screening threshold as the next key frame picture. Any picture meeting the standard can be selected as the next key frame picture.

[0076] Further, the frame rate of the camera is set to match the moving speed of the robot, so as to ensure that a picture containing a sufficient number of feature points of the previous key frame picture can be collected, facilitate the extraction of the next key frame picture, and prevent the tracking of the key frame picture from being lost.

[0077] Further, if the feature point data of the current extracted picture is greater than the threshold range compared with the previous key frame picture, it is indicated that the position of the current picture extracted by the robot is too close to the position of the previous key frame picture. In this case, the extracted picture is a redundant information picture and is not used as a key frame picture.

[0078] Further, if the feature point data of the current extracted picture is less than the threshold range compared with the previous key frame picture, it is indicated that the position of the current picture extracted by the robot is far away from the position of the previous key frame picture. The pose information obtained by matching the current extracted picture as a key frame picture has a relatively large error, and the environmental features are too few to affect the calculation result. Therefore, at least one picture between the current picture and the previous key frame picture needs to be selected as a key frame picture.

[0079] Further, if the number of matching feature points between the current extracted key frame picture and the last key frame picture is small, or there is no same feature point, it indicates that the current scene has a large change relative to the environment of the last key frame picture, at this time, a new key frame picture needs to be extracted between the current extracted key frame picture and the last key frame picture, so as to avoid the front end tracking of the robot platform from failing.

[0080] In the embodiment, by matching the feature points of the pictures collected by the robot, it is judged whether the current extracted picture can be used as a key frame picture, and the key frame picture is limited according to the number of extracted same features, which can effectively filter out redundant information and avoid excessive data processing of the robot platform, while ensuring the accuracy of the composition, the efficiency of the composition is also improved.

[0081] Referring to Figure 5 , Figure 5 The flowchart of the fourth embodiment of the map construction method for robot navigation of the application is shown.

[0082] Based on the above Figure 2 embodiment, in the embodiment, step S20 includes:

[0083] Step S21, acquiring a known grid map according to the known map;

[0084] In the embodiment, the robot platform generates a corresponding grid map and a point cloud map every time the map is expanded, which are used as the known grid map and the known point cloud map as the reference basis for the next expansion of the map.

[0085] Step S22, detecting the position of the robot through the known grid map;

[0086] In the embodiment, the known grid map includes the obstacles and known areas explored by the last expansion of the map, and the unknown areas that have not been explored. The known grid map can mark the obstacles as black squares, the known areas as white squares, and the unknown areas as gray squares, so as to perform semantic annotation on the known grid map, and facilitate the distinction and detection.

[0087] Step S23, when it is detected that the position of the robot is located in the known area in the known grid map, the known pose map and the reconstructed pose map are matched by algorithm to determine the constraint relationship between the known pose map and the reconstructed pose map.

[0088] In the embodiment, after establishing the pose node of the robot at the current position, the current position of the robot is searched through the known grid map to search the area where the current robot is located in the known grid map.

[0089] Further, when it is detected that the current position of the robot is in the gray square of the known grid map, it can be determined that the current position of the robot is in the unexplored unknown area, and the robot platform does not process the current established supplementary pose graph, but only performs loop detection on the current pose node established at the current position.

[0090] Further, when it is detected that the current position of the robot is in the white square of the known grid map, it can be determined that the current position of the robot is in the known area, that is, the current position of the robot is in the known map, and there is a pose node in the known map; then the robot platform performs algorithm matching on the current established supplementary pose graph and the known pose graph.

[0091] In this embodiment, the grid map generated by the last mapping is used to search the position of the robot being mapped, and the supplementary pose graph and the known pose graph are matched for the overlapping area between the supplementary mapping area and the known area. By searching the position of the robot, it is determined whether the supplementary pose graph and the known pose graph need to be matched, so that the supplementary pose graph and the known pose graph can be constrained in time, thereby facilitating timely and accurate correction of the supplementary pose graph, avoiding identification errors due to too small overlapping area when the supplementary pose graph area is too large, and thus avoiding too large deviation of the optimization result of the supplementary pose graph.

[0092] Further, based on the above Figure 5 The step S23 specifically includes:

[0093] According to the supplementary pose graph and the picture collected by the robot, a supplementary point cloud graph is generated;

[0094] A known point cloud graph corresponding to the known map is obtained, and algorithm matching is performed on the supplementary point cloud graph according to the known point cloud graph to obtain an overlapping area;

[0095] According to the overlapping area, the pose node and the pose edge that overlap between the supplementary pose graph and the known pose graph are determined;

[0096] According to the overlapping pose node and the pose edge, a constraint is established for the known pose graph and the supplementary pose graph, and a constraint relationship of the known pose graph and the supplementary pose graph is obtained.

[0097] In this embodiment, when the supplementary pose graph and the known pose graph need to be matched, the robot platform extracts corresponding point clouds from the supplementary pose graph and the corresponding collected picture to generate a supplementary point cloud graph. By matching the supplementary point cloud graph and the known point cloud graph corresponding to the known pose graph, the point clouds with the same position in the two point cloud graphs are extracted.

[0098] Further, according to the repeated point cloud information, a pose node corresponding to the point cloud information of the known pose graph is extracted as a known registration pose node, a pose node corresponding to the point cloud information of the supplemented pose graph is extracted as a supplemented registration pose node, the supplemented registration pose node is associated with the known registration pose node and a corresponding pose edge by a registration algorithm, and a new pose constraint is established.

[0099] Further, for the convenience of persons skilled in the art, the registration algorithm for establishing a target pose constraint is further explained mathematically in the embodiment. Specifically as follows:

[0100] Generally, the slam problem can be expressed as a least square problem as follows:

[0101]

[0102] wherein x is pose information of all nodes at present, is an information matrix, representing a distribution of errors, wherein e ij is a predicted and observed error between node i and node j.

[0103] Further, the prediction is generally obtained by a front-end registration algorithm, and the observation is generally obtained by a registration algorithm when loop detection is performed; a least square solver in the back end can obtain an x value that minimizes F(x), thereby obtaining a maximun likelihood solution.

[0104] In the embodiment, the application adds the supplemented registration pose node as a new constraint to the known pose graph, which is mathematically expressed as:

[0105]

[0106] is a pose estimation of the pose node, is a pose estimation of the pose node, is an information matrix, representing a probability distribution of the matching.

[0107] Further, after the least square problem is constructed, a Newton-Gauss method or a Levenberg-Marquardt (nonlinear regression least square) method can be used to solve the least square problem by a typical solution method.

[0108] In this embodiment, the robot platform adds a supplementary registration pose node to the known pose graph according to the established pose constraint, thereby connecting the supplementary pose graph with the known pose graph, that is, adding the supplementary pose graph to the known pose graph; then performing loop detection on all pose nodes in the connected pose graph, if there is a loop, establishing a constraint on the pose node with the loop, performing maximum likelihood optimization on the associated pose graph after the constraint is established, and correcting the known pose graph and the supplementary pose graph, so that the obtained supplementary pose graph is more accurate.

[0109] Further, based on the above Figure 5 As shown in the embodiment, step S23 further includes:

[0110] According to the pictures collected by the robot, performing loop detection on all the pose nodes in the known pose graph and the supplementary pose graph;

[0111] According to the loop result obtained by the loop detection, establishing a loop constraint on the pose node with the loop;

[0112] According to the loop constraint, performing correction optimization on the pose nodes and the pose edges in the known pose graph and the supplementary pose graph;

[0113] According to the correction optimization result, determining the relative position relationship between the supplementary pose graph and the known pose graph.

[0114] Further, loop detection is to detect that in addition to the adjacent last pose node, there is another pose node associated with the current pose node, that is, the current pose node can be calculated through other pose nodes, at this time, the current pose node and other pose nodes associated with it are established pose constraint, that is, loop constraint is obtained.

[0115] Further, if the supplementary pose graph and the known pose graph have a constraint relationship, the supplementary pose graph and the known pose graph after the constraint are established are simultaneously subjected to loop detection.

[0116] Further, if there is a loop in the loop detection, a constraint is established on the pose node with the loop, and optimization is performed, and then maximum likelihood optimization is performed on all the pose nodes.

[0117] Further, if there is no loop in the loop detection, maximum likelihood optimization is directly performed on all the pose nodes.

[0118] Further, if the supplementary pose graph and the known pose graph do not have a constraint relationship, that is, the robot has not traveled into the known area, then a new pose node is established, and loop detection is performed on the pose node.

[0119] Further, if there is a loop in the loop detection, constraints are established for the pose nodes with loops in the supplemented pose graph, and maximum likelihood optimization is performed on all pose nodes in the supplemented pose graph.

[0120] Further, if there is no loop in the loop detection, no processing is performed on the supplemented pose graph, and the robot continues to establish new pose nodes; until the robot moves to a known map, the supplemented pose graph and the known pose graph have a position coinciding pose node.

[0121] Further, the optimized supplemented pose graph and the known pose graph are closer to the real environment, so that the relative position relationship between the supplemented pose graph and the known pose graph can be determined; and the supplemented map is constructed according to the supplemented pose graph and the environmental information, so that the position relationship between the supplemented map and the known map can be mapped according to the relative position relationship.

[0122] Further, according to the constraint relationship between the supplemented pose graph and the known pose graph, the connection area of the supplemented map and the known map can be determined, and then the supplemented map and the known map can be connected through an algorithm by combining the position relationship between the supplemented map and the known map, so as to generate an expanded map.

[0123] In the embodiment, the pose nodes in the supplemented pose graph and the known pose graph after the connection are adjusted through loop detection and maximum likelihood optimization, so that the pose nodes in the target pose graph obtained are closer to the real camera pose; the expanded map established is also closer to the real running site, and the accuracy of the expanded map is improved; and according to the constraint relationship between the supplemented pose graph and the known map, the position relationship and the connection area between the supplemented map and the known map are mapped, so that the supplemented map is added to the known map, and the expansion of the known map is completed.

[0124] In addition, the embodiment of the present application also provides a map construction device for robot navigation.

[0125] Reference Figure 6 , Figure 6 It is a functional module schematic diagram of the first embodiment of the map construction device for robot navigation of the present application.

[0126] In the embodiment, the map construction device for robot navigation comprises:

[0127] The pose graph and map construction module 10 is configured to construct a supplemented pose graph and a supplemented map according to the travel route of the robot and the collected pictures.

[0128] The pose graph matching module 20 is configured to acquire a known map and a known pose graph, and determine a constraint relationship between the constructed pose graph and the known pose graph by algorithm matching of the known pose graph and the constructed pose graph.

[0129] The map matching module 30 is configured to map a position relationship between the constructed map and the known map according to the constraint relationship.

[0130] The expanded map generation module 40 is configured to splice the constructed map into the known map according to the position relationship, and generate an expanded map.

[0131] Further, the target pose constraint establishment module 10 specifically comprises:

[0132] The pose information acquisition submodule is configured to acquire pose nodes of the robot on a travel route and pose edges between the pose nodes by feature point matching on key frame pictures collected by the robot; wherein the feature points include lines, inflection points and easily distinguishable markers.

[0133] The constructed pose graph construction submodule is configured to construct the constructed pose graph according to the pose nodes and the pose edges between the pose nodes.

[0134] The environment information acquisition submodule is configured to acquire environment information of the travel route according to pictures collected by the robot; wherein the environment information includes obstacles, obstacle distances, corners and road widths.

[0135] The constructed map construction submodule is configured to construct a constructed map according to the constructed pose graph and the environment information.

[0136] Further, the pose information acquisition submodule specifically comprises:

[0137] The key frame selection unit is configured to select key frame pictures according to a preset overlap feature ratio of pictures collected by the robot.

[0138] The pose transformation information acquisition unit is configured to acquire pose transformation information between two positions of the robot when collecting adjacent two key frame pictures by feature point matching on the adjacent two key frame pictures.

[0139] The pose node establishment unit is configured to establish a pose node corresponding to a next picture according to a pose node corresponding to a previous picture as a constraint.

[0140] The pose edge acquisition unit is configured to acquire pose edges between the pose nodes according to constraint relationships of the pose nodes.

[0141] Further, the key frame selection unit specifically comprises:

[0142] a feature point extraction subunit configured to extract identifiable features in the previous key frame picture as feature points of the previous frame picture;

[0143] a key frame picture selection subunit configured to select a picture containing a certain number of the feature points as a next key frame picture from pictures collected after the previous key frame picture;

[0144] an inserted key frame determination subunit configured to set the threshold of the number of the feature points, and determine an inserted key frame picture between the previous key frame picture and the next key frame picture according to the number of the feature points contained in the next key frame picture and the threshold.

[0145] Further, the pose graph matching module 20 comprises:

[0146] a known grid map acquisition sub-module configured to acquire a known grid map according to the known map;

[0147] a robot position detection sub-module configured to detect the position of the robot through the known grid map;

[0148] a pose graph matching sub-module configured to, when the position of the robot is detected to be located in a known area in the known grid map, perform algorithm matching on the known pose graph and the reconstructed pose graph to determine the constraint relationship between the known pose graph and the reconstructed pose graph.

[0149] Further, the pose graph matching sub-module specifically comprises:

[0150] a reconstructed point cloud map generation unit configured to generate a reconstructed point cloud map according to the reconstructed pose graph and the pictures collected by the robot;

[0151] a point cloud map matching module unit configured to acquire a known point cloud map corresponding to the known map, perform algorithm matching on the reconstructed point cloud map according to the known point cloud map, and acquire an overlapping area;

[0152] an overlapping pose determination unit configured to determine the pose nodes and the pose edges that overlap between the reconstructed pose graph and the known pose graph according to the overlapping area;

[0153] a pose graph relative position acquisition unit configured to establish a constraint between the known pose graph and the reconstructed pose graph according to the overlapping pose nodes and the overlapping pose edges, and acquire the relative position relationship between the known pose graph and the reconstructed pose graph.

[0154] Further, the target pose constraint establishing module 10 further comprises a pose graph optimization submodule, which specifically comprises:

[0155] a loop detection unit, configured to perform loop detection on all the pose nodes in the known pose graph and the reconstructed pose graph according to pictures collected by the robot;

[0156] a loop constraint establishing unit, configured to establish a loop constraint for the pose nodes with loop according to the loop detection result;

[0157] a pose graph optimization unit, configured to correct and optimize the pose nodes and the pose edges in the known pose graph and the reconstructed pose graph according to the loop constraint.

[0158] Corresponding to each step in the above-mentioned map construction method for robot navigation, each module in the above-mentioned map construction device for robot navigation has the same function and implementation process, which will not be described here.

[0159] In addition, the embodiment of the present application further provides a computer readable storage medium.

[0160] The computer readable storage medium of the present application stores a map construction program for robot navigation, wherein the map construction program for robot navigation, when executed by a processor, implements the steps of the above-mentioned map construction method for robot navigation.

[0161] The method implemented when the map construction program for robot navigation is executed can refer to each embodiment of the map construction method for robot navigation of the present application, which will not be described here.

[0162] It should be noted that in this paper, the term "comprise", "include" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or system including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or system. Without more limitations, the element defined by the statement "comprises a" does not exclude the presence of other identical elements in the process, method, article or system including the element.

[0163] The above-mentioned embodiment number of the present application is only for description, not representing the pros and cons of the embodiment.

[0164] The application is operable with numerous general purpose or special purpose computing system environments or configurations. Examples of well-known computing systems, environments, and / or configurations that can be suitable for use with the application include personal computers, server computers, handheld or laptop devices, tablet devices, multiprocessor systems, microprocessor-based systems, set top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, distributed computing environments that include any of the above systems or devices, and the like. The application can be described in the general context of computer-executable instructions, such as program modules, being executed by a computer. Generally, program modules include routines, programs, objects, components, data structures, and the like, that perform particular tasks or implement particular abstract data types. The application can also be practiced in distributed computing environments where tasks are performed by remote processing devices that are linked through a communications network. In a distributed computing environment, program modules can be located in both local and remote computer storage media including memory storage devices.

[0165] Those skilled in the art can clearly understand that the above-mentioned embodiment method can be realized by means of software and necessary general hardware platform, of course, it can also be realized by hardware, but in many cases, the former is a better embodiment. Based on such understanding, the technical solutions of the present application can be embodied in the form of a software product, which is stored in a storage medium (such as a ROM / RAM, a magnetic disk, or an optical disc) as described above, and includes a number of instructions for making a terminal device (which can be a mobile phone, a computer, a server, an air conditioner, or a network device) execute the methods described in various embodiments of the present application.

[0166] The above is only the preferred embodiment of the present application, and does not limit the patent scope of the present application, and any equivalent structure or equivalent process transformation, or direct or indirect application in other related technical fields, is also included in the patent protection scope of the present application.

Claims

1. A map construction method for robot navigation, characterized by, The method comprises the following steps: According to the travel route of the robot and the collected pictures, a reconstructed pose graph and a reconstructed map are constructed; Obtain a known map and a known pose graph, and determine the constraint relationship between the reconstructed pose graph and the known pose graph through algorithm matching of the known pose graph and the reconstructed pose graph; According to the constraint relationship, the positional relationship between the reconstructed map and the known map is mapped; According to the positional relationship, the reconstructed map is connected to the known map to generate an expanded map; Wherein, the known map and the known pose graph are obtained, and the constraint relationship between the reconstructed pose graph and the known pose graph is determined through algorithm matching of the known pose graph and the reconstructed pose graph, comprising: According to the known map, a known grid map is obtained; The position of the robot is detected through the known grid map; After detecting that the position of the robot is located in a known area in the known grid map, algorithm matching is performed on the known pose graph and the reconstructed pose graph to determine the constraint relationship between the known pose graph and the reconstructed pose graph; The algorithm matching of the known pose graph and the reconstructed pose graph to determine the constraint relationship between the known pose graph and the reconstructed pose graph comprises: According to the reconstructed pose graph and the pictures collected by the robot, a reconstructed point cloud graph is generated; Obtain the known point cloud graph corresponding to the known map, and perform algorithm matching on the reconstructed point cloud graph according to the known point cloud graph to obtain an overlapping area; According to the overlapping area, the pose nodes and pose edges overlapped by the reconstructed pose graph and the known pose graph are determined; According to the overlapped pose nodes and pose edges, constraints are established between the known pose graph and the reconstructed pose graph to obtain the constraint relationship between the known pose graph and the reconstructed pose graph.

2. The map construction method for robot navigation according to claim 1, wherein, According to the travel route of the robot and the collected pictures, a reconstructed pose graph and a reconstructed map are constructed, comprising: By matching feature points of key frame pictures collected by the robot, pose nodes of the robot on the travel route and pose edges between the pose nodes are obtained; wherein the feature points include lines, inflection points and easily distinguishable markers; According to the pose nodes and the pose edges, the reconstructed pose graph is constructed; According to the pictures collected by the robot, environmental information of the travel route is obtained; wherein the environmental information includes obstacles, obstacle distances, corners and road widths; According to the reconstructed pose graph and the environmental information, the reconstructed map is constructed.

3. The map construction method for robot navigation according to claim 2, wherein, The feature point matching of the key frame pictures collected by the robot to obtain the pose nodes of the robot on the travel route and the pose edges between the pose nodes comprises: According to a preset coincidence feature ratio of the pictures collected by the robot, the key frame pictures are selected; By matching feature points of adjacent two key frame pictures, pose transformation information between two positions of the robot when collecting the adjacent two key frame pictures is obtained; According to the pose transformation information, the pose node corresponding to a next frame of picture is established as a constraint with the pose node corresponding to a previous frame of picture; According to the constraint of the pose node, the pose edges between the pose nodes are obtained.

4. The map construction method for robot navigation according to claim 3, wherein, The selecting of the key frame pictures according to the preset proportion of the overlapping features of the pictures collected by the robot comprises: extracting easily recognizable features in a previous key frame picture as feature points of the previous frame of picture; selecting a picture containing a certain number of the feature points from pictures collected after the previous key frame picture as a next key frame picture; setting a threshold of the number of the feature points, and determining the key frame pictures inserted between the previous key frame picture and the next key frame picture according to the number of the feature points contained in the next key frame picture and the threshold.

5. The map construction method for robot navigation according to claim 1, wherein, After the constraint of the known pose graph and the reconstructed pose graph is established according to the overlapping pose nodes and the pose edges, and the constraint relationship of the known pose graph and the reconstructed pose graph is obtained, the method further comprises: performing loop detection on all the pose nodes in the known pose graph and the reconstructed pose graph according to the pictures collected by the robot; establishing a loop constraint for the pose nodes with loop according to a loop result obtained by the loop detection; correcting and optimizing the pose nodes and the pose edges in the known pose graph and the reconstructed pose graph according to the loop constraint; determining a relative position relationship between the reconstructed pose graph and the known pose graph according to a correction and optimization result.

6. A map construction apparatus for robot navigation, characterized by, The map construction device for robot navigation comprises: a pose graph and map construction module configured to construct a reconstructed pose graph and a reconstructed map according to a travel route of a robot and pictures collected by the robot; a pose graph matching module configured to obtain a known map and a known pose graph, and determine a constraint relationship between the reconstructed pose graph and the known pose graph by algorithm matching of the known pose graph and the reconstructed pose graph; a map matching module configured to map a position relationship between the reconstructed map and the known map according to the constraint relationship; an expanded map generation module configured to link the reconstructed map to the known map to generate an expanded map according to the position relationship; The pose graph matching module comprises a known grid map obtaining submodule, a robot position detection submodule, and a pose graph matching submodule. The known grid map obtaining submodule is configured to obtain a known grid map according to the known map. The robot position detection submodule is configured to detect a position of the robot through the known grid map. The pose graph matching submodule is configured to, when the position of the robot is detected to be located in a known area in the known grid map, perform algorithm matching of the known pose graph and the reconstructed pose graph to determine the constraint relationship between the known pose graph and the reconstructed pose graph. The pose graph matching sub-module comprises a supplementary point cloud graph generation unit, a point cloud graph matching module unit, an overlapping pose determination unit, and a pose graph relative position acquisition unit; the supplementary point cloud graph generation unit is configured to generate a supplementary point cloud graph according to the supplementary pose graph and the pictures collected by the robot; the point cloud graph matching module unit is configured to acquire a known point cloud graph corresponding to the known map, perform algorithm matching on the supplementary point cloud graph according to the known point cloud graph, and acquire an overlapping region; the overlapping pose determination unit is configured to determine pose nodes and pose edges of the supplementary pose graph and the known pose graph that overlap according to the overlapping region; and the pose graph relative position acquisition unit is configured to establish constraints on the known pose graph and the supplementary pose graph according to the overlapping pose nodes and the overlapping pose edges, and acquire a relative position relationship of the known pose graph and the supplementary pose graph.

7. A map construction device for robot navigation, characterized by, The map construction device for robot navigation comprises a processor, a memory, and a map construction program for robot navigation stored on the memory and executable by the processor, wherein the map construction program for robot navigation, when executed by the processor, implements the steps of the map construction method for robot navigation according to any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a map construction program for robot navigation, wherein the map construction program for robot navigation, when executed by the processor, implements the steps of the map construction method for robot navigation according to any one of claims 1 to 5.

Images