Robot, map construction method, device and readable storage medium
By combining two-dimensional LiDAR and visual cooperative identification, and using an optimization algorithm to construct a global probabilistic grid map, the mapping error problem of two-dimensional LiDAR positioning in scenarios such as long corridors and glass was solved, and stable robot localization was achieved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SHENZHEN PUDU TECH CO LTD
- Filing Date
- 2021-12-31
- Publication Date
- 2026-05-29
AI Technical Summary
Existing two-dimensional laser positioning methods have limited observation constraints in scenarios such as long corridors and glass, which can easily lead to mapping errors or incompleteness, affecting the accuracy of robot positioning.
By combining 2D LiDAR and visual cooperative markers, sensor data and image data of the robot along the mapping path are acquired. A stable global probabilistic grid map is constructed using optimization algorithms. The observation values of markers in the environment are added as constraints to optimize the robot's pose nodes and local probabilistic grid map.
In scenarios with limited identifiers or observation constraints, it can establish a stable robot localization map, breaking through the limitations of two-dimensional LiDAR localization and improving the accuracy and completeness of localization.
Smart Images
Figure CN115540867B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robotics, and in particular to a robot, a map-building method, an apparatus, and a readable storage medium. Background Technology
[0002] Currently, robots performing indoor delivery tasks typically locate themselves using two-dimensional (2D) lasers or visual cooperative markers. Each method has its advantages and disadvantages. 2D laser positioning requires the creation of a localization map through Simultaneous Localization and Mapping (SLAM), and it does not rely on markers or requires only a few. However, 2D lasers have limited observation constraints in scenarios such as long corridors and glass surfaces, easily leading to mapping errors or incompleteness, severely impacting the robot's subsequent positioning and normal operation. Summary of the Invention
[0003] This application provides a robot, a map building method, an apparatus, and a readable storage medium to enable the creation of stable maps using laser and visual cooperative markers in scenarios with limited markers or limited observation constraints.
[0004] On one hand, this application provides a robot, the robot comprising:
[0005] Memory, processor, sensor devices, and image acquisition devices;
[0006] The memory stores executable program code;
[0007] The processor coupled to the memory calls executable program code stored in the memory to execute a map construction method, the method comprising:
[0008] The processor coupled to the memory calls executable program code stored in the memory to execute a map construction method, the method comprising:
[0009] Acquire multiple frames of sensor data collected by the sensors as the robot moves along the mapped path;
[0010] Based on the multi-frame sensor data, multiple pose nodes and multiple local probability grid maps of the robot are obtained;
[0011] Based on the multi-frame sensing data, multiple pose nodes, and multiple local probability grid maps, the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps are obtained.
[0012] The image acquisition device acquires multiple frames of image data related to the identification code.
[0013] The multi-frame image data is associated with the corresponding pose nodes to obtain multiple observation values of the pose nodes and the identification codes;
[0014] The plurality of pose nodes, the plurality of local probabilistic grid maps, and the observed values are optimized using the first relative pose relationship, the second relative pose relationship, and the observed values as constraints.
[0015] A localization map is constructed using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observed values, and the multi-frame sensor data.
[0016] On the other hand, this application provides a map building apparatus for use in a robot including sensor devices and image acquisition devices, the apparatus comprising:
[0017] The first acquisition module is used to acquire multiple frames of sensor data collected by the sensor device when the robot moves along the mapping path;
[0018] The second acquisition module is used to obtain multiple pose nodes and multiple local probability grid maps of the robot based on the multi-frame sensing data.
[0019] The third acquisition module is used to obtain the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps based on the multi-frame sensing data, multiple pose nodes and multiple local probability grid maps; the fourth acquisition module is used to acquire the multi-frame image data acquired by the image acquisition device for the identification code.
[0020] The association module is used to associate the multi-frame image data with the corresponding pose nodes to obtain multiple observation values of the pose nodes and the identification codes.
[0021] An optimization module is used to optimize the plurality of pose nodes, the plurality of local probability grid maps, and the observation values, constrained by the first relative pose relationship, the second relative pose relationship, and the observation values.
[0022] The map building module is used to construct a positioning map using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observation values, and the multi-frame sensor data.
[0023] Thirdly, this application provides a map construction method, the method comprising:
[0024] Acquire multiple frames of sensor data collected by the sensors as the robot moves along the mapped path;
[0025] Based on the multi-frame sensor data, multiple pose nodes and multiple local probability grid maps of the robot are obtained;
[0026] Based on the multi-frame sensing data, multiple pose nodes, and multiple local probability grid maps, the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps are obtained.
[0027] The image acquisition device acquires multiple frames of image data related to the identification code.
[0028] The multi-frame image data is associated with the corresponding pose nodes to obtain multiple observation values of the pose nodes and the identification codes;
[0029] The plurality of pose nodes, the plurality of local probabilistic grid maps, and the observed values are optimized using the first relative pose relationship, the second relative pose relationship, and the observed values as constraints.
[0030] A localization map is constructed using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observed values, and the multi-frame sensor data.
[0031] Fourthly, this application provides a readable storage medium having a computer program stored thereon, the computer program being executed by a processor to implement a map building method, the map building method being the map building method implemented by the robot described in the first aspect.
[0032] As can be seen from the technical solution provided in this application, when optimizing the robot's latest pose node, the constraint of the robot's observation of the markers in the environment is added. In other words, it is only necessary to paste a limited number of markers in an environment where 2D LiDAR positioning is not applicable, and then combine the robot's latest pose node, local probabilistic grid map, and laser point cloud data associated with the latest pose node obtained by the 2D LiDAR on the robot. After optimizing the robot's latest pose node and the pose data of the markers in the environment, a global probabilistic grid map and marker map that can be stably output can be established, thereby breaking through the limitations of 2D LiDAR positioning in certain scenarios. Attached Figure Description
[0033] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0034] Figure 1 This is a schematic diagram of the structure of the robot provided in the embodiments of this application;
[0035] Figure 2 This is a flowchart of the map construction method provided in the embodiments of this application;
[0036] Figure 3 This is a schematic diagram of the structure of the map building apparatus provided in the embodiments of this application;
[0037] Figure 4 This is a schematic diagram of the device provided in the embodiments of this application. Detailed Implementation
[0038] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.
[0039] In this specification, adjectives such as "first" and "second" are used only to distinguish one element or action from another, without necessarily requiring or implying any actual such relationship or order. Where circumstances permit, reference to an element, component, or step (etc.) should not be construed as limited to only one element, component, or step, but may include one or more of the elements, components, or steps, etc.
[0040] For ease of description, the dimensions of the various parts shown in the accompanying drawings are not drawn to actual scale.
[0041] Please see the appendix Figure 1 This application provides a schematic diagram of the structure of a robot according to an embodiment. For ease of explanation, only the parts relevant to the embodiment are shown. The robot may include:
[0042] The robot includes a memory 10, a processor 20, sensors, and an image acquisition device (not shown in the figure). The processor 20 is the core of the robot's computation and control, and is the final execution unit for information processing and program execution. The memory 10 may be, for example, a hard disk drive, a non-volatile memory (such as flash memory or other electronically programmable erasure-restricted memory used to form a solid-state drive), or a volatile memory (such as static or dynamic random access memory), etc. The embodiments of this application are not limited to these types of memory.
[0043] The memory 10 stores executable program code; the processor 20 coupled to the memory 10 calls the executable program code stored in the memory 10 to execute the following map construction method: acquiring multiple frames of sensor data collected by the sensors as the robot moves along the mapping path; obtaining multiple pose nodes and multiple local probability grid maps of the robot based on the multiple frames of sensor data; obtaining a first relative pose relationship between adjacent pose nodes and a second relative pose relationship between pose nodes and local probability grid maps based on the multiple frames of sensor data, multiple pose nodes, and multiple local probability grid maps; acquiring multiple frames of image data collected by the image acquisition device on the identification code; associating the multiple frames of image data with the corresponding pose nodes to obtain the observation value of the identification code; optimizing the multiple pose nodes, the multiple local probability grid maps, and the observation value using the first relative pose relationship, the second relative pose relationship, and the observation value as constraints; and constructing a localization map using the optimized multiple pose nodes, the multiple local probability grid maps, the observation value, and the multiple frames of sensor data.
[0044] See Figure 2 This application provides a map construction method, which mainly includes steps S201 to S207, as described below:
[0045] Step S201: Acquire multiple frames of sensor data collected by the sensor devices as the robot moves along the mapped path.
[0046] In this embodiment of the application, the mapping path is the route taken by the robot when it moves for the purpose of mapping. The sensors here include two-dimensional LiDAR and wheeled odometers, etc. Therefore, the multi-frame sensing data collected by the sensors include LiDAR point cloud data and wheeled odometer data, i.e., position, attitude and other information.
[0047] Step S202: Obtain multiple pose nodes and multiple local probability grid maps of the robot through multi-frame sensor data.
[0048] In this embodiment, as described above, the robot is equipped with sensors including a wheeled odometer and a two-dimensional LiDAR. Multiple frames of sensor data collected by these two sensors can yield relatively accurate multiple pose nodes for the robot. It should be noted that the robot's multiple pose nodes include historical pose nodes and the latest pose node. The latest pose node is also the robot's current pose node. This is because the robot's pose nodes include a time variable; that is, the robot's pose nodes are pose nodes at different times. The pose node obtained at the current time is the latest pose node or the current pose node, while the pose nodes obtained before the current time are historical pose nodes. Furthermore, the local probability grid map is relative to the global probability grid map, meaning that the grid map is limited to a certain range. As an embodiment of this application, obtaining multiple pose nodes and multiple local probability grid maps for the robot from multiple frames of sensor data can be achieved through the following steps S2021 to S2025:
[0049] Step S2021: Create an initial pose node and an initial local probability grid map.
[0050] The initial pose node refers to the pose node being 0, and the initial local probability grid map refers to the local probability grid map where the pose of the pose node is 0.
[0051] Step S2022: Create a new pose node based on odometry data and / or laser point cloud data.
[0052] Generally, creating more pose nodes means improved accuracy in subsequent mapping. However, creating more pose nodes comes at the cost of increased computational cost or reduced real-time performance. Therefore, careful selection is needed when creating new pose nodes. Specifically, creating a new pose node based on odometry data and / or laser point cloud data can be done as follows: if the odometry data and / or laser point cloud data indicate that the robot's translational distance compared to the previous pose node is greater than or equal to a first translational threshold, and / or the robot's rotational distance is greater than or equal to a first rotational threshold, then a new pose node is created.
[0053] Step S2023: Obtain the pose prediction value of the new pose node based on the odometry data corresponding to the new pose node and the previous pose node.
[0054] In this embodiment of the application, the previous pose node is the pose node obtained in the previous event cycle according to steps S2021 to S2024.
[0055] Step S2024: Use the laser point cloud data corresponding to the new pose node and the local probabilistic grid map to correct the pose prediction value to obtain the latest pose node.
[0056] Since the pose prediction value of the new pose node is obtained based on the odometry data corresponding to the new pose node and the previous pose node, there is a certain error or uncertainty. Therefore, it is necessary to use the laser point cloud data corresponding to the new pose node and the local probabilistic grid map to correct the pose prediction value to obtain the latest pose node.
[0057] Step S2025: Create a new local probability grid map based on the latest pose node and the previous local probability grid map.
[0058] Specifically, creating a new local probability grid map based on the latest pose node and the previous local probability grid map can be done as follows: if the translation distance between the latest pose node and the previous local probability grid map is greater than or equal to a second translation threshold, then a new local probability grid map is created, and the pose value of the latest pose node is used as the pose value of the local probability grid map. In other words, the pose value of the first pose in the local probability grid map is used as the pose value of that local probability grid map.
[0059] For the initial local probabilistic grid map, its first pose node is the initial pose node. Therefore, the pose value of the local probabilistic grid map is the same as the pose value of the initial pose node (e.g., it can be (0, 0, 0)).
[0060] After step S2025, return to the step of creating a new pose node and / or a new local probabilistic grid map based on odometry data and / or laser point cloud data to obtain multiple pose nodes and multiple local probabilistic grid maps, that is, repeat steps S2023 to S2025.
[0061] Step S203: Based on multi-frame sensor data, multiple pose nodes, and multiple local probability grid maps, obtain the first relative pose relationship between adjacent pose nodes and the second relative pose relationship between pose nodes and local probability grid maps.
[0062] It should be noted that the second relative pose relationship in step S203 includes both map relative relationship and closed-loop constraint relationship. Specifically, step S203 can be achieved through steps S2031 to S2033, as explained below:
[0063] Step S2031: Obtain the first relative pose relationship between every two adjacent pose nodes based on multiple pose nodes.
[0064] Specifically, based on every two adjacent pose nodes among multiple pose nodes, the first relative pose relationship between each pair of adjacent pose nodes is calculated. This results in multiple first relative pose relationships. Optionally, the first relative pose relationship between two adjacent pose nodes specifically refers to the relative pose relationship between the pose values of the two adjacent pose nodes.
[0065] Step S2032: Obtain the map relative relationship based on each pose node and the corresponding local probabilistic grid map.
[0066] Specifically, the laser point cloud data associated with the latest pose node among multiple pose nodes of the robot is matched with the corresponding local probabilistic grid map. If the laser point cloud data associated with the latest pose node of the robot successfully matches the corresponding local probabilistic grid map, the relative pose relationship C between the latest pose node of the robot and the successfully matched local probabilistic grid map is obtained. ij The map relationship between the pose node and the corresponding local probability grid map.
[0067] In the continuous process of creating poses, each latest pose node is matched with the local probability grid map where that pose node is located (and its corresponding location), thereby obtaining the map relative relationship between each pose node and the local probability grid map, thus obtaining multiple map relative relationships. Similarly, the map relative relationship between each pose node and the local probability grid map is also the relative positional relationship of its pose value.
[0068] Step S2033: Through loop closure detection, obtain the closed loop constraint relationship based on the laser point cloud data corresponding to the latest pose node and other local grid probability maps. The other local grid probability maps refer to the local grid probability maps that are not corresponding to the latest pose node.
[0069] In the process of continuously creating poses, for each latest pose node, loop closure detection is performed with other local grid probability maps to determine whether there are any loop constraints. If there are, the loop constraints are obtained; otherwise, they are considered non-existent.
[0070] The loop closure detection here is also known as loop loop detection, which is an existing technology and will not be elaborated on here. Once a loop closure frame is detected, the loop closure constraint relationship can be obtained based on the laser point cloud data corresponding to the latest pose node and other local grid probability maps, that is, the edge between two points with a loop closure relationship in graph optimization.
[0071] Step S204: Acquire multiple frames of image data collected by the image acquisition device from the identification code.
[0072] In this embodiment, the identification code can be a marker affixed to an environment with limited observation constraints for two-dimensional LiDAR positioning, such as a long corridor or glass wall. The image acquisition device carried by the robot can be a monocular, binocular, or tricular camera. By capturing images of the identification code using these image acquisition devices, multiple frames of image data can be obtained.
[0073] Step S205: Associate the multiple frames of image data acquired by the image acquisition device with the corresponding pose nodes to obtain multiple observation values of pose nodes and identification codes.
[0074] Since the technical solution of this application aims to address scenarios unsuitable for 2D LiDAR positioning, in this embodiment, the identification codes can be markers affixed in environments with limited observation constraints for 2D LiDAR positioning, such as long corridors or glass walls. The image acquisition device on the robot can be a monocular, binocular, or tricular camera, which can determine the pose of these identification codes by acquiring them in the environment. This pose is the robot's observation value of the identification codes in the environment. Furthermore, multiple frames of image data can be associated with the robot's pose nodes. Thus, when the pose of the identification codes in the environment cannot be acquired through the robot's image acquisition device, the pose of the identification codes, i.e., the observation value of the identification codes, can be obtained through the above association relationship. As for the specific association method, it can be a time association method, that is, associating the robot's observation values of the same identification code in the environment at different times with different pose nodes of the robot, or it can be an ID association method, that is, associating the robot's observation values of different identification codes in the environment with the same pose node of the robot.
[0075] Optionally, multiple identification codes may be observed for the same pose node. Similarly, the same identification code may be observed by multiple pose nodes. Therefore, by associating multiple frames of image data acquired by the image acquisition device with the corresponding pose nodes, multiple observation values of the identification code can be obtained.
[0076] In some cases, there may be one observation, or at least two or more. Step S206: Optimize multiple pose nodes, multiple local probabilistic grid maps, and observations using the first relative pose relationship, the second relative pose relationship, and the observations as constraints.
[0077] Specifically, step S206 can be implemented as follows: A nonlinear least squares problem is constructed using the first relative pose relationship, the second relative pose relationship, and the observed values as constraints, and multiple pose nodes, multiple local probability grid maps, and the observed values as optimization objectives. The nonlinear least squares problem is then solved using a graph optimization algorithm, and the optimal solution of the nonlinear least squares problem is used as the optimized multiple pose nodes, multiple local probability grid maps, and the observed values. Here, the graph optimization algorithm can be the Levenberg-Marquardt algorithm or the Gauss-Newton method, etc., and this application does not limit its implementation.
[0078] In an optional embodiment, multiple or all first relative pose relationships, multiple or all second relative pose relationships, and multiple or all observation values are constrained and uniformly optimized to obtain multiple pose nodes, multiple local probability grid maps, and observation values.
[0079] Step S207: Construct a localization map using the optimized multiple pose nodes, multiple local probabilistic grid maps, observations, and multiple frames of sensor data.
[0080] Specifically, step S207 can be implemented as follows: establishing a global probability grid map based on the optimized multiple pose nodes and their associated multi-frame sensor data and multiple local probability grid maps; establishing an identification map based on the optimized observations; and associating the identification map with the global probability grid map to form a localization map. Unlike existing technologies that stitch together local probability grid maps to form a global probability grid map, this embodiment establishes the global probability grid map using the optimized multiple pose nodes and their associated multi-frame sensor data and multiple local probability grid maps. Since the map construction method no longer relies on local probability grid maps but strictly follows the SLAM mapping process, and the mapping parameters are all optimized data, establishing the global probability grid map using the optimized multiple pose nodes and their associated multi-frame sensor data and multiple local probability grid maps as parameters reduces mapping errors. Similarly, writing the optimized observations into the identification map-related files and constructing the identification map can also reduce mapping errors.
[0081] From the above appendix Figure 2As can be seen from the example map construction method, when optimizing the robot's latest pose node, the constraint of the robot's observation of the environmental markers is added. In other words, it is only necessary to paste a limited number of markers in the environment where 2D LiDAR positioning is not applicable, and combine the robot's latest pose node, local probabilistic grid map, and LiDAR point cloud data associated with the latest pose node obtained by the robot's 2D LiDAR, etc., after optimizing the robot's latest pose node and the pose data of the environmental markers, a global probabilistic grid map and marker map that can be stably output can be established, thereby breaking through the limitations of 2D LiDAR positioning in certain scenarios.
[0082] Please see Figure 3 This application provides a map building device, which may be the central processing unit of a robot or a functional module therein. The device may include a first acquisition module 301, a second acquisition module 302, a third acquisition module 303, a fourth acquisition module 304, an association module 305, an optimization module 306, and a map building module 307, as detailed below:
[0083] The first acquisition module 301 is used to acquire multiple frames of sensor data collected by the sensor devices when the robot moves along the mapping path;
[0084] The second acquisition module 302 is used to obtain multiple pose nodes and multiple local probability grid maps of the robot based on multi-frame sensing data.
[0085] The third acquisition module 303 is used to obtain, based on the multi-frame sensing data, multiple pose nodes and multiple local probability grid maps, the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps.
[0086] The fourth acquisition module 304 is used to acquire multiple frames of image data acquired by the image acquisition device from the identification code;
[0087] The association module 305 is used to associate multiple frames of image data with corresponding pose nodes to obtain multiple observation values of pose nodes and identification codes;
[0088] The optimization module 306 is used to optimize multiple pose nodes, multiple local probability grid maps and observations, constrained by the first relative pose relationship, the second relative pose relationship and the observation values.
[0089] The map building module 307 is used to build a localization map using optimized multiple pose nodes, multiple local probabilistic grid maps, observations and multiple frames of sensor data.
[0090] In one embodiment of this application, the above appendix Figure 3The example's second acquisition module 302 may include an initialization unit, a first creation unit, a predicted value acquisition unit, a correction unit, and a second creation unit, wherein:
[0091] The initialization unit is used to create an initial pose node and an initial local probabilistic grid map.
[0092] The first creation unit is used to create a new pose node based on odometry data and / or laser point cloud data;
[0093] The prediction value acquisition unit is used to acquire the pose prediction value of the new pose node based on the odometry data corresponding to the new pose node and the previous pose node.
[0094] The correction unit is used to correct the pose prediction value using the laser point cloud data corresponding to the new pose node and the local probabilistic grid map to obtain the latest pose node.
[0095] The second creation unit is used to create a new local probability grid map based on the latest pose node and the previous local probability grid map.
[0096] Optionally, in another embodiment of this application, the first creation unit may include a new pose node creation unit, and the second creation unit may include a new map creation unit, wherein:
[0097] The new pose node creation unit is used to create a new pose node if odometry data and / or laser point cloud data indicate that the translational distance of the robot compared to the previous pose node is greater than or equal to a first translation threshold, and / or the rotational distance of the robot is greater than or equal to a first rotation threshold.
[0098] The new map creation unit is used to create a new local probability grid map if the translation distance between the latest pose node and the previous local probability grid map is greater than or equal to a second translation threshold, and to use the latest pose node as the pose of the local probability grid map.
[0099] Optionally, in another embodiment of this application, the above-mentioned appendix... Figure 3 The example's third acquisition module 303 may include a first relative pose relationship acquisition unit, a map relative relationship acquisition unit, and a closed-loop constraint relationship acquisition unit, wherein:
[0100] The first relative pose relationship acquisition unit is used to acquire the first relative pose relationship between every two adjacent pose nodes based on multiple pose nodes.
[0101] The map relative relationship acquisition unit is used to acquire the map relative relationship based on each pose node and the corresponding local probabilistic grid map.
[0102] The closed-loop constraint relationship acquisition unit is used to obtain the closed-loop constraint relationship based on the laser point cloud data corresponding to the latest pose node and other local grid probability maps through loop closure detection. The other local grid probability maps refer to the local grid probability maps that are not corresponding to the latest pose node.
[0103] Optionally, in another embodiment of this application, the above-mentioned appendix... Figure 3 The example optimization module 306 may include a problem building unit and a problem solving unit, wherein:
[0104] The problem construction unit is used to construct a nonlinear least squares problem with constraints of the first relative pose relationship, the second relative pose relationship and the observation value, and with multiple pose nodes, multiple local probability grid maps and the observation value as optimization objectives.
[0105] The problem-solving unit is used to solve nonlinear least squares problems using graph optimization algorithms. The optimal solution of the nonlinear least squares problem is used as multiple pose nodes, multiple local probability grid maps, and observations after optimization.
[0106] Optionally, in another embodiment of this application, the above-mentioned appendix... Figure 3 The example map building module 307 may include a first map generation unit, a second map generation unit, and an association unit, wherein:
[0107] The first image generation unit is used to build a global probability grid map based on the optimized multiple pose nodes and their associated multi-frame sensor data and multiple local probability grid maps.
[0108] The second map generation unit is used to build a label map based on the optimized observations;
[0109] The association unit is used to associate the identifier map with the global probabilistic raster map to form a location map.
[0110] From the appendix Figure 3 As can be seen from the example device, when optimizing the robot's latest pose node, the constraint of the robot's observation of the markers in the environment is added. In other words, it is only necessary to paste a limited number of markers in an environment where 2D LiDAR positioning is not applicable, and combine the robot's latest pose node, local probabilistic grid map, and laser point cloud data associated with the latest pose node obtained by the robot's 2D LiDAR, after optimizing the robot's latest pose node and the pose data of the markers in the environment, a global probabilistic grid map and marker map that can be stably output can be established, thereby breaking through the limitations of 2D LiDAR positioning in certain scenarios.
[0111] Figure 4 This is a schematic diagram of the structure of a device provided in one embodiment of this application. For example... Figure 4As shown, the device 4 in this embodiment can be a robot or a module thereof, mainly including: a processor 40, a memory 41, and a computer program 42 stored in the memory 41 and executable on the processor 40, such as a map building method program. When the processor 40 executes the computer program 42, it implements the steps in the above-described map building method embodiment, for example... Figure 2 The steps S201 to S207 are shown. Alternatively, when the processor 40 executes the computer program 42, it implements the functions of each module / unit in the above-described device embodiments, for example... Figure 3 The functions of the first acquisition module 301, the second acquisition module 302, the third acquisition module 303, the fourth acquisition module 304, the association module 305, the optimization module 306, and the map building module 307 are shown.
[0112] For example, the computer program 42 of the map construction method mainly includes: acquiring multiple frames of sensor data collected by sensors when the robot moves along the mapping path; obtaining multiple pose nodes and multiple local probability grid maps of the robot based on the multiple frames of sensor data; obtaining a first relative pose relationship between adjacent pose nodes and a second relative pose relationship between pose nodes and local probability grid maps based on the multiple frames of sensor data, multiple pose nodes, and multiple local probability grid maps; acquiring multiple frames of image data collected by the image acquisition device on the identification code; associating the multiple frames of image data with the corresponding pose nodes to obtain the observation value of the identification code; optimizing the multiple pose nodes, multiple local probability grid maps, and observation values with constraints of the first relative pose relationship, the second relative pose relationship, and the observation values; and constructing a map using the optimized multiple pose nodes, multiple local probability grid maps, observation values, and multiple frames of sensor data. The computer program 42 can be divided into one or more modules / units, one or more modules / units are stored in the memory 41, and executed by the processor 40 to complete this application. One or more modules / units can be a series of computer program instruction segments capable of performing specific functions. These instruction segments describe the execution process of computer program 42 in device 4. For example, computer program 42 can be divided into the functions of a first acquisition module 301, a second acquisition module 302, a third acquisition module 303, a fourth acquisition module 304, an association module 305, an optimization module 306, and a map building module 307 (a module in the virtual device). The specific functions of each module are as follows: The first acquisition module 301 is used to acquire multiple frames of sensor data collected by sensors when the robot moves along the mapping path; the second acquisition module 302 is used to obtain multiple pose nodes and multiple local probability grid maps of the robot based on the multiple frames of sensor data; the third acquisition module 303 is used to obtain multiple pose nodes and multiple local probability grid maps of the robot based on the multiple frames of sensor data, multiple pose nodes, and multiple local probability grid maps. The system obtains the first relative pose relationship between adjacent pose nodes and the second relative pose relationship between pose nodes and local probability grid maps; the fourth acquisition module 304 is used to acquire multiple frames of image data acquired by the image acquisition device for the identification code; the association module 305 is used to associate the multiple frames of image data with the corresponding pose nodes to obtain the observation value of the identification code; the optimization module 306 is used to optimize multiple pose nodes, multiple local probability grid maps and observation values with the first relative pose relationship, the second relative pose relationship and the observation value as constraints; the map construction module 307 is used to construct a map using the optimized multiple pose nodes, multiple local probability grid maps, observation values and multiple frames of sensor data.
[0113] Device 4 may include, but is not limited to, processor 40 and memory 41. Those skilled in the art will understand that... Figure 4This is merely an example of device 4 and does not constitute a limitation on device 4. It may include more or fewer components than shown, or combine certain components, or different components. For example, a computing device may also include input / output devices, network access devices, buses, etc.
[0114] The processor 40 may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.
[0115] The memory 41 can be an internal storage unit of the device 4, such as a hard disk or RAM of the device 4. The memory 41 can also be an external storage device of the device 4, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, or Flash Card equipped on the device 4. Furthermore, the memory 41 can include both internal and external storage units of the device 4. The memory 41 is used to store computer programs and other programs and data required by the device. The memory 41 can also be used to temporarily store data that has been output or will be output.
[0116] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed. That is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above-described device can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.
[0117] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.
[0118] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.
[0119] In the embodiments provided in this application, it should be understood that the disclosed apparatus / device and method can be implemented in other ways. For example, the apparatus / device embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.
[0120] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.
[0121] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.
[0122] If an integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it may be stored in a non-transitory computer-readable storage medium. Based on this understanding, the implementation of all or part of the processes in the above-described embodiments can also be accomplished by a computer program instructing related hardware. The computer program for the map building method can be stored in a computer-readable storage medium. When executed by a processor, the computer program can implement the steps of the various method embodiments described above, namely: acquiring multiple frames of sensor data collected by sensors as the robot moves along the mapping path; obtaining multiple pose nodes and multiple local probability grid maps of the robot based on the multiple frames of sensor data; obtaining a first relative pose relationship between adjacent pose nodes and a second relative pose relationship between pose nodes and local probability grid maps based on the multiple frames of sensor data, multiple pose nodes, and multiple local probability grid maps; acquiring multiple frames of image data collected by an image acquisition device from an identification code; associating the multiple frames of image data with the corresponding pose nodes to obtain the observed value of the identification code; optimizing the multiple pose nodes, multiple local probability grid maps, and observed values using the first relative pose relationship, the second relative pose relationship, and the observed values as constraints; and constructing a map using the optimized multiple pose nodes, multiple local probability grid maps, observed values, and multiple frames of sensor data. Computer programs include computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. Non-transitory computer-readable media can include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in non-transitory computer-readable media can be appropriately added to or removed according to the requirements of legislation and patent practice in a jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, non-transitory computer-readable media do not include electrical carrier signals and telecommunication signals. The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of this application. It should be understood that the above description is only a specific embodiment of this application and is not intended to limit the scope of protection of this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this invention.
Claims
1. A robot, characterized in that, The robot includes: Memory, processor, sensor devices, and image acquisition devices; The memory stores executable program code; The processor coupled to the memory calls executable program code stored in the memory to execute a map construction method, the method comprising: Acquire multiple frames of sensor data collected by the sensors as the robot moves along the mapped path; Based on the multi-frame sensor data, multiple pose nodes and multiple local probability grid maps of the robot are obtained; Based on the multi-frame sensing data, multiple pose nodes, and multiple local probability grid maps, the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps are obtained. The image acquisition device acquires multiple frames of image data related to the identification code. The multi-frame image data is associated with the corresponding pose nodes to obtain multiple observation values of the pose nodes and the identification codes; The plurality of pose nodes, the plurality of local probabilistic grid maps, and the observed values are optimized using the first relative pose relationship, the second relative pose relationship, and the observed values as constraints. A localization map is constructed using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observed values, and the multi-frame sensor data.
2. The robot as described in claim 1, characterized in that, The sensor devices include a wheeled odometer and a two-dimensional lidar, and the sensing data includes odometer data and lidar point cloud data. The processor calls executable program code stored in the memory, and the step of obtaining multiple pose nodes and multiple local probabilistic grid maps of the robot based on the multi-frame sensing data in the map construction method includes: Create an initial pose node and an initial local probabilistic grid map; Create a new pose node based on odometry data and / or laser point cloud data; The pose prediction value of the new pose node is obtained based on the odometry data corresponding to the new pose node and the previous pose node. The latest pose node is obtained by correcting the pose prediction value using the laser point cloud data corresponding to the new pose node and the local probabilistic grid map. A new local probability grid map is created based on the latest pose node and the previous local probability grid map; Return to the step of creating a new pose node and / or a new local probabilistic raster map based on odometry data and / or laser point cloud data to obtain multiple pose nodes and multiple local probabilistic raster maps.
3. The robot according to claim 2, characterized in that, The processor calls the executable program code stored in the memory to execute the step of creating a new pose node based on odometry data and / or laser point cloud data in the map construction method, which includes: If the odometry data and / or laser point cloud data indicate that the translational distance of the robot compared to the previous pose node is greater than or equal to a first translation threshold, and / or the rotational distance of the robot is greater than or equal to a first rotation threshold, then a new pose node is created. The processor calls the executable program code stored in the memory to execute the map construction method, specifically the step of creating a new local probabilistic raster map based on the latest pose node and the previous local probabilistic raster map, which includes: If the translation distance between the latest pose node and the previous local probability grid map is greater than or equal to the second translation threshold, a new local probability grid map is created, and the latest pose node is used as the pose of the local probability grid map.
4. The robot as described in claim 1, characterized in that, The second relative pose relationship includes map relative relationships and closed-loop constraint relationships. The processor calls the executable program code stored in the memory to execute the map construction method. The steps of obtaining the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probabilistic grid maps based on the multi-frame sensor data, multiple pose nodes, and multiple local probabilistic grid maps include: The first relative pose relationship between every two adjacent pose nodes is obtained based on multiple pose nodes. The relative relationship between the map and the corresponding local probability grid map is obtained based on each pose node; By detecting loop closures, the closed-loop constraint relationship is obtained based on the laser point cloud data corresponding to the latest pose node and other local grid probability maps. The other local grid probability maps refer to local grid probability maps that are not corresponding to the latest pose node.
5. The robot as described in claim 1, characterized in that, The processor calls the executable program code stored in the memory to execute a map construction method, in which the step of optimizing the plurality of pose nodes, the plurality of local probabilistic grid maps, and the observation values under constraints of the first relative pose relationship, the second relative pose relationship, and the observation values includes: Using the first relative pose relationship, the second relative pose relationship, and the observed value as constraints, and the plurality of pose nodes, the plurality of local probabilistic grid maps, and the observed value as optimization objectives, a nonlinear least squares problem is constructed. The nonlinear least squares problem is solved using a graph optimization algorithm, and the optimal solution of the nonlinear least squares problem is used as the optimized multiple pose nodes, the multiple local probability grid maps, and the observations.
6. The robot as described in claim 1, characterized in that, The processor calls the executable program code stored in the memory to execute the map construction method, and the step of constructing a localization map using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observation values, and the multi-frame sensor data includes: A global probability grid map is established based on the optimized multiple pose nodes, their associated multi-frame sensor data, and multiple local probability grid maps. A labeling map is created based on the optimized observations; The identification map is associated with the global probability grid map to form the positioning map.
7. A map-building device, applied to a robot including sensor components and image acquisition devices, characterized in that, The device includes: The first acquisition module is used to acquire multiple frames of sensor data collected by the sensor device when the robot moves along the mapping path; The second acquisition module is used to obtain multiple pose nodes and multiple local probability grid maps of the robot based on the multi-frame sensing data. The third acquisition module is used to obtain, based on the multi-frame sensing data, multiple pose nodes and multiple local probability grid maps, the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps. The fourth acquisition module is used to acquire multiple frames of image data captured by the image acquisition device for the identification code; The association module is used to associate the multi-frame image data with the corresponding pose nodes to obtain multiple observation values of the pose nodes and the identification codes. An optimization module is used to optimize the plurality of pose nodes, the plurality of local probability grid maps, and the observation values, constrained by the first relative pose relationship, the second relative pose relationship, and the observation values. The map building module is used to construct a positioning map using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observation values, and the multi-frame sensor data.
8. A map construction method, characterized in that, The method is applied to a robot, including robot sensors and image acquisition devices, and the method includes: Acquire multiple frames of sensor data collected by the sensors as the robot moves along the mapped path; Based on the multi-frame sensor data, multiple pose nodes and multiple local probability grid maps of the robot are obtained; Based on the multi-frame sensing data, multiple pose nodes, and multiple local probability grid maps, the first relative pose relationship between every two adjacent pose nodes and the second relative pose relationship between each pose node and the multiple local probability grid maps are obtained. The image acquisition device acquires multiple frames of image data related to the identification code. The multi-frame image data is associated with the corresponding pose nodes to obtain multiple observation values of the pose nodes and the identification codes; The plurality of pose nodes, the plurality of local probabilistic grid maps, and the observed values are optimized using the first relative pose relationship, the second relative pose relationship, and the observed values as constraints. A localization map is constructed using the optimized multiple pose nodes, the multiple local probabilistic grid maps, the observed values, and the multi-frame sensor data.
9. The map construction method as described in claim 8, characterized in that, The sensor devices include a wheeled odometer and a two-dimensional lidar, and the sensing data includes odometer data and lidar point cloud data. The step of obtaining multiple pose nodes and multiple local probabilistic grid maps of the robot based on the multi-frame sensing data includes: Create an initial pose node and an initial local probabilistic grid map; Create a new pose node based on odometry data and / or laser point cloud data; The pose prediction value of the new pose node is obtained based on the odometry data corresponding to the new pose node and the previous pose node. The latest pose node is obtained by correcting the pose prediction value using the laser point cloud data corresponding to the new pose node and the local probabilistic grid map. A new local probability grid map is created based on the latest pose node and the previous local probability grid map; Return to the step of creating a new pose node and / or a new local probabilistic raster map based on odometry data and / or laser point cloud data to obtain multiple pose nodes and multiple local probabilistic raster maps.
10. A readable storage medium having a computer program stored thereon, characterized in that, The computer program is used to implement a map building method when executed by a processor, the map building method being the map building method implemented by the robot according to any one of claims 1 to 6.