Robot positioning method, apparatus, device, and computer-readable storage medium

Abstract

The application discloses a robot positioning method, comprising: creating a plurality of pose nodes according to preset conditions in sequence, and taking the latest created pose node as a new pose node; after creating each current pose node of the plurality of pose nodes and before creating a next pose node, establishing a first constraint, a second constraint and a third constraint of each current pose node; then optimizing all the first constraints, the second constraints and the third constraints of adjacent preset number of pose nodes to obtain an optimized pose of each pose node, and taking the optimized pose of the new pose node in the adjacent preset number of pose nodes as a pose of the robot. The method can reduce time consumption while ensuring positioning stability and positioning accuracy. The application also discloses a robot positioning device, equipment and a computer readable storage medium, which have the above technical effects.

Description

Technical Field

[0001] This application relates to the field of robotics, and in particular to a robot positioning method; it also relates to a robot positioning device, equipment, and computer-readable storage medium. Background Technology

[0002] Currently, indoor delivery robots typically use 2D LiDAR for positioning or visual tags. For 2D LiDAR, the limited observation constraints in scenarios like long corridors and glass walls lead to delayed correction of positioning errors, reduced accuracy, and even, with excessive accumulated errors, incorrect delivery locations. Visual tags, usually affixed to the ceiling, can ensure stable positioning when sufficient numbers are present; however, applying a sufficiently dense array of tags is time-consuming.

[0003] In view of this, how to reduce time consumption while ensuring positioning stability and accuracy has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0004] The purpose of this application is to provide a robot localization method that can reduce time consumption while ensuring localization stability and accuracy. Another purpose of this application is to provide a robot localization device, equipment, and computer-readable storage medium, all of which have the above-mentioned technical effects.

[0005] To address the aforementioned technical problems, this application provides a robot localization method, which includes:

[0006] Multiple pose nodes are created sequentially according to preset conditions, and the newly created pose node is used as the new pose node.

[0007] After creating each current pose node of the plurality of pose nodes and before creating the next pose node, a first constraint, a second constraint, and a third constraint are established for each current pose node. Specifically, an odometer constraint is established between the current pose node and the previous pose node based on the robot's odometry data and pose at the current pose node and the odometry data and pose of the previous pose node, and this constraint serves as the first constraint. A laser constraint is established based on the robot's LiDAR data, pose, and grid map at the current pose node, and this constraint serves as the second constraint. When a visual marker is detected at the current pose node, a constraint is established between the pose of the visual marker and the pose corresponding to the current pose node, based on the robot's pose at the current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, and this constraint serves as the third constraint.

[0008] A predetermined number of adjacent pose nodes are selected from the plurality of pose nodes, and the first constraint, the second constraint, and the third constraint of each pose node in the predetermined number of adjacent pose nodes are optimized to obtain the optimized pose of each pose node in the predetermined number of adjacent pose nodes. The predetermined number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

[0009] To address the aforementioned technical problems, this application also provides a robot positioning device, comprising:

[0010] The pose node creation module is used to create multiple pose nodes sequentially according to preset conditions, and the latest created pose node is used as the new pose node.

[0011] The constraint module is used to establish a first constraint, a second constraint, and a third constraint for each current pose node after the creation of each of the plurality of pose nodes and before the creation of the next pose node. The constraint module includes a first constraint establishment module, a second constraint establishment module, and a third constraint establishment module.

[0012] The first constraint establishment module is used to establish a mileage constraint between the current pose node and the previous pose node based on the odometry data and pose of the robot at the current pose node and the odometry data and pose of the previous pose node, and use it as the first constraint.

[0013] The second constraint establishment module is used to establish laser constraints based on the robot's lidar data, pose, and grid map at the current pose node, and use these constraints as the second constraint.

[0014] The third constraint establishment module is used to establish a constraint between the visual label and the pose corresponding to the current pose node when a visual label is identified at the current pose node, based on the pose of the robot at the current pose node, the pose of the visual label in the robot coordinate system, and the pose of the visual label in the grid map coordinate system, and to serve as the third constraint.

[0015] The graph optimization module is used to select a preset number of adjacent pose nodes from the plurality of pose nodes and graph optimize the first constraint, the second constraint, and the third constraint of each pose node in the preset number of adjacent pose nodes to obtain the optimized pose of each pose node in the preset number of adjacent pose nodes. The preset number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

[0016] To address the aforementioned technical problems, this application also provides a robot, comprising:

[0017] A processor and a memory connected to the processor, the memory including a computer program, wherein the processor executes the computer program to perform the following steps:

[0018] Multiple pose nodes are created sequentially according to preset conditions, and the newly created pose node is used as the new pose node.

[0019] After creating each current pose node of the plurality of pose nodes and before creating the next pose node, a first constraint, a second constraint, and a third constraint are established for each current pose node. Specifically, an odometer constraint is established between the current pose node and the previous pose node based on the robot's odometry data and pose at the current pose node and the odometry data and pose of the previous pose node, and this constraint serves as the first constraint. A laser constraint is established based on the robot's LiDAR data, pose, and grid map at the current pose node, and this constraint serves as the second constraint. When a visual marker is detected at the current pose node, a constraint is established between the pose of the visual marker and the pose corresponding to the current pose node, based on the robot's pose at the current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, and this constraint serves as the third constraint.

[0020] A predetermined number of adjacent pose nodes are selected from the plurality of pose nodes, and the first constraint, the second constraint, and the third constraint of each pose node in the predetermined number of adjacent pose nodes are optimized to obtain the optimized pose of each pose node in the predetermined number of adjacent pose nodes. The predetermined number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

[0021] To address the aforementioned technical problems, this application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the robot localization method as described in any of the preceding claims.

[0022] The robot localization method provided in this application includes: creating pose nodes sequentially according to preset conditions, and using the latest created pose node as the new pose node; during the creation of the plurality of pose nodes, establishing a first constraint, a second constraint, and a third constraint for each current pose node after its creation and before the creation of the next pose node; wherein...

[0023] Based on the odometry data and pose of the robot at the current pose node and the odometry data and pose of the previous pose node, the mileage constraint between the current pose node and the previous pose node is established and used as the first constraint.

[0024] The laser constraint is established based on the robot's LiDAR data, pose, and grid map at the current pose node, and serves as the second constraint.

[0025] When a visual marker is identified at the current pose node, based on the robot's pose at each current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, constraints are established between the visual marker and the pose corresponding to the current pose node, and these constraints are used as the third constraint. All the first, second, and third constraints of a predetermined number of adjacent pose nodes are optimized to obtain an optimized pose for each of the predetermined number of adjacent pose nodes, wherein the predetermined number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the robot's pose.

[0026] As can be seen, the robot localization method provided in this application integrates LiDAR localization and visual marker localization, combining the advantages of both to optimize the pose of the robot's latest pose nodes, thereby ensuring the accuracy of the output pose. While maintaining localization stability and accuracy, it can reduce the number of visual markers and lower processing time. Furthermore, this application employs graph optimization to integrate LiDAR localization and visual marker localization, achieving even higher localization accuracy.

[0027] The robot positioning device, equipment, and computer-readable storage medium provided in this application all have the aforementioned technical effects. Attached Figure Description

[0028] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the prior art and embodiments 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.

[0029] Figure 1 This is a flowchart illustrating a robot localization method provided in an embodiment of this application.

[0030] Figure 2 This is a schematic diagram of a graph optimization provided in an embodiment of this application;

[0031] Figure 3 This is a schematic diagram of a robot positioning device provided in an embodiment of this application;

[0032] Figure 4 This is a schematic diagram of a robot provided in an embodiment of this application. Detailed Implementation

[0033] The core of this application is to provide a robot localization method that reduces time consumption while ensuring localization stability and accuracy. Another core aspect of this application is to provide a robot localization device, equipment, and computer-readable storage medium, all of which possess the aforementioned technical effects.

[0034] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, 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, 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.

[0035] Please refer to Figure 1, Figure 1 This is a flowchart illustrating a robot localization method provided in an embodiment of this application. (Refer to...) Figure 1 As shown, the method includes:

[0036] S101: Create multiple pose nodes sequentially according to preset conditions, and use the latest created pose node as the new pose node;

[0037] Specifically, this application achieves robot localization based on LiDAR data, wheeled odometer data, images captured by cameras, and grid maps and label maps. The grid map is used for LiDAR localization. The label map is used for visual label localization. The label map contains the ID of each visual label. This step aims to continuously create pose nodes according to preset conditions. Each pose node contains the pose, creation time, and node number.

[0038] In one specific implementation, creating multiple pose nodes sequentially according to preset conditions includes: initializing the robot's pose to obtain an initial pose node; determining whether the robot's pose meets the preset conditions for adding pose nodes; and adding a new pose node whenever the robot's pose meets the preset conditions for adding pose nodes.

[0039] In other words, based on the creation of the initial pose node, a new pose node is created whenever the robot's pose meets the preset conditions.

[0040] One method for initializing the robot pose and obtaining the initial pose node is as follows:

[0041] Run the first initialization thread and the second initialization thread; the first initialization thread obtains the robot's pose based on the visual labels in the current image; the second initialization thread obtains the robot's pose based on the current LiDAR data;

[0042] When either the first initialization thread or the second initialization thread is successfully initialized, the robot's pose obtained by the successfully initialized thread is used as the initial pose, and the initial pose node corresponding to the initial pose is created.

[0043] Specifically, running the first initialization thread and the second initialization thread can be done by running the first initialization thread and the second initialization thread synchronously.

[0044] Specifically, during initialization, the first initialization thread and the second initialization thread run synchronously. Initialization is considered successful when either thread successfully initializes, and the robot's pose obtained by the successfully initialized thread is used as the initial pose, and the corresponding initial pose node is created. If neither thread fails to initialize, both threads continue to run until initialization is successful.

[0045] During the execution of the first initialization thread, the visual marker recognition module identifies the input image and determines whether visual markers exist within it. If a visual marker is present, it outputs the marker's ID and its pose in the robot's coordinate system. Based on the marker map, the robot's initial pose is then calculated. The robot's initial pose is represented as follows:

[0046] w T r = w T mi *( r T mi ) -1 ; w T r This indicates the robot's initial pose. w T mi This indicates the pose of the visual representation in the grid map coordinate system. r T mi This represents the pose of the visual label in the robot coordinate system. The subscript i represents the ID of the visual label.

[0047] Conversely, if there are no visual markers in the image, the first initialization thread fails to initialize and continues with the initialization process.

[0048] When the second initialization thread runs, the robot's pose is calculated based on LiDAR data using a scanning matching algorithm. Initialization is considered successful when the matching score is higher than a preset threshold, and the calculated robot pose is used as the initial pose. For example, initialization is considered successful when the matching score is higher than 0.65.

[0049] Conversely, if the matching score is not higher than the preset threshold, the second initialization thread is considered to have failed in its current initialization and will continue with the initialization process.

[0050] If both the first and second initialization threads are successfully initialized, the two threads will generate two poses. At this point, one of the poses can be randomly selected as the robot's initial pose.

[0051] Alternatively, the method for determining whether the robot's pose meets the preset conditions for adding pose nodes can be:

[0052] Determine whether the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset mileage value, or whether the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset angle value.

[0053] If the mileage difference is greater than the preset mileage value or the orientation angle difference is greater than the preset angle value, then the robot's pose satisfies the condition for adding pose nodes.

[0054] Specifically, upon initialization, only one pose node, the initial pose node, is created. Based on this, after calculating the robot's current pose using LiDAR data or visual markings, the robot's current pose is compared to the pose corresponding to the previous pose node. If the odometer difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset odometer value, a new pose node corresponding to the current pose is added. Alternatively, if the angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset angle value, a new pose node corresponding to the current pose is added. Similarly, if both the odometer difference between the robot's current pose and the pose corresponding to the previous pose node are greater than a preset odometer value and the angle difference are greater than a preset angle value, a new pose node corresponding to the current pose is also added. If the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is not greater than a preset mileage value, and the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is not greater than a preset angle value, then no new pose node corresponding to the current pose will be added.

[0055] This application does not impose a single, fixed value on the aforementioned preset mileage and preset angle values; they can be set differently. For example, the preset mileage can be set to 10cm, and the preset angle to 5°. Therefore, if the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is greater than 10cm, a new pose node corresponding to the current pose is added. Alternatively, if the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than 5°, a new pose node corresponding to the current pose is added.

[0056] During the continuous creation of pose nodes, that is, after creating each current pose node and before creating the next pose node, the first constraint, the second constraint, and the third constraint of each current pose node are established.

[0057] Taking a specific scenario as an example, within a certain time period, a pose node is created every second from second 0 to second 10. When the pose node created in the first second is created, its first, second, and third constraints are immediately established. Similarly, when the pose node created in the fourth second is created, its first, second, and third constraints are immediately established.

[0058] S102: Based on the odometry data and pose of each current pose node of the robot and the previous pose node, establish the odometry constraint between each current pose node and the previous pose node, and use it as the first constraint of the current pose node.

[0059] S103: Establish laser constraints based on the lidar data, pose, and grid map corresponding to the current pose node, and use them as the second constraint for each current pose node;

[0060] S104: When a visual marker is identified at the current pose node, a constraint is established between the visual marker and the pose corresponding to the new pose node based on the pose of the current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, and this constraint is used as the third constraint.

[0061] Specifically, for each newly created pose node (i.e., at the moment of its creation, the newly created pose node is the current pose node), a mileage constraint is established between the current pose node and the previous pose node based on their odometry data and poses, and this constraint serves as the first constraint for the new pose node. The mileage constraint is specifically represented as follows:

[0062] O ij =(T k-1 ) -1 *T k - ok T ok-1 Among them, O ij T represents the mileage constraint. k-1 T represents the pose corresponding to the (k-1)th pose node. k This represents the pose corresponding to the k-th pose node. ok T ok-1 This represents the mileage change between the pose corresponding to the (k-1)th pose node and the pose corresponding to the kth pose node.

[0063] In addition, for each new pose node created, a laser constraint is established based on the corresponding LiDAR data, pose, and grid map, and this constraint serves as the second constraint for the new pose node. The laser constraint is specifically represented as follows:

[0064] m j =1-M smooth (T k *h k ); where m j M represents laser constraint. smooth T represents the bicubic interpolation function that smooths the probabilities of points in a raster map. kh represents the pose corresponding to the k-th pose node. k This represents the k-th laser point.

[0065] Furthermore, for each newly created pose node, when a visual identifier is recognized at that current pose node, a constraint is established between the visual identifier and the pose corresponding to the current pose node, based on the pose of the new pose node, the pose of the visual identifier in the robot coordinate system, and the pose of the visual identifier in the grid map coordinate system. This constraint serves as the third constraint for the current pose node. The specific representation of the constraint between the visual identifier and the pose is as follows:

[0066] I ij =(T k ) -1 * w T mi – r T mi Among them, I ij T represents the constraint of the pose corresponding to the visual label and the pose node. k This represents the pose corresponding to the k-th pose node. w T mi This indicates the pose of the visual representation in the grid map coordinate system. r T mi This indicates the pose of the visual label in the robot's coordinate system.

[0067] It is clear that for each pose node created, a first constraint and a second constraint will be established for that pose node. For pose nodes where visual markings can be identified, in addition to establishing the first and second constraints, a third constraint will also be established for that pose node.

[0068] It is also important to understand that the first, second, and third constraints are established between the creation of this pose node and the start of the next pose node. That is, when this pose node is the current pose node.

[0069] refer to Figure 2 As shown, Figure 2 In the diagram, x0, x1...x5 represent the robot's pose nodes, M1 and M2 represent visual markers, and the regular hexagon represents the grid map. Figure 2 Direct connections between the elements indicate constraints between them. It can be seen that pose nodes x0 to x4 have a first constraint, a second constraint, and a third constraint. Specifically, since two visual markers are identified at pose node x1, pose node x1 has two third constraints. For pose node x5, pose node x5 has the first and second constraints, but no visual markers are identified at pose node x5, so pose node x5 has no third constraint.

[0070] S105: Select a preset number of adjacent pose nodes from the plurality of pose nodes and optimize the first constraint, the second constraint, and the third constraint of each pose node in the preset number of adjacent pose nodes to obtain the optimized pose of each pose node in the preset number of adjacent pose nodes, wherein the preset number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

[0071] Specifically, this embodiment can perform graph optimization on all the first constraints, second constraints, and third constraints of a preset number of pose nodes. In one embodiment, a preset sliding window can be used to load the preset number of pose nodes, and graph optimization calculations can be performed on the first constraints, second constraints, and third constraints of all the preset number of pose nodes within the preset sliding window.

[0072] In other embodiments, a preset sliding window may not be used; instead, a preset number of adjacent pose nodes may be directly selected, and graph optimization may be performed on the first, second, and third constraints of each pose node within the preset number of adjacent pose nodes. Optionally, here, a preset number of adjacent pose nodes means that the preset number of pose nodes are adjacent to each other, i.e., a consecutive preset number of pose nodes.

[0073] Optionally, graph optimization of the first, second, and third constraints of each pose node in a preset number of adjacent nodes in the graph optimization refers to simultaneously performing optimization calculations on the first, second, and third constraints of each pose node in a preset number of adjacent nodes in the graph optimization.

[0074] In an optional embodiment, the adjacent preset number of pose nodes includes a new pose node (i.e., the latest created pose node). Specifically, the new pose node is the last pose node among the adjacent preset number of pose nodes.

[0075] In an optional embodiment, the preset quantity can be one, two, or more, and is not limited here. That is, the preset capacity of the sliding window can be one, two, or more, and is not limited here.

[0076] This embodiment uses a sliding window for cyclic localization as an example. A sliding window is pre-defined. While continuously creating pose nodes, whenever the number of created pose nodes reaches the capacity of the preset sliding window (i.e., the maximum number of pose nodes that the preset sliding window can load), graph optimization is performed on the first constraint, the second constraint, and the third constraint of all pose nodes within the preset sliding window to obtain the optimized pose of each pose node within the preset sliding window. Furthermore, the optimized pose of the last pose node within the preset sliding window is used as the robot's pose; wherein, the last pose node within the preset sliding window is the most recently created pose node.

[0077] For example, if the capacity of the preset sliding window is 10, that is, the corresponding preset quantity is 10, then when the number of pose nodes first reaches 10 (numbered pose nodes 1 to 10), graph optimization is performed on the first, second, and third constraints of pose nodes 1 to 10 within the preset sliding window to obtain the optimized poses of pose nodes 1 to 10. Furthermore, since pose node 10 is the last pose node within the preset sliding window, that is, pose node 10 is the newly created pose node, the optimized pose of pose node 10 is used as the robot's pose. As the number of pose nodes increases, when the number of pose nodes reaches 11 (numbered pose nodes 1 to 11), the preset sliding window will move. At this point, graph optimization is performed on the first, second, and third constraints of pose nodes 2 to 11 within the preset sliding window to obtain the optimized poses of pose nodes 2 to 11. Furthermore, since pose node 11 is the last pose node within the preset sliding window, i.e., pose node 11 is the most recently created pose node, the optimized pose of pose node 11 is used as the robot's pose. This process is repeated to perform cyclic localization of the robot.

[0078] In an optional embodiment, for multiple pose nodes within a preset sliding window, each node is actually used as the current pose node during creation, and its first constraint, second constraint, and third constraint are calculated. Therefore, for each of the 10 pose nodes within the preset sliding window, each pose node corresponds to a first constraint, a second constraint, and a third constraint, resulting in graph optimization of a total of 30 constraints.

[0079] In an optional embodiment, although the pose of the pose node is continuously optimized, its corresponding first constraint, second constraint and third constraint no longer change with the pose.

[0080] Similarly, as the number of pose nodes increases, the sliding window will keep moving, keeping the latest pose node as the last pose node of the sliding window.

[0081] In an optional embodiment, a time constraint can be preset to ensure that the pose optimized by the previous sliding window has been output before a new pose node arrives.

[0082] In the optional embodiment, when establishing the first constraint for the next pose node of the latest pose node, the calculation can be based on the pose of the latest pose node or the optimized pose of the latest pose node; no limitation is made here.

[0083] In one specific implementation, the graph optimization involves taking all the first constraints, second constraints, and third constraints of a predetermined number of adjacent pose nodes among a plurality of pose nodes to obtain the optimized pose of each pose node among the predetermined number of adjacent pose nodes: the LM algorithm is used to calculate the minimum value of the sum of squares of the first constraints, second constraints, and third constraints of the pose nodes among the predetermined number of adjacent pose nodes; and each pose corresponding to the minimum value is taken as the optimized pose of each pose node.

[0084] This application will not elaborate on the LM algorithm; please refer to existing records.

[0085] In addition to using the LM algorithm for graph optimization, other algorithms such as Gauss-Newton can also be used.

[0086] Furthermore, based on the above embodiments, it also includes:

[0087] Each time a new pose node is created, the factors and the pose node with the earliest creation time in the sliding window are removed, and the new pose node is added to the preset sliding window.

[0088] Specifically, after completing one graph optimization, for each newly created pose node, the earliest created pose node in the preset sliding window, along with its first, second, and third constraints, is removed. Then, the newly created pose node is added to the preset sliding window, and graph optimization is performed again on each pose node within the preset sliding window, and this process is repeated.

[0089] For example, after graph optimization of the first, second, and third constraints of pose nodes 1 to 10, when pose node 11 is created, pose node 1 and its constraints are discarded, and pose node 11 is added to a preset sliding window.

[0090] In summary, the robot localization method provided in this application integrates LiDAR localization and visual marker localization. By combining the advantages of both methods, it can reduce the number of visual markers and lower the time consumption while ensuring localization stability and accuracy. Furthermore, this application employs graph optimization to fuse LiDAR localization and visual marker localization, achieving even higher localization accuracy.

[0091] This application also provides a robot positioning device, which is described below and can be referred to in conjunction with the method described above. Please refer to... Figure 3 , Figure 3 This is a schematic diagram of a robot positioning device provided in an embodiment of this application, combined with... Figure 3 As shown, the device includes:

[0092] The pose node creation module 10 is used to continuously create pose nodes according to preset conditions and use the latest created pose node as a new pose node.

[0093] The constraint module is used to establish a first constraint, a second constraint, and a third constraint for each current pose node after the creation of each current pose node of the plurality of pose nodes and before the creation of the next pose node. The constraint module includes a first constraint establishment module 20, a second constraint establishment module 30, and a third constraint establishment module 40.

[0094] The first constraint establishment module 20 is used to establish the mileage constraint between the new pose node and the previous pose node based on the odometry data and pose of the new pose node and the previous pose node, and to use it as the first constraint of the new pose node.

[0095] The second constraint establishment module 30 is used to establish laser constraints based on the lidar data, pose and grid map corresponding to the new pose node, and to serve as the second constraint of the new pose node.

[0096] The third constraint establishment module 40 is used to establish a constraint between the visual label and the pose corresponding to the new pose node when a visual label is identified at the new pose node, based on the pose of the new pose node, the pose of the visual label in the robot coordinate system and the pose of the visual label in the grid map coordinate system, and to serve as the third constraint of the new pose node.

[0097] The graph optimization module 50 is used to graph optimize all the first constraints, second constraints and third constraints of a preset number of adjacent pose nodes in a plurality of pose nodes, to obtain the optimized pose of each pose node in the preset number of adjacent pose nodes, wherein the preset number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

[0098] Based on the above embodiments, optionally, the pose node creation module 10 includes:

[0099] The initial pose node creation unit is used to initialize the robot's pose and obtain the initial pose node;

[0100] The judgment unit is used to determine whether the robot's pose meets the preset conditions for adding pose nodes;

[0101] The pose node adding unit is used to add a new pose node whenever the pose of the robot meets the preset conditions for adding a pose node.

[0102] Based on the above embodiments, optionally, the initial pose node creation unit is specifically used for:

[0103] Run the first initialization thread and the second initialization thread; the first initialization thread obtains the robot's pose based on the visual labels in the current image; the second initialization thread obtains the robot's pose based on the current LiDAR data;

[0104] When either the first initialization thread or the second initialization thread is successfully initialized, the robot's pose obtained by the successfully initialized thread is used as the initial pose, and the initial pose node corresponding to the initial pose is created.

[0105] Based on the above embodiments, optionally, the determining unit is specifically used for:

[0106] Determine whether the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset mileage value, or whether the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset angle value.

[0107] If the mileage difference is greater than the preset mileage value or the orientation angle difference is greater than the preset angle value, then the robot's pose satisfies the preset condition for adding pose nodes.

[0108] Based on the above embodiments, optionally, the graph optimization module 50 is specifically used for:

[0109] The LM algorithm is used to calculate the minimum value of the sum of squares of the first constraint, the second constraint, and the third constraint of the adjacent preset number of pose nodes.

[0110] Each pose corresponding to the minimum value is taken as the optimized pose of each pose node.

[0111] In addition to the above embodiments, optionally, the following further includes:

[0112] The elimination module is used to eliminate the pose node with the earliest creation time in the preset sliding window each time a new pose node is created, and add the new pose node to the preset sliding window.

[0113] This application also provides a robot, see reference. Figure 4 As shown, the device includes a memory 1 and a processor 2.

[0114] Memory 1 is used to store computer programs;

[0115] Processor 2 is used to execute computer programs to perform the following steps:

[0116] Multiple pose nodes are created sequentially according to preset conditions, and the newly created pose node is used as the new pose node.

[0117] After creating each current pose node of the plurality of pose nodes and before creating the next pose node, a first constraint, a second constraint, and a third constraint are established for each current pose node. Specifically, an odometer constraint is established between the current pose node and the previous pose node based on the robot's odometry data and pose at the current pose node and the odometry data and pose of the previous pose node, and this constraint serves as the first constraint. A laser constraint is established based on the robot's LiDAR data, pose, and grid map at the current pose node, and this constraint serves as the second constraint. When a visual marker is detected at the current pose node, a constraint is established between the pose of the visual marker and the pose corresponding to the current pose node, based on the robot's pose at the current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, and this constraint serves as the third constraint.

[0118] A predetermined number of adjacent pose nodes are selected from the plurality of pose nodes, and the first constraint, the second constraint, and the third constraint of each pose node in the predetermined number of adjacent pose nodes are optimized to obtain the optimized pose of each pose node in the predetermined number of adjacent pose nodes. The predetermined number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

[0119] Based on the above embodiments, as a specific implementation method, when the processor executes the computer subroutine stored in the memory, it specifically implements the following steps:

[0120] Initialize the robot's pose to obtain the initial pose node;

[0121] Determine whether the robot's pose meets the preset conditions for adding pose nodes;

[0122] Whenever the robot's pose meets the preset conditions for adding a pose node, a new pose node is added.

[0123] Based on the above embodiments, as a specific implementation method, when the processor executes the computer subroutine stored in the memory, it specifically implements the following steps:

[0124] Run the first initialization thread and the second initialization thread; the first initialization thread obtains the robot's pose based on the visual labels in the current image; the second initialization thread obtains the robot's pose based on the current LiDAR data;

[0125] When either the first initialization thread or the second initialization thread is successfully initialized, the robot's pose obtained by the successfully initialized thread is used as the initial pose, and the initial pose node corresponding to the initial pose is created.

[0126] Based on the above embodiments, as a specific implementation method, when the processor executes the computer subroutine stored in the memory, it specifically implements the following steps:

[0127] Determine whether the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset mileage value, or whether the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset angle value.

[0128] If the mileage difference is greater than the preset mileage value or the orientation angle difference is greater than the preset angle value, then the robot's pose satisfies the preset condition for adding pose nodes.

[0129] Based on the above embodiments, as a specific implementation method, when the processor executes the computer subroutine stored in the memory, it specifically implements the following steps:

[0130] The LM algorithm is used to calculate the minimum value of the sum of squares of the first constraint, the second constraint, and the third constraint of the adjacent preset number of pose nodes.

[0131] Each pose corresponding to the minimum value is taken as the optimized pose of each pose node.

[0132] Based on the above embodiments, as a specific implementation method, when the processor executes the computer subroutine stored in the memory, it further performs the following steps:

[0133] Each time a new pose node is created, the pose node with the earliest creation time in the preset sliding window is removed, and the new pose node is added to the preset sliding window.

[0134] For a description of the equipment provided in this application, please refer to the above method embodiments; further details will not be provided here.

[0135] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, can perform the following steps:

[0136] Pose nodes are continuously created according to preset conditions, and the newly created pose node is used as the new pose node. Based on the odometry data and pose of the new pose node and the previous pose node, an odometry constraint is established between the new pose node and the previous pose node, serving as the first constraint for the new pose node. Based on the LiDAR data, pose, and grid map corresponding to the new pose node, a LiDAR constraint is established, serving as the second constraint for the new pose node. When a visual marker is detected at the new pose node, based on the pose of the new pose node, the pose of the visual marker in the robot coordinate system, and the... The pose of the visual identifier in the grid map coordinate system establishes the constraints of the pose corresponding to the new pose node, and serves as the third constraint of the new pose node; multiple pose nodes are loaded using a preset sliding window, and the first, second, and third constraints of all pose nodes within the preset sliding window are optimized to obtain the optimized pose of each pose node, and the optimized pose of the last pose node within the preset sliding window is used as the pose of the robot; wherein, the last pose node within the preset sliding window is the most recently created pose node.

[0137] The computer-readable storage medium may include various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0138] For a description of the computer-readable storage medium provided in this application, please refer to the above method embodiments; further details will not be repeated here.

[0139] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatuses, devices, and computer-readable storage media disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant details can be found in the method section.

[0140] Those skilled in the art will further 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, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. 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.

[0141] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented directly by hardware, a software module executed by a processor, or a combination of both. The software module can be located in random access memory (RAM), main memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art.

[0142] The robot positioning method, apparatus, device, and computer-readable storage medium provided in this application have been described in detail above. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the embodiments above are merely for the purpose of helping to understand the method and its core ideas. It should be noted that those skilled in the art can make various improvements and modifications to this application without departing from its principles, and these improvements and modifications also fall within the protection scope of the claims of this application.

Claims

1. A robot localization method, characterized in that, The robot localization method includes: Multiple pose nodes are created sequentially according to preset conditions, and the newly created pose node is used as the new pose node; the pose node includes pose, creation time and node number. After creating each current pose node of the plurality of pose nodes and before creating the next pose node, a first constraint, a second constraint, and a third constraint are established for each current pose node. Specifically, an odometer constraint is established between the current pose node and the previous pose node based on the robot's odometry data and pose at the current pose node and the odometry data and pose of the previous pose node, and this constraint serves as the first constraint. A laser constraint is established based on the robot's LiDAR data, pose, and grid map at the current pose node, and this constraint serves as the second constraint. When a visual marker is detected at the current pose node, a constraint is established between the pose of the visual marker and the pose corresponding to the current pose node, based on the robot's pose at the current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, and this constraint serves as the third constraint. A predetermined number of adjacent pose nodes are selected from the plurality of pose nodes, and the first constraint, the second constraint, and the third constraint of each pose node in the predetermined number of adjacent pose nodes are optimized to obtain the optimized pose of each pose node in the predetermined number of adjacent pose nodes. The predetermined number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

2. The robot localization method according to claim 1, characterized in that, The step of creating pose nodes sequentially according to preset conditions includes: Initialize the robot's pose to obtain the initial pose node; Determine whether the robot's pose meets the preset conditions for adding pose nodes; Whenever the robot's pose meets the preset conditions for adding a pose node, a new pose node is added.

3. The robot localization method according to claim 2, characterized in that, Initialize the robot pose to obtain the initial pose nodes, including: Run the first initialization thread and the second initialization thread; the first initialization thread obtains the robot's pose based on the visual labels in the current image; the second initialization thread obtains the robot's pose based on the current LiDAR data; When either the first initialization thread or the second initialization thread is successfully initialized, the robot's pose obtained by the successfully initialized thread is used as the initial pose, and the initial pose node corresponding to the initial pose is created.

4. The robot localization method according to claim 2, characterized in that, Determining whether the robot's pose meets the preset conditions for adding pose nodes includes: Determine whether the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset mileage value, or whether the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset angle value. If the mileage difference is greater than the preset mileage value or the orientation angle difference is greater than the preset angle value, then the robot's pose satisfies the preset condition for adding pose nodes.

5. The robot localization method according to claim 1, characterized in that, The graph optimization involves taking a predetermined number of adjacent pose nodes from multiple pose nodes and optimizing all the first, second, and third constraints to obtain the optimized pose of each pose node from the predetermined number of adjacent pose nodes. This includes: The LM algorithm is used to calculate the minimum value of the sum of squares of the first constraint, the second constraint, and the third constraint of the adjacent preset number of pose nodes. Each pose corresponding to the minimum value is taken as the optimized pose of each pose node.

6. The robot localization method according to claim 1, characterized in that, Before the graph optimization of all the first constraints, second constraints, and third constraints of a predetermined number of adjacent pose nodes among multiple pose nodes, the method further includes: The preset number of pose nodes are loaded using a preset sliding window; The graph optimization includes all the first constraints, second constraints, and third constraints of a predetermined number of adjacent pose nodes among multiple pose nodes, including: The graph optimizes the first constraint, the second constraint, and the third constraint of all pose nodes within the preset sliding window.

7. The robot localization method according to claim 6, characterized in that, The method further includes: Each time a new pose node is created, the pose node with the earliest creation time in the preset sliding window is removed, and the new pose node is added to the preset sliding window.

8. A robot positioning device, characterized in that, include: The pose node creation module is used to create multiple pose nodes sequentially according to preset conditions, and the latest created pose node is used as the new pose node. The pose node includes the pose, creation time, and node number; The constraint module is used to establish a first constraint, a second constraint, and a third constraint for each current pose node after the creation of each of the plurality of pose nodes and before the creation of the next pose node. The constraint module includes a first constraint establishment module, a second constraint establishment module, and a third constraint establishment module. The first constraint establishment module is used to establish a mileage constraint between the current pose node and the previous pose node based on the odometry data and pose of the robot at the current pose node and the odometry data and pose of the previous pose node, and use it as the first constraint. The second constraint establishment module is used to establish laser constraints based on the robot's lidar data, pose, and grid map at the current pose node, and use these constraints as the second constraint. The third constraint establishment module is used to establish a constraint between the visual label and the pose corresponding to the current pose node when a visual label is identified at the current pose node, based on the pose of the robot at the current pose node, the pose of the visual label in the robot coordinate system, and the pose of the visual label in the grid map coordinate system, and to serve as the third constraint. The graph optimization module is used to select a preset number of adjacent pose nodes from the plurality of pose nodes and graph optimize the first constraint, the second constraint, and the third constraint of each pose node in the preset number of adjacent pose nodes to obtain the optimized pose of each pose node in the preset number of adjacent pose nodes. The preset number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

9. A robot comprising a processor and a memory connected to the processor, the memory including a computer program, wherein the processor, when executing the computer program, performs the following steps: Multiple pose nodes are created sequentially according to preset conditions, and the newly created pose node is used as the new pose node; the pose node includes pose, creation time and node number. After creating each current pose node of the plurality of pose nodes and before creating the next pose node, establish a first constraint, a second constraint, and a third constraint for each current pose node; wherein... Based on the odometry data and pose of the robot at the current pose node and the odometry data and pose of the previous pose node, an odometry constraint is established between the current pose node and the previous pose node, which serves as the first constraint; a laser constraint is established based on the LiDAR data, pose, and grid map of the robot at the current pose node, which serves as the second constraint; when a visual marker is identified at the current pose node, a constraint is established between the pose of the visual marker and the pose corresponding to the current pose node based on the pose of the robot at the current pose node, the pose of the visual marker in the robot coordinate system, and the pose of the visual marker in the grid map coordinate system, which serves as the third constraint; A predetermined number of adjacent pose nodes are selected from the plurality of pose nodes, and the first constraint, the second constraint, and the third constraint of each pose node in the predetermined number of adjacent pose nodes are optimized to obtain the optimized pose of each pose node in the predetermined number of adjacent pose nodes. The predetermined number of adjacent pose nodes includes the new pose node, and the optimized pose of the new pose node is used as the pose of the robot.

10. The robot according to claim 9, characterized in that... The step of creating pose nodes sequentially according to preset conditions includes: Initialize the robot's pose to obtain the initial pose node; Determine whether the robot's pose meets the preset conditions for adding pose nodes; Whenever the robot's pose meets the preset conditions for adding a pose node, a new pose node is added.

11. The robot according to claim 10, characterized in that, The initialization of the robot pose, obtaining the initial pose node, includes: Run the first initialization thread and the second initialization thread; the first initialization thread obtains the robot's pose based on the visual labels in the current image; the second initialization thread obtains the robot's pose based on the current LiDAR data; When either the first initialization thread or the second initialization thread is successfully initialized, the robot's pose obtained by the successfully initialized thread is used as the initial pose, and the initial pose node corresponding to the initial pose is created.

12. The robot according to claim 10, characterized in that, Determining whether the robot's pose meets the preset conditions for adding a pose node includes: determining whether the mileage difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset mileage value, or whether the orientation angle difference between the robot's current pose and the pose corresponding to the previous pose node is greater than a preset angle value. If the mileage difference is greater than the preset mileage value or the orientation angle difference is greater than the preset angle value, then the robot's pose satisfies the preset condition for adding pose nodes.

13. The robot according to claim 9, characterized in that, The graph optimization involves taking a predetermined number of adjacent pose nodes from multiple pose nodes and optimizing all the first, second, and third constraints to obtain the optimized pose of each pose node from the predetermined number of adjacent pose nodes. This includes: The LM algorithm is used to calculate the minimum value of the sum of squares of the first constraint, the second constraint, and the third constraint of the adjacent preset number of pose nodes. Each pose corresponding to the minimum value is taken as the optimized pose of each pose node.

14. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the steps of the robot localization method as described in any one of claims 1 to 7.

Images