Autonomous driving system of robot

Abstract

The present invention provides an autonomous driving system of a robot. According to one embodiment of the present invention, the autonomous driving system of a robot in which node optimization of a pose graph generated in graph-based SLAM is implemented is disclosed. In addition, according to one embodiment of the present invention, the autonomous driving system of a robot in which integration with pre-constructed spatial information from partial mapping is implemented is disclosed. Moreover, according to one embodiment of the present invention, the autonomous driving system of a robot in which visualization of a pose graph generated in graph-based SLAM is implemented is disclosed. Furthermore, according to one embodiment of the present invention, the autonomous driving system of a robot in which editing for observation associated with a node on a pose graph is implemented is disclosed.

Description

Autonomous driving system for robots

[0001] The present invention relates to an autonomous driving system for a robot.

[0002] Robots that transport logistics, such as cargo or products of various sizes in warehouses and production lines, can be operated in autonomous driving mode, where they recognize their current location based on an autonomous driving algorithm, generate an optimized path to the target location, and are controlled to follow that path.

[0003] In autonomous navigation of such robots, path planning, which plans the path of the robot from the starting position of the robot or the loading position for loading the transport object to the goal position of the robot or the unloading position for unloading the transport object, or motion planning, which plans the pose of the robot including the heading or rotation of the robot along the path of the robot, together with the position of the robot as the state of the robot at each position of the robot, can be planned, global path planning, which plans the optimal global path from the starting position of the robot or the starting pose of the robot to the goal position of the robot or the goal pose of the robot, and local path planning for obstacle avoidance or collision avoidance maneuver according to observation observed in real time from the sensors of the robot can be planned, and in addition to the global path planning and local path planning as described above, behavior for defining the interaction with objects in the surrounding environment surrounding the robot can be planned. You can also plan your behavior.

[0004] One embodiment of the present invention includes an autonomous driving system for a robot in which node optimization of a pose graph generated from graph-based SLAM is implemented.

[0005] One embodiment of the present invention includes an autonomous driving system for a robot in which integration with spatial information constructed from partial mapping is implemented.

[0006] One embodiment of the present invention includes an autonomous driving system of a robot in which visualization of a pose graph generated from graph-based SLAM is implemented.

[0007] One embodiment of the present invention includes an autonomous driving system of a robot in which editing of observations associated with nodes on a pose graph is implemented.

[0008] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which node optimization of a pose graph generated from a graph-based SLAM according to one embodiment of the present invention is implemented,

[0009] A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment.

[0010] It includes an operation unit that eliminates a first node that satisfies an elimination condition among the nodes on the pose graph.

[0011] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which integration with spatial information constructed from partial mapping according to one embodiment of the present invention is implemented,

[0012] A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment.

[0013] A computation unit is included that generates an integrated, interconnected pose graph for the first and second workspaces by implementing a partial mapping with a non-contiguous time gap from the mapping for the first workspace, for a first workspace where the above mapping is implemented and a second workspace that is spatially interconnected.

[0014] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which visualization of a pose graph generated from graph-based SLAM according to one embodiment of the present invention is implemented,

[0015] A computational unit that generates a pose graph including nodes related to the pose of a robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization for inferring the pose of a robot on a map including spatial information about the surrounding environment and mapping for inferring spatial information about the surrounding environment.

[0016] It includes a computational unit that provides visualized information about the generated pose graph through a user interface.

[0017] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which editing of observations linked to nodes on a pose graph according to one embodiment of the present invention is implemented,

[0018] A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM (simultaneous localization and mapping) for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment.

[0019] It includes an operation unit that performs editing on an observation based on an observation linked to an editing target node that is created earlier in time and an editing reference node that is created later in time, as an editing target node and an editing reference node that are created with a time gap within a preset third work area.

[0020] In one embodiment of the present invention, by implementing optimization of nodes generated in graph-based SLAM (simultaneous localization and mapping), waste of computational resources or inefficient waste of storage capacity due to inefficient and redundant storage of data of each node or observations associated with each node can be prevented, and problems of excessive computational resources or computational time required for generation and calling of local maps or global maps generated from overlapping or combining observations observed at each node can be prevented, and ultimately, system-down of the entire robot system including the back-end can be prevented.

[0021] In one embodiment of the present invention, by deleting the oldest node or the observation associated with the oldest node based on the time of creation within the search range for node optimization, or deleting the observation with the lowest matching rate with the observation of the center node or the latest node within the search range for node optimization, or the node associated with the observation with the lowest matching rate, it is possible to reduce inefficient and redundant data storage among the multiple observations observed from overlapping poses regarding similar positions and postures while multiple nodes are connected in a complex manner along the driving path of the robot within the limited work area of ​​the robot, and to reduce excessive waste of computational resources or computational time required for generating a local map or a global map due to the overlapping or combination of multiple observations observed from overlapping poses, and to shorten the computational time required for generating and calling the map.

[0022] In one embodiment of the present invention, integration with previously constructed spatial information from partial mapping can be implemented, and partial mapping that is continuously connected to the mapping of the first workspace can be implemented for an additional second workspace that is spatially connected to the first workspace in which mapping is implemented as previously constructed spatial information.

[0023] In one embodiment of the present invention, a second pose graph for a second workspace can be generated through additional partial mapping while being connected to a first pose graph for graph-based SLAM for mapping and localization for a first workspace in which mapping is implemented as previously constructed spatial information, and the first pose graph for the first workspace and the second pose graph for the second workspace are generated sequentially so that a local map generated from an observation associated with a node on the first pose graph for the first workspace and a local map generated from an observation associated with a node on the second pose graph for the second workspace are connected to each other, and the first pose graph for the first workspace and the second pose graph for the second workspace are optimized through loop closure detection, thereby implementing a full mapping that connects these first and second workspaces to each other, and an entire global mapping that integrates a first workspace in which mapping is implemented as previously constructed spatial information and a second workspace in which mapping is implemented through partial mapping can be implemented.

[0024] In one embodiment of the present invention, while utilizing the existing spatial information regarding the first workspace in which mapping is implemented, it is possible to implement integrated spatial information or integrated mapping regarding the first and second workspaces while additionally implementing partial mapping only for the second workspace.

[0025] In one embodiment of the present invention, by implementing visualization of a pose graph generated from graph-based SLAM, a robot operator can recognize a problem in which pose graph optimization is delayed or hindered, such as when the robot's driving is concentrated in a certain section of the robot's workspace, but loop closure detection is not performed or a sufficient number of loop closure detections are not performed.

[0026] In one embodiment of the present invention, the operator of the robot can confirm from visualized information about the pose graph that a relatively large number of nodes are intensively generated in some sections of the workspace where the robot's driving is concentrated, but a sufficient number of loop closures or loop closure detections are not performed, due to the intensive driving of the robot, and can confirm the presence of obstacles on the driving path of the robot that may delay or hinder loop closure detection, such as the presence of obstacles that are difficult to avoid due to motion constraints that limit the motion of the robot.

[0027] In one embodiment of the present invention, for the configurations of a pose graph, visual information expressed with differential display elements can be provided, and for a node in a pose graph that requires loop closure and has not yet been loop-closed, a differential display element is displayed from other nodes in the pose graph that have already been loop-closed, or for the last node in the pose graph that has not yet been loop-closed and has not yet been loop-closed, a differential display element is displayed from other nodes that have not yet been loop-closed and nodes that have already been loop-closed, thereby providing visual information that highlights the visibility of the last node that has been created latest among the nodes that have not yet been loop-closed, and in the future, in the robot's driving path planning, by inducing the robot to plan a driving path toward the last node that has been created latest among the nodes that have not yet been loop-closed, loop closure detection can be induced for nodes that have not yet been loop-closed, and pose graph optimization can be promoted for all nodes created in the pose graph.

[0028] In one embodiment of the present invention, by implementing editing of observations associated with nodes on a pose graph generated in graph-based SLAM, an update of observations associated with previously generated nodes can be implemented based on observations associated with recently generated nodes at or near the current time within a pre-set spatial range, and observations observed from previously generated nodes that are spatially adjacent within a pre-set spatial range can be updated based on observations observed from recently generated nodes that reflect changes in the surrounding environment caused by a gap in the generation time, even for changes in the dynamic surrounding environment surrounding the robot due to changes in the dynamic surrounding environment or static surrounding environments with relatively less environmental change.

[0029] In one embodiment of the present invention, by updating previously generated observations based on recently generated nodes linked to updated observations according to a time gap in the generation time, the entire global map can be updated based on observations from each node forming the pose graph, thereby resolving distortion of the global map due to mismatch between observations of recently generated nodes and observations of previously generated nodes.

[0030] In one embodiment of the present invention, a local map is generated from the overlapping or combination of observations associated with a group of nodes within a pre-set spatial range based on a recently generated node, and an editing range can be set on the local map by comparing the generated local map with observations associated with the recently generated node, and by setting the editing range on the local map generated from the overlapping or combination of a plurality of observations associated with the group of nodes, the scale of observations for setting the editing range can be expanded to the object scale, and while setting the editing range on the observation associated with each of the group of nodes, it is possible to prevent errors in the editing range from being sensitively affected by noise mixed in each point of a group of point clouds forming the observation associated with each node or observation associated with each node, and for example, confusion between observations that need to be deleted due to errors in the editing range and observations that need to be maintained can be prevented.

[0031] Figure 1 is a diagram illustrating an example of localization for inferring the position of a robot by applying a Kalman filter, in which a prior probability distribution (previous correction) for the pose of the robot in the previous time step and a prediction probability distribution for the pose of the robot in the next time step are predicted from the motion model and control or odometry of the robot, and a posterior probability distribution (correction) that corrects the prediction probability distribution for the pose of the robot from the observation model of the robot and observations observed from the sensors of the robot is illustrated to explain how the accuracy of the prediction is improved as covariance decreases from the prediction and correction.

[0032] In Fig. 2, in the application of a particle filter, a diagram is shown showing an example of a probability distribution (proposal, probability) predicted in the prediction step, a probability distribution (proposal) predicted by a Gaussian distribution, or a frequency of particle samples for the probability distribution, and an example of expressing the weight of a Gaussian distribution as a spatial frequency.

[0033] Figure 3 illustrates an example of a weight distribution for compensating from a predicted probability distribution (Gaussian distribution) in the prediction step to a corrected probability distribution (arbitrary function) in the correction step in the application of a particle filter.

[0034] Figure 4 shows a diagram showing an example of converting from a differential weight distribution to a uniform spatial frequency.

[0035] FIG. 5a shows a drawing representing a pose of a robot, including a position and posture of the robot taken along the robot's driving path, and FIG. 5b shows an exemplary drawing showing a pose graph generated along the robot's driving path represented in FIG. 5a.

[0036] FIG. 6 illustrates a diagram for explaining a start condition of node optimization, which is a condition for starting node optimization, that is, a first number or more nodes are created within a first work area set to a first radius (node ​​optimization search distance) from a central node or the latest node.

[0037] FIG. 7 is a diagram illustrating the relationship between a first working area set as a first radius with respect to the initiation condition of node optimization and a second working area (loop closure detection search distance) with respect to the search condition of loop closure detection.

[0038] Figure 8 illustrates a diagram for explaining reconnection, which searches for a reconnection node to replace a first node that satisfies the elimination condition for node optimization, and creates an edge connecting the second and third nodes that were neighbors of the first node and the reconnection node.

[0039] Figure 9 illustrates a diagram for explaining reconnection that creates an edge that directly connects the second and third nodes that were adjacent to the first node that satisfies the elimination condition for node optimization.

[0040] FIG. 10 illustrates a diagram for explaining that the erasure of the first node is limited depending on the presence of an obstacle recognized between the central node or the latest node and the first node to be erased, as a condition for initiating erasure or a restriction on the erasure condition for node optimization.

[0041] FIG. 11 is a drawing showing an example of an embodiment in which partial mapping is applied, in which a first workspace in which mapping is implemented, including an existing process line and an existing shelf, is spatially connected to a second workspace in which mapping is not yet implemented, while a new line is added, and a second workspace in which partial mapping is applied is shown.

[0042] FIG. 12 is a drawing illustrating a robot's travel path for a first workspace in which mapping is implemented, including the existing process line and existing shelves illustrated in FIG. 11.

[0043] FIGS. 13 and 14 illustrate an initial estimation of a starting position of a partial mapping, and illustrate the generation of a starting node based on observations observed from a robot's sensor from a position candidate regarding a starting position of a partial mapping represented in FIG. 13, or a position or pose corrected from observations observed from a robot's sensor from a position candidate regarding a starting position of a partial mapping and spatial information (map) regarding a first workspace, as a corrected position or an initially estimated starting position represented in FIG. 14.

[0044] FIG. 15 is a diagram illustrating an observation observed from an initiating node, which is an example, to illustrate a connection between an initiating node and the fourth node on the first pose graph that is the closest to the initiating position of a partial mapping, or a connection between an initiating node and the last node with the latest generation time on the first pose graph, with respect to the initiating position of a partial mapping.

[0045] FIG. 16 is a diagram illustrating the generation of a second pose graph for a second workspace according to a partial mapping initiated from an initiation node, and an example diagram illustrating observations observed from nodes on the second pose graph is shown.

[0046] FIG. 17 illustrates a diagram for explaining pose graph optimization for nodes on a loop formed across the first and second pose graphs, while forming a pose graph that is a combination of a first pose graph for a first workspace and a second pose graph for a second workspace.

[0047] Figure 18 shows a drawing showing an example of a robot's driving path.

[0048] FIG. 19 is a diagram showing an example of visualized information provided for a pose graph generated along the driving path illustrated in FIG. 18.

[0049] FIG. 20 is a diagram showing another example of visualized information provided for a pose graph generated along the driving path illustrated in FIG. 18, showing an example of representing loop closures as differential display elements from edges connecting neighboring nodes, together with node identification numbers that follow a chronological order based on the time of generation.

[0050] FIG. 21 is another example of visualized information provided for a pose graph generated along a driving path illustrated in FIG. 18, which also expresses the driving direction of the robot on an edge connecting neighboring nodes, and shows an example of expressing loop closure as a differential display element from the edge connecting neighboring nodes, and expressing nodes where loop closure detection has been performed and nodes where loop closure detection has not been performed but loop closure detection is required as differential display elements.

[0051] FIG. 22a is a drawing for explaining an embodiment in which the spatial conditions of a third work area set as a first distance scale from an editing reference node are used as editing start conditions, and FIG. 22b is a drawing exemplarily showing a local map generated according to the overlapping or combination of observations observed from an editing target node.

[0052] Figure 23 illustrates a diagram exemplarily showing observations observed from each of the editing target nodes illustrated in Figure 22a.

[0053] FIG. 24(a) illustrates an example of a local map generated according to the overlapping or combination of observations observed from each of the editing target nodes illustrated in FIG. 23, and FIG. 24(b) illustrates an example of a mismatch (an editing target or deletion target on the local map that is not observed in the observation of the editing target node) with the observation observed from the editing reference node on the local map illustrated in FIG. 24(a).

[0054] Figure 25 illustrates an example of an editing range set according to a second distance scale from an editing reference node on the local map of Figure 24(b).

[0055] FIG. 26 illustrates an editing restriction for an observation linked to an editing target node that is blocked by an obstacle between the editing target node and the editing reference node, based on the observation of the editing reference node, and on the left is a drawing showing an obstacle that exists between the editing reference node and the observation of the editing target node, and on the right is an enlarged drawing of the left side, and is a drawing for explaining ray tracing for capturing the obstacle as described above.

[0056] Figures 27(a) to 27(c) illustrate drawings for explaining deletion within the editing range set on the observation of the editing target node and maintenance outside the editing range set on the observation of the editing target node by applying the editing range set on the local map as in Figure 25 to the observation of the editing target node.

[0057] Figure 28 illustrates a diagram for explaining that a portion of a local map that is outside the editing range is maintained on a local map that overlaps or combines each observation of an editing target node.

[0058] FIG. 29 illustrates a drawing showing an Aruco marker as an example of a positioning code or positioning marker that is visually observed from a sensor of the robot as a landmark installed around the robot's driving path.

[0059] FIG. 30 illustrates a drawing showing April-tag as another example of a positioning code or positioning marker visually observed from a robot's sensor as a landmark installed around the robot's driving path.

[0060] FIG. 31 illustrates a drawing showing a QR marker as another example of a positioning code or positioning marker that is visually observed from a sensor of the robot as a landmark installed around the periphery of the robot's driving path.

[0061] FIG. 32 illustrates a drawing for explaining positioning codes or positioning markers of different identification marks recognized within a landmark detection distance centered on the position of the robot along the robot's driving path.

[0062] On the left side of Fig. 33, a drawing is shown to explain how a landmark node is generated for a pose of a landmark based on the accumulation of multiple observations of the same landmark from a robot along the robot's driving path as a generation condition, and on the right side of Fig. 33, a drawing is shown to explain how a mean and covariance are extracted from a cluster of data with a relatively high density from clustering (DBSCAN) for multiple data predicted as a pose of a landmark from an observation model regarding the robot's observation and the pose of the robot that observed each of the multiple accumulated observations, and data scattered with a relatively low density are removed as noise or outliers.

[0063] On the left side of Fig. 34, there is shown a drawing exemplarily showing one form of a pose graph that does not include a landmark node, and on the right side of Fig. 34, there is shown a drawing exemplarily showing one form of a pose graph that includes a node related to a pose of a robot that follows a driving path of the robot, and to which a landmark node and an observation node related to a pose of a robot that observes the landmark node are added, and node identification numbers with different signs (+ / -) are assigned to the landmark node related to the pose of the landmark and the node related to the pose of the robot.

[0064] Figure 35 is a drawing for explaining the uncertainty of odomerty, in which the odometry of the wheel and the actual driving distance of the robot are mismatched depending on the actual driving distance of the robot and the slip (e.g., the wheel spinning in place) or drift (e.g., the wheel slipping) of the wheel of the robot.

[0065] On the left side of Fig. 36, there is shown a drawing for explaining how to adjust the probability distribution or the particle set regarding the probability distribution regarding the pose of the robot in a divergent form in a moving state of an accelerated driving environment toward a target speed, and different drawings are shown for explaining how to adjust the frequency of particle samples in a relatively wide range and a relatively narrow range in low-speed driving where the target speed is relatively low and high-speed driving where the target speed is relatively high, and on the right side of Fig. 36, there is shown a drawing for explaining how to adjust the probability distribution or the particle set regarding the probability distribution regarding the pose of the robot in a convergent form in an approach state of a decelerated driving environment toward a target speed.

[0066] Figure 37 illustrates a drawing for explaining the convergence of a probability distribution or a particle set regarding a probability distribution regarding a robot's pose in a static environment where it is difficult to extract features of the surrounding environment from observations of the surrounding environment, such as an open area or a straight corridor, or where there is little possibility of collision with obstacles or collisions in the surrounding environment.

[0067] Figure 38 shows different diagrams for explaining how to adjust the probability distribution of the robot's pose or the particle set for the probability distribution in different convergence and divergence forms, respectively, in a static environment where the surrounding environment changes relatively little over time and in a dynamic environment where the surrounding environment changes relatively much over time.

[0068] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which node optimization of a pose graph generated from a graph-based SLAM according to one embodiment of the present invention is implemented,

[0069] A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment.

[0070] It includes an operation unit that eliminates a first node that satisfies an elimination condition among the nodes on the pose graph.

[0071] For example, the above operation unit,

[0072] Along with the first node of the deletion target that satisfies the above deletion condition, the observation linked to the first node of the deletion target can also be deleted.

[0073] For example, the above operation unit,

[0074] By forming a node on the above pose graph, observations observed from the robot's sensor regarding the pose for the node can be stored in association with the corresponding node.

[0075] For example, the above operation unit,

[0076] As the above elimination condition,

[0077] The condition for starting erasure is that a first number or more of nodes are formed within a first work area set in advance on the work space of the robot.

[0078] Among the nodes created within the first work area, the node that is the fastest or the node that was created the longest time ago can be selected as the first node to be deleted.

[0079] For example, the observation that is erased along with the first node of the above erasure target is,

[0080] It can be generated at the earliest or oldest point in time among the multiple observations associated with each node generated within the first work area.

[0081] For example, the above operation unit,

[0082] As the above elimination condition,

[0083] The condition for starting erasure is that a first number or more of nodes are formed within a first work area set in advance on the work space of the robot.

[0084] Among the nodes created within the first work area, the node corresponding to the center position of the first radius that sets the first work area or the node linked to the observation with the lowest matching rate with the observation linked to the most recent node with the latest creation time can be selected as the first node to be deleted.

[0085] For example, the observation that is erased along with the first node of the above erasure target is,

[0086] It may be an observation with the lowest matching rate produced from one-to-one matching between an observation associated with each node generated within the first work area and an observation associated with the central node or the latest node.

[0087] For example, the above operation unit,

[0088] A group of point clouds as observations associated with the central node or the latest node, and point clouds associated with each node as observations associated with nodes generated within the first work area are sequentially made into another group of point clouds, and a point-to-point matching of the ICP (Iterative Closet Point) algorithm is performed between the group of point clouds and the other group of point clouds, and a matching rate between the observations associated with the central node or the latest node and the observations associated with each node generated within the first work area can be calculated based on the result of the ICP (Iterative Closet Point) algorithm.

[0089] For example, the above operation unit,

[0090] In order to prevent the isolation of the second node and the third node connected to the first node through an edge according to the deletion of the first node of the deletion target, the node that is linked to the observation with the highest matching rate with the observation of the first node of the deletion target within the first work area can be searched for, and the searched node can be selected as a reconnection node that will replace the first node and create a connection with the second node and the third node adjacent to the first node.

[0091] For example, the above operation unit,

[0092] The matching rate is calculated by sequentially comparing one-to-one the observations linked to the first node of the above-mentioned deletion target and the observations stored linked to the remaining nodes excluding the first to third nodes among the nodes created within the first work area.

[0093] From the comparison of the generated matching rates, the reconnection node that is linked to the observation stored in connection with the first node to be eliminated and the observation with the highest matching rate can be selected.

[0094] For example, in calculating the above matching rate,

[0095] A matching rate can be calculated between the observation linked to the first node of the above-mentioned deletion target and the observation linked to the node of the comparison target.

[0096] For example, in calculating the above matching rate,

[0097] The relative translational and rotational components that allow different observations between the first node of the above-mentioned elimination target and the node of the comparison target to be aligned with each other can be calculated, and the degree of alignment with each other can be calculated through a rigid body transformation including the relative translational and rotational components.

[0098] For example, in calculating the above matching rate

[0099] By applying an ICP (iterative closest point) algorithm between a group of point clouds observed from a robot's lidar sensor as an observation linked to the first node of the above-mentioned elimination target and another group of point clouds observed from a robot's lidar sensor as an observation linked to the node of the above-mentioned comparison target, the degree of matching between them can be calculated from a rigid body transformation including relative translational and rotational components.

[0100] For example, the above operation unit,

[0101] After the first node of the above-mentioned deletion target is deleted, pose graph optimization (PGO) can be implemented for all nodes including the second node, the third node, and the reconnected node connected on the loop of the pose graph including the edge that forms a connection between the second node or the third node that was connected to the deleted first node through an edge and the reconnected node selected from the comparison with the observation of the first node, according to loop closure detection.

[0102] For example, the edge between the second node and the reconnection node and the edge between the third node and the reconnection node do not include odometry information that follows the robot's driving path.

[0103] The above computational unit predicts a predicted observation between the second node and the reconnection node or between the third node and the reconnection node from the pose of the robot with respect to the second node and the third node and the pose of the robot with respect to the reconnection node,

[0104] Pose graph optimization can be implemented by applying least square optimization to the Mahalanobis distance with squared errors and measurement uncertainty added between the predicted observations predicted between the second node and the reconnection node and between the third node and the reconnection node and the real observations observed between the second node and the reconnection node and between the third node and the reconnection node.

[0105] For example, the above operation unit,

[0106] In order to prevent the isolation of the second node and the third node that are connected to the first node through the edge according to the deletion of the first node of the above deletion target, an edge that directly connects the second node and the third node that are adjacent to each other with the first node in between can be created.

[0107] For example, the above operation unit,

[0108] After the first node of the above-mentioned deletion target is deleted, pose graph optimization (PGO) can be implemented for all nodes including the second node and the third node connected on the loop of the pose graph including the edge that directly connects the second node and the third node, according to loop closure detection.

[0109] For example, the edge between the second and third nodes does not contain odometry information that follows the robot's driving path.

[0110] The above computational unit predicts the predicted observation between the second and third nodes from the pose of the robot with respect to the second node and the pose of the robot with respect to the third node,

[0111] Pose graph optimization can be implemented by applying least square optimization to the Mahalanobis distance, which adds the squared error and measurement uncertainty between the predicted observation predicted between the second and third nodes and the real observation observed between the second and third nodes.

[0112] For example, the uncertainty of measurement for applying least square optimization to the edge directly connecting the second node and the third node can be calculated from the uncertainty of measurement between the eliminated first node and the second node and the uncertainty of measurement between the eliminated first node and the third node.

[0113] For example, the uncertainty of the measurement between the first and second nodes that have been deleted is calculated from the matching rate between a group of point clouds observed from the first node that has been deleted and a group of point clouds observed from the second node.

[0114] The uncertainty of the measurement between the first node and the third node that has been deleted can be calculated from the matching rate between a group of point clouds observed from the first node that has been deleted and another group of point clouds observed from the third node.

[0115] For example, the first working area corresponds to a circumferential range of the first radius,

[0116] The above first radius may be smaller than the second radius that sets the second working area for the search condition of loop closure detection in pose graph optimization (PGO), in which optimization is performed on nodes generated on a loop of a pose graph according to loop closure detection.

[0117] For example, the first radius is

[0118] It may be half of the second radius that sets the second working area for the search condition of the above loop closure detection.

[0119] For example, the above operation unit,

[0120] Set a second radius limit for the second working area corresponding to the search condition of the above loop closure detection,

[0121] It is possible to avoid generating loop closure constraints that extend excessively beyond the second radius and to avoid performing pose graph optimization based on loop closure detection.

[0122] For example, as the above elimination condition,

[0123] The first radius of the first working area is set to 5 m, which is half of 10 m, as the second radius for setting the second working area for the search condition of loop closure detection.

[0124] The formation of 5 to 20 or more nodes, which is set as the first number, within the first working space of 5 m set as the first radius may be set as the initiation condition for erasure.

[0125] For example, the above operation unit,

[0126] As a limitation for the above-mentioned elimination condition, the presence of an obstacle recognized between the first node and the latest node or the central node corresponding to the center position of the first radius for setting the first work area set in advance on the work space of the robot as a condition for starting elimination can be set as a limitation for the above-mentioned elimination condition.

[0127] For example, the above operation unit,

[0128] Subject to the restrictions on the above deletion conditions, if the presence of an obstacle is recognized between the central node or the latest node and the first node, the first node can be maintained without being deleted.

[0129] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which integration with spatial information constructed from partial mapping according to one embodiment of the present invention is implemented,

[0130] A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment.

[0131] A computation unit is included that generates an integrated, interconnected pose graph for the first and second workspaces by implementing a partial mapping with a non-contiguous time gap from the mapping for the first workspace, for a first workspace where the above mapping is implemented and a second workspace that is spatially interconnected.

[0132] For example, the above operation unit,

[0133] By connecting a first pose graph for a first workspace and a second pose graph for a second workspace, which are generated with a non-contiguous visual gap, an integrated and connected pose graph for the first and second workspaces can be generated.

[0134] For example, the above operation unit,

[0135] A second pose graph for a second workspace can be generated from a partial mapping for the second workspace while maintaining the first pose graph for the first workspace and the observations associated with the nodes on the first pose graph, and the second pose graph can be generated as being connected to the first pose graph.

[0136] For example, the above operation unit,

[0137] Partial mapping for the second workspace can be initiated by setting the starting position of the partial mapping to a position within the first workspace where the mapping is implemented as a starting condition of the partial mapping.

[0138] For example, the above operation unit,

[0139] As an initial estimate of the starting position of the above partial mapping, the starting position of the partial mapping can be inferred from the spatial information mapped with respect to the first work space.

[0140] For example, the above operation unit,

[0141] The starting position of the partial mapping can be initially estimated from observations observed from the starting position of the partial mapping and observations observed from nodes on the first pose graph for the first workspace, thereby generating a starting node for the starting position of the partial mapping on the first pose graph, and storing the observations observed from the starting position of the partial mapping in association with the starting node.

[0142] For example, the above operation unit,

[0143] On the first pose graph, search for the fourth node closest to the starting node with respect to the starting position of the partial mapping,

[0144] By creating an edge connecting the explored fourth node and the starting node, the starting position of the partial mapping can be incorporated into a node connected to the first pose graph.

[0145] For example, the above operation unit,

[0146] On the first pose graph before the connection of the above initiation node, the last node with the latest creation time is searched along the robot's driving path,

[0147] An edge connecting the last node explored and the starting node for the starting position of the partial mapping can be created, thereby incorporating the starting position of the partial mapping into a node connected to the first pose graph.

[0148] For example, the above operation unit,

[0149] By creating an edge connecting the final node and the starting node on the first pose graph, the first pose graph including the edge connecting the final node and the starting node can be connected to each other as a whole, and no disconnection or isolation can be formed along the first pose graph.

[0150] For example, the above operation unit,

[0151] A second pose graph can be generated that advances according to a time step from a starting position of a partial mapping as a node incorporated into a first pose graph for the first workspace to a second workspace as a target of the partial mapping.

[0152] For example, the starting position of the partial mapping may form a common node connecting the first and second pose graphs with respect to the first and second work spaces.

[0153] For example, the above operation unit,

[0154] According to the loop closure detection detected during the above partial mapping, pose graph optimization (PGO) can be implemented for all nodes on the loop connected across the first and second pose graphs for the first and second workspaces.

[0155] For example, the edge formed between the start node regarding the start position of the partial mapping and the fourth node closest to the start node regarding the start position of the partial mapping on the first pose graph does not include odometry information that follows the robot's driving path.

[0156] The above computational unit predicts the predicted observation between the initiation node and the fourth node from the pose of the robot with respect to the initiation node and the pose of the robot with respect to the fourth node,

[0157] Pose graph optimization can be implemented through least square optimization for the Mahalanobis distance, which adds the squared error and measurement uncertainty between the predicted observation predicted between the initiation node and the fourth node and the real observation observed between the initiation node and the fourth node.

[0158] For example, the edge formed between the starting node regarding the starting position of the partial mapping and the last node with the latest generation time on the first pose graph before the connection of the starting node does not include odometry information that follows the robot's driving path.

[0159] The above computational unit predicts the predicted observation between the start node and the final node from the pose of the robot with respect to the start node and the pose of the robot with respect to the final node,

[0160] Pose graph optimization can be implemented through least square optimization for the Mahalanobis distance, which adds the squared error and measurement uncertainty between the predicted observation predicted between the start node and the last node and the real observation observed between the start node and the last node.

[0161] For example, the above operation unit,

[0162] Generate a first pose graph for the first workspace and a second pose graph for the second workspace according to the advancement of the time step along the driving path of the robot,

[0163] The difference in the first generation time between adjacent nodes on the first pose graph before the partial mapping,

[0164] The difference between the start node of the start position of the above partial mapping and the second generation time point between the last node on the first pose graph before the above partial mapping is

[0165] The relationship of difference at the second generation point > difference at the first generation point can be satisfied.

[0166] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which visualization of a pose graph generated from graph-based SLAM according to one embodiment of the present invention is implemented,

[0167] A computational unit that generates a pose graph including nodes related to the pose of a robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization for inferring the pose of a robot on a map including spatial information about the surrounding environment and mapping for inferring spatial information about the surrounding environment.

[0168] It includes a computational unit that provides visualized information about the generated pose graph through a user interface.

[0169] For example, the above operation unit,

[0170] As visualized information about nodes on a pose graph generated according to the advancement of time steps along the driving path of the robot, along with each node, a node identification number following a chronological order based on the time of generation and / or a driving direction of the robot can be displayed on an edge connecting neighboring nodes.

[0171] For example, the above operation unit,

[0172] According to loop closure detection on the driving path of the robot, pose graph optimization (PGO) can be implemented to perform optimization on nodes generated on the loop of the pose graph.

[0173] For example, the above operation unit,

[0174] It is possible to provide visualized information about nodes generated in relation to the pose of the robot according to the advancement of time steps along the driving path of the robot, edges connecting adjacent nodes generated in relation to the time steps, and loop closures generated according to loop closure detection.

[0175] For example, the above operation unit,

[0176] As visualized information about the loop closures generated according to the above loop closure detection, the last loop closure with the latest generation time and the other loop closures excluding the last loop closure can be displayed with different display elements.

[0177] For example, the above operation unit,

[0178] As visualized information about loop closure generated according to the above loop closure detection, nodes in a pose graph requiring loop closure but for which loop closure has not yet been achieved can be displayed with a differential display element compared to other nodes in a pose graph for which loop closure has already been achieved.

[0179] For example, the above operation unit,

[0180] As visualized information about loop closure generated according to the above loop closure detection, among the nodes in the pose graph that require loop closure and for which loop closure has not yet been performed, the last node with the latest generation time point can be displayed as a differential display element between the nodes that have not yet been loop closed and the nodes that have already been loop closed, excluding the last node.

[0181] For example, the above operation unit,

[0182] On the pose graph provided with the above visualized information, the edges between neighboring nodes including the odomerty information of the robot and the loop closure generated according to loop closure detection without including the odometry information of the robot can be visually distinguished from each other by displaying the edges and the loop closure as different display elements.

[0183] For example, the above operation unit,

[0184] On the pose graph provided by the above visualized information, the loop closure,

[0185] Or, highlight it with a line of a different color from the above edge,

[0186] It can be highlighted with an arrow different from the line indicating the edge above.

[0187] For example, a loop closure displayed on a pose graph provided with the above visualized information may indicate that pose graph optimization (PGO) has been implemented or is to be implemented for nodes generated on a closed loop from the displayed loop closure.

[0188] For example, the above operation unit,

[0189] Visualized information about the above pose graph can provide an opportunity to recognize delay or interference environments in pose graph optimization (PGO).

[0190] For example, the above operation unit,

[0191] The visualized pose graph information for a work area in which loop closure detection has not been performed or in which loop closure detection has not been performed in sufficient numbers, compared to the occupancy distribution or occupancy density occupied by the robot along the driving path of the robot, can provide an opportunity to recognize a delay environment or an obstruction environment for the pose graph optimization.

[0192] For example, the delay environment or hindrance environment for the pose graph optimization may include the presence of a motion constraint that limits the motion of the robot or an obstacle as a surrounding environment that hinders the driving of the robot.

[0193] For example, the above operation unit,

[0194] Based on the judgment on the presence of a delay environment or an obstruction environment in the pose graph optimization according to loop closure detection for the pose graph provided with the above visualized information,

[0195] With the visualized information of the above pose graph, it is possible to provide notifications about delay or interference environments in pose graph optimization.

[0196] For example, the above operation unit,

[0197] i) No loop closure was created according to loop closure detection for the pose graph for which the above visualized information is provided,

[0198] ii) A notification regarding a delay environment or an interference environment regarding pose graph optimization can be provided from a comparison of the range of the work area for the pose graph in which the visualized information is provided and a threshold range set in advance to exceed the range in which the loop closure detection is searched or the average range for the work area in which the loop closure detection is performed.

[0199] For example, the above operation unit,

[0200] i) No loop closure was created according to loop closure detection for the pose graph for which the above visualized information is provided,

[0201] iii) By comparing the number of nodes generated on the pose graph provided with the above visualized information and the maximum number of critical nodes that a loop on the pose graph can accommodate according to the above loop closure detection, it is possible to provide notification of a delay environment or an interference environment with respect to pose graph optimization.

[0202] For example, the above operation unit,

[0203] i) No loop closure was created according to loop closure detection for the pose graph for which the above visualized information is provided,

[0204] ii) The radius of the working area for the pose graph in which the above visualized information is provided is greater than the preset threshold range of 10 m or

[0205] iii) If the number of nodes generated on the pose graph provided with the above visualized information is greater than the preset threshold number of 20,

[0206] Along with visual information about the pose graph, it can provide notifications about delays or interference environments for pose graph optimization.

[0207] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which editing of observations linked to nodes on a pose graph according to one embodiment of the present invention is implemented,

[0208] A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM (simultaneous localization and mapping) for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment.

[0209] It includes an operation unit that performs editing on an observation based on an observation linked to an editing target node that is created earlier in time and an editing reference node that is created later in time, as an editing target node and an editing reference node that are created with a time gap within a preset third work area.

[0210] For example, the above operation unit,

[0211] Regarding the selection of the nodes for the above editing target and the nodes for the editing criteria,

[0212] Among the nodes generated within the third work area, the nodes for editing and the nodes for editing criteria can be selected such that the nodes are generated at different times with a time gap from each other, and the different observations associated with each node have a matching rate lower than a threshold matching rate set in advance.

[0213] For example, the above operation unit,

[0214] Among the nodes generated within the third work area, the nodes of the editing target and the nodes of the editing criteria can be selected such that the nodes are generated at different times with a time gap from each other, and the different observations associated with each node have a matching rate higher than the first threshold matching rate but lower than the second threshold matching rate.

[0215] For example, the first critical matching rate provides a measure for keeping observations from two different nodes belonging to different sections spatially separated from each other in the third workspace independent of each other,

[0216] The above second critical matching rate can provide a measure for preventing waste of computational resources due to editing of observations between two different nodes, such that changes in the surrounding environment over time between two different nodes created at different creation times with a time gap are negligible or relatively minor in the third workspace.

[0217] For example, in the calculation of the matching rate between different observations stored in connection with each node between two different nodes that were created at different times within the third work area,

[0218] By applying a rigid body transformation on the relative translational and rotational components derived from the ICP (iterative closest point) algorithm between a group of point clouds observed from the robot's lidar sensor as observations stored in association with one of the two different nodes and another group of point clouds observed from the robot's lidar sensor as observations stored in association with the other nodes, a matching rate between different observations stored in association with two different nodes having different generation times can be derived.

[0219] For example, between the node of the above-mentioned editing target and two different nodes selected as the node of the above-mentioned editing reference, some of the observations observed from the node of the editing reference can be incorporated into the observations observed from the node of the editing target through a transformation matrix including relative translational components and rotational components derived from different observations observed from each node.

[0220] For example, the above operation unit,

[0221] The nodes for the editing target and the nodes for the editing criteria can be selected by calculating a matching rate from a mutual comparison of observations stored one-to-one with respect to each node among a group of nodes created within a pre-set third work area.

[0222] For example, the above operation unit,

[0223] Among a group of nodes created within the third work area, nodes created in different sections that are blocked by obstacles or blocked by occupied cells on the occupancy grid mapped from the SLAM may not be selected as nodes for editing or nodes for editing criteria.

[0224] For example, the above operation unit,

[0225] Based on the observation linked to the node of the above editing standard, edit the observation linked to the node of the above editing target.

[0226] Some deletions and some additions to observations linked to the nodes of the above editing target are allowed, but

[0227] Complete deletion or complete replacement of observations associated with the node being edited above may not be permitted.

[0228] For example, the observation of the node of the above-mentioned editing target and the observation of the node of the editing criterion may include observations of the same landmark and observations of different landmarks as the surrounding environment surrounding the robot.

[0229] For example, the above operation unit can edit an observation observed from a node of the editing target based on an observation observed from a node of the editing criterion, according to the following editing initiation conditions.

[0230] 1) A temporal criterion regarding the time gap between the creation time of the node to be edited and the node of the editing standard.

[0231] 2) Spatial criteria for creating nodes for editing and nodes for editing criteria within the third work area.

[0232] 3) Matching rate between different observations observed from the nodes of the editing target and the nodes of the editing criteria.

[0233] For example, the above operation unit,

[0234] A global map is created by connecting local maps based on observations or observations observed from nodes on the above pose graph.

[0235] As an observation linked to the node of the above-mentioned editing target, a global map can be updated from an observation edited based on the node of the above-mentioned editing standard so that the observation linked to the node of the above-mentioned editing target and the observation linked to the node of the above-mentioned editing standard are aligned with each other.

[0236] For example, the above operation unit,

[0237] A local map is created by superimposing or combining observations linked to a group of editing target nodes belonging to a third work area from the nodes of the above editing criteria along the poses of each node of a group of editing targets that follow the robot's driving path.

[0238] The editing range set on the above local map can be applied to observations associated with each node of a group of editing targets.

[0239] For example, the above operation unit,

[0240] Observation of the surrounding environment surrounding the robot can be expanded to the object scale from a local map generated through overlapping or combining observations linked to a group of editing target nodes within the above third work area.

[0241] For example, the above operation unit,

[0242] When setting the editing range set on the above local map,

[0243] A limited editing range can be set based on a distance scale centered on the node of the above editing criteria.

[0244] For example, the above operation unit,

[0245] Select a node to be edited within the third work area according to the first distance measure centered on the node of the above editing standard,

[0246] The scope of editing for observation of the node of the editing target can be set according to the second distance scale centered on the node of the above editing standard.

[0247] For example, the above operation unit,

[0248] The following restrictions may be applied to the editing scope for observations linked to the nodes of the above editing target.

[0249] 1) Radius range centered around the node of the editing criteria.

[0250] 2) Observation linked to a node of the target of editing that is blocked by an obstacle between it and the node of the editing standard.

[0251] For example, the above operation unit, with respect to the limitation of the editing range of 1),

[0252] For observations observed within a close distance of the second distance scale from the node of the above editing standard, it is provided as a standard for editing for the observation of the node to be edited.

[0253] Observations observed at a distance beyond the second distance scale from the node of the above editing criteria may not be provided as a criterion for editing the observation of the node to be edited.

[0254] For example, the above operation unit, with respect to the limitation of the editing range of the above 2),

[0255] Based on an observation that cannot be observed due to an obstacle from the node of the above editing standard, a mismatch that is not present in the observation of the node of the editing standard is prevented from being deleted from the observation of the node of the editing target, thereby preventing loss of spatial information about the surrounding environment of the robot.

[0256] For example, the above operation unit, with respect to the limitation of the editing range of the above 2),

[0257] From the node of the above editing standard, the restriction of the editing range of 2) can be applied depending on whether a group of point clouds regarding obstacles exists between the node of the above editing standard and the observation node of the editing target according to ray tracing.

[0258] Hereinafter, with reference to the attached drawings, an autonomous driving system of a robot according to one embodiment of the present invention will be described.

[0259] <SLAM, 그래프 기반의 SLAM, Kalman 필터, particle 필터, ICP 알고리즘>

[0260] Figure 1 is a diagram illustrating an example of localization for inferring the position of a robot by applying a Kalman filter, in which a prior probability distribution (previous correction) for the pose of the robot in the previous time step and a prediction probability distribution for the pose of the robot in the next time step are predicted from the motion model and control or odometry of the robot, and a posterior probability distribution (correction) that corrects the prediction probability distribution for the pose of the robot from the observation model of the robot and observations observed from the sensors of the robot is illustrated to explain how the accuracy of the prediction is improved as covariance decreases from the prediction and correction.

[0261] In Fig. 2, in the application of a particle filter, a diagram is shown showing an example of a probability distribution (proposal, probability) predicted in the prediction step, a probability distribution (proposal) predicted by a Gaussian distribution, or a frequency of particle samples for the probability distribution, and an example of expressing the weight of a Gaussian distribution as a spatial frequency.

[0262] Figure 3 illustrates an example of a weight distribution for compensating from a predicted probability distribution (Gaussian distribution) in the prediction step to a corrected probability distribution (arbitrary function) in the correction step in the application of a particle filter.

[0263] Figure 4 shows a diagram showing an example of converting from a differential weight distribution to a uniform spatial frequency.

[0264] FIG. 5a shows a drawing representing a pose of a robot, including a position and posture of the robot taken along the robot's driving path, and FIG. 5b shows an exemplary drawing showing a pose graph generated along the robot's driving path represented in FIG. 5a.

[0265] SLAM (simultaneous localization and mapping) simultaneously performs localization, which infers the pose of a robot, and mapping, which creates a map using spatial information surrounding the robot. It can mean simultaneous position estimation and map creation. In this specification, the pose of a robot can include both the position and posture of the robot. For example, in a robot moving on a two-dimensional plane, the instantaneous pose (position and posture) of the robot can be defined as three parameters: two-dimensional coordinates and a rotation angle. For example, the pose of the robot can include information on the position coordinates of the robot on a map as spatial information about the surrounding environment of the robot, and heading information regarding the rotation direction of the robot.

[0266] In this way, SLAM, which simultaneously implements localization to estimate the pose of the robot according to the robot's driving and mapping to infer spatial information about the surrounding environment surrounding the robot, or mapping to infer spatial information about the surrounding environment including pose information of landmarks as the surrounding environment surrounding the robot, can be implemented with various algorithms. For example, SLAM can be implemented with filtering-based and smoothing-based algorithms.

[0267] For example, in a SLAM based on filtering, a SLAM based on a Kalman filter can perform a prediction to predict a probability distribution about the pose of the robot (or the position of a landmark) at the current time step from a belief at the previous time step (e.g., a probability distribution about the pose of the robot, a prior probability distribution at the current time step, or a posterior probability distribution at the previous time step) and a motion model, and for example, the motion model (the pose Xt of the robot at the current time step) can include a transformation matrix A multiplied by a state vector X about the pose of the robot (the pose Xt-1 of the robot at the previous time step), a transformation matrix B multiplied by control or odometry (such as wheel odometry or IMU odometry), and noise ε (e.g., covariance of a Gaussian distribution). After the prediction that predicts the probability distribution regarding the pose of the robot (or the position of the landmark) in this way, a correction can be performed, and the pose of the robot (or the position of the landmark) predicted in the prediction can be corrected from the measurement of a sensor such as a lidar sensor or a camera and an observation model. For example, the observation model (observation Z) can include a transformation matrix C multiplied by the state vector X regarding the pose of the robot and noise δ (e.g., covariance of a Gaussian distribution).

[0268] Motion model

[0269] Xt = AtXt-1+BtUt+εt

[0270] Observation model

[0271] Zt=CtXt+δt

[0272] In this way, in SLAM based on the Kalman filter, the prediction that generates a prior probability distribution for the robot's pose (or the location of the landmark) and the correction that generates a posterior probability distribution for the robot's pose (or the location of the landmark) can be repeated, and SLAM can be implemented by repeating the prediction and correction for the robot's pose (or the location or map of the landmark) at the next step while advancing the time step according to the discretized time step, while reducing the uncertainty increased according to the covariance of the motion model in the prediction through observation, thereby increasing the accuracy of the robot's pose (or the location or map of the landmark) (by reducing the covariance). For example, in the SLAM or localization based on the Kalman filter, the motion model and observation model can be modeled as linear systems, but in the Extended Kalman Filter (EKF), the motion model and observation model modeled as nonlinear systems can be linearly approximated through first order Taylor expansion, and the Kalman filter can be applied to nonlinear systems by approximating them as linear systems through different transformation matrices At, Bt, and Ct at different center positions of the state vector for each robot's pose (EKF, Extended Kalman Filter).

[0273] For example, in particle filter-based SLAM as a filter-based SLAM, it is possible to approximate a random probability distribution rather than a parametric probability distribution of a Gaussian distribution like the Kalman filter described above (for example, the prior probability distribution for the pose of a robot or the position or map of a landmark and the posterior probability distribution for them are defined by the mean and covariance). For example, in a particle filter, a particle set can approximate any arbitrary function. For example, in the particle filter, the prior probability distribution and the posterior probability distribution for the pose of a robot (or the position or map of a landmark) can be expressed as a random probability distribution (or arbitrary function). For example, in a particle filter, the probability distribution for the pose of a robot (or the position or map of a landmark) can be expressed in the form of a particle set X that sums the multiplication of the hypothesis pose (x(j)) for each robot's pose and the importance weight (w(j)) for each hypothesis pose (x(j)).

[0274] X={x(j), w(j)}, j=1,..,n

[0275] w(j) = proposal / target = f(x(j)) / π(x(j))

[0276] Here, the importance weight (w) is for compensation between the probability distribution predicted in the previous step (proposal, f(x)), for example, the initial random probability distribution or the belief or prior probability distribution in the previous step) and the probability distribution modified from the observation (target, π(x)). The importance weight (w) for compensation between the proposal (f(x)) and the target (π(x)) can be reflected in the new probability distribution in the form of different frequencies (frequencies, which represent the probability of being sampled when resampling particles) of uniform weights.

[0277] For example, the particle filter may repeat the process of predicting a probability distribution about the pose of the robot from the previous belief (prior probability distribution about the pose of the robot) and the motion model of the robot, and the process of generating a posterior probability distribution about the pose of the robot from the observation about the sensor readings of a sensor such as a lidar sensor or a vision sensor and the observation model of the robot, performing resampling from the posterior probability distribution, applying the motion model of the robot to each resampled particle (for example, when applying the motion model of the robot to each resampled particle, it may be applied stochastically so as not to be deterministic), performing observation about the measurements of a sensor such as a lidar sensor or a vision sensor at a new position according to the advancement of the time step, and performing weighting for each particle in a way that reduces the error between the observation about the sensor readings and the observation predicted from each particle to which the motion model of the robot is applied, and in this way, the probability distribution to which weighting is applied or the probability distribution in which the weighting is expressed in the form of frequency may be re-calculated. By resampling and repeating the prediction and correction described above for the new resampled particle set, the robot's pose (or landmark position or map) can be inferred by repeating the prediction and correction until the robot's position (or landmark position or map) converges.

[0278] For example, by applying the particle filter, localization can be performed to infer the pose of the robot on a map as spatial information about the surrounding environment of the robot, and Monte Carlo Localization (MCL) can be performed, and for example, while updating the probability distribution of particle samples to reduce the error between the predicted observation applied to the motion model of the robot and the real observation measured from the sensor of the robot, for example, the expected measurement (sensory reading) from the pose predicted from the motion model of the robot and the spatial information (map) surrounding the robot can be predicted from the sensor measurement, and based on the error between the predicted observation and the real observation measured from the sensor of the robot, the importance weight for each particle can be calculated, and the weighting is performed by performing resampling from a probability distribution (particle set) expressed in the form of frequency, applying the motion model to the resampled particle samples, and performing weighting for each particle in a way that reduces the error between the observation predicted from each particle to which the motion model is applied and the observation about the sensor measurement. Through this MCL (Monte Carlo Localization) Localization can be implemented to infer the pose of a robot on a map of spatial information surrounding the robot.For example, since computational resources are required to calculate predicted observations expected at each position while applying the robot's motion model to each particle sampled or resampled in the particle filter, Adaptive Monte Carlo Localization (AMCL) may be applied, which adaptively reduces the number of resampled particles as the robot's pose converges through the gradually resampled particle set.

[0279] As described above, the Kalman filter or particle filter can be applied as a filtering-based SLAM algorithm for simultaneously generating a map as spatial information about the pose (position and attitude) of the robot and the surrounding environment surrounding the robot, or as a localization algorithm for inferring the pose (position and attitude) of the robot on a given map in a situation where a map as spatial information surrounding the surrounding environment of the robot is constructed.

[0280] In one embodiment of the present invention, in SLAM (graph-based SLAM) that simultaneously implements localization for inferring the pose (position and posture) of a robot on a map including spatial information about the surrounding environment surrounding the robot and mapping for inferring spatial information about the surrounding environment surrounding the robot, the pose of the robot in the next stage can be predicted from a motion model and control or odometry corresponding to the system information of the robot (prediction), and the pose of the robot can be corrected from an observation model corresponding to the system information of the robot and an observation observed from a sensor of the robot (correction). For example, in one embodiment of the present invention, a recursive process can be performed in which the pose of the robot is predicted by repeating prediction and correction along the driving path of the robot, the predicted pose of the robot is corrected, and the posterior probability distribution for the corrected pose of the robot is used as a prior probability distribution in the next stage, and prediction and correction are repeated again.For example, in one embodiment of the present invention, the sensor of the robot may include a lidar sensor that generates a group of point clouds for the surrounding environment or landmarks as observations, and the relative translation and rotation of the robot between the neighboring time steps can be inferred by matching the group of point clouds observed from the lidar sensor at neighboring time steps with another group of point clouds at discrete time steps, for example, by matching observations through the ICP algorithm (inferring the relative translation and rotation of the robot between neighboring nodes), and while correction is performed from the observation model corresponding to the observation and the system information of the robot, the prior probability distribution of the pose of the robot in the prediction before the correction and the probability distribution of the pose of the robot predicted from the motion model and control or odometry corresponding to the system information of the robot can be corrected, for example, the covariance of the probability distribution of the pose of the robot can be reduced, and a more accurate pose of the robot can be inferred. For example, in one embodiment of the present invention, the probability distribution of the pose of the robot can be updated by repeating the prediction and correction described above by applying a Kalman filter or a particle filter.In addition, in SLAM (graph-based SLAM) according to one embodiment of the present invention, optimization can be performed through least square optimization (e.g., non-linear least square optimization) for all nodes forming a loop on a pose graph according to loop closure detection, and, for example, a solution that gradually converges in discrete steps according to a first order Taylor expansion can be approached, and pose graph optimization can be implemented by applying least square optimization to the Mahalanobis distance that reflects the squared error between the predicted observation and the real observation before correction (the square of the error function between the predicted observation and the real observation) and the uncertainty of the measurement between adjacent nodes forming a loop on the pose graph.

[0281] For example, in the above Kalman filter, the probability distribution regarding the pose of the robot can be assumed as a Gaussian distribution, and a parametric probability distribution in which the probability distribution regarding the pose of the robot can be defined from two parameters, mean and covariance, can be assumed, and for example, the noise ε of the motion model of the robot applied in prediction and the noise δ of the observation model of the robot applied in correction can also be defined as Gaussian distributions, and for example, the noises ε and δ of the motion model and observation model of the robot, respectively, can be expressed as the covariance of the Gaussian distribution.

[0282] For example, in the above Kalman filter, a recursive process can be performed in which the posterior probability distribution in the previous stage is input as the prior probability distribution (or the belief of the previous stage) in the next stage, and for example, in localization using the Kalman filter in which prediction and correction are repeated, in prediction, the mean and covariance that define the prediction probability distribution in the current stage can be calculated from the belief (posterior probability distribution of the previous stage) and motion model of the previous stage, and in correction, the mean and covariance of the prediction probability distribution calculated in the previous stage can be corrected from the observation of the measurement of a sensor such as a lidar sensor or a vision sensor and the observation model of the robot. At this time, the Kalman gain involved in the correction of the prediction probability distribution calculated in the previous stage can be calculated, and the mean value (mean corrected by the Kalman gain) of the posterior probability distribution (posterior probability distribution on which correction is performed) corrected to some value between the mean of the prediction probability distribution and the mean of the probability distribution of the observation (the probability distribution or likelihood assumed to be a Gaussian distribution depending on the uncertainty of the observation) can be calculated, and the prediction probability distribution Reduced than the covariance of the probability distribution of covariance and observation (the probability distribution or likelihood assumed to be a Gaussian distribution depending on the uncertainty of observation) (reduced than the covariance of the prediction probability distribution to which each motion model is applied, and reduced than the covariance of the probability distribution of observation).The covariance value (covariance corrected by Kalman gain) of the corrected posterior probability distribution (posterior probability distribution with correction) can be calculated, and for example, the covariance of the prediction probability distribution increased compared to the covariance of the prior probability distribution according to the covariance of the robot's motion model can generate a posterior probability distribution with reduced uncertainty or improved reliability to have reduced covariance through observations regarding sensor observations.

[0283] Kalman Filter Algorithm

[0284] Prediction

[0285] (μt^ and Σt^ are the mean and covariance predicted from the prediction, and Rt is the covariance of the motion model)

[0286] μt^ = At ​​μt-1 + Bt Ut

[0287] Σt^ = At ​​Σt-1 At T + Rt

[0288] Correction

[0289] (μt and Σt are the mean and covariance predicted from the correction, Qt is the covariance of the observation model, and Kt is the Kalman gain)

[0290] Kt = Σt^ Ct T (Ct Σt^ Ct T +Qt) -1

[0291] μt= μt^ + Kt (Zt - Ct μt^)

[0292] Σt = (I-Kt Ct) Σt^

[0293] Similar to the Kalman filter described above, the particle filter can be applied as a filtering-based algorithm for SLAM that simultaneously implements localization to infer the pose of the robot on a map of spatial information about the surrounding environment surrounding the robot and mapping to create a map of spatial information about the surrounding environment surrounding the robot, or it can be applied as a localization algorithm to infer the pose of the robot on an already constructed map. For example, the robot's motion model can be applied to each particle sampled from the probability distribution of the belief or prior in the previous step, and the importance weight for each particle can be calculated based on the error between the predicted observation (prediction) and the real observation measured from the robot's sensor (correction). By repeating the prediction and correction described above for particle samples resampled from the probability distribution expressed in the form of frequencies through weighting for each particle, the pose of the robot can be inferred on a given map according to the convergence of the particle samples (concentration of frequencies with respect to the probability distribution).

[0294] For example, SLAM for simultaneously implementing localization for inferring the pose of a robot and mapping for inferring spatial information about the surrounding environment of the robot can be implemented as a graph-based SLAM such as a pose graph (see also) or a factor graph, as well as a filter-based SLAM that applies a Kalman filter or particle filter as described above, and a smoothing-based SLAM. In the pose graph or factor graph, a node (node ​​or vertex) can mean the position or pose (state vector) of the robot at each time step, and can correspond to a variable that is the target of optimization in pose graph optimization, and an edge connecting adjacent nodes (or vertices) can mean a spatial constraint, where the spatial constraint can include a control or odometry constraint, a loop closure constraint, etc.The above pose graph optimization or factor graph optimization can be a problem of maximizing the joint probability distribution regarding the probability distribution of each node (or vertex), and by taking the negative log likelihood for this joint probability distribution, least square error minimization or least square optimization can be applied in the form of the sum of the square errors between the predicted value (predicted observation) and the measured value (real observation), and at this time, considering the uncertainty of the measurement, least square error minimization or least square optimization can be applied to the form of the Mahalanobis distance that assumes the uncertainty of the measurement to be Gaussian distributed and is summed over all nodes (or vertices).

[0295] The following formula: Pose graph optimization

[0296] (X *is the state vector of the robot's pose searched from the least square optimization for pose graph optimization, e(X) is the error between the real observation Z observed from the robot's sensor and the predicted observation f(x) predicted from the observation model, and the state vector X of the robot's pose is searched to minimize the error between the real observation Z and the predicted observation f(x), Ω is the uncertainty of the measurement, i is the subscript for each edge forming the pose graph, and optimization is performed for all edges forming the pose graph, e i t (X)Ω i e i (X) implements least square optimization for the sum of Mahalanobis distance - Mahalanobis distance, which includes the squared error between the real observation and the predicted observation for each edge forming the pose graph and the uncertainty Ω of the measurement.

[0297] X * = argmin(X) Σ e i t (X)Ω i e i (X)

[0298] e i (X) = Z i -f i (X)

[0299] For example, in pose graph optimization or factor graph optimization, non-linear least square optimization can be performed, and a numerical solution can be produced by repeating iterations until the state vector corresponding to the node (or vertex) or the pose of the robot converges from the first-order linear approximation (first-order Taylor expansion) centered on each node (or vertex) from the Taylor expansion.

[0300] non-linear least square optimization

[0301] i)X * = argmin(X) Σ f i (X) 2

[0302] ii) Differentiation and linearization (first order Taylor expansion, linearized at X=X0, J is the Jacobian for partial differentiation)

[0303] ΔX * = argmin(ΔX) Σ J i ΔX + f i (X0) 2 , J i = ∂ f i (X) / ∂X│x=x0

[0304] iii) Numerically calculate the solution by repeating the iteration to zero the inside of the magnitude.

[0305] For example, in the pose graph optimization or factor graph optimization, sensory readings observed along the robot's driving path can be accumulated and databased, and the instantaneous observations can be compared with the accumulated observations in the database to capture previously visited locations, and loop closure constraints can be formed from the capture of previously visited locations. For example, sensory readings sequentially acquired from the robot's sensors along the driving path can be 1D (1-dimensional) and stored in a database, and features can be extracted for the instantaneous observations and similar sensor observations, and the presence or absence of loop closure can be determined by comparing the extracted features. For example, the robot extracts image features from a video image captured by a vision sensor at the current point in time, extracts image features from a video image determined to have similar observation values ​​from a database in which observation values ​​up to the current point in time are accumulated, and then compares the image features extracted from each video image to determine whether a loop is closed (loop closure detection). In this way, loop closure detection can be implemented by capturing observation values ​​that match each other through mutual matching (matching or association) of observation values ​​observed at different time steps along the robot's driving path.For example, if it is finally determined as the above loop closure, a local SLAM (e.g., a step of implementing a local mapping associated with each node or vertex) prior to loop closure detection) or a global SLAM (e.g., a step of implementing a global mapping expressed on a global coordinate system) that requires relatively more computational resources than the front end process of SLAM or a back end process of SLAM, a loop closure constraint can be formed between corresponding different nodes (or vertices) on the pose graph, and each node (or vertex, the pose of the robot corresponding to each node or vertex) can be optimized through non-linear least square error minimization for the sum of the Mahalanobis distances (e.g., including a Gaussian distribution that reflects the squared error between the predicted observation and the observed real observation and the uncertainty about the measurement) on all edges including the edges forming the loop closure constraint and the edges between neighboring nodes (or vertices) formed along the robot's driving path. For example, in the above non-linear least square optimization, a solution that gradually converges can be produced by adding an increment (Δ) to each node (or vertex) as the discretized step advances.

[0306] For example, the sensor of the robot may include a lidar sensor, which is a type of time of flight (TOF) sensor that calculates the distance to a landmark from the flight distance from the emission of light to the capture of reflected light, and the lidar sensor may generate a group of point clouds as sensory readings of the sensor, and may generate a 2D point cloud or a 3D point cloud from a 2D lidar sensor and a 3D lidar sensor, respectively. For example, for the loop closure detection, matching (or association) can be performed between a group of point clouds observed from a lidar sensor at different time steps along the driving path of the robot and a group of point clouds, and for example, by calculating translational transformation and rotational transformation (rigid body transformation) to match a group of point clouds that have captured the same landmark with a group of point clouds, a rigid transformation can be calculated between the poses of a robot observing a group of point clouds and the poses of a robot observing a group of point clouds (by applying the ICP algorithm), so that a real observation between the poses of different robots can be formed, and for example, the pose of the robot can be corrected according to the predicted observation between the poses of different robots (localization that infers the pose of the robot by repeating prediction and correction), or the pose of the robot can be inferred in a direction that minimizes the error between the real observation formed through matching between the point clouds of the group and the group of points observed from the poses of different robots and the predicted observation (pose graph optimization according to loop closure detection).

[0307] For example, the observation values ​​(e.g., 1D-ized observation values) of a group of point clouds observed from the robot's lidar sensor at the current point in time can be compared with the observation values ​​of the point clouds observed up to the current point in time accumulated in the database, and similar observation values ​​can be extracted, and the ICP (iterative closest point) algorithm can be performed between the group of point clouds observed at the current point in time and the group of point clouds observed at the previous point in time that are determined to be similar observation values, so that the translational transformation and rotational transformation (rigid body transformation) that can most closely match the group of point clouds and the group of point clouds can be derived, and by applying the ICP algorithm as described above, the rigid body transformation including the relative translation and rotation between different nodes (or vertices) on the pose graph can be predicted, and a real observation between neighboring nodes (or vertices) can be formed.

[0308] For example, in an ICP algorithm for matching a group of point clouds output from a robot's sensor (a lidar sensor) at different time steps along the robot's driving path with a group of point clouds, since each point forming a group of point clouds and a group of point clouds does not have an association with each other (e.g., a one-to-one correspondence between the points forming a group of point clouds and a group of point clouds), the ICP algorithm as described above can be iteratively repeated, for example, until the matching between the point clouds of different groups converges.

[0309] ICP algorithm

[0310] (Search for the rotational transformation R and translational transformation t that minimize the distance between pi and qi by applying the rotational transformation R and translational transformation t between a group of point clouds pi and another group of point clouds qi)

[0311] min(R,t) Σ (Rpi+t)-qi 2

[0312] As described above, the ICP as an algorithm for matching a group of point clouds corresponding to observations from a lidar sensor as a sensor of a robot and another group of point clouds may be implemented as a point-to-point ICP or a point-to-plane ICP. For example, in the point-to-point ICP, a translational transformation and a rotational transformation that minimize the distance between points between a group of point clouds and another group of point clouds are calculated, thereby forming a spatial constraint or a loop closure constraint as a real observation that forms an edge connecting adjacent nodes (or vertices) on a pose graph. In this case, the matching between a plurality of points forming a point cloud, for example, the matching between a plurality of points that do not include association information between each other, requires excessive computation, so that it may be implemented as a point-to-plane ICP. For example, it may be implemented by minimizing the normal length from one point to the tangent plane of another point.

[0313] As described above, in SLAM (graph-based SLAM), a spatial constraint corresponding to an edge or a loop closure constraint can be formed between neighboring nodes (or vertices) along the robot's driving path, and for example, a bundle adjustment can be performed on all nodes (or vertices) forming a closed path from loop closure detection or on the poses of robots forming nodes (or vertices), and a least square optimization can be performed on the sum of Mahalanobis distances that include the squared error between the predicted observation and the observed (real observation) at each node (or vertex) and reflect the uncertainty of the measurement. For example, the uncertainty of the measurement can be calculated from the matching rate between a group of point clouds and another group of point clouds as observations observed at different robot poses, and in various embodiments of the present invention, the uncertainty of the measurement can be assumed to be a Gaussian distribution.

[0314] For example, in the pose graph or fact graph-based SLAM, the poses for all nodes (or vertices) forming a closed loop can be updated in bulk through loop closure detection (bundle adjustment), which may be different from the Kalman filter or particle filter that infers the pose of the robot at the next time step on a time step basis at each time step.

[0315] In SLAM, which simultaneously implements localization for inferring the pose (location and posture) of the robot and mapping for generating spatial information about the surrounding environment surrounding the robot, if the uncertainty of the observation model regarding measurements from sensors for inferring the relative position or posture of the robot from landmarks as the surrounding environment surrounding the robot is low, or in other words, if the reliability of the measurements from the robot's sensors is high, the uncertainty of the robot's motion model, for example, the uncertainty of the robot's control or odometry, can be compensated for. Conversely, if the uncertainty of the robot's motion model is low, or in other words, if the reliability of the robot's control or odometry is high, the uncertainty of the observation model regarding measurements from the robot's sensors can be compensated for. However, in a general SLAM operating environment that simultaneously implements localization and mapping, it may be necessary to consider both the uncertainty of the robot's motion model and the uncertainty of the observation model regarding measurements from the robot's sensors.

[0316] In one embodiment of the present invention, in SLAM for simultaneously implementing localization for inferring a pose regarding the position and posture of the robot itself on a map regarding spatial information regarding the surrounding environment surrounding the robot and mapping for inferring spatial information regarding the surrounding environment surrounding the robot, a graph-based SLAM such as a pose graph (or factor graph, see also) can be implemented, and in the pose graph, a node (or vertex) formed according to the advancement of a discrete time step along the driving path of the robot (see also) can mean a pose including the position and posture of the robot, and an edge connecting neighboring nodes (or vertices) can include a spatial constraint between neighboring nodes (or vertices) or connectivity information between neighboring nodes (or vertices), and optimization or bundle adjustment for all nodes (or vertices) in the loop can be performed in loop closure detection based on the spatial constraint or connectivity information of the edge connecting neighboring nodes (or vertices), and in this way, in the loop closure In pose graph optimization (PGO), the edge between neighboring nodes (or vertices) can correspond to the Mahalanobis distance, which includes the squared error between the predicted observation and the observed real observation and the uncertainty about the observation from the robot's sensor, and can be understood as the reliability of the observation from the robot's sensor, and for example, in pose graph optimization (PGO) due to loop closure,For spatial constraints forming all edges of the loop, or the squared error between predicted observations and observed real observations between neighboring nodes (or vertices) considering the uncertainty about observations from the robot's sensors, for example, the sum of the squared error between predicted observations from the observation model and real observations measured from the robot's sensors between neighboring nodes (or vertices) and the multiplication of the uncertainty about observations from the robot's sensors (corresponding to Mahalanobis distance), the least square optimization or non-linear least square optimization can be performed for all nodes (or vertices) forming the loop.

[0317] The following formula: Pose graph optimization

[0318] (X * is the state vector of the robot's pose searched from the least square optimization for pose graph optimization, e(X) is the error between the real observation Z observed from the robot's sensor and the predicted observation f(x) predicted from the observation model, and the state vector X of the robot's pose is searched to minimize the error between the real observation Z and the predicted observation f(x), Ω is the uncertainty of the measurement, i is the subscript for each edge forming the pose graph, and optimization is performed for all edges forming the pose graph, e i t (X)Ω i e i(X) implements least square optimization for the sum of Mahalanobis distance - Mahalanobis distance, which includes the squared error between the real observation and the predicted observation for each edge forming the pose graph and the uncertainty Ω of the measurement.

[0319] X * = argmin Σ e i t (X)Ω i e i (X)

[0320] e i (X) = Z i -f i (X)

[0321] Node optimization

[0322] FIG. 6 illustrates a diagram for explaining a start condition of node optimization, which is a condition for starting node optimization, that is, a first number or more nodes are created within a first work area set to a first radius (node ​​optimization search distance) from a central node or the latest node.

[0323] FIG. 7 is a diagram illustrating the relationship between a first working area set as a first radius with respect to the initiation condition of node optimization and a second working area (loop closure detection search distance) with respect to the search condition of loop closure detection.

[0324] Figure 8 illustrates a diagram for explaining reconnection, which searches for a reconnection node to replace a first node that satisfies the elimination condition for node optimization, and creates an edge connecting the second and third nodes that were neighbors of the first node and the reconnection node.

[0325] Figure 9 illustrates a diagram for explaining reconnection that creates an edge that directly connects the second and third nodes that were adjacent to the first node that satisfies the elimination condition for node optimization.

[0326] FIG. 10 illustrates a diagram for explaining that the erasure of the first node is limited depending on the presence of an obstacle recognized between the central node or the latest node and the first node to be erased, as a condition for initiating erasure or a restriction on the erasure condition for node optimization.

[0327] In a graph-based SLAM according to one embodiment of the present invention, some nodes (or vertices) satisfying a deletion condition can be deleted so as to optimize nodes (or vertices) formed instantaneously according to discretized time steps along a robot's driving path. In a graph-based SLAM according to a comparative example that is in contrast to the present invention, the generation of nodes (or vertices) can be continued in a time-series manner along the driving path of the robot within the workspace of the robot in a limited area. According to this comparative example, as a large number of nodes (or vertices) are connected to each other in a complex form within the workspace of the robot in a limited area, a large number of overlapping nodes (or vertices) with respect to overlapping poses with respect to similar positions and postures in the pose space of the robot can be generated, which can result in inefficient and redundant data storage. Such inefficient and redundant data storage can result in inefficient waste of computational resources or storage capacity, and excessive time can be required to call a map (e.g., a global map) that is updated from observations measured at the positions of each node (or vertex), which can ultimately result in a system-down of the entire robot system including the robot and the back-end of the robot.In addition, the surrounding environment surrounding the robot may be a dynamic environment that can change frequently, or even a static environment with relatively little change, but the surrounding environment surrounding the robot may change over a long period of time. Despite such changes in the surrounding environment, observations from nodes (or vertices) accumulated over a long period of time or from sensors of the robot that are stored in connection with nodes (or vertices) may distort spatial information about the surrounding environment of the robot. For example, observations from nodes (or vertices) accumulated over a long period of time or from sensors of the robot that are stored in connection with nodes (or vertices) may cause a mismatch with the latest information of observations from nodes (or vertices) generated in the most recent time step adjacent to the current time point or from sensors of the robot that are stored in connection with nodes (or vertices). For example, a mismatch between different observations captured from similar positions or postures in the pose space of the robot, for example, an observation from a node (or vertex) accumulated over a long period of time and an observation from a node (or vertex) generated in the most recent time step adjacent to the current time point, may cause a mismatch between these nodes (or vertices). This can also cause distortions in maps that overlap or combine observations (e.g., global maps). For example, this can lead to a mismatch between local maps associated with nodes (or vertices) that have accumulated over time and local maps associated with recently created nodes (or vertices).

[0328] In one embodiment of the present invention, in order to avoid storing redundant and inefficient data that may result from the creation of excessive nodes (or vertices) for a limited work area of ​​a robot, node (or vertex) optimization can be implemented to delete or eliminate observations from a sensor of a robot stored in association with a node (or vertex) for a node (or vertex) that satisfies a certain erasure condition, and by eliminating a large number of redundant nodes (or vertices) within a limited work area of ​​a robot, computational resources and storage capacity can be reduced, and mismatch of observations between nodes having a time gap from each other (e.g., mismatch of observations or mismatch of local maps depending on whether or not updates are made according to changes in the robot's surrounding environment between nodes related to a similar pose of the robot) can be fundamentally resolved.

[0329] In one embodiment of the present invention, node optimization can be initiated under a condition that a predetermined first number (e.g., 5 to 20) or more nodes are created within a region within a predetermined radius range on a pose graph or within a first work area of ​​a robot (corresponding to a node optimization search distance), as a starting condition for node optimization. For example, in one embodiment of the present invention, a first working area (node ​​optimization search distance) corresponding to half of a second working area (loop closure detection search distance) for a search condition of loop closure detection as a preset working area may be set as a first working area preset for node (or vertex) optimization, and in a specific embodiment, when the second working area (loop closure detection search distance) corresponding to a search condition of loop closure detection is set to a radius of 10 m (second radius), the first working area (node ​​optimization search distance) preset for node (or vertex) optimization may be set to 5 m (first radius), and the first and second working areas may correspond to, for example, a range within a circle measured in a radial direction from a centrifugal position with a node generated at the current time step as the centrifugal position, and for example, the first and second working areas may correspond to a range within a circle with a radius of 5 m (first radius) and a range within a circle with a radius of 10 m (second radius), respectively.

[0330] In one embodiment of the present invention, loop closure detection for optimization (pose graph optimization, PGO) for all nodes (or vertices) forming a loop of a pose graph can be searched within a second work area of ​​a robot set in advance, and for example, loop closure detection may not be performed outside the second work area set in advance. That is, in one embodiment of the present invention, the search for loop closure detection may be restricted so as not to be performed outside the second work area set in advance, and a limit of the second work area may be set for loop closure detection.

[0331] In one embodiment of the present invention, when setting the limit of the second working area for loop closure detection, forming a loop closure constraint that is excessively extended beyond the second working area (loop closure detection search distance) may cause an error in pose graph optimization by forming a loop closure constraint that connects nodes related to poses of different robots that are excessively distant from each other while judging nodes (or vertices) generated with edges corresponding to excessively distant spatial constraints as origin regression according to similar real observations measured from the robot's sensors. For example, by excluding loop closure detections with low accuracy, it is possible to prevent the accuracy of the entire pose graph from deteriorating due to an error in pose graph optimization. For example, in one embodiment of the present invention, within a limited area of ​​a second work area corresponding to a range of a second radius centered on a node generated at a current time step, an observation associated with a node generated at a current time step and an observation associated with another node belonging to the second work area are compared, and nodes associated with observations similar to the node generated at the current time step can be connected with a loop closure constraint, and pose graph optimization according to loop closure detection can be initiated.For example, in one embodiment of the present invention, even considering uncertainty in odometry and uncertainty in observation, creating a loop closure constraint connecting poses of robots that are excessively distant from each other may cause errors in pose graph optimization according to loop closure detection, and may cause distortion of the entire pose graph due to the erroneous loop closure constraint.

[0332] In this way, in one embodiment of the present invention, a limit of a first work area (node ​​optimization search distance) is set for each node (or vertex) optimization, and a limit of a second work area is set for loop closure detection, while the first work area for node (or vertex) optimization can be set to half of the second work area for loop closure detection. At this time, the first and second work areas may be set to an actual scale of the work space rather than being set on a pose graph, and for example, may be set to a range within a circumference limited by a radius range, and more specifically, each of the first and second work areas can be set to 5 m (first radius) and 10 m (second radius). In one embodiment of the present invention, a first working area for node (or vertex) optimization can be set to half of a second working area for searching for loop closure detection, and considering that a complex trajectory can be formed within the second working area while forming a loop of a pose graph according to origin regression according to loop closure detection, the first working area for node (or vertex) optimization can be restricted to be narrower than the second working area for searching for loop closure detection, and that a complex trajectory can be formed within the second working area while forming a loop of a pose graph due to origin regression, and that a large number of nodes (or vertices) that are overlapping or close to overlapping in the pose graph can be generated, the first radius for setting the first working area with respect to the start condition of node (or vertex) optimization can be set as a restriction for node (or vertex) optimization a first working area that corresponds to half of the second radius for setting the second working area with respect to the search condition of loop closure detection.

[0333] In one embodiment of the present invention, the creation of a first number (e.g., 5 to 20) or more nodes (or vertices) within a first work area may be set as a starting condition for node (or vertex) optimization. For example, in one embodiment of the present invention, the first work area may be set as a first radius that is half of a second radius that sets a second work area that limits loop closure detection, and the creation of a first number (e.g., 5 to 20) or more nodes (or vertices) within the first work area may be set as a starting condition, and searching for a first node to be deleted from nodes less than the first number may reduce the need to implement reduction in computational load or storage capacity while deleting overlapping or duplicated data.

[0334] In a specific embodiment of the present invention, the initiation condition of node (or vertex) optimization may be that a first number (e.g., 5 to 20) or more nodes are created within a first work area (first radius) set in advance, and nodes to be eliminated (first nodes) may be selected based on the first number (e.g., 5 to 20) or more nodes created within the first work area.

[0335] In one embodiment of the present invention, in selecting a node to be erased, the first node whose creation time is the earliest within the first work area can be selected as the node to be erased. For example, the first node created the longest within the first work area can be selected as the node to be erased. In this way, in the node optimization according to one embodiment of the present invention, the first node created at the earliest time or the first node created the oldest time is selected as the node to be eliminated for node optimization based on the time of creation within the first work area where the node optimization is set as a limitation, because the observation stored in connection with the first node created the oldest time reflects the measurement from the robot's sensor the oldest time ago, and accordingly, for example, the local map from the oldest time is stored from the observation based on the oldest time measurement, and there is a high possibility that it does not reflect the change in the surrounding environment surrounding the robot, and for example, even in a dynamic surrounding environment where the spatial information of the surrounding environment changes frequently depending on the surrounding environment surrounding the robot, or a static surrounding environment where the change in the spatial information of the surrounding environment is relatively small, the local map associated with the first node can be eliminated based on the observation associated with the first node measured a long time ago or the observation associated with the first node, which does not reflect the change accumulated over a long period of time, and in one embodiment of the present invention, the first node created the oldest time or the first node created the oldest time within the first work area where the node optimization is set as a limitation, based on the time of creation. The first node created a long time ago can be selected as the node to be eliminated for node optimization.In node optimization according to one embodiment of the present invention, the first node selected as the target of elimination and the observations stored in connection with the first node can all be deleted or removed, and by deleting or removing the first node and the observations linked to the first node in this way, the second node and the third node adjacent to the first node can lose their connectivity information and form isolated nodes on the pose graph, and in this way, for the isolated second and third nodes adjacent to the first node to be elided and whose connectivity information has been lost on the pose graph due to the deletion or elimination of the first node, an edge connecting the second and third nodes adjacent to the first node and the reconnected node before the first node is deleted can be created from a search for a reconnected node that replaces the first node, as described below.

[0336] In selecting a node to be reconnected according to one embodiment of the present invention, among the nodes in the first work area, a node linked to an observation most similar to an observation linked to the first node to be deleted may be selected as a reconnection node that will create a connection with the second node and the third node adjacent to the first node by replacing the first node. In node optimization according to one embodiment of the present invention, a reconnection node is selected to replace the first node to be eliminated, and a connection is created between the second node and the third node adjacent to the first node and the reconnection node, because, as a group of nodes connected from the second node and the third node adjacent to the first node remain as isolated nodes with lost connectivity information on the pose graph due to deletion or elimination of the first node, a loop on the pose graph that can be optimized through least square optimization according to loop closure detection cannot be formed, and therefore pose graph optimization (PGO) cannot be performed on these isolated nodes, and because the connectivity information with the second node and the third node adjacent to the first node is lost, or because the connectivity information with other parts of the pose graph is lost, editing of observations stored in connection with the isolated nodes or editing of a local map cannot be performed on the isolated nodes on the pose graph. In this regard, in node optimization according to one embodiment of the present invention, a node having an observation similar to the observation associated with the first node is selected to replace the first node to be eliminated. A reconnection node can be searched, a reconnection between a second node and a third node that are adjacent to the first node and whose connectivity information with the first node has been lost can be created, and an edge can be created between the first node that is the target of erasure and the second and third nodes that are adjacent to the reconnection node.

[0337] In node optimization according to one embodiment of the present invention, searching for a reconnection node to replace the first node to be eliminated based on observation can be performed based on real observation in pose graph optimization (PGO) according to loop closure detection, and for example, least square optimization is performed on the squared error between the predicted observation and the observed real observation between neighboring nodes. Therefore, based on real observation, a node that is linked to an observation similar to the first node to be eliminated can be selected as a reconnection node that will replace the first node and create a connection with the second node and the third node neighboring the first node.

[0338] In one embodiment of the present invention, in searching for a reconnection node that will replace the first node and create a connection with the second node and the third node adjacent to the first node, the reconnection node that has the highest matching rate of observation with the first node within the first work area can be searched for, for example, from a matching between a group of point clouds observed from the robot's lidar sensor at the first node to be erased and another group of point clouds observed from the robot's lidar sensor at another node within the first work area, another node within the first work area in which the group of point clouds has the highest matching rate with the group of point clouds can be selected as a reconnection node that will replace the first node and create a connection with the other second node and the third node adjacent to the first node.

[0339] More specifically, in one embodiment of the present invention, a matching rate of observation can be calculated between a group of point clouds observed from a first node of a target for erasure (a group of point clouds stored in connection with the first node, and another group of point clouds observed from other nodes (a group of point clouds stored in connection with other nodes) excluding the first node and the second and third nodes adjacent to the first node (the second and third nodes directly connected to the first node through an edge on the pose graph) within the first work area, and in one embodiment of the present invention, calculating the matching rate of observation between the first node of a target for erasure and another node within the first work area, for example, a point cloud observed from a lidar sensor of a robot can be exemplified as the observation, and a matching rate of observation can be calculated as a measure of consistency between a group of point clouds observed from the first node of a target for erasure and another group of point clouds observed from other nodes excluding the first node and the second and third nodes directly connected to the first node on the pose graph within the first work area, and for example, Through an ICP (iterative closest point) algorithm between a group of point clouds observed from a first node and a group of point clouds observed from other nodes within the first work area, relative translational and rotational components for matching between these different groups of point clouds and the other group of point clouds can be predicted, and from an ICP algorithm that sequentially inputs a group of point clouds observed from the first node and a group of point clouds observed from other nodes within the first work area, relative translational and rotational components can be predicted, and a transformation matrix (for example,According to the degree of overlap between a group of point clouds as observations associated with the first node and another group of point clouds as observations associated with other nodes in the first work area, a matching rate of observations between the first node and other nodes can be calculated, based on a transformation matrix or homogeneous matrix.

[0340] In this way, in one embodiment of the present invention, based on the observation observed at each node, a reconnection node that will replace the first node to be erased and create a connection with the second node and the third node adjacent to the first node, for example, the second node and the third node that are directly connected to the first node to be erased through an edge, can be selected, and an observation matching rate can be calculated through observation matching (ICP algorithm) between the first node to be erased and the reconnection node that will replace the first node and create a connection, for example, a node that has a high observation matching rate or matching rate with the first node is selected as a reconnection node that will replace the first node and create a connection with the second node and the third node adjacent to the first node, thereby forming a loop on the pose graph according to loop closure detection from the reconnection node that replaces the first node to be erased and creates a connection with the second node and the third node adjacent to the first node, and based on the observation between the second node-reconnection node and the third node-reconnection node that form the loop, the pose graph Optimization (PGO) can be performed.

[0341] In the node optimization according to one embodiment of the present invention, the second node-reconnected node and the third node-reconnected node, where connectivity information or edges are created between each other on the pose graph according to the elimination of the first node, do not contain control or odometry information as information about the motion model (absence of odometry information between the second node-reconnected node and the third node-reconnected node, which are not actual driving paths of the robot), but the second node-reconnected node and the third node-reconnected node contain pose information about the relative position (e.g., translation) and relative rotation (e.g., rotation) between each other while creating nodes on the previous pose graph, and accordingly, the predicted observation according to the movement between the second node-reconnected node and the third node-reconnected node can be predicted from the observation model, and the real observation according to the movement between the second node-reconnected node and the third node-reconnected node can be obtained from the real observation stored in association with each of the second node, the third node, and the connection node, and therefore, the second node-reconnected node and the third node-reconnected node Pose graph optimization (PGO) based on loop closure detection may also be possible for loops connecting the third node and the reconnection node.In other words, for the reconnected nodes that do not form the actual driving path of the robot and are connected to the second and third nodes adjacent to the first node by replacing the first node eliminated from node optimization, the pose graph optimization (PGO) can be implemented by applying least square optimization to the squared error (Mahalanobis distance that adds measurement uncertainty to the squared error) between the predicted observation and the real observation from the robot's sensor according to loop closure detection, even for the edges or spatial constraints between these second node-reconnected nodes and third node-reconnected nodes.

[0342] In one embodiment of the present invention, among the nodes generated over time as the time step advances according to the movement of the robot, the first node to be deleted can be selected based on whether an deletion start condition is satisfied, centered on the most recent node generated at the current time point or the most recent node closest to the current time point, for example, by setting the deletion start condition as being whether a first number (for example, between 5 and 20) or more nodes are generated within a first work area of ​​a radius range centered on the most recent node (for example, the first radius setting the first work area is half of the second radius setting the second work area with respect to the search condition of loop closure detection).

[0343] In various embodiments of the present invention, the selection of the first node to be purged may be performed by selecting the node with the earliest creation time within the first work area, that is, the oldest node in terms of time, as the first node to be purged, or the node associated with the observation with the lowest matching rate with the observation associated with the most recent node generated at the current time or closest to the current time, more specifically, the node with the lowest observation matching rate with the most recent node among the plurality of nodes generated within the first work area, as the first node to be purged. For example, a group of point clouds as observations associated with the most recent nodes, and point clouds associated with each node as observations associated with the nodes generated within the first work area may be sequentially used as another group of point clouds, and a point-to-point matching of the ICP (Iterative Closet Point) algorithm may be performed between the group of point clouds and the other group of point clouds, thereby calculating the matching rate between the observation associated with the most recent nodes and the observation associated with each node generated within the first work area. In this way, in an embodiment of selecting a first node to be deleted based on a matching rate, an observation to be deleted together with the first node to be deleted may correspond to an observation with the lowest matching rate calculated from a one-to-one matching between an observation associated with each node generated within the first work area and an observation associated with the latest node.

[0344] In one embodiment of the present invention, in order to prevent the isolation of the second node and the third node connected to the first node through an edge according to the erasure of the first node of the erasure target, a node that is linked to an observation with the highest matching rate with the observation of the first node of the erasure target within the first work area is searched for, and the searched node can be selected as a reconnection node that replaces the first node and creates a connection with the second node and the third node adjacent to the first node.

[0345] In various embodiments of the present invention, before erasing the first node to be erased, various algorithms may be applied to prevent the isolation of the second and third nodes that were adjacent to each other with the first node interposed between them, and as described above, a reconnection node that is linked to an observation observed from the first node to be erased and an observation that has the highest matching rate may be selected, and an edge connecting the reconnection node and the adjacent second and third nodes may be formed by replacing the first node to be erased, and in addition, an edge that directly connects the second and third nodes that were adjacent to each other with the first node to be erased interposed between them may be created.

[0346] For example, in an embodiment in which an edge is formed that directly connects the second and third nodes that were adjacent to the first node that is the target of erasure, after the first node that is the target of erasure is erased, pose graph optimization (PGO) can be implemented for all nodes including the second node and the third node that are connected on the loop of the pose graph that includes the edge that directly connects the second node and the third node, based on loop closure detection.

[0347] In the above pose graph optimization (PGO), the edge between the second and third nodes does not include odometry information for tracking the robot's driving path, but the predicted observation between the second and third nodes can be predicted from the robot's pose with respect to the second node and the robot's pose with respect to the third node (e.g., from the robot's pose with respect to the second and third nodes and an observation model of the robot), and the pose graph optimization can be implemented by applying least square optimization to the Mahalanobis distance with added squared error and measurement uncertainty between the predicted observation predicted between the second and third nodes and the real observation associated with the second and third nodes (e.g., the observation associated with the second and third nodes).

[0348] In the pose graph optimization (PGO) described above, the uncertainty of the measurement for applying the least square optimization to the edge directly connecting the second node and the third node can be calculated from the uncertainty of the measurement between the eliminated first node and the second node and the uncertainty of the measurement between the eliminated first node and the third node. More specifically, in one embodiment of the present invention, the uncertainty of the measurement between the eliminated first node and the second node can be calculated from the matching rate (ICP algorithm) between a group of point clouds observed from the eliminated first node and another group of point clouds observed from the second node, and similarly, the uncertainty of the measurement between the eliminated first node and the third node can be calculated from the matching rate (ICP algorithm) between a group of point clouds observed from the eliminated first node and another group of point clouds observed from the third node.

[0349] In various embodiments of the present invention, the measurement uncertainty between the second and third nodes connected to both sides of the first node according to the elimination of the first node, which is the target of elimination, may be calculated from the point-to-point matching rate of the ICP algorithm between a group of point clouds as observations associated with the second node and another group of point clouds as observations associated with the third node, but since calculating the association information and matching rate between the group of point clouds and the other group of point clouds from a number of iterations in the absence of association information between the group of point clouds and the other group of point clouds requires allocation of additional computational resources and computational time, in one embodiment of the present invention, before the elimination of the first node, which is the target of elimination, the node of the pose graph is generated along the driving path of the robot, and the relative pose between the first node and the second node is calculated from the observation (relative rigid body transformation according to the observation), so that the matching (ICP algorithm) between the point clouds as observations of the first node and the second node can be implemented, for example, from the odometery predicted between the first node and the second node. Observation can be corrected from real observation between the first node and the second node, and a rigid transformation for matching (ICP algorithm) of point clouds between the first node and the second node as the real observation between the first node and the second node can be calculated, and the pose of the robot can be inferred while correcting the predicted observation, and accordingly, the first,The matching rate (corresponding to the uncertainty of measurement or the reliability of measurement) between the observations associated with the second node may have been previously calculated, and similarly to the first and second nodes along the robot's driving path, the matching rate between the observations associated with these first and third nodes, i.e., the uncertainty of measurement, may also have been previously calculated. In order to save computational resources and computational time for calculating the matching rate between the observations associated with these second and third nodes between the second and third nodes that form a direct connection upon elimination of the first node that is the target of elimination, in one embodiment of the present invention, the matching rate between the observations of these second and third nodes as the uncertainty of measurement for performing pose graph optimization (PGO) between the second and third nodes that form a direct connection upon elimination of the first node that is the target of elimination may be calculated based on the matching rate between the observations of the first and second nodes that have been previously calculated and the matching rate between the observations of the first and third nodes that have been previously calculated. For example, the first node-second node The uncertainty of measurement can be calculated in pose graph optimization for edges that directly connect the second and third nodes, which are the target of elimination, by relatively simple operations such as the average or weighted average of the matching rate between the observations linked to the nodes and the matching rate between the observations linked to the first and third nodes, and the uncertainty of measurement between the second and third nodes calculated as above,Pose graph optimization can be implemented from least square optimization for the Mahalanobis distance, which includes the squared error between the predicted observation and the real observation between the second and third nodes.

[0350] In node optimization according to one embodiment of the present invention, a first node to be erased can be selected by setting the generation of a first number or more nodes within a first work area set to a first radius centered on the latest node generated at the current time step as a start condition for erasure for node optimization, and in various embodiments of the present invention, even if the first node to be erased is selected by satisfying the above-described start condition for erasure or satisfying the start condition for erasure, a restriction on erasure or deletion of the first node can be set, and if erasure of the first node is restricted, erasure of the first node may not be performed. For example, in various embodiments of the present invention, even if the node that is the earliest or oldest based on the time of creation within the first work area, or the node that has the lowest matching rate with the observation associated with the latest node within the first work area, is selected as the first node to be eliminated, centered on the node created at the current time step or the most recently created node based on the time of creation, if an obstacle is recognized between the latest node and the first node, the first node that is blocked by the obstacle recognized between the latest node and the first node may not be eliminated. For example, in one embodiment of the present invention, ray tracing may be applied from the latest node, and a point related to the obstacle (for example, a point of a group of point clouds as an observation observed from a sensor of the robot) is captured from an outgoing ray from the latest node toward the first node, and the presence of the obstacle can be recognized by searching for adjacent points from the captured point.

[0351] For example, in an instance where an obstacle is recognized between the latest node and the first node (the node selected as the first node), the latest node and the first node can be understood as forming different sections within the first work area, and maintaining observations that observed different sections in this way can be advantageous for generating an accurate global map from the overlapping or combination of observations that observed different sections, and it may be desirable to maintain observations that observed different sections for the purpose of saving computational resources and storage capacity as overlapping or duplicated data, rather than for the purpose of generating a global map that accurately reflects spatial information about the surrounding environment including the obstacle, for example. In other words, the existence of the obstacle can be understood as setting a restriction on the initiation condition of elimination (a first number or more of nodes within the first workspace) or the elimination condition (the node with the earliest or oldest creation time within the first workspace / the node associated with the observation with the lowest matching rate within the first workspace), so that even if the first node satisfying the elimination condition is selected, the first node and the observation associated with the first node can be maintained without elimination or deletion of the first node.

[0352] In various embodiments of the present invention, when the presence of an obstacle is recognized between the center position (corresponding to the center node) of the first radius that sets the first working radius, which is the starting condition for erasure or a restriction on the erasure condition, and the first node, erasure or deletion of the first node may not be performed, and node optimization for at least the first working area set around the center node may not be performed any further, and erasure of the node may be terminated, or another first node in which the presence of an obstacle is not recognized within the first working radius may be selected to perform node optimization for the first working area.

[0353] In one embodiment of the present invention, the center position (corresponding to the node of the center) of the first radius for setting the first work area as the starting condition of elimination for node optimization may correspond to the node generated at the current time step or the latest node with the latest generation time point as the time step following the robot's travel path advances, and in one embodiment of the present invention, the first radius for setting the first work area in the starting condition of elimination for node optimization may mean the first radius centered on the node generated at the current time step or the latest node with the latest generation time point as a basis for generation, and in other words, the node generated at the current time step or the latest node with the latest generation time point as a basis for generation may mean the center position (node ​​of the center) of the first radius for setting the first work radius, and in an embodiment for selecting the node with the lowest matching rate observation linked among a plurality of nodes within the first work radius as the first node to be eliminated with respect to the elimination condition, the matching rate is the difference between the node of the center position corresponding to the first radius for setting the first work area, which is the starting condition of elimination, and other nodes in the first work area. It can mean the matching rate of observation between, and generally, for the initiation condition of elimination or the elimination condition, the initiation condition of elimination or the elimination condition can be set based on the central position (center node) of the first radius that sets the first working area.

[0354] <Partial Mapping>

[0355] FIG. 11 is a drawing showing an example of an embodiment in which partial mapping is applied, in which a first workspace in which mapping is implemented, including an existing process line and an existing shelf, is spatially connected to a second workspace in which mapping is not yet implemented, while a new line is added, and a second workspace in which partial mapping is applied is shown.

[0356] FIG. 12 is a drawing illustrating a robot's travel path for a first workspace in which mapping is implemented, including the existing process line and existing shelves illustrated in FIG. 11.

[0357] FIGS. 13 and 14 illustrate an initial estimation of a starting position of a partial mapping, and illustrate the generation of a starting node based on observations observed from a robot's sensor from a position candidate regarding a starting position of a partial mapping represented in FIG. 13, or a position or pose corrected from observations observed from a robot's sensor from a position candidate regarding a starting position of a partial mapping and spatial information (map) regarding a first workspace, as a corrected position or an initially estimated starting position represented in FIG. 14.

[0358] FIG. 15 is a diagram illustrating an observation observed from an initiating node, which is an example, to illustrate a connection between an initiating node and the fourth node on the first pose graph that is the closest to the initiating position of a partial mapping, or a connection between an initiating node and the last node with the latest generation time on the first pose graph, with respect to the initiating position of a partial mapping.

[0359] FIG. 16 is a diagram illustrating the generation of a second pose graph for a second workspace according to a partial mapping initiated from an initiation node, and an example diagram illustrating observations observed from nodes on the second pose graph is shown.

[0360] FIG. 17 illustrates a diagram for explaining pose graph optimization for nodes on a loop formed across the first and second pose graphs, while forming a pose graph that is a combination of a first pose graph for a first workspace and a second pose graph for a second workspace.

[0361] In one embodiment of the present invention, in SLAM for simultaneously implementing localization for inferring the location of the robot itself on a map including spatial information about the surrounding environment surrounding the robot and mapping for inferring spatial information about the surrounding environment surrounding the robot, partial mapping can be implemented for an additional space for which spatial information was not inferred in a previous step. For example, in one embodiment of the present invention, separate from the first workspace for which mapping was implemented in the previous step, partial mapping that is continuously connected to the mapping of the first workspace can be implemented for an additional second workspace for which mapping was not implemented in the previous step and which is spatially connected to the first workspace for which mapping was implemented in the previous step.

[0362] In a partial mapping according to one embodiment of the present invention, a pose graph for a second workspace can be generated while being connected to a pose graph for graph-based SLAM for mapping and localization for a first workspace, for example, by sequentially generating a first pose graph for the first workspace and a second pose graph for the second workspace, a local map generated from an observation associated with a node on the first pose graph for the first workspace and a local map generated from an observation associated with a node on the second pose graph for the second workspace are connected to each other, for example, while the first pose graph for the first workspace and the second pose graph for the second workspace are pose graph optimized through loop closure detection, for example, while a pose graph that progresses from the first workspace toward the second workspace and then returns toward the first workspace is pose graph optimized through loop closure detection, an entire mapping that connects these first and second workspaces can be implemented, and a first workspace in which mapping is implemented in a previous step, and a new second workspace in which mapping is not implemented in a previous step and spatial information is not generated A complete global mapping can be implemented that links workspaces to each other.Partial mapping according to one embodiment of the present invention utilizes information about the first workspace previously generated in the previous step (e.g., the first pose graph about the first workspace and observations stored in connection with each node generated on the first pose graph, etc.) while implementing integrated mapping as spatial information about the entire workspace that integrates the first and second workspaces with only partial mapping about an additional second workspace connected to the first workspace, and in particular, while utilizing information about the first workspace whose mapping was implemented in the previous step (e.g., the first pose graph about the first workspace and observations stored in connection with each node on the first pose graph about the first workspace, etc.) while additionally implementing partial mapping only for the second workspace, it is possible to implement integrated mapping about the first and second workspaces. In a comparative example contrasting with the present invention, for integrated mapping about the first and second workspaces, while generating pose graphs again for the first and second workspaces, information about the first workspace generated in the previous step may not be utilized and may be discarded, and a graph-based method including generating a pose graph about the first workspace again is implemented. Starting the SLAM algorithm from scratch again can result in waste of computational resources and computational time.

[0363] More specifically, the partial mapping according to one embodiment of the present invention can be connected to the pose graph for the first workspace through a node (the fourth node) adjacent to the start position of the partial mapping on the pose graph for the first workspace from the start position of the partial mapping set within the first workspace where the mapping was implemented in the previous step. For example, the starting position of the partial mapping can be set to a position within the first workspace where the mapping was implemented in the previous step (start condition of the partial mapping: the starting position of the partial mapping is set to a position within the first workspace where the mapping was implemented), and while the starting position of the partial mapping is set to a position within the first workspace where the mapping is implemented, a connection with the first pose graph for the first workspace can be sought, and for example, the starting position of the partial mapping can be inferred through an initial estimation from a landmark of the mapping on the first workspace where the mapping is implemented as the starting position of the partial mapping, and in this way, through the initial estimation, a connection with the fourth node closest to the starting position of the partial mapping can be created from the initially estimated starting position of the partial mapping on the first workspace or on the first pose graph for the first workspace.

[0364] In a partial mapping according to one embodiment of the present invention, by searching for a fourth node closest to a starting position of partial mapping on a first pose graph for a first workspace and generating a connection between the starting position of partial mapping and the fourth node, the starting position of partial mapping can be incorporated into a pose graph for the first workspace (a node on the first pose graph for the first workspace), and in this way, a pose graph (a second pose graph) that advances in time steps toward a second workspace as a mapping target from the starting position of partial mapping connected to the first pose graph for the first workspace can be generated, and for example, the pose graphs for the first and second workspaces can be sequentially connected to each other.

[0365] For example, the starting position (pose) of the partial mapping can be initially estimated from a relative position (translation along two axes) or a relative pose (rotation) from a mapped landmark on the first workspace where the mapping is implemented or on the first pose graph for the first workspace where the mapping is implemented, and can be initially estimated, for example, based on observations from sensors of the robot. The starting position of the partial mapping predicted from such an initial estimation can be generated as a node (the starting node of the partial mapping) on ​​the first pose graph for the first workspace where the mapping is implemented, and a search for a fourth node closest to the starting position of the partial mapping on the first pose graph for the first workspace where the mapping is implemented can be performed, and among the nodes formed on the first pose graph for the first workspace, the fourth node closest to the initially estimated starting position (start node) of the partial mapping can be searched based on the distance from the initially estimated starting position (start node) of the partial mapping. For example, in one embodiment of the present invention, the initial estimate of the starting position of the partial mapping may mean a position corrected from a position candidate for the starting position of the partial mapping, and may mean, for example, a position or a corrected pose based on observation from a sensor of the robot from the position candidate for the starting position of the partial mapping.

[0366] According to one embodiment of the present invention, a partial mapping can generate an edge or spatial constraint connecting the starting position (start node) of the partial mapping and the searched fourth node, and for example, the connection information or the information about the motion model between the starting position (start node) of the partial mapping and the fourth node on the pose graph for the first workspace does not include control or odometry information (absence of odometry information between the starting position of the partial mapping - the starting node - which is not the actual driving path of the robot - and the fourth node), but the starting position (start node) of the partial mapping and the fourth node include pose information about the relative position (e.g., two-axis translation) and relative rotation (e.g., rotation) between each other while generating nodes on the first pose graph for the first workspace (start position or start node of the partial mapping - initial estimate; fourth node - generated on the first pose graph for the first workspace), and accordingly, the movement between the starting position (start node) of the partial mapping and the fourth node is predicted from the observation model. Since observations can be predicted, and real observations according to movement between the starting position (start node) of the partial mapping and the fourth node can be obtained from real observations stored in association with the starting position (start node) of each partial mapping and the fourth node, pose graph optimization according to loop closure detection can also be possible for the loop connecting the starting position (start node) of the partial mapping and the fourth node.That is, the robot's actual driving path is not formed, and the initial estimated starting position (start node) of the partial mapping is a relative pose with respect to the landmark of the mapping with respect to the first workspace where the mapping is implemented, or an observation observed at the starting position of the partial mapping, etc., and the fourth node is generated on the pose graph with respect to the first workspace where the mapping is implemented, and the starting position (start node) of the partial mapping is initially estimated from the observation observed at the starting position of the partial mapping, etc., and the least square optimization is applied to the squared error (Mahalanobis distance with measurement uncertainty added to the squared error) between the predicted observation and the real observation according to loop closure detection for the edge or spatial constraint between the starting position (start node) of the partial mapping and the fourth node, so that the pose graph optimization (PGO) can be implemented.

[0367] For example, in partial mapping according to one embodiment of the present invention, a pose graph that integrally connects the first and second workspaces can be generated while advancing from a first workspace where mapping is implemented as a starting position (start node) of partial mapping toward a second workspace where mapping is not implemented, and for example, a pose graph generated through partial mapping can perform pose graph optimization (PGO) on a loop that includes an edge that connects the starting position (start node) of partial mapping and a fourth node according to loop closure detection while extending from the first workspace corresponding to the starting position (start node) of partial mapping toward the second workspace as a target of mapping and then returning toward the first workspace.

[0368] In pose graph optimization according to one embodiment of the present invention, a first pose graph for a first workspace and a second pose graph for a second workspace, which are generated by partial mapping according to one embodiment of the present invention, can be generated as a single integrated pose graph, and the integration of the first and second pose graphs can be performed, for example, by setting a start node for a start position of the partial mapping within a first workspace in which the first pose graph is previously generated, comparing an observation observed from the start position (start node) of the partial mapping with an observation associated with a node of the first pose graph for the first workspace, and performing an initial estimation for the start position (start node) of the partial mapping based on a matching rate, and in this way, the start node for the start position of the partial mapping can be generated from the initial estimation for the start position of the partial mapping. At this time, the initiation node regarding the initiation position of the partial mapping can be generated as a common node connecting the first pose graph regarding the first workspace and the second pose graph regarding the second workspace, and a connection between the initiation node and the first pose graph can be formed so as to incorporate the initiation node regarding the initiation position of the partial mapping into a node of the first pose graph, and as described above, a connection can be formed between the initiation node and the fourth node closest to the initiation node among the first pose graphs, and for example, by generating an edge connecting the initiation node and the fourth node generated at adjacent positions to each other, the accuracy of the position prediction of the relatively adjacent initiation node and the fourth node or the accuracy of the spatial information about the surrounding environment according to the overlapping or combination of observations based on the predicted positions can be improved from pose graph optimization for the edge connecting between the initiation node and the fourth node according to loop closure detection.

[0369] In various embodiments of the present invention, the connection between the starting node and the first pose graph with respect to the starting position of the partial mapping or the starting node of the partial mapping may be implemented by searching for the fourth node closest to the starting position or the starting node of the partial mapping and connecting the searched fourth node and the starting node, as described above, or by searching for the last node whose generation time is later over time on the starting position or the starting node of the partial mapping and the first pose graph and connecting the searched last node and the starting node. For example, in the embodiment of connecting the starting node and the nearest fourth node as described above, depending on the position of the nearest fourth node on the first pose graph, a group of nodes generated after the fourth node may be disconnected or isolated from the first pose graph, that is, the connection between the fourth node and the starting node may cause a group of nodes generated between the time of generation of the fourth node and the time of generation of the starting node to be disconnected or isolated from the first pose graph over time, for example, the fourth node on the first pose graph before the connection of the starting node may form a connection with another neighboring node generated after the fourth node, and the fourth node may have a double connection through the connection with the starting node, and at this time, a group of nodes generated after the fourth node may form an open branch without forming a closed loop on the pose graph according to loop closure detection on the pose graph developed after the starting node, and in this way, a group of nodes generated after the fourth node may be a disconnected node on the first pose graph or a node for which pose graph optimization cannot be implemented. An isolated node can be formed on the first pose graph.For example, in one embodiment of the present invention, a disconnection or isolation is formed on the pose graph according to a connection between the initiating node and the nearest fourth node, which may mean that the fourth node forms a double connection, thereby forming a node that is substantially disconnected from the first pose graph or an isolated node from the first pose graph on which pose graph optimization cannot be performed according to loop closure detection.

[0370] As described above, by connecting the initiating node regarding the initiating position of the partial mapping to the first pose graph, disconnection or isolation of some nodes on the first pose graph can be prevented by creating an edge connecting the initiating node and the last node whose creation time is latest on the first pose graph before the connection of the initiating node, for example, while the initiating node regarding the initiating position of the partial mapping forms a connection with the last node whose creation time is latest on the first pose graph, for example, a group of nodes created after the last node may not form a disconnected or isolated state in which pose graph optimization cannot be substantially implemented, and while the connection between the initiating node regarding the initiating position of the partial mapping from the last node on the first pose graph is made, while the initiating node creates the last node whose creation time is latest on the first pose graph to which the initiating node is connected, the second pose graph can be developed from the initiating node, and the first and second pose graphs can be implemented to create an integrated single pose graph.

[0371] In an embodiment of generating an edge connecting an initiating node and a fourth node closest to the initiating position of a partial mapping, the edge formed between the initiating node and the fourth node closest to the initiating position of a partial mapping does not include odometry information for tracking a driving path of a robot, but can predict a predicted observation between the initiating node and the fourth node from the pose of the robot with respect to the initiating node and the pose of the robot with respect to the fourth node, and in this way, pose graph optimization can be implemented through least square optimization for a Mahalanobis distance with added squared errors and measurement uncertainties between the predicted observation predicted between the initiating node and the fourth node and real observations stored in association with the initiating node and the fourth node.

[0372] Similarly, in an embodiment of generating an edge connecting an initiating node with respect to an initiating position of a partial mapping and a last node generated most recently based on a time of generation on a first pose graph prior to the connection of the initiating node, the edge formed between the initiating node with respect to the initiating position of the partial mapping and the last node generated most recently on the first pose graph prior to the connection of the initiating node does not include odometry information for tracking a robot's driving path, but can predict a predicted observation between the initiating node and the last node from the pose of the robot with respect to the initiating node and the pose of the robot with respect to the last node, and in this way, pose graph optimization can be implemented through least square optimization for a Mahalanobis distance with added squared errors and measurement uncertainties between predicted observations predicted between the initiating node and the last node and real observations stored in association with the initiating node and the last node.

[0373] In one embodiment of the present invention, a first pose graph for the first workspace and a second pose graph for the second workspace are generated according to an advance of a time step along a driving path of the robot, wherein a difference in a first generation time between adjacent nodes on the first pose graph before the partial mapping and a difference in a second generation time between a start node for the start position of the partial mapping and a last node on the first pose graph before the partial mapping can satisfy a relationship of difference in the second generation time > difference in the first generation time.

[0374] For example, partial mapping according to one embodiment of the present invention may be implemented in an environment where, after the generation of a first pose graph for a first workspace or the implementation of graph-based SLAM for the first workspace, additional generation of a second pose graph for a second workspace in addition to the previously generated first pose graph is required, with some lapse of time or some temporal separation therebetween, rather than implying the generation of a continuous pose graph for a first and second workspaces that are spatially connected to each other or the implementation of a continuous graph-based SLAM for the first and second workspaces that are spatially connected to each other, and may not imply, for example, the generation of a continuous pose graph for a first and second workspaces that are spatially connected to each other or the implementation of a continuous graph-based SLAM for the first and second workspaces that are spatially connected to each other.

[0375] More specifically, the difference in the second generation time between the last node with the latest generation time on the first pose graph generated before the initiation of the partial mapping and the generation time of the initiation node generated according to the initiation of the partial mapping may be greater than the difference in the first generation time between adjacent nodes on the first pose graph formed from the generation of the continuous pose graph, and for example, the difference in the second generation time between the initiation node with respect to the initiation position of the partial mapping and the last node on the first pose graph before the partial mapping may satisfy the relationship of difference in the second generation time > difference in the first generation time.

[0376] Pose Graph Visualization

[0377] Figure 18 shows a drawing showing an example of a robot's driving path.

[0378] FIG. 19 is a diagram showing an example of visualized information provided for a pose graph generated along the driving path illustrated in FIG. 18.

[0379] FIG. 20 is a diagram showing another example of visualized information provided for a pose graph generated along the driving path illustrated in FIG. 18, showing an example of representing loop closures as differential display elements from edges connecting neighboring nodes, together with node identification numbers that follow a chronological order based on the time of generation.

[0380] FIG. 21 is another example of visualized information provided for a pose graph generated along a driving path illustrated in FIG. 18, which also expresses the driving direction of the robot on an edge connecting neighboring nodes, and shows an example of expressing loop closure as a differential display element from the edge connecting neighboring nodes, and expressing nodes where loop closure detection has been performed and nodes where loop closure detection has not been performed but loop closure detection is required as differential display elements.

[0381] In graph-based SLAM according to one embodiment of the present invention, visualization of a pose graph can be implemented. For example, in the visualization of the pose graph according to one embodiment of the present invention, by providing visual information about the pose graph in which a new node is generated as the time step advances, in other words, by providing visual information about the pose graph generated through the previous steps, for example, by providing visual information about the pose graph including the edges connecting the nodes generated through the previous steps and the neighboring nodes, the robot operator can recognize a motion constraint that restricts the motion of the robot, a problem in the control of the robot, or a problem in the driving environment of the robot, and can recognize a problem in which pose graph optimization is delayed or hindered, such as, for example, when the robot's driving is concentrated in a certain workspace, a loop closure detection is not performed or a sufficient number of loop closure detections are not performed, and for example, when the robot's driving is concentrated in a certain section of the workspace, the robot operator can recognize, from the visual information about the pose graph, that a relatively large number of nodes are concentratedly generated in a certain section of the workspace in which the robot's driving is concentrated, but a sufficient number of loops are not generated despite the large number of nodes generated. In other words, despite the large number of nodes generated, a sufficient number of loops are not generated. It can be confirmed that loop closure detection has not been performed, and based on this recognition, it can provide an opportunity to recognize environments where pose graph optimization is delayed or hindered, so that problems such as motion constraints that limit the motion of the robot, problems in the robot control, and problems in the robot's driving environment can be checked and resolved.For example, in one embodiment of the present invention, the presence of an obstacle on a driving path of the robot that may delay or hinder loop closure detection on the driving path of the robot as a driving environment of the robot (for example, the presence of an obstacle that is not easy to avoid due to a motion constraint that restricts the motion of the robot) can be confirmed, and loop closure detection can be promoted by removing such obstacles, and a global map can be formed by matching observations from each node through optimization for all nodes forming a loop from pose graph optimization.

[0382] Visualization of a pose graph according to one embodiment of the present invention may include generating a pose graph regarding a workspace as visual information and displaying it as a user interface. In various embodiments of the present invention, in addition to visualization of the pose graph, it may comprehensively mean various forms of user interfaces, such as any type of notification or message, that can provide awareness of an environment in which pose graph optimization is delayed or hindered from loop closure detection as described above. In one embodiment of the present invention, by comparing the number of nodes generated on the pose graph with the maximum number of critical nodes that the loop generated from loop closure detection can accommodate, it is recognized that a loop is not generated according to loop detection despite generation of a sufficient number of nodes or that a relatively small number of loops are generated, so that there is a problem that delays or hinders pose graph optimization, and a notification or the like can be provided as a user interface.

[0383] For example, in one embodiment of the present invention, i) no loop closure is generated according to loop closure detection for the pose graph for which the visualized information is provided, and ii) a notification for a delay environment or an obstruction environment for pose graph optimization can be provided from a comparison of the range of a work area for the pose graph for which the visualized information is provided and a threshold range (e.g., a radius of 10 m) set in advance to exceed an average range for the work area for which the loop closure detection is performed.

[0384] In addition, in one embodiment of the present invention, i) no loop closure is generated according to loop closure detection for the pose graph for which the visualized information is provided, and iii) a notification about a delay environment or an obstruction environment for pose graph optimization can be provided from a size comparison of the number of nodes generated on the pose graph for which the visualized information is provided and the maximum number of critical nodes (e.g., 20) that a loop on the pose graph for which the visualized information is provided can accommodate.

[0385] In one embodiment of the present invention, in the average pose graph optimization, the number of nodes forming a loop on the pose graph can be counted from loop closure detection, and according to the statistics collected from the average pose graph optimization, for example, considering that the number of nodes forming a loop on the pose graph is less than a second number (e.g., 20) or considering a maximum acceptable second number (e.g., 20) on the loop generated according to the loop detection, the second number regarding the critical number of nodes can be compared with the total number of nodes forming the pose graph for which visualization information is provided, and, for example, from the count regarding the total number of nodes generated on the pose graph for which visualization information is provided (e.g., the total number of nodes in which a loop has not yet been formed on the pose graph according to the loop closure detection), a user interface such as a notification requesting an inspection for an environment that delays or impedes the pose graph optimization can be provided.

[0386] For example, in the visualization of a pose graph according to one embodiment of the present invention, a pose graph including edges where odometry is observed along the actual driving path of the robot can be provided, and loop closures generated according to loop closure detection that do not form the actual driving path of the robot and thus do not observe odometry can also be provided as visualization information about the pose graph on the user interface. For example, information about a loop formed on the pose graph according to loop closure detection is important information indicating that optimization (pose graph optimization) has been performed on all nodes on these loops, and can be provided as visualization information about the pose graph on the user interface, and can be expressed as different display elements (e.g., edges or arrows of different colors) so as to be distinguished from edges corresponding to the actual driving path of the robot. In this way, a user or an operator of the robot who recognizes a loop closure can recognize that pose graph optimization has been performed on nodes on a loop closed from the loop closure. For example, a loop closure displayed on a pose graph provided with the above visualized information may indicate that pose graph optimization (PGO) has been implemented or is to be implemented for nodes generated on a closed loop from the displayed loop closure.

[0387] For example, in one embodiment of the present invention, on the pose graph provided as the visualized information, the loop closure may be highlighted with a line of a different color from the edge, or may be highlighted with an arrow different from the line indicating the edge.

[0388] In the visualization of a pose graph according to one embodiment of the present invention, as visualized information about nodes on a pose graph generated according to the advancement of time steps along a driving path of a robot, a node identification number following a chronological order based on the generation time can be displayed together with each node, and thus, information about a driving history that follows the driving direction of the robot can be provided based on the generation time of the node on the pose graph, and for example, the last node with the latest generation time can be identified based on the generation time, and a reference can be provided for planning a future driving path of the robot, and for example, in order to facilitate the generation of a loop closure according to loop closure detection, a driving path can be planned from a position adjacent to the last node with the latest generation time (the last node with the latest generation time on the pose graph) toward a node for which a loop closure has not yet been generated (for example, the last node with the latest generation time among the nodes for which a loop closure has not yet been generated - the last node for which a loop closure has not yet been generated), as described below.

[0389] In the visualization of a pose graph according to one embodiment of the present invention, visualized information about nodes on a pose graph generated according to the advancement of a time step along a driving path of a robot and edges connecting neighboring nodes can be provided, and as major information about the pose graph, visualized information about loop closure according to loop closure detection can be provided, and in various embodiments of the present invention, various information about loop closure on a pose graph for which visualized information is provided can be provided as follows.

[0390] 1) Visualized information about loop closures generated according to loop closure detection, with the latest loop closure generated over time and the remaining loop closures excluding the last loop closure displayed as differential display elements.

[0391] For example, for multiple loop closures generated on a pose graph, by displaying the most recently generated loop closure and the remaining loop closures with different display elements (e.g., display elements of different colors) based on the generation time, it is possible to provide an opportunity to review the most recent robot driving path related to the most recently generated last loop closure.

[0392] 2) Visualized information about loop closure generated according to loop closure detection, where nodes in the pose graph that require loop closure but have not yet been loop closed are displayed with different display elements from other nodes in the pose graph that have already been loop closed.

[0393] For example, by classifying a number of nodes generated on a pose graph into two classes: nodes that have already undergone loop closure and nodes that have not yet undergone loop closure, the nodes that form a closed loop because the loop closure has already occurred and the nodes that form an open loop because the loop closure has not yet occurred can be displayed with different display elements, for example, they can be displayed with different colored display elements, and by providing visualization information that highlights the visibility of nodes that require loop closure, for example, nodes that require pose graph optimization according to loop closure detection, with different display elements, in the future, in planning the robot's driving path, by planning the robot's driving path toward the nodes that require loop closure (nodes that have not yet undergone loop closure), loop closure detection can be induced for nodes that have not yet undergone loop closure, and optimization for all nodes generated on the pose graph can be promoted.

[0394] 3) Visualized information on loop closure generated according to loop closure detection, where among the nodes in the pose graph that require loop closure and for which loop closure has not yet occurred, the last node with the lowest generation time is displayed as a differential display element for the remaining nodes, excluding the last node, compared to nodes for which loop closure has not yet occurred and nodes for which loop closure has already occurred.

[0395] In one embodiment of the present invention, a plurality of nodes generated on a pose graph are classified into two categories: nodes in which loop closure has already occurred and nodes in which loop closure has not yet occurred, and nodes forming a closed loop due to loop closure and nodes forming an open loop due to loop closure not yet occurring can be displayed with different display elements. In addition, among the nodes in which loop closure has not yet occurred, visualization information can be provided in which visibility of the last node, which is generated the latest, is highlighted with a different display element from that of other nodes except for the last node. For example, in an embodiment providing visualized information about nodes on a pose graph, the nodes for which visualized information is provided are classified into three categories: nodes that have already undergone loop closure, nodes that have not yet undergone loop closure but are the latest node created, and nodes that have not yet undergone loop closure but are the latest node created, and nodes of different categories can be displayed with different display elements, and in providing visualized information about nodes of different categories in this way, visualized information about nodes of different categories can be provided with visibility levels in the following order.

[0396] The last node with the latest creation time among the nodes without loop closure > Nodes excluding the last node with the latest creation time among the nodes without loop closure > Nodes with loop closure.

[0397] For example, the visibility level may be given by a color display element that can be sensitively captured by the naked eye for each category node (red color > blue color > black color) or a set of relatively large numbers of pixels (e.g., displayed relatively large or thick).

[0398] The pose graph visualization according to this embodiment can provide a reference for the future driving path of the robot, and by inducing the robot to plan the driving path toward the last node whose creation time is the latest among the nodes where the loop closure has not yet been achieved, loop closure detection can be induced for nodes where the loop closure has not yet been achieved, and pose graph optimization for all nodes created on the pose graph can be promoted.

[0399] <observation 편집>

[0400] FIG. 22a is a drawing for explaining an embodiment in which the spatial conditions of a third work area set as a first distance scale from an editing reference node are used as editing start conditions, and FIG. 22b is a drawing exemplarily showing a local map generated according to the overlapping or combination of observations observed from an editing target node.

[0401] Figure 23 illustrates a diagram exemplarily showing observations observed from each of the editing target nodes illustrated in Figure 22a.

[0402] FIG. 24(a) illustrates an example of a local map generated according to the overlapping or combination of observations observed from each of the editing target nodes illustrated in FIG. 23, and FIG. 24(b) illustrates an example of a mismatch (an editing target or deletion target on the local map that is not observed in the observation of the editing target node) with the observation observed from the editing reference node on the local map illustrated in FIG. 24(a).

[0403] Figure 25 illustrates an example of an editing range set according to a second distance scale from an editing reference node on the local map of Figure 24(b).

[0404] FIG. 26 illustrates an editing restriction for an observation linked to an editing target node that is blocked by an obstacle between the editing target node and the editing reference node, based on the observation of the editing reference node, and on the left is a drawing showing an obstacle that exists between the editing reference node and the observation of the editing target node, and on the right is an enlarged drawing of the left side, and is a drawing for explaining ray tracing for capturing the obstacle as described above.

[0405] Figures 27(a) to 27(c) illustrate drawings for explaining deletion within the editing range set on the observation of the editing target node and maintenance outside the editing range set on the observation of the editing target node by applying the editing range set on the local map as in Figure 25 to the observation of the editing target node.

[0406] Figure 28 illustrates a diagram for explaining that a portion of a local map that is outside the editing range is maintained on a local map that overlaps or combines each observation of an editing target node.

[0407] In one embodiment of the present invention, for each node forming a pose graph and associated observation (or local map), a matching rate between observations associated with different nodes that are spatially adjacent to each other and generated with a time gap within a third work area is calculated, and among the observations associated with different nodes for which a relatively low matching rate is calculated, an update or edit can be implemented based on an observation associated with a node that is generated relatively early (a node generated a long time ago) and an observation associated with a node that is generated late (a node generated recently close to the current time).

[0408] In one embodiment of the present invention, while additionally generating nodes on a pose graph according to an advancement of a time step, between different nodes (recently generated nodes and previously generated nodes) having a time gap, according to the difference in creation time between the different nodes, and according to the difference in creation time, as a change in the surrounding environment surrounding the robot, for example, a change in the dynamic surrounding environment or a static surrounding environment with relatively less environmental change, based on the observation observed from the recently generated node, the observation observed from the nodes generated a long time ago (previously generated nodes) that are spatially adjacent to the recently generated node within a preset third work area can be updated. For example, by updating observations (or local maps) generated long ago based on recently generated nodes linked to updated observations (or local maps) based on the time gap between recently generated nodes and previously generated nodes with a time gap between them, the entire global map based on observations from each node forming the pose graph can be updated, and distortion of the global map due to mismatch between observations of recently generated nodes and observations of previously generated nodes can be resolved.

[0409] With respect to the above observation or local map editing conditions, the recently created node and the previously created node are created within a third work area as a pre-set spatial range, and the matching rate of the observation observed from the recently created node and the previously created node is calculated to be relatively low, and thus, the editing of the observation or local map can be initiated. For example, in various embodiments of the present invention, the time gap between the recently created node and the previously created node can also be pre-set. For example, as the observation or local map editing initiation condition, a temporal condition (a time gap between a recently created node that provides a criterion for editing and a previously created node that is the target of editing), a spatial condition (a third work area as a spatial range between a recently created node that provides a criterion for editing and a previously created node that is the target of editing), and the matching rate of the observation or local map between different nodes between the recently created node that provides a criterion for editing and the previously created node that is the target of editing can be set as the observation or local map editing initiation condition according to one embodiment of the present invention.

[0410] In one embodiment of the present invention, the matching rate between a recently generated observation (observation from a recently generated node) and a previously generated observation (observation from a previously generated node) is a function of the relative translational components (e.g., two-axis translation) and rotational components (e.g., rotation) for matching between a group of point clouds observed from a lidar sensor as a recently generated observation and another group of point clouds observed from a lidar sensor as a previously generated observation, from an ICP (iterative closet point) algorithm, and the matching rate between the group of point clouds and the other group of point clouds can be estimated from a rigid transformation to which these translational components and rotational components are applied, and, for example, through a rigid transformation expressed as a transformation matrix (transformation matrix or homogeneous matrix) having translational components and rotational components, for example, a plurality of editing candidate nodes (previously generated nodes) that satisfy a spatial reference (third work area) and a temporal condition corresponding to an editing start condition, a recently generated node and a plurality of editing candidates can be matched. For the observation of two different nodes of one node among the nodes (previously created nodes), the matching rate can be sequentially calculated one-to-one, and editing can be initiated for the observation or local map of a previously created node for which the matching rate is lower than a preset threshold matching rate.For example, in one embodiment of the present invention, in the editing of observations or local maps of previously generated nodes, unlike node optimization that involves the complete deletion of observations of previously generated nodes (e.g., deleting all observations associated with the previously generated first node that satisfies the initiation condition of node optimization), deletion of some observations of previously generated nodes or addition of observations of previously generated nodes is possible. In this sense, editing of observations of previously generated nodes in one embodiment of the present invention may not mean the complete deletion of observations of previously generated nodes or the complete replacement of observations of previously generated nodes. For example, observations of previously generated nodes and observations of recently generated nodes may capture different landmarks, and thus, deleting or replacing observations that capture different landmarks can prevent the accuracy of pose graph optimization from deteriorating due to editing of these observations (e.g., real observations rather than predicted observations in pose graph optimization initiated by loop closure detection).

[0411] In various embodiments of the present invention, the matching rate of observations observed from previously generated nodes and recently generated nodes adjacent to each other within a third workspace may be set to a range higher than a first threshold matching rate but lower than a second threshold matching rate, which may be a condition for initiating editing. For example, the first threshold matching rate may provide a criterion for determining that observations observed from previously generated nodes and observations observed from recently generated nodes belong to different sections of the workspace. For example, even if previously generated nodes and recently generated nodes are spatially adjacent to each other within the third workspace, if the nodes are linked to observations belonging to different sections of the workspace, the first threshold matching rate may be set so that the observations for these different sections are maintained as they are. The second threshold matching rate may be higher than this because a matching rate between previously generated nodes and recently generated nodes can be regarded as substantially maintaining the surrounding environment surrounding the robot as it is, and therefore, it may be determined that there is no need to edit or update the previously generated observation based on the recently generated observation.

[0412] In various embodiments of the present invention, the editing initiation condition may also include the presence or absence of an obstacle between a recently created node and a previously created node. For example, in one embodiment of the present invention, even if the matching rate of observations between a previously created node and a recently created node is low, the presence of an obstacle between a previously created node and a recently created node means that the previously created node and the recently created node belong to different sections in the robot's workspace, and therefore, it is desirable to independently maintain previously created observations and recently created observations that capture different sections, and it may not be desirable to edit one of the different observations regarding different sections based on the other. For example, observations from each node forming the pose graph can be matched or collected to create a global map, and for path planning of the robot, for example, for path planning for the robot to drive from the start position of the robot to the target position, an obstacle occupying space on the global map can be represented by bit 1 (grid cell is filled), and an empty space can be represented by bit 0 (grid cell is empty), and the presence of a filled cell (obstacle, occupied cell) between a previously created node and a recently created node on an occupancy grid expressed as filled and empty cells depending on the presence of an object can mean the presence of an obstacle between these previously created nodes and recently created nodes, and by recognizing observations for different sections of the workspace, editing one of the different observations regarding the different sections based on the other may not be allowed (edit initiation condition is not satisfied).In various embodiments of the present invention, the presence of an obstacle between a previously created node and a recently created node may be determined on the occupancy grid as described above, or may be determined based on observations observed from sensors of the robot, for example, ray tracing may be applied to a previously created node or a recently created node according to ray tracing from a recently created node, and for example, a point regarding an obstacle may be observed from an outgoing ray from a recently created node toward a previously created node, and the presence of an obstacle may be recognized by searching for adjacent points from the captured point.

[0413] In one embodiment of the present invention, while generating nodes of a pose graph according to the advancement of a time step along a driving path of a robot, a local map can be generated from observations associated with other nodes (corresponding to nodes with a later generation time, the editing target node) within a third work area, centered around the generated nodes (corresponding to recently generated nodes close to the current time point, the editing reference node). For example, the local map can be generated by combining observations associated with each node according to the pose of the robot with respect to other nodes (e.g., the editing target node) within the third work area along the driving path of the robot. In one embodiment of the present invention, a local map is generated from observations associated with other nodes (the editing target node) within the third work area, centered around a node (the editing reference node) at or close to the current time point along the driving path of the robot, and an editing range of observations associated with other nodes within the third work area can be set by comparing the local map generated from observations associated with other nodes (the editing target node) within the third work area with observations observed from the current time point or a recently generated node (the editing reference node) close to the current time point.

[0414] In various embodiments of the present invention, the observations between a recently created node (edit reference node) or a node close to the current time point in time and a previously created node (edit target node) may be compared one-to-one, for example, through the ICP algorithm, to set the editing range of the observations associated with the edit target node by comparing the observations associated with the edit reference node and the observations associated with the edit target node one-to-one, or, as described above, a local map may be generated from a group of other nodes (a group of edit target nodes) within the third work area centered on the recently created node, and the local map generated from the group of edit target nodes in this way may be compared with the observations associated with the edit reference node, to set the editing range on the local map generated from the group of edit target nodes, and the editing range set on the local map may be applied to each observation associated with the group of edit target nodes.

[0415] For example, in an embodiment in which an editing range is set on the observation of an editing target node by comparing an observation linked to the above-mentioned editing reference node and an observation linked to one editing target node on a one-to-one basis, since the editing range is set based on an observation linked to one editing target node, there is a possibility that an error in the editing range of the observation linked to the editing target node may occur due to the reliability of the observation or the uncertainty of the measurement of the robot's sensor, and for example, when an observation observing the surrounding environment from the editing target node is captured, noise may be generated in the observation from the editing target node as the robot shakes according to the driving environment or the robot's sensor shakes due to an obstacle avoidance or collision avoidance maneuver. In this way, setting the editing range on the observation of the editing target node by a one-to-one comparison of the observation of the editing target node containing noise and the observation of the editing reference node may be a possibility that an error in the editing range may occur. For example, confusion may occur between an observation part that needs to be deleted and an observation part that needs to be maintained among the observations of the editing target node. For example, a landmark or obstacle in the surrounding environment observed at a previous point in time may be removed at a recent point in time. The observation part of the edit target node that needs to be deleted,There is a possibility that an error in the editing scope may have occurred due to confusion about the observation part of the target node to be edited that needs to be maintained as an observation of landmarks or obstacles in the surrounding environment observed at both the previous and the latest time points, while the observation part of the target node to be edited that needs to be deleted is maintained, while the observation part of the target node to be edited that needs to be maintained is deleted. For example, in a one-to-one matching between a group of point clouds as observations of an editing reference node and another group of point clouds as observations of an editing target node, the ICP algorithm can be applied. However, in the absence of association information between a group of point clouds of an editing reference node and another group of point clouds of an editing target node, and between a group of point clouds and another group of point clouds that may contain noise, comparing the observations of the editing reference node and the observations of the editing target node from multiple iterations according to the ICP algorithm to set the editing range from the observations of the editing target node may cause errors in setting the editing range despite the relatively high computational cost of the ICP algorithm requiring multiple iterations. Therefore, in various embodiments of the present invention, as described below, rather than setting the editing range from the observations of one editing target node, a local map is generated by linking the observations of a group of editing target nodes belonging to a third work area centered on an editing reference node generated at a recent point in time with the poses of each editing target node according to the driving path of the robot, and by setting the editing range from the local map thus generated, errors in the editing range can be reduced.

[0416] That is, in various embodiments of the present invention, rather than setting the editing range from the observation image of a single editing target node, a local map is created from a group of editing target nodes created at a previous time point within a third work area centered on an editing reference node created at a recent time point, and the editing range is set from the created local map, and by applying the editing range set on the local map in this way to the observation of each of the editing target nodes in the group, for example, the influence of noise that may exist in the observation of each of the editing target nodes in the group can be canceled out from the local map that overlaps or combines the observations of the group of editing target nodes, and for example, the observation of the surrounding environment can be expanded to the object unit or object scale from the local map that overlaps or combines the observations of the group of editing target nodes, thereby reducing the sensitivity of each point of a group of point clouds forming the observation of each of the editing target nodes in the group and eliminating the influence of noise.

[0417] In various embodiments of the present invention, that is, as described above, the editing range of an observation associated with an editing target node is set from an observation associated with each editing target node, or from a local map that overlaps or combines observations of a group of editing target nodes. In various embodiments of the present invention, the editing range may mean a range to be deleted from the observation of the editing target node or added to the observation of the editing target node, and this editing range may be limited by a distance measure called a radius range centered on each editing reference node. For example, in various embodiments of the present invention, in selecting an editing target node, the editing target node may be searched for within the third work area as a distance measure of a radius range centered on the editing reference node, and in editing the observation of the editing target node, the distance measure of a radius range centered on the editing reference node may be set. For example, a third work area for selecting a letter target node may be applied with a first distance measure, which is a radius range centered on an editing reference node, and an editing range for observation of the editing target node may be applied with a second distance measure, which is a radius range centered on an editing reference node. The first distance measure for selecting the editing target node (corresponding to the third work area) and the second distance measure for the editing range for observation of the editing target node may be set differently or identically.

[0418] More specifically, the setting of the editing range or the limitation of the editing range according to various embodiments of the present invention can be exemplified as follows.

[0419] 1) Radial range centered on the editing reference node (second distance measure)

[0420] 2) Observation of an edit target node blocked by an obstacle between it and the edit reference node.

[0421] The setting of the editing range or the limitation of the editing range of the above 1) may be done by differentially evaluating the reliability of observations observed close to the editing reference node and the reliability of observations observed far away, so that, for example, for observations observed close to within the second distance scale, an editing standard may be provided for observations of editing target nodes within the third work area set as the first distance scale, but for observations observed far away beyond the second distance scale, an editing standard may not be provided for observations of editing target nodes within the third work area set as the first distance scale. For example, when setting an editing range on a local map generated according to the overlapping or combination of observations of a group of editing target nodes belonging to the third work area, a second distance scale centered on the editing reference node may be applied to set an editing range within the second distance scale. For example, editing may be allowed for objects on the local map within the editing range within the second distance scale, but editing may not be allowed for objects on the local map outside the second distance scale.

[0422] The setting of the editing range or the limitation of the editing range in 2) above may be to prevent an observation that cannot be observed because it is blocked by an obstacle from the editing reference node from being deleted from the observation of the editing target node as a mismatch that does not exist in the observation of the editing reference node. For example, even if an observation that cannot be observed because it is blocked by an obstacle from the editing reference node can be observed from, for example, an editing target node of a pose different from the pose of the editing reference node, and in this case, even if a mismatch exists between the observation of the editing reference node and the observation of the editing target node, if an observation that can be observed from an editing target node of a pose different from the editing reference node is deleted based on the observation of the editing reference node that cannot be observed due to the presence of an obstacle, spatial information about the surrounding environment is lost, so the setting of the editing range or the limitation of the editing range as in 2) may be applied.

[0423] In various embodiments of the present invention, the setting of the editing range or the limitation of the editing range in 2) may be applied depending on whether a group of point clouds regarding obstacles exist between the observation of the editing target node according to ray tracing from the editing reference node, and for example, when an object on a local map generated by overlapping or combining observations of a group of editing target nodes does not exist on the observation of the editing reference node, but a group of point clouds that can be recognized as obstacles exist between the editing reference node and the objects on the local map, the objects on the local map may be excluded from the editing range. For example, in applying ray tracing from the editing reference node, a point regarding an obstacle is captured from an outgoing ray from the editing reference node toward an object on the local map, and the presence of the obstacle can be recognized by searching for adjacent points from the captured point, and accordingly, the setting of the editing range or the limitation of the editing range in 2) may be applied. In one embodiment of the present invention, the detection of the presence or absence of an obstacle may be performed by searching a group of point clouds in the vicinity rather than by capturing a single point by ray tracing from an editing reference node. For example, when a single point is captured from an outgoing ray directed toward an object on a local map that overlaps or combines observations of a group of editing target nodes from an editing reference node, it may be detected whether a group of point clouds containing other points are captured around the captured point. Taking into account the possibility of noise, when a group of point clouds containing a large number of points are searched, the corresponding point cloud may be recognized as an obstacle, and the editing range setting or editing range restriction of 2) above may be applied.

[0424] Landmark

[0425] FIG. 29 illustrates a drawing showing an Aruco marker as an example of a positioning code or positioning marker that is visually observed from a sensor of the robot as a landmark installed around the robot's driving path.

[0426] FIG. 30 illustrates a drawing showing April-tag as another example of a positioning code or positioning marker visually observed from a robot's sensor as a landmark installed around the robot's driving path.

[0427] FIG. 31 illustrates a drawing showing a QR marker as another example of a positioning code or positioning marker that is visually observed from a sensor of the robot as a landmark installed around the periphery of the robot's driving path.

[0428] FIG. 32 illustrates a drawing for explaining positioning codes or positioning markers of different identification marks recognized within a landmark detection distance centered on the position of the robot along the robot's driving path.

[0429] On the left side of Fig. 33, a drawing is shown to explain how a landmark node is generated for a pose of a landmark based on the accumulation of multiple observations of the same landmark from a robot along the robot's driving path as a generation condition, and on the right side of Fig. 33, a drawing is shown to explain how a mean and covariance are extracted from a cluster of data with a relatively high density from clustering (DBSCAN) for multiple data predicted as a pose of a landmark from an observation model regarding the robot's observation and the pose of the robot that observed each of the multiple accumulated observations, and data scattered with a relatively low density are removed as noise or outliers.

[0430] On the left side of Fig. 34, there is shown a drawing exemplarily showing one form of a pose graph that does not include a landmark node, and on the right side of Fig. 34, there is shown a drawing exemplarily showing one form of a pose graph that includes a node related to a pose of a robot that follows a driving path of the robot, and to which a landmark node and an observation node related to a pose of a robot that observes the landmark node are added, and node identification numbers with different signs (+ / -) are assigned to the landmark node related to the pose of the landmark and the node related to the pose of the robot.

[0431] In one embodiment of the present invention, the sensors of the robot may include a lidar sensor that generates a point cloud observation as an observation of the surrounding environment or a landmark, and a vision sensor that generates image information (image frames) as an observation of the surrounding environment or a landmark. In one embodiment of the present invention, in localization for inferring the pose of the robot, a prediction is performed from a motion model and odometry along the driving path of the robot from sensors of different types of robots, and after the prediction, different observations can be generated from observations of different types of sensors, and a correction can be performed from different observations observed from different steps or different poses of the robots. For example, in the correction, the relative translation (two-axis translation) and rotation of the robot can be inferred from observations observed from different poses of the robots, and for example, a probability distribution regarding the pose of the robot predicted in the prediction can be corrected from odometry.

[0432] In one embodiment of the present invention, a positioning code or a positioning marker may be included as a landmark visually observed from a sensor of the robot, and for example, a relative position (two-axis translation) and / or posture (rotation) with respect to the robot may be captured from the positioning code or the positioning marker. For example, the positioning marker or the positioning marker may include a QR code or an April Tag, and in one embodiment of the present invention, in localization for inferring the pose of the robot, a relative translation and relative rotation between poses of different robots at different time steps may be derived from an observation that captures a positioning code or a positioning marker as an observation observed from a vision sensor of the robot. More specifically, in one embodiment of the present invention, in localization for inferring a pose of a robot, observation of a positioning code or a positioning marker installed on the workspace of the robot as a surrounding environment or a landmark surrounding the robot (observation from a vision sensor that observes the positioning code or the positioning marker) can provide information about a relative position or relative posture between the positioning code or the positioning marker and the robot, and the relative position (e.g., two-axis translation) and / or relative posture (e.g., rotation) of the robot observed from the positioning code or the positioning mark as a static surrounding environment or a static landmark surrounding the robot can provide information about the relative position and relative posture of the robot taken at different time steps along the driving path of the robot, and for example, the relative position (e.g., two-axis translation) and rotation (e.g., rotation) can be predicted between poses of different robots represented by different nodes on a pose graph.For example, in localization for inferring the pose of a robot in one embodiment of the present invention, along the driving path of the robot, the pose of the robot in the next time step (prediction probability distribution) can be predicted from the pose of the robot in the previous time step (posterior probability distribution in the previous time step) from the control or odometry together with the motion model, and together with the observation model, the relative positions and relative attitudes between the poses of the robot taken at different time steps can be predicted from the observations in which the positioning codes or positioning markers observed from the vision sensor are observed, and the prediction probability distribution can be corrected to the posterior probability distribution from the observations observed from the vision sensor of the robot (correction). For example, in one embodiment of the present invention, the positioning codes or positioning markers as described above may be observed consecutively at neighboring nodes along the driving path of the robot, or may be observed at nodes separated from each other with different nodes in between along the driving path of the robot.For example, in localization for inferring the pose of a robot, positioning codes or positioning markers observed from nodes that are spaced apart from each other with different nodes in between can provide observations about the relative positions and / or relative poses between the spaced apart nodes (poses of the robot), and for example, different nodes connected by observations about their relative positions or poses from observations of positioning codes or positioning markers do not contain odometry information along the robot's driving path (observations about their relative positions or poses can be connected from observations of positioning codes or positioning markers, but these two nodes that are spaced apart from each other do not contain odometry information according to the robot's driving), but two nodes that are spaced apart from each other can contain information about their relative positions and poses while creating nodes on the pose graph, and accordingly, predicted observations according to the movement between the spaced apart nodes connected by observations of positioning codes or positioning markers from the observation model can be predicted, and positioning codes or positioning markers that connect the spaced apart nodes can be used to predict the predicted observations. Since observations regarding positioning markers are considered real observations, the pose of the robot can be inferred to reduce the error between the predicted observation and the real observation.

[0433] For example, in localization for inferring the pose of a robot as described above, a relative pose of the robot can be inferred between two nodes that are distant from each other and do not include odometry information along the robot's driving path, but are connected through observations of the positioning code or positioning marker, including the relative position and relative posture.

[0434] For example, in localization for inferring the pose of a robot, observations observed from two different nodes along the driving path of the robot may include observations of a point cloud observed from a lidar sensor of the robot and observations of a positioning code or a positioning marker observed from a vision sensor of the robot, which may form a real observation between the two nodes, and for example, the pose of the robot may be inferred from the relative positions and relative poses of the two nodes and the predicted observation predicted from an observation model and the real observation from the lidar sensor and the vision sensor, so as to reduce the error between these predicted observations and the real observation from the lidar sensor and the vision sensor.

[0435] In localization according to one embodiment of the present invention, at least one of a relative position and a relative posture of the robot can be inferred from different observations of the positioning code or the positioning marker visually observed from a sensor of the robot at first and second poses of different robots along the transport path of the robot, as a positioning code or a positioning marker installed in the vicinity of the robot, and in the localization, between a first image frame as an observation of the positioning code or the positioning marker observed from the first node and a second image frame as an observation of the positioning code or the positioning marker observed from the second node, at least one of a relative translational component and a relative rotational component can be calculated as a relative rigid body transformation for mutual matching of the first and second image frames.

[0436] In the application of a visually observed positioning code or positioning marker (hereinafter referred to as a landmark) as a landmark according to one embodiment of the present invention, the process for observing the landmark may be different from that in SLAM (graph-based SLAM) for simultaneously implementing mapping for inferring spatial information about the surrounding environment and localization for inferring the pose of the robot, and in localization for inferring the pose of the robot based on spatial information about the surrounding environment that has already been generated.

[0437] For example, in mapping to generate spatial information about the surrounding environment, such as in SLAM, landmark nodes can be generated on a pose graph from observations of landmarks observed from the robot, and for example, in the mapping, the pose of the robot may or may not be predicted or corrected from the landmark. For example, in one embodiment of the present invention, if observations of the same landmark (e.g., landmarks with the same identification mark) are captured a certain number of times or more, a node (landmark node) related to the pose of the landmark can be generated on a pose graph that unfolds along the robot's driving path. If a landmark node is generated under the condition that observations of the same landmark are captured a plurality of times, for example, if observations of the same landmark are captured a single time or a number of times less than a threshold number of times, it may be difficult to accurately predict the pose of the landmark from a relatively small number of observations. In addition, if localization is performed to predict or correct the pose of the robot by referring to the pose of the landmark that has been spatially informatized about the surrounding environment through mapping, the accuracy of prediction of the pose of the robot may decrease from the pose of the inaccurately predicted landmark. In addition, in pose graph optimization starting from loop closure detection, pose graph optimization can also be performed on the edge that connects the landmark node related to the pose of the landmark and the node related to the pose of the robot. At this time, the edge that connects the nodes (nodes related to the pose of the robot) generated on the pose graph and Together, if we perform least square optimization on the edges connecting the nodes regarding the robot's pose and the landmark nodes regarding the landmark's pose,In particular, when the accuracy of the landmark node is relatively low, errors can occur not only between the landmark node for the landmark's pose and the node for the robot's pose, but also between the nodes generated on the pose graph (nodes for the robot's pose). Therefore, rather than generating inaccurate landmark nodes, for example, landmark nodes for the pose of the corresponding landmark may not be generated from observations for the same landmark that have not been observed more than a preset threshold number of times. For example, while odometry information exists between nodes generated along the robot's driving path, odometry information may be absent between landmark nodes that do not form the robot's driving path and nodes for the pose of the robot that observed the landmark. In order to infer the pose of a landmark based on observations of the landmark from the robot, as explained above, it may be necessary to accumulate observations for the same landmark more than a certain number of times.

[0438] In one embodiment of the present invention, in mapping for inferring spatial information about the surrounding environment and localization for inferring the pose of the robot, different processes may be performed for observations that observe landmarks. For example, in the mapping, a landmark node for the pose of the landmark may be generated on a pose graph from an observation that observes the landmark, and in the localization, the pose of the robot may be predicted on spatial information about the surrounding environment including the landmark. For example, the pose of the robot may be predicted (prediction) from an observation that observes the landmark, or the predicted pose of the robot may be corrected (correction).

[0439] In one embodiment of the present invention, in mapping for generating spatial information about the surrounding environment, the same landmark needs to be observed multiple times along the robot's driving path or across different poses of the robot along the robot's driving path, and for the same landmark observed multiple times along the robot's driving path or across different poses of the robot along the robot's driving path, a landmark node for the pose of the corresponding landmark can be generated under an evaluation that the corresponding landmark is relatively robust. For example, in one embodiment of the present invention, the pose of the landmark can be predicted from multiple observations of the same landmark from different robots' poses, and for example, the pose of the landmark can be predicted from the poses of different robots that observed the same landmark and an observation model of the robot, and clustering can be performed using density-based DBSCAN with the data distribution of the pose of the landmark as input, and for example, the mean and covariance of the pose of the landmark can be extracted from a cluster of data with a relatively high density, and data with a relatively low density and sparse distribution can be removed as noise or outliers. For example, in one embodiment of the present invention, among a plurality of data predicted as the pose of a landmark, a threshold of positional deviation and a threshold of angular deviation can be applied, so that data exceeding the threshold of positional deviation and the threshold of angular deviation can be excluded as noise or outliers.For example, in one embodiment of the present invention, clustering is performed on a plurality of data predicted about the pose of a corresponding landmark from different observations of the same landmark from different poses of different robots, noise or outliers are removed based on the density of the data, the mean and covariance are extracted from a cluster of data with a relatively high density, and a landmark node about the pose of the landmark can be generated based on the mean extracted in this way. In one embodiment of the present invention, the landmark node generated as described above and the node about the pose of the robot that observed the landmark node can be generated together. In this way, by generating the landmark node about the pose of the landmark and the observation node about the pose of the robot together, and connecting the landmark node about the pose of the landmark and the observation node about the pose of the robot that observed the corresponding landmark with an edge, the landmark node can be incorporated into the pose graph as a node on the pose graph, and both the node about the pose of the robot and the landmark node about the pose of the landmark can be optimized together on the pose graph according to loop closure detection.

[0440] As described above, a plurality of data for predicting the pose of a landmark can be generated from observations observed from poses of different robots, and at this time, the mean and covariance can be extracted from clustering of a plurality of data regarding the pose of the landmark, the pose of the landmark can be predicted from the extracted mean, and accordingly, a landmark node regarding the pose of the landmark can be generated on a pose graph, and from the covariance, in pose graph optimization according to loop closure detection, the uncertainty of the measurement can be provided in least square optimization for the edge connecting the landmark node and the observation node regarding the pose of the robot that observed the corresponding landmark. For example, for the edge connecting the landmark node and the observation node regarding the pose of the robot that observed the corresponding landmark, the predicted observation can be generated from the pose of the landmark with respect to the landmark node and the pose of the robot with respect to the observation node, and the Mahalanobis distance including the squared error between the observation node and the real observation that observed the landmark and the uncertainty of the measurement calculated as described above can be applied to implement the pose graph optimization.

[0441] For example, in mapping for generating spatial information about the surrounding environment, a landmark node for a landmark can be generated on a pose graph based on observations of landmarks from a robot. In other words, in the mapping, the pose of the robot can be predicted or the predicted pose can not be corrected based on observations of landmarks from the robot. For example, in mapping according to one embodiment of the present invention, the pose of a landmark can be predicted from observations of the landmark, a landmark node for the pose of the landmark can be generated, and the pose of the robot can be predicted or not corrected from the pose of the landmark before the landmark node is generated.In other words, in one embodiment of the present invention, the generation of a landmark node for the pose of a landmark can be performed based on observations of the same landmark for a plurality of times along the driving path of the robot and for different poses of the robot, and localization can be performed on the poses of different robots that observed the same landmark, so that, for example, a prediction can be performed on the pose of the robot from odometry and a motion model of the robot, and a correction can be performed on the predicted pose of the robot from observations observed from a lidar sensor of the robot, and for the pose of the robot inferred from the prediction and correction on the pose of the robot in this way, that is, as the pose of the robot that observed the same landmark along the driving path of the robot, observations of the landmark from the poses of the robots for which the prediction and correction as described above have been performed and the poses of each robot can be accumulated, and when observations of the same landmark are accumulated more than a preset number of times, a plurality of data of the pose of the landmark can be generated from the plurality of accumulated observations, for example, from the pose of the robot in which each of the plurality of observations is accumulated and each observation and the observation model of the robot, in order to predict the pose of the landmark, For example, clustering based on the density of data, such as DBSCAN, can be used to derive the mean and remove noise and outliers, and the pose of a landmark can be predicted from the derived mean and landmark nodes can be created.At this time, a landmark node can be created by predicting the pose of the landmark from the mean based on the result of collecting multiple observations that observe the same landmark from nodes regarding the poses of different robots, and in this way, the poses of multiple landmarks can be predicted from observations observed from the poses of multiple different robots, and a node regarding the pose of a robot that observed a landmark node calculated as the mean among the poses of multiple landmarks can be created as an observation node that will create a connection with the landmark node. For example, in one embodiment of the present invention, after creating a landmark node, an observation node that observes the landmark can be created, and then an edge that connects the landmark node and the observation node can be created.

[0442] In one embodiment of the present invention, a node related to the pose of the robot can be generated at each time step along the driving path of the robot at intervals of discrete time steps, and in this way, a node related to the pose of the robot can be generated at each discrete time step. However, in the embodiment of generating a landmark node related to the pose of a landmark as described above, since the observation node is generated according to the time point at which the landmark is observed, that is, according to the time point at which the landmark node and the observation node are generated, for example, rather than being formed at equal intervals along the driving path of the robot at predetermined discrete time intervals, the nodes can be formed at unequal intervals according to the time point at which the landmark is observed.

[0443] In one embodiment of the present invention, nodes related to the pose of the robot are generated at each discrete time step, and nodes are generated at a first interval along the driving path of the robot, and while generating landmark nodes according to observation of the landmark and observation nodes that observe the landmark nodes, a second interval narrower than the first interval can be formed between the observation nodes and other neighboring nodes along the driving path of the robot.

[0444] In the pose graph optimization for the edges connecting the landmark nodes and observation nodes, optimization for the landmark nodes and observation nodes can be implemented by applying the covariance calculated as described above.For example, in the pose graph optimization according to one embodiment of the present invention, a predicted observation can be predicted from a pose of a robot and an observation model for two nodes between two neighboring nodes on a pose graph, and a least square optimization can be applied to a Mahalanobis distance including a squared error and an uncertainty of measurement between a predicted observation and a real observation from a real observation of a landmark from a pose of a robot for two nodes, and additionally, a predicted measurement can be predicted from a pose of a landmark for a landmark node and a pose of a robot for an observation node and an observation model between a landmark node and an observation node, and a Mahalanobis distance including a squared error and an uncertainty of measurement (calculated from covariance extracted from a plurality of data for predicting the position of a landmark, as described above) between a predicted observation and a real observation from a pose of a robot for an observation node, and for example, a Mahalanobis distance for an edge between nodes generated along a driving path of a robot and a Mahalanobis distance for an edge between a landmark node and an observation node. For the summation, a bundle adjustment can be implemented for the nodes generated along the robot's driving path and for the landmark nodes and observation nodes by applying least square optimization.

[0445] <particle 필터>

[0446] Figure 35 is a drawing for explaining the uncertainty of odomerty, in which the odometry of the wheel and the actual driving distance of the robot are mismatched depending on the actual driving distance of the robot and the slip (e.g., the wheel spinning in place) or drift (e.g., the wheel slipping) of the wheel of the robot.

[0447] On the left side of Fig. 36, there is shown a drawing for explaining how to adjust the probability distribution or the particle set regarding the probability distribution regarding the pose of the robot in a divergent form in a moving state of an accelerated driving environment toward a target speed, and different drawings are shown for explaining how to adjust the frequency of particle samples in a relatively wide range and a relatively narrow range in low-speed driving where the target speed is relatively low and high-speed driving where the target speed is relatively high, and on the right side of Fig. 36, there is shown a drawing for explaining how to adjust the probability distribution or the particle set regarding the probability distribution regarding the pose of the robot in a convergent form in an approach state of a decelerated driving environment toward a target speed.

[0448] Figure 37 illustrates a drawing for explaining the convergence of a probability distribution or a particle set regarding a probability distribution regarding a robot's pose in a static environment where it is difficult to extract features of the surrounding environment from observations of the surrounding environment, such as an open area or a straight corridor, or where there is little possibility of collision with obstacles or collisions in the surrounding environment.

[0449] Figure 38 shows different diagrams for explaining how to adjust the probability distribution of the robot's pose or the particle set for the probability distribution in different convergence and divergence forms, respectively, in a static environment where the surrounding environment changes relatively little over time and in a dynamic environment where the surrounding environment changes relatively much over time.

[0450] In one embodiment of the present invention, a particle filter may be applied in SLAM (filtering-based SLAM) for simultaneously implementing localization for inferring a pose (state vector) regarding the position and attitude of a robot on a map containing spatial information regarding the surrounding environment surrounding the robot, or mapping for inferring spatial information regarding the surrounding environment surrounding the robot along with such localization. Unlike the Kalman filter, this particle filter can approximate a random probability distribution rather than a parametric probability distribution of a Gaussian distribution, and for example, a particle set in the particle filter can approximate an arbitrary arbitrary function. For example, in the particle filter, the probability distribution regarding the pose of the robot (or the position or map of the landmark) can be expressed as a random probability distribution (or arbitrary function) rather than as a mean and covariance, unlike in the Kalman filter. For example, in the particle filter, the probability distribution regarding the pose of the robot (or the position or map of the landmark) can be expressed in the form of a particle set X that is the sum of the multiplication of the hypothesis pose(x(j))) regarding each robot's pose and the importance weight(w(j))) regarding each hypothesis pose(x(j)).

[0451] X={x(j), w(j)}, j=1,..,n

[0452] w(j) = proposal / target = f(x(j)) / π(x(j))

[0453] For example, the importance weight (w(j)) of the particle filter is for compensation between the probability distribution predicted in the previous step (proposal, f(x), e.g., the initial random probability distribution or the belief or prior probability distribution of the previous step) and the probability distribution modified from the observation (target, π(x)). The importance weight (w(j)) for compensation between the proposal (f(x)) and the target (π(x)) can be reflected in the new probability distribution in the form of different spatial frequencies of uniform weights, e.g., the spatial frequency (frequency, which expresses the probability of being sampled when resampling particles) of a particle set in the robot's workspace.

[0454] In one embodiment of the present invention, when applying a particle filter to localization and / or filtering-based SLAM, the spatial frequency of the particles expressing the probability distribution of the robot in the particle filter, for example, the spatial frequency (frequency, frequency expressing the probability of being sampled when resampling the particles) of a particle set in the workspace of the robot, can be differentially set according to different driving states of the robot and whether the robot is in an abnormal state. For example, in one embodiment of the present invention, in SLAM for simultaneously implementing localization for inferring the pose of the robot or mapping for inferring spatial information of the surrounding environment surrounding the robot together with localization, the probability distribution regarding the pose of the robot can be predicted from the motion model and control or odometry of the robot (prediction), and the predicted probability distribution (prediction probability distribution) regarding the pose of the robot can be corrected (correction) from the observation model of the robot and observations observed from the sensors of the robot. At this time, in order to predict the probability distribution regarding the pose of the robot in the above prediction, the probability distribution regarding the pose of the robot can be predicted in the next step from the motion model and control and / or odometry based on the posterior probability distribution corrected in the previous step, and according to the driving state of the robot, as the probability distribution regarding the pose of the robot, for example, the spatial frequency of the particle set, more specifically, the frequency expressing the probability of being sampled when resampling particles in the work space of the robot can be adaptively set.For example, in one embodiment of the present invention, a driving state in which the spatial frequency of the particle set is differentially set may include a moving state and an approach state, and for example, in the moving state, the driving of the robot may include a driving state in which the driving of the robot is accelerated toward a target speed according to a command value (or a control value), and in an accelerated driving environment, when a relatively strong traction force is applied to the wheels of the robot and friction with the ground is insufficient, there may be a high probability that a slippery environment of the wheels of the robot will be formed, and further, in an accelerated driving environment, there may be a high probability that a slippery environment of the wheels will be formed while following a detour path (collision avoidance or obstacle avoidance) that deviates from a driving path set according to behavior planning or local planning set in advance in response to an unexpected situation.

[0455] For example, in a slippery environment of a robot's wheels, such as during an accelerated driving situation of a robot, as the uncertainty of odometry deepens, it may be necessary to adaptively adjust the probability distribution (e.g., a probability distribution corrected through correction) corresponding to the frequency sampled during particle resampling, reflecting the steering component (e.g., the position and posture of the robot) due to the unbalanced rotation between the wheels on both sides, along with the displacement component (e.g., the distance traveled by the robot, the position of the robot) in the driving direction. In one embodiment of the present invention, in a particle filter applied to SLAM that simultaneously implements location and location and mapping, a posterior probability distribution corrected through prediction, which predicts a probability distribution about the pose of the robot from odometry, and correction, which corrects the predicted probability distribution about the pose of the robot from observation, can be generated. At this time, the probability distribution including the uncertainty of odometry is a prediction probability distribution predicted from prediction, but the uncertainty of odometry may also be included in the probability distribution (e.g., a posterior probability distribution) corrected from the prediction probability distribution through observation.For example, in a Kalman filter that infers the pose of a robot by repeating prediction and correction, similar to a particle filter, the mean value of the posterior probability distribution corrected through correction can be calculated as any value between the mean value of the probability distribution that considers the uncertainty of odometry (Gaussian noise) in prediction and the mean value of the probability distribution that considers the uncertainty of measurement (Gaussian noise) in observation. In this case, the mean value of the posterior probability distribution corrected through correction can be calculated by reflecting the mean value of the probability distribution of prediction and the mean value of the probability distribution of observation at different ratios according to the weights calculated from the Kalman gain.

[0456] In this way, in the application of the particle filter according to one embodiment of the present invention, depending on the driving state of the robot, for example, in a moving state in which the robot accelerates by following a command value (control value), the uncertainty of the odometry that may relatively increase in an accelerated driving environment (moving state) may be considered, and the probability distribution regarding the pose of the robot may be adjusted by considering the uncertainty of the displacement component (position of the robot) along the driving direction and the uncertainty of the steering component (attitude of the robot) diverging in both directions away from the driving direction. In addition, for example, in an accelerated driving situation in which the uncertainty of the odometry increases, such as a moving state, the probability distribution (posterior probability distribution) regarding the pose of the robot corrected through correction may be adjusted.

[0457] In the application of the particle filter according to one embodiment of the present invention, as a probability distribution corrected through prediction and correction, for example, a particle set weighted with differential importance weights for correction between the probability distribution of a proposal and the probability distribution of a target through prediction and correction can be expressed as a particle set of different spatial frequencies with uniform importance weights, and in this way, by resampling particles from the particle set of different spatial frequencies on the workspace of the robot, a recursive process of repeating prediction and correction in the next step can be performed. At this time, in various embodiments of the present invention, adjusting the probability distribution regarding the pose of the robot according to the driving state of the robot, such as the moving state, is to reflect the uncertainty of odometry in the resampling of particles (for example, the uncertainty of displacement in the driving direction in an accelerated driving situation and the uncertainty of steering outside the driving direction), and to adjust the spatial probability distribution in which the particles are resampled (for example, the frequency in which the probability distribution regarding the pose of the robot is expressed as a spatial frequency in the workspace of the robot), and this adjustment of the probability distribution can be applied to the posterior probability distribution corrected through prediction and correction.

[0458] In a comparative example contrasting with the present invention, resampling of particles can be performed from a posterior probability distribution corrected through prediction and correction (or a frequency expressed as a spatial frequency in the workspace of the robot in terms of a probability distribution regarding the pose of the robot), and no separate adjustment may be made to the probability distribution for which particles are resampled depending on whether the robot is in a driving state or an abnormal state. In contrast, in one embodiment of the present invention, resampling of particles can be performed from a probability distribution adaptively adjusted depending on whether the robot is in a driving state (e.g., a moving state) or an abnormal state for the posterior probability distribution corrected through prediction and correction (a frequency expressed as a spatial frequency in the workspace of the robot as a probability distribution).

[0459] In one embodiment of the present invention, depending on the driving state of the robot, for example, in an accelerated driving situation in which the robot accelerates toward a target speed, a relatively high traction force is applied to the wheels of the robot, and thus the uncertainty of odometry may increase due to the slippery environment of the wheels of the robot. In order to cope with an unexpected situation that may arise in an accelerated driving situation, the robot may follow a detour path and deviate from the initially set driving path, for example, according to collision avoidance or obstacle avoidance, based on behavior planning or local path planning that has been set in advance to escape the unexpected situation. In this collision avoidance or obstacle avoidance, the uncertainty of odometry may further increase as the slippery environment of the wheels of the robot is added. In one embodiment of the present invention, depending on the driving state of the robot, for example, in an accelerated driving situation such as a moving state, the probability distribution regarding the pose of the robot may be adjusted in consideration of the uncertainty of the displacement (distance traveled, position of the robot) that follows the driving direction of the robot and the uncertainty of the steering (position of the robot and posture of the robot) that deviates from the driving direction of the robot.

[0460] In the application of the particle filter according to one embodiment of the present invention, in consideration of the uncertainty of the displacement (travel distance, position of the robot) that follows the driving direction of the robot, the probability distribution of the pose of the robot can be extended further along the driving direction of the robot, and particle samples from a particle set (a particle set for resampling) corresponding to the probability distribution of the pose of the robot can be distributed to a range extended further along the driving direction of the robot. However, in various embodiments of the present invention, when the uncertainty of the displacement (travel distance) along the driving direction is considered, in consideration of the uncertainty of the displacement (travel distance) along the driving direction, the probability distribution of the pose of the robot can be extended further along the driving direction of the robot, or particle samples from a particle set can be distributed to a range extended further along the driving direction of the robot, the driving direction of the robot can mean both directions including the forward direction of the robot, for example, the forward direction toward the target position of the robot, as well as the backward direction opposite to the forward direction.

[0461] In one embodiment of the present invention, depending on the driving state of the robot, for example, in an accelerated driving situation such as a moving state, a slippery environment of the wheels of the robot may be formed, and accordingly, the uncertainty of odometry may increase, and for example, when considering a driving situation in which the uncertainty of odometry increases to a maximum or close to a maximum, for example, a situation in which the robot does not actually drive due to an obstacle (for example, the position of the robot remains the same) despite the rotation of the wheels of the robot, such as an idle rotation of the wheels of the robot, in the application of a particle filter that considers a situation in which the uncertainty of odometry may increase, such as a moving state, depending on the driving state of the robot, the posterior probability distribution of the pose of the robot calculated through prediction and correction may need to include the probability of the same position as the previous step, or in other words, it may need to distribute particle samples of the same position as the previous step in the particle set for resampling, and for example, the probability distribution of the pose of the robot is not biased in one direction along the driving direction of the robot, and is biased in one direction along the driving direction of the robot. Symmetrically and evenly distributing particle samples from the particle set for resampling along both directions in the opposite direction, that is, symmetrically and evenly distributing particle samples along both directions in one direction and the opposite direction along the driving direction, can be advantageous for inferring the pose of a robot with virtually no positional movement through mutual comparison of observations observed from opposite directions.

[0462] For example, in the application of the particle filter according to one embodiment of the present invention, for the posterior probability distribution to which the prediction and correction are applied (e.g., particle samples of different frequencies in the workspace or pose space of the robot), in a driving situation where the uncertainty of the odometry increases, such as a driving state of moving, the posterior probability distribution can be adaptively adjusted according to the driving state of the robot, and when the adjusted probability distribution is used as the prior probability distribution in the next step (e.g., particle samples of different frequencies in the workspace or pose space of the robot) and the motion model is applied to each particle sample, and the importance weight is calculated through comparison of the prediction or predicted observation and the real observation, even if the motion model is applied with the probability distribution (posterior probability distribution) adjusted according to the driving state of the robot as the prior probability distribution in the next step, some particle samples among the particle samples that are symmetrically and balancedly distributed along both one direction and the opposite direction along the driving direction of the robot on the prior probability distribution can be distributed at the same position as in the previous step, thereby improving the odometry. It may be advantageous to predict the pose of a robot that is substantially in the same position as in the previous step, for example, when the robot's wheels are spinning due to an obstacle blocking it, where the uncertainty may be at or near the maximum.In addition, in the application of a particle filter that infers a probability distribution (posterior probability distribution) regarding a robot's pose by comparing a prediction or predicted observation using a motion model of the robot with a real observation, it is advantageous to secure predicted observations observed at positions before and after positions where predicted observations or predicted observations similar to real observations are predicted, in that it can be advantageous to increase the accuracy of the probability distribution of the robot's pose. In one embodiment of the present invention, the probability distribution regarding the pose of the robot can be adjusted according to the driving state of the robot, for example, in a moving state, and at this time, by expressing the probability distribution, it can be advantageous to increase the accuracy of the probability distribution regarding the pose of the robot by symmetrically and evenly distributing the frequency or distribution of spatial particle samples in the workspace or pose space of the robot in both one direction and the opposite direction along the driving direction of the robot.

[0463] In this way, in the application of the particle filter according to one embodiment of the present invention, in consideration of the uncertainty of the steering (travel distance, position of the robot) outside the driving direction of the robot, the probability distribution of the pose of the robot can be extended further along the driving direction of the robot (both directions, one direction and the opposite direction along the driving direction), and particle samples from a particle set (a particle set for resampling) corresponding to the probability distribution of the pose of the robot can be distributed to a range extended further along the driving direction of the robot. For example, in the application of the particle filter according to one embodiment of the present invention, in consideration of the uncertainty of the displacement (travel distance, position of the robot) following the driving direction of the robot, the probability distribution of the pose of the robot can be extended further along the driving direction of the robot (both directions, one direction and the opposite direction of the driving direction), and particle samples corresponding to the probability distribution of the pose of the robot can be distributed to a range extended further along the driving direction of the robot (both directions, one direction and the opposite direction). In addition, in the application of the particle filter according to one embodiment of the present invention, by considering the uncertainty of the steering (position of the robot and posture of the robot) outside the driving direction of the robot, the probability distribution of the pose of the robot can be extended to a wider angular range outside the driving direction of the robot, and for example, particle samples can be distributed to a wider angular range outside the driving direction of the robot in a particle set (a particle set for resampling) corresponding to the probability distribution of the pose of the robot.For example, in the application of a particle filter according to one embodiment of the present invention, by considering the uncertainty of steering (position of the robot and posture of the robot) outside the driving direction of the robot, the probability distribution regarding the pose of the robot can be extended to a wide angular range outside the driving direction of the robot, and particle samples corresponding to the probability distribution regarding the pose of the robot can be distributed to a wider angular range outside the driving direction of the robot.

[0464] For example, in one embodiment of the present invention, adjusting the probability distribution of the robot may mean that, as determined by comparing the probability distribution before and after the adjustment with the probability distribution after the adjustment, the interval between the two ends of the probability distribution, which corresponds to a frequency of substantially zero (0 or a probability of substantially zero), is extended longer along the driving direction of the robot (taking into account the uncertainty of the displacement of the robot) or extended more widely outside the driving direction of the robot (taking into account the uncertainty of the steering of the robot).

[0465] In this way, in the application of the particle filter according to one embodiment of the present invention, depending on the driving state of the robot, for example, in a driving situation where the uncertainty of odometry may increase, such as a moving state, by expressing a probability distribution or probability distribution regarding the pose of the robot, the frequency or distribution of spatial particle samples in the work space or pose space of the robot can be adjusted, and for example, while extending the probability distribution regarding the pose of the robot in both directions along the driving direction, in one direction and the opposite direction, in other words, particle samples can be distributed in a spatially extended range along both directions in the driving direction of the robot in a particle set for expressing the probability distribution, and also while broadly extending the probability distribution regarding the pose of the robot in a symmetrical and balanced manner in an angular range on both sides outside the driving direction, in other words, particle samples can be distributed in a spatially extended range in an angular range on both sides outside the driving direction of the robot in a particle set for expressing the probability distribution.In this situation where the uncertainty of odometry increases, for example, in an acceleration driving situation where the robot accelerates toward the target speed to follow the command value (or control value) such as a moving state, the uncertainty of odometry may increase, for example, depending on the draft or slip environment of the robot due to a detour path that deviates from the original path plan due to collision avoidance or obstacle avoidance maneuvers depending on a slip environment or an unexpected situation, and accordingly, the probability distribution regarding the pose of the robot can be adjusted to a divergent form, and for example, the spatial probability distribution regarding the pose of the robot can be adjusted to a longer and wider range to generate the possibility or probability of the robot's pose being taken for a wider spatial range (considering various states in the pose space considering the position and posture of the robot) by considering various unexpected possibilities. In other words, the probability distribution can be adjusted to a divergent form so that inference can be made about various unexpected poses of the robot (so that probabilities or particles can be sampled even for various unexpected poses of the robot), and in the particle set for expressing the probability distribution, particles The distribution of the particle set can be adjusted in a divergent manner, with the samples distributed over a longer and wider extended range.

[0466] In one embodiment of the present invention, the driving state of the robot considered for adjusting the probability distribution or the particle set for expressing the probability distribution may include an approach state in addition to the moving state described above. For example, the moving state may be a state in which the robot accelerates toward a target speed to follow a command value (or control value), and may correspond to, for example, a state at some distance from the target position of the robot. In contrast, the approach state may be a state in which the robot decelerates toward a target speed to follow a command value (or control value), and may correspond to a state in which the robot approaches the target position. In one embodiment of the present invention, in an approach state where the robot approaches a target position, the probability distribution regarding the pose of the robot can be adjusted in a convergent manner. For example, for a posterior probability distribution generated through prediction from odometry and correction from observation, the probability distribution can be adjusted so that the probability exists only in a shorter range along the driving direction of the robot (along both one direction and the opposite direction along the driving direction of the robot), and further, the probability distribution can be adjusted so that the probability exists only in a narrower angular range on either side off the driving direction of the robot. In other words, in a particle set for expressing the probability distribution regarding the pose of the robot, the distribution of the particle set can be adjusted so that particle samples are distributed only in a shorter length along the driving direction of the robot (along both one direction and the opposite direction along the driving direction of the robot), and further, the distribution of the particle set can be adjusted so that particle samples are distributed only in a narrower angular range on either side off the driving direction of the robot.

[0467] As explained above, in an accelerated driving environment such as a moving state, the uncertainty of odometry, for example, in an emergency driving environment such as a robot's drift or slip environment or a collision avoidance or obstacle avoidance maneuver, can be considered to generate a probability (substantially non-zero probability) for an unexpected pose of the robot, for example, a probability (substantially non-zero probability) can be generated even in a longer range along the robot's driving direction, and the probability distribution can be adjusted in a divergent form to generate a probability (substantially non-zero probability) even in a wider range on both sides away from the robot's driving direction.

[0468] Unlike the moving state described above, in the approach state, the probability distribution can be adjusted in a convergent manner. For example, the posterior probability distribution generated through prediction and correction can be adjusted in a convergent manner so that the probability (substantially non-zero probability) exists only in a shorter range along the driving direction of the robot, and further, the posterior probability distribution can be adjusted in a convergent manner so that the probability (substantially non-zero probability) exists only in a narrower angular range on either side off the driving direction of the robot. In other words, the particle set for expressing the probability distribution regarding the pose of the robot can be adjusted in a convergent manner so that particle samples in the particle set for expressing the probability distribution regarding the pose of the robot are distributed only in a shorter range along the driving direction of the robot, and further, the particle set for expressing the probability distribution regarding the pose of the robot can be adjusted in a convergent manner so that particle samples in the particle set for expressing the probability distribution regarding the pose of the robot are distributed only in a narrower angular range on either side off the driving direction of the robot.

[0469] In this way, in the approach state, the probability distribution or particle set regarding the robot's pose is adjusted to convergence form because, in the approach state, as the robot's position approaches the target position, the uncertainty regarding the robot's pose, including the robot's position, can decrease, and adjusting the probability distribution to convergence form while the uncertainty regarding the robot's pose decreases is desirable for saving computational resources. For example, a probability distribution about the pose of the robot can be derived according to the time step by repeating the prediction based on the robot's motion model and odometry and the correction based on the robot's observation model and observation. As the robot's position approaches the target position, the accuracy of the inference about the pose of the robot can be improved based on the observation of the target position, and the uncertainty about the pose of the robot can be gradually reduced. For example, in a recursive process where the posterior probability distribution of the previous step is input as the prior probability distribution of the next step, the accuracy of the prior probability distribution can be improved as the accuracy of the posterior probability distribution in the previous step is improved. In this way, as the accuracy of the posterior probability distribution is improved through correction to the prior probability distribution with improved accuracy, the uncertainty about the pose of the robot is reduced, and even if the probability distribution is adjusted in a convergent manner accordingly, the probability of the pose of the robot that deviates from the prediction can be close to zero (0), and computational resources can be saved as the frequency of particles in the probability distribution or the particle set representing the probability distribution from the probability distribution in a convergent manner is reduced.For example, as the position of a robot in an approach state approaches a target position, the pose of the robot can be corrected with high accuracy based on observations of landmarks, and as a robot in an approach state that has approached the target position observes the target position or a landmark of the target position at an adjacent position in a close approach state, the uncertainty of the observation can be reduced, and as the correction of the pose of the robot based on the observation is accurately performed, the accuracy of the probability distribution of the pose of the robot in the current step and the accuracy of the probability distribution in the subsequent step can be improved.

[0470] In one embodiment of the present invention, a robot in an approach state that has approached a target position can gradually reach a stationary state while its driving speed decreases, and in order to infer a pose of the robot with a decreased driving speed or a pose of the robot in a state where the driving speed is close to a stationary state, the probability distribution regarding the pose of the robot can be adjusted in a convergent manner so that a probability (a probability that is substantially not zero) exists only in a short range symmetrically and balancedly along both one direction and the opposite direction along the driving direction of the robot. In other words, the particle set for expressing the probability distribution regarding the pose of the robot can be adjusted in a convergent manner so that particles of the particle set for expressing the probability distribution regarding the pose of the robot are distributed only in a short range along both one direction and the opposite direction along the driving direction of the robot. According to one embodiment of the present invention, the adjustment of the probability distribution regarding the pose of the robot can be performed on a particle set (corresponding to a probability distribution) for resampling particles, and since the particle sets are symmetrically and balancedly distributed along both one direction and the opposite direction along the driving direction of the robot in the adjusted probability distribution or the adjusted particle set, when generating the next stage prediction or prediction probability distribution from the motion model and odometry for the particle samples of the adjusted particle set in this way, for example, the pose of a robot with a reduced driving speed or a robot with a driving speed close to zero (0) can be more accurately predicted, and for example, it can be advantageous to infer the pose of a robot with a reduced driving speed or with substantially little positional movement through a mutual comparison of observations observed from opposite sides.

[0471] In the application of a particle filter according to one embodiment of the present invention, depending on the driving state of the robot, in a moving state or an approach state, a probability distribution or a particle set expressing a probability distribution regarding the pose of the robot can be adaptively adjusted in a divergent form and a convergent form, respectively, and at this time, the probability distribution can be adjusted to have substantially equal probabilities in a symmetrical and balanced manner along both directions of the driving direction of the robot and the opposite direction, and the probability distribution can be adjusted to have substantially equal probabilities in a symmetrical and balanced manner along both angular directions outside the driving direction of the robot, and in this way, by adjusting the probability distribution or the particle set expressing a probability distribution regarding the pose of the robot in a symmetrical and balanced manner along both directions of the driving direction and both angular directions outside the driving direction, the probability distribution regarding the pose of the robot can be more accurately inferred through a mutual comparison of observations observed from opposite sides.

[0472] In the application of a particle filter according to one embodiment of the present invention, the probability distribution of the robot can be adaptively adjusted to a divergent form and a convergent form according to a driving state such as a moving state and an approach state, and the driving state of the robot can include a moving state in which the robot accelerates to follow a target speed according to a command value and an approach state in which the robot decelerates to follow a target speed according to a command value, and in addition to the moving state and the approach state, the robot can include a pause state corresponding to a stationary state of 2 seconds or less and a stop state corresponding to a stationary state of more than 2 seconds, and in addition to such driving states of the robot, the probability distribution regarding the pose of the robot can be adaptively controlled even for an abnormal state of the robot.

[0473] In the application of the particle filter according to one embodiment of the present invention, the abnormal state of the robot may include an abnormal state of the drive motor for providing driving power to the robot and an abnormal state of the robot's sensor. For example, the abnormal state of the drive motor may comprehensively mean an abnormal state of the drive motor, such as a power cut of the drive motor, an odometry abnormality such as an encoder of the drive motor, and an inability to control the drive motor. In addition, the abnormal state of the robot's sensor may comprehensively mean an abnormal state of the robot's sensor in which normal observation is not provided from the robot's sensor, such as a loss of observation from the robot's sensor. For example, in the application of the particle filter according to one embodiment of the present invention, the reason for adjusting the probability distribution regarding the pose of the robot with respect to the abnormal state of the drive motor and the abnormal state of the sensor as the abnormal state of the robot is that, in the inference regarding the pose of the robot, the abnormal state of the drive motor may affect the prediction from the control or odometry, and the abnormal state of the sensor may affect the correction from the observation. In this way, the abnormal state of the robot can decrease the accuracy of the probability distribution about the robot's pose by increasing the uncertainty of odometry or increasing the uncertainty of observation, thereby affecting the prediction and correction for inferring the robot's pose.

[0474] In the application of the particle filter according to one embodiment of the present invention, instead of stopping the inference of the pose of the robot according to the application of the particle filter despite the abnormal state of the robot, such as the abnormal state of the drive motor and the abnormal state of the sensor, by adjusting the probability distribution of the pose of the robot, the pose of the robot can be inferred immediately when the robot is restored from the abnormal state to the normal state, thereby preventing an unexpected situation of the robot in advance, and for example, an unexpected situation, such as an obstacle or collision, can be prevented from the pose of the robot that is not predicted in the abnormal state of the robot, and for example, by adjusting the probability distribution of the pose of the robot to a diverging probability distribution that is sparse and expands over a wide range (a probability distribution that is distributed over a wide range with a relatively low probability), the pose of the robot can be predicted over a wide range, and accordingly, the pose of the robot can be inferred immediately when the robot is restored from the abnormal state to the normal state, thereby avoiding an unexpected situation, and for example, collision avoidance or obstacle avoidance can be initiated according to behavior planning or local path planning planned in advance.

[0475] In the application of the particle filter according to one embodiment of the present invention, even in an abnormal state of the robot, a probability distribution regarding the pose of the robot can be inferred according to the application of the particle filter including prediction and correction. However, in the abnormal state of the robot, as the uncertainty of odometry and / or observation increases, the accuracy of the probability distribution regarding the pose of the robot according to the prediction and / or correction decreases. Therefore, the probability distribution of the robot, for example, the probability distribution for resampling of particle samples, can be expanded to a wide spatial range while being sparse, and thus, the probability distribution (probability distribution for resampling) can be adjusted over a wide spatial range after the adjustment so that a probability can be generated even for a pose of the robot that could not be predicted before the adjustment. In addition, the particle set can be adjusted so that particle samples are distributed over a wide spatial range while being sparse in the particle set expressing the probability distribution.

[0476] In the application of the particle filter according to one embodiment of the present invention, the probability distribution regarding the pose of the robot may be adjusted depending on the state of the surrounding environment surrounding the robot. For example, in a surrounding environment such as an open area where it is difficult to expect correction of the pose of the robot according to observations observed from the sensor of the robot, the probability distribution regarding the pose of the robot may depend on odermetry, and accordingly, the probability distribution regarding the pose of the robot may be adjusted so that the probability is generated mainly along the driving direction of the robot. In other words, the probability distribution may be adjusted so that the probability of the robot deviating from the driving direction is reduced.

[0477] In one embodiment of the present invention, unlike an open area, for an environment surrounding a robot where obstacles are scattered, the probability distribution of the robot can be adjusted in a divergent manner so that probabilities exist along angular directions on both sides off the direction of movement of the robot, taking into account the possibility of obstacles and collisions during the robot's movement. For example, depending on the surrounding environment of the robot, in an environment (static environment) such as an open area or a long corridor with few obstacles around the robot or a low possibility of collision, observations observed from the robot's sensors may be insufficient (for example, it is difficult to predict the robot's pose from observations of the surrounding environment because the features of the surrounding environment observed from the robot's sensors are insufficient). Here, insufficient observations in a static environment such as an open area or a long corridor mean, for example, that even if observations are obtained from the robot's sensors, the possibility that the robot's pose can be corrected from observations with few characteristic features is low. In other words, it may mean that the difference between the robot's pose and the observation model may not be discernible.In this way, in a static surrounding environment such as an open area or a long corridor where observation is insufficient or features observed from observation are not sufficient, the inference of the probability distribution regarding the pose of the robot may depend on the odometry of the robot, and thus the probability distribution regarding the pose of the robot may be adjusted so that the probability exists along the driving direction of the robot. Since the possibility of obstacles or collisions around the robot is low, there may be a relatively low need to infer the pose of the robot over a wide range along the robot's periphery. In a situation where observation is rare, predicting and correcting the pose of the robot over a wide range may result in a waste of computational resources. For example, in the application of the particle filter according to one embodiment of the present invention, since distributing many particle samples over an angular range on both sides outside the driving direction of the robot and performing prediction and correction on these many particle samples requires the allocation of a lot of computational resources, the distribution of the probability distribution regarding the pose of the robot or the particle set representing the probability distribution may be concentrated primarily in the driving direction of the robot to prevent the waste of computational resources.

[0478] Unlike static surroundings such as open areas or long corridors, in dynamic surroundings with many obstacles or high collision potential along the surroundings of the robot, the probability distribution of the pose of the robot or the distribution of particle samples related to the probability distribution can be expanded to a wide range over an angular range on both sides of the robot's driving direction, thereby enabling the robot to perform collision avoidance and obstacle avoidance maneuvers according to its surroundings, and the robot's pose can be inferred despite the collision avoidance and obstacle avoidance maneuvers that deviate from the original driving path in such unexpected situations.

[0479] For example, depending on the surrounding environment surrounding the robot (static surrounding environment / dynamic surrounding environment), adjustments can be made to the probability distribution of the pose of the robot or the particle set expressing the probability distribution. At this time, the surrounding environment surrounding the robot can include consideration of obstacles or collisions existing in the surrounding environment surrounding the robot over time. For example, for a dynamic surrounding environment with relatively high changes over time, a static surrounding environment with relatively low changes over time, or a normal surrounding environment with changes in the surrounding environment in between the dynamic and static surrounding environments, the probability distribution can be adjusted in a divergent manner to a wider range (dynamic surrounding environment), or in a convergent manner to a narrower range (static surrounding environment), or no separate adjustment can be made (normal surrounding environment). In this way, the classification of the surrounding environment according to the changes over time of the surrounding environment surrounding the robot, for example, the classification of the surrounding environment into dynamic, static, and normal, can be performed by considering changes over time and including obstacles or collisions in the current surrounding environment (dynamic surrounding environment), obstacles, etc. Alternatively, the probability distribution as described above may be adjusted depending on the surrounding environment where the frequency of collisions is relatively low (static surrounding environment) and the surrounding environment where the frequency of these obstacles or collisions is moderate (normal surrounding environment). For example, the probability distribution regarding the robot's pose may be adjusted to a divergent form and a convergent form, respectively, or the probability distribution regarding the robot's pose may not be adjusted.

[0480] That is, in the application of the particle filter according to one embodiment of the present invention, the distribution of particles in a particle set expressing a probability distribution or probability distribution regarding the pose of the robot can be adjusted to a divergent form or a convergent form according to the state of the robot, for example, according to the driving state (moving state and approach state) of the robot, the abnormal state of the robot, and the surrounding environment surrounding the robot.

[0481] For example, in one embodiment of the present invention, depending on the driving state of the robot, depending on whether the robot is in an abnormal state, and depending on the surrounding environment surrounding the robot, the probability distribution or the particle set expressing the probability distribution regarding the pose of the robot may be adjusted in a divergent manner so that the probability exists up to a first range that is relatively widely expanded, or the frequency of the particle set regarding the probability distribution exists up to a first range that is relatively widely expanded, or the frequency of the particle set regarding the probability distribution may be adjusted in a convergent manner so that the probability exists only up to a second range that is relatively narrowly limited, or the frequency of the particle set regarding the probability distribution exists only up to a second range that is relatively narrowly limited, and adaptively adjusting the frequency of the probability distribution or the particle set regarding the probability distribution regarding the pose of the robot in a divergent manner or a convergent manner in this way may be implemented according to an increase or decrease in an adaptive gain involved in the generation of the probability distribution or the particle set regarding the probability distribution, and the probability of the probability distribution or the frequency of the particle set regarding the probability distribution regarding the pose of the robot, for example, may be expanded to a first range that is relatively widely expanded (divergent manner, increase in adaptive gain) or relatively By setting an adaptive gain that adjusts the probability distribution or the particle set for the probability distribution in a divergent or convergent manner to limit it only to a narrow second range (convergent form, reduction of adaptive gain), and increasing or decreasing the set adaptive gain according to the driving state of the robot, whether the robot is in an abnormal state, and the surrounding environment of the robot, the probability distribution or the particle set for the probability distribution can be adjusted in a divergent or convergent manner.

[0482] In this specification, for an autonomous driving system of a robot in which node optimization of a pose graph generated from graph-based SLAM is implemented, an autonomous driving system of a robot in which integration with previously constructed spatial information from partial mapping is implemented, an autonomous driving system of a robot in which visualization of a pose graph generated from graph-based SLAM is implemented, an autonomous driving system of a robot in which editing of observations linked to nodes on a pose graph is implemented, an autonomous driving system of a robot in which robust pose estimation from landmarks is implemented, and an autonomous driving system of a robot in which pose estimation based on a probability distribution or a frequency of particle samples for a pose of a robot that is adaptively adjusted according to a driving state of the robot and a surrounding environment is implemented, a computational unit for performing necessary operations to perform the autonomous driving system of the present invention and controlling or controlling the autonomous driving system as a whole performs the above-described technical operations such as node optimization of the pose graph, partial mapping integrated into previously constructed spatial information, visualization of the pose graph, editing of observation linked to the pose graph, pose estimation robust to noise and outliers from landmarks, and pose estimation based on a probability distribution or a frequency of particle samples for a pose of a robot that is adaptively adjusted according to a driving state of the robot or a surrounding environment. The configurations can be implemented, and for the convenience of understanding, in several places of this specification, even if the computational unit is not specified as the subject performing the autonomous driving system in which the technical configurations of the present invention are implemented, or as the subject of calculation and / or control for implementing the autonomous driving system of the robot described above, as the subject for implementing the technical configurations described above, the computational unit of the present invention can perform the actions and functions necessary to perform and implement the corresponding technical configurations.

[0483] Although the present invention has been described with reference to the embodiments shown in the attached drawings, these are merely exemplary, and those skilled in the art to which the present invention pertains will understand that various modifications and equivalent other embodiments are possible therefrom.

[0484] The present invention can be applied to an autonomous driving system for a robot and industrial fields related to autonomous driving of a robot.

Claims

1. A computational unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement localization for inferring the pose of the robot on a map containing spatial information about the surrounding environment and graph-based SLAM for inferring spatial information about the surrounding environment. An autonomous driving system for a robot, characterized in that it includes a computational unit that eliminates a first node that satisfies an elimination condition among the nodes on the pose graph.

2. In paragraph 1, The above operation unit is, As the above elimination condition, The condition for starting erasure is that a first number or more of nodes are formed within a first work area set in advance on the work space of the robot. An autonomous driving system for a robot, characterized in that among the nodes created within the first work area, the node that is the fastest or the node that was created the longest time ago is selected as the first node to be deleted.

3. In paragraph 1, The above operation unit is, As the above elimination condition, The condition for starting erasure is that a first number or more of nodes are formed within a first work area set in advance on the work space of the robot. An autonomous driving system for a robot, characterized in that, among the nodes generated within the first work area, a node corresponding to the center position of the first radius that sets the first work area or a node linked to an observation with the lowest matching rate with an observation linked to the most recent node with the latest generation time is selected as the first node to be deleted.

4. In paragraph 1, The above operation unit is, An autonomous driving system for a robot, characterized in that, in order to prevent isolation of a second node and a third node connected to the first node through an edge according to the deletion of the first node of the above deletion target, a node linked to an observation having the highest matching rate with the observation of the first node of the deletion target within a first work area is searched for, and the searched node is selected as a reconnection node that replaces the first node and creates a connection with the second node and the third node adjacent to the first node.

5. In paragraph 4, The above operation unit is, The matching rate is calculated by sequentially comparing one-to-one the observations linked to the first node of the above-mentioned deletion target and the observations stored linked to the remaining nodes excluding the first to third nodes among the nodes created within the first work area. An autonomous driving system for a robot, characterized in that, based on a comparison of the magnitude of the produced matching rates, a reconnection node is selected in which the observation stored in connection with the first node to be eliminated has the highest matching rate.

6. In paragraph 1, The above operation unit is, An autonomous driving system for a robot, characterized in that an edge is created that directly connects the second node and the third node, which are adjacent to each other with the first node in between, to prevent isolation of the second node and the third node, which are connected to the first node through the edge, according to the deletion of the first node of the above deletion target.

7. In paragraph 1, The above first working area corresponds to the circumferential range of the first radius, An autonomous driving system for a robot, characterized in that the first radius is smaller than a second radius that sets a second working area for a search condition of loop closure detection in pose graph optimization (PGO), in which optimization is performed on nodes generated on a loop of a pose graph according to loop closure detection.

8. In paragraph 1, The above operation unit is, An autonomous driving system for a robot, characterized in that the presence of an obstacle recognized between the first node and the central node or the latest node corresponding to the central position of the first radius for setting a first work area set in advance in the work space of the robot as a limitation on the above-mentioned elimination condition is set as a limitation on the above-mentioned elimination condition.

9. In paragraph 8, The above operation unit is, An autonomous driving system for a robot, characterized in that, if the presence of an obstacle is recognized between the central node or the latest node and the first node, the first node is maintained without being erased, according to the restrictions on the above-mentioned erasure conditions.

10. A computation unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization to infer the pose of the robot on a map containing spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment. An autonomous driving system for a robot, comprising a computational unit that generates an integrated, interconnected pose graph for the first and second workspaces by implementing a partial mapping with a non-contiguous time gap from the mapping for the first workspace, for a first workspace in which the above mapping is implemented and a second workspace that is spatially interconnected.

11. In paragraph 10, The above operation unit is, An autonomous navigation system for a robot, characterized in that a first pose graph for a first workspace and a second pose graph for a second workspace are connected to each other with a non-contiguous visual gap, thereby generating an integrated connected pose graph for the first and second workspaces.

12. In paragraph 10, The above operation unit is, An autonomous navigation system for a robot, characterized in that a second pose graph for a second workspace is generated from a partial mapping for the second workspace while maintaining the first pose graph for the first workspace and the observations associated with the nodes on the first pose graph, and the second pose graph is generated as connected to the first pose graph.

13. In paragraph 10, The above operation unit is, An autonomous driving system for a robot, characterized in that partial mapping for the second workspace is initiated under a condition that the starting position of the partial mapping is set to a position within the first workspace where mapping is implemented.

14. In paragraph 13, The above operation unit is, An autonomous driving system for a robot, characterized in that the starting position of the partial mapping is inferred from spatial information mapped with respect to the first work space as an initial estimate of the starting position of the partial mapping.

15. In paragraph 14, The above operation unit is, An autonomous navigation system for a robot, characterized in that the starting position of the partial mapping is initially estimated from observations observed from the starting position of the partial mapping and observations observed from nodes on a first pose graph regarding the first workspace, a starting node regarding the starting position of the partial mapping is created on the first pose graph, and the observations observed from the starting position of the partial mapping are stored in association with the starting node.

16. In paragraph 10, The above operation unit is, On the first pose graph, search for the fourth node closest to the starting node with respect to the starting position of the partial mapping, An autonomous driving system for a robot, characterized in that an edge connecting the explored fourth node and the starting node is created, and the starting position of the partial mapping is incorporated into a node connected to the first pose graph.

17. In paragraph 10, The above operation unit is, On the first pose graph before the connection of the above initiation node, the last node with the latest creation time is searched along the robot's driving path, An autonomous driving system for a robot, characterized in that an edge is created between the last node explored and the starting node for the starting position of the partial mapping, and the starting position of the partial mapping is incorporated into a node connected to the first pose graph.

18. In paragraph 17, The above operation unit is, An autonomous driving system for a robot, characterized in that an edge connecting the final node and the start node is created on the first pose graph so that the first pose graph including the edge connecting the final node and the start node is connected to each other as a whole, and no disconnection or isolation is formed along the first pose graph.

19. In paragraph 10, The above operation unit is, An autonomous navigation system for a robot, characterized in that it generates a second pose graph that advances according to a time step from a starting position of a partial mapping as a node incorporated into a first pose graph for the first workspace to a second workspace as a target of the partial mapping.

20. A computation unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM for localization to infer the pose of the robot on a map including spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment. An autonomous driving system for a robot, comprising a computational unit that provides visualized information about a generated pose graph through a user interface.

21. In paragraph 20, The above operation unit is, An autonomous driving system for a robot, characterized in that it displays visualized information about nodes on a pose graph generated according to the advancement of time steps along the driving path of the robot, and, together with each node, a node identification number following a chronological order based on the time of generation and / or a driving direction of the robot on an edge connecting neighboring nodes.

22. In paragraph 20, The above operation unit is, An autonomous driving system for a robot, characterized in that pose graph optimization (PGO) is implemented to perform optimization on nodes generated on a loop of the pose graph according to loop closure detection on the driving path of the robot.

23. In paragraph 22, The above operation unit is, An autonomous driving system for a robot, characterized in that it provides visualized information about a node generated with respect to a pose of a robot according to the advancement of a time step along the driving path of the robot, an edge connecting adjacent nodes generated in the order of time steps, and a loop closure generated according to loop closure detection.

24. In paragraph 23, The above operation unit is, An autonomous driving system for a robot, characterized in that it displays visualized information about loop closures generated according to the above loop closure detection, with differential display elements for the last loop closure with the latest generation time and the remaining loop closures excluding the last loop closure.

25. In paragraph 23, The above operation unit is, A robot autonomous driving system characterized in that, as visualized information about loop closure generated according to the above loop closure detection, nodes on a pose graph requiring loop closure, for which loop closure has not yet been achieved, are displayed with differential display elements compared to other nodes on a pose graph for which loop closure has already been achieved.

26. In paragraph 23, The above operation unit is, A robot autonomous driving system characterized in that, as visualized information about a loop closure generated according to the above loop closure detection, among the nodes in a pose graph requiring loop closure and for which loop closure has not yet been performed, the last node with the latest generation time is displayed with a differential display element from the nodes for which loop closure has not yet been performed and the nodes for which loop closure has already been performed, excluding the last node.

27. In paragraph 23, The above operation unit is, An autonomous driving system for a robot, characterized in that, on a pose graph provided with the above visualized information, edges between neighboring nodes including odomerty information of the robot and loop closures generated according to loop closure detection without including odometry information of the robot are displayed as different display elements so that the edges and loop closures can be visually distinguished from each other.

28. In paragraph 20, The above operation unit is, An autonomous driving system for a robot, characterized in that it provides an opportunity to recognize a delay environment or an obstacle environment for the pose graph optimization from visualized pose graph information for a work area in which loop closure detection has not been performed or in which loop closure detection has not been performed in a sufficient number of cases, in comparison with the occupancy distribution or occupancy density occupied by the robot along the driving path of the robot.

29. In paragraph 20, The above operation unit is, Based on the judgment on the presence of a delay environment or an obstruction environment in the pose graph optimization according to loop closure detection for the pose graph provided with the above visualized information, An autonomous driving system for a robot, characterized in that it provides a notification of a delay environment or an obstruction environment of pose graph optimization along with visualized information of the pose graph.

30. A computation unit that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes, to implement graph-based SLAM (simultaneous localization and mapping) for localization to infer the pose of the robot on a map including spatial information about the surrounding environment and mapping to infer spatial information about the surrounding environment. An autonomous driving system for a robot, comprising a computational unit that performs editing on an observation based on an observation linked to an editing target node that is generated earlier in time and an editing reference node that is generated later in time, with respect to an editing target node that is generated later in time within a preset third work area.

31. In paragraph 30, The above operation unit is, Regarding the selection of the nodes for the above editing target and the nodes for the editing criteria, A robot autonomous driving system characterized in that, among the nodes generated within the third work area, the nodes to be edited and the nodes for editing criteria are selected such that the nodes are generated at different times with a time gap from each other and the different observations associated with each node have a matching rate lower than a threshold matching rate set in advance.

32. In paragraph 31, The above operation unit is, A robot autonomous driving system characterized in that, among the nodes generated within the third work area, the nodes to be edited and the nodes for editing are selected so that the nodes are generated at different times with a time gap from each other, and the different observations associated with each node have a matching rate higher than the first threshold matching rate but lower than the second threshold matching rate.

33. In paragraph 30, The above operation unit is, A robot autonomous navigation system characterized in that, among a group of nodes generated within the third work area, nodes generated in different sections blocked by obstacles or blocked by occupied cells on the occupancy grid mapped from the SLAM are not selected as nodes to be edited or nodes for editing criteria.

34. In paragraph 30, The above operation unit is, Based on the observation linked to the node of the above editing standard, edit the observation linked to the node of the above editing target. Some deletions and some additions to observations linked to the nodes of the above editing target are allowed, but An autonomous driving system for a robot, characterized in that complete deletion or complete replacement of observations linked to the nodes of the above-mentioned editing target is not allowed.

35. In paragraph 30, An autonomous driving system for a robot, characterized in that the above-mentioned operation unit edits an observation observed from a node of the editing target based on an observation observed from a node of the editing standard according to the following editing initiation conditions. 1) A temporal criterion regarding the time gap between the creation time of the node to be edited and the node of the editing standard. 2) Spatial criteria for creating nodes for editing and nodes for editing criteria within the third work area. 3) Matching rate between different observations observed from the nodes of the editing target and the nodes of the editing criteria.

36. In paragraph 30, The above operation unit is, A local map is generated by superimposing or combining observations linked to a group of editing target nodes belonging to a third work area from the nodes of the above editing criteria along the poses of each node of a group of editing targets that follow the robot's driving path. An autonomous driving system for a robot, characterized in that the editing range set on the local map is applied to observations associated with each node of a group of editing targets.

37. In paragraph 36, The above operation unit is, An autonomous driving system for a robot, characterized in that it expands the observation of the surrounding environment surrounding the robot to an object scale from a local map generated through overlapping or combining observations linked to a group of editing target nodes belonging to the third work area.

38. In paragraph 36, The above operation unit is, When setting the editing range set on the above local map, An autonomous driving system for a robot, characterized in that a limited editing range is set according to a distance scale centered on a node of the above editing standard.

39. In paragraph 38, The above operation unit is, Select a node to be edited within the third work area according to the first distance measure centered on the node of the above editing standard, An autonomous driving system for a robot, characterized in that the editing range for observation of a node of an editing target is set according to a second distance scale centered on a node of the above editing standard.

40. In paragraph 38, The above operation unit is, An autonomous driving system for a robot, characterized in that the following restrictions are applied to the editing range for observations linked to the nodes of the above-mentioned editing target. 1) Radius range centered around the node of the editing criteria. 2) Observation linked to a node of the target of editing that is blocked by an obstacle between it and the node of the editing standard.

Images