Autonomous driving system of robot

Abstract

According to the present invention, disclosed is an autonomous driving system of a robot. According to one embodiment of the present invention, disclosed is an autonomous driving system of a robot in which robust pose estimation is achieved from landmarks. In addition, according to one embodiment of the present invention, disclosed is an autonomous driving system of a robot in which pose estimation is achieved on the basis of a probability distribution or the frequencies of particle samples pertaining to the pose of the robot adaptively adjusted according to a driving state of the robot and a surrounding environment.

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 robust pose estimation from landmarks is implemented.

[0005] One embodiment of the present invention includes an autonomous driving system for a robot in which pose estimation based on a probability distribution or frequency of particle samples regarding a pose of the robot is adaptively adjusted according to the driving state of the robot and the surrounding environment is implemented.

[0006] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which robust pose estimation from landmarks is implemented according to one embodiment of the present invention,

[0007] A computational unit that implements simultaneous localization and mapping (SLAM) for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment or mapping to infer spatial information about the surrounding environment together with the localization,

[0008] A landmark installed around the driving path of the robot includes an operation unit that infers a relative pose between the robot and the positioning code or positioning marker from observation of the positioning code or positioning marker visually observed from a sensor of the robot.

[0009] In order to solve the above-mentioned and other problems, an autonomous driving system of a robot is provided, in which pose estimation is implemented based on a probability distribution or a frequency of particle samples for a pose of a robot that is adaptively adjusted according to the driving state of the robot and the surrounding environment according to one embodiment of the present invention.

[0010] A computational unit that applies a particle filter for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment or simultaneous localization and mapping (SLAM) to simultaneously implement localization and mapping to infer spatial information about the surrounding environment.

[0011] For a particle set that adaptively represents a probability distribution or probability distributions about the pose of the robot, depending on whether the robot is in a driving state or an abnormal state,

[0012] i) adjusting the probability distribution to be divergent so that the probability exists over a relatively wide first range, or adjusting the frequency of the particle set to be divergent so that the particle samples are distributed over a relatively wide first range; or

[0013] ii) Includes an operation unit that adjusts the probability distribution to converge so that the probability exists only up to a relatively narrow second range, or adjusts the frequency of the particle set to converge so that the particle samples are distributed only up to a relatively narrow second range.

[0014] In one embodiment of the present invention, a pose of a robot robust to noise and outliers can be inferred based on observations of landmarks, and clustering (DBSCAN) is applied to a plurality of data predicted as the pose of a landmark from a plurality of accumulated observations of the same landmark, and a mean and covariance are extracted from a cluster of data with a relatively high density from the clustering, and the pose of the landmark can be inferred from the extracted mean, and an uncertainty of measurement can be calculated in pose graph optimization (PGO) for an edge connecting a landmark node and an observation node that observed the landmark node with respect to the pose of the landmark from the extracted covariance, and data scattered with a relatively low density from the clustering are removed as noise or outliers, that is, noise and outliers can be removed from observations of landmarks by applying clustering to a plurality of observations of the same landmark, and the accuracy of prediction of the pose of the robot from observations observed from the pose of the landmark predicted with improved accuracy and the pose of the robot can be improved. , and as a result, inference of the pose of a robot that is robust to noise and outliers can be implemented.

[0015] In one embodiment of the present invention, in mapping for inferring spatial information about a surrounding environment, a landmark node and an observation node that observed the landmark node can be generated on a pose graph for a predicted landmark with improved accuracy while removing noise and outliers by applying clustering to a plurality of accumulated observations that observed the same landmark, and in localization for inferring a pose of a robot on the mapped pre-built spatial information, a pose of a robot can be predicted (prediction) from an observation that observed a landmark, or a predicted pose of a robot can be corrected (correction).

[0016] In one embodiment of the present invention, pose estimation based on the spatial frequency of a probability distribution or particle sample for a pose of a robot that is adaptively adjusted according to the driving state of the robot and the surrounding environment is implemented, so that the spatial frequency in a probability distribution or a particle set for the probability distribution for adaptively predicting the pose of the robot is adjusted in a divergent or convergent form according to the driving state of the robot, such as a driving environment in which the robot is accelerating or decelerating, or a driving environment in which the robot is approaching a target position, or by adjusting the spatial frequency in a probability distribution or a particle set for the probability distribution for predicting the pose of the robot in a divergent or convergent form according to a dynamic surrounding environment that is relatively highly variable or a static surrounding environment that is relatively less variable depending on the surrounding environment, thereby increasing the accuracy of prediction of the pose of the robot by considering different driving environments according to each driving state, while efficiently saving computational resources for applying a motion model to each particle sample forming a particle set in order to predict the pose of the robot, and increasing the accuracy of prediction of the pose of the robot by considering the dynamic / static characteristics of the surrounding environment depending on the surrounding environment. Inefficient waste of resources can be prevented, and the spatial distribution of the probability distribution or particle set for predicting the pose of the robot in different divergence and convergence forms by considering the driving state of the robot and the surrounding environment can be adaptively adjusted, thereby improving the accuracy of prediction of the pose of the robot while reducing the computational burden compared to a comparative example that does not differentiate according to the driving state of the robot and the surrounding environment.

[0017] In one embodiment of the present invention, even in an abnormal state of a robot, such as an abnormal state of a driving motor for providing driving power of the robot or an abnormal state of a sensor of the robot, instead of stopping the prediction of the pose of the robot by applying a particle filter, by adjusting the spatial frequency of particle samples forming a probability distribution or a particle set regarding the probability distribution regarding the pose of the robot, for example, in a divergent form in which low probability is widely distributed or in a divergent form in which relatively low frequency particle samples are scattered in a wide space, the pose of the robot can be immediately inferred 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. By adjusting the probability distribution regarding the pose of the robot to a divergent form that expands in a wide range while being sparse, the pose of the robot can be predicted over a wide range, and accordingly, the pose of the robot can be immediately predicted when the robot is restored from the abnormal state to the normal state. By inferring poses, it is possible to avoid unexpected situations, for example, by initiating collision avoidance or obstacle avoidance according to pre-planned behavior planning or local path planning.

[0018] 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.

[0019] 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.

[0020] 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.

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

[0022] 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.

[0023] 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.

[0024] 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.

[0025] 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.

[0026] 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.

[0027] 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.

[0028] 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.

[0029] 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.

[0030] 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.

[0031] 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.

[0032] 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.

[0033] 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.

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

[0035] 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.

[0036] 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.

[0037] 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.

[0038] 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.

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

[0040] 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).

[0041] 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).

[0042] 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.

[0043] 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.

[0044] 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.

[0045] 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.

[0046] 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.

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

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

[0049] 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.

[0050] 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.

[0051] 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.

[0052] 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.

[0053] 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.

[0054] 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.

[0055] (5) In order to solve the above-mentioned and other problems, an autonomous driving system of a robot in which robust pose estimation from landmarks is implemented according to one embodiment of the present invention,

[0056] A computational unit that implements simultaneous localization and mapping (SLAM) for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment or mapping to infer spatial information about the surrounding environment together with the localization,

[0057] A landmark installed around the driving path of the robot includes an operation unit that infers a relative pose between the robot and the positioning code or positioning marker from observation of the positioning code or positioning marker visually observed from a sensor of the robot.

[0058] For example, an observation from the positioning code or positioning marker may provide information about the relative pose between the positioning code or positioning marker and the robot.

[0059] For example, an observation from the positioning code or positioning marker may provide information about at least one of a two-axis translational position and a rotation as a relative posture between the positioning code or positioning marker and the robot.

[0060] For example, the above operation unit, in the localization,

[0061] As a positioning code or positioning marker installed around the movement path of the robot, at first and second nodes of different robots along the movement path of the robot, at least one of the relative position and relative attitude of the pose of the robot taken at the first and second nodes where different observations are observed from different observations regarding the positioning code or positioning marker visually observed from the sensors of the robots can be inferred.

[0062] For example, the above operation unit, in the localization,

[0063] A first image frame as an observation regarding the positioning code or positioning marker observed from the first node,

[0064] Between the second image frames, as an observation regarding the positioning code or positioning marker observed from the second node,

[0065] As a relative rigid body transformation for mutual matching of the first and second image frames, at least one of a relative translation component and a relative rotation component can be produced.

[0066] For example, the above operation unit,

[0067] Graph-based SLAM can be implemented to generate a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes.

[0068] For example, the above operation unit,

[0069] According to loop closure detection for the above pose graph optimization (PGO), optimization is performed on nodes including the first and second nodes that observed the positioning code or positioning marker on the loop of the pose graph.

[0070] Predict the predicted observation between the first and second nodes from the pose of the robot with respect to the first node and the pose of the robot with respect to the second node,

[0071] Observations regarding the positioning code or positioning marker observed from the first and second nodes are considered real observations.

[0072] Pose graph optimization (PGO) can be implemented by applying least square optimization to the Mahalanobis distance including the squared error between the predicted observation and the real observation and the uncertainty of measurement.

[0073] For example, the above operation unit,

[0074] According to loop closure detection for the above pose graph optimization (PGO), optimization is performed on nodes including the first and second nodes that observed the positioning code or positioning marker on the loop of the pose graph.

[0075] Predict the predicted observation between the first and second nodes from the pose of the robot with respect to the first node and the pose of the robot with respect to the second node,

[0076] As observations observed between the first and second nodes, the observations regarding the positioning code or positioning marker observed in the form of an image frame from the vision sensor of the robot and the observations regarding the surrounding environment observed in the form of a point cloud from the lidar sensor of the robot are different real observations.

[0077] By applying least square optimization to the Mahalanobis distance including the uncertainty of the measurement of the observation from the vision sensor of the robot and the uncertainty of the measurement of the observation from the lidar sensor as the Mahalanobis distance with the squared error between the predicted observation and the real observation and the uncertainty of the measurement, pose graph optimization (PGO) can be implemented.

[0078] For example, the positioning code or positioning marker may include a QR code or an April-Tag.

[0079] For example, the above operation unit,

[0080] In mapping to generate spatial information about the surrounding environment, a landmark node can be generated regarding the pose of the landmark.

[0081] For example, the above operation unit,

[0082] An observation node regarding the pose of the robot that observed the landmark node and the landmark node can be created, and an edge connecting the landmark node and the observation node can be created.

[0083] For example, the above operation unit,

[0084] The landmark node can be created by setting a creation condition that a number of observations of the same landmark from the robot are accumulated more than a threshold number of times along the driving path of the robot.

[0085] For example, the above operation unit,

[0086] By applying clustering to the accumulated plurality of observations, poses of different robots that observed each of the accumulated plurality of observations, and a plurality of data predicted as poses of the landmarks from an observation model regarding the observation of the robots, the mean and covariance can be extracted from a cluster of data that is densely packed with relatively high density from the clustering, and data scattered with relatively low density can be removed as noise or outliers.

[0087] For example, the above operation unit,

[0088] The landmark node can be generated from a mean extracted from a plurality of data predicted as the pose of the landmark, an observation node can be generated regarding the pose of the robot that observed the generated landmark node, and an edge can be generated between the landmark node and the observation node.

[0089] For example, the above operation unit,

[0090] From the covariance extracted from a plurality of data predicted as the pose of the above landmark, the uncertainty of the measurement for optimizing the pose graph for the edge connecting the landmark node for the pose of the landmark and the observation node for the pose of the robot that observed the corresponding landmark can be calculated.

[0091] For example, the above operation unit,

[0092] It is possible to implement an edge connecting the landmark node and an observation node regarding the pose of the robot that observed the landmark node, and a pose graph optimization for bundle adjustment of a plurality of nodes generated along the driving path of the robot.

[0093] For example, the above operation unit,

[0094] Generate a node for the pose of the robot at each discrete time step, and generate nodes at a first interval along the driving path of the robot.

[0095] While generating a landmark node according to the observation of the landmark and an observation node that observed the landmark node, a second interval narrower than the first interval can be formed between the observation node and another neighboring node along the driving path of the robot.

[0096] For example, the above operation unit,

[0097] In mapping for generating spatial information about the surrounding environment, a landmark node for the pose of the landmark and an observation node for the pose of the robot that observed the landmark node are generated from the observation that observed the landmark, and an edge connecting the landmark node and the observation node is generated.

[0098] In localization for predicting the pose of a robot on spatial information about the surrounding environment including the above landmark, the pose of the robot that observed the landmark can be predicted from the observation that observed the landmark, or the predicted pose of the robot can be corrected.

[0099] (6) In order to solve the above-mentioned and other problems, an autonomous driving system of a robot is provided, which implements pose estimation based on the frequency of particle samples or probability distribution of the pose of the robot adaptively adjusted according to the driving state of the robot and the surrounding environment according to one embodiment of the present invention.

[0100] A computational unit that applies a particle filter for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment or simultaneous localization and mapping (SLAM) to simultaneously implement localization and mapping to infer spatial information about the surrounding environment.

[0101] For a particle set that adaptively represents a probability distribution or probability distributions about the pose of the robot, depending on whether the robot is in a driving state or an abnormal state,

[0102] i) adjusting the probability distribution to be divergent so that the probability exists over a relatively wide first range, or adjusting the frequency of the particle set to be divergent so that the particle samples are distributed over a relatively wide first range; or

[0103] ii) Includes an operation unit that adjusts the probability distribution to converge so that the probability exists only up to a relatively narrow second range, or adjusts the frequency of the particle set to converge so that the particle samples are distributed only up to a relatively narrow second range.

[0104] For example, the above operation unit,

[0105] i) adjusting the frequency of the probability distribution or the particle set representing the probability distribution with respect to the pose of the robot in a divergent form so that there is a probability that is substantially non-zero (0) within a relatively widely extended first range or there are particle samples within a relatively widely extended range; or

[0106] ii) The frequency of the probability distribution or the particle set expressing the probability distribution regarding the pose of the robot can be adjusted in a convergent manner so that a probability that is substantially non-zero (0) exists only up to a relatively narrowly limited second range, or particle samples exist only up to a relatively narrowly limited second range.

[0107] For example, the above i) adjusting the probability distribution of the robot or the frequency of the particle set to a divergent form,

[0108] i-1) Adjusting the frequency of a probability distribution or a particle set representing a probability distribution on the pose of the robot in a diverging form so that the probability exists up to a first range that is relatively long along the driving direction of the robot or so that particle samples are distributed up to a first range that is relatively long; or

[0109] i-2) It may include adjusting the frequency of particle samples representing the probability distribution or probability distribution on the pose of the robot in a divergent form so that the probability exists up to a first range that is relatively widely extended along an angular range on both sides outside the driving direction of the robot, or the particle samples are distributed up to a first range that is relatively widely extended.

[0110] For example, adjusting the probability distribution of the robot or the frequency of the particle set in the above ii) convergence form,

[0111] ii-1) Adjusting the frequency of the particle set representing the probability distribution or the probability distribution on the pose of the robot to converge so that the probability exists only up to a second range that is relatively shortened along the driving direction of the robot or so that particle samples are distributed only up to a second range that is relatively shortened; or

[0112] ii-2) It may include adjusting the distribution of particle samples representing the probability distribution or probability distribution with respect to the pose of the robot to a convergent form so that the probability exists only in a relatively narrowly limited second range along an angular range on either side outside the driving direction of the robot, or the particle samples are distributed only in a relatively narrowly limited second range.

[0113] For example, in the application of particle filters to localization for inferring the pose of a robot,

[0114] The above operation unit is,

[0115] Following the driving path of the robot, a posterior probability distribution for the pose of the robot generated in the previous step is used as a prior probability distribution for the next step, and a prediction for predicting a probability distribution for the pose of the robot from a motion model and odometry corresponding to the robot system is generated based on the prior probability distribution, and a correction for correcting the probability distribution of the prediction based on an observation (observation) observed from a sensor of the robot and an observation model corresponding to the robot system is generated to generate a posterior probability distribution, and particles are resampled from the generated posterior probability distribution.

[0116] For the posterior probability distribution in which the particles are resampled, the frequency of the particle set expressing the probability distribution or the probability distribution regarding the pose of the robot can be adaptively adjusted, i) in a divergent form, depending on whether the robot is in a driving state or an abnormal state, or ii) in a convergent form, the frequency of the particle set expressing the probability distribution or the probability distribution regarding the pose of the robot can be adjusted.

[0117] For example, the above operation unit,

[0118] Depending on the driving status of the above robot,

[0119] In the moving state of an accelerated driving environment toward a target speed, the frequency of the particle set expressing the probability distribution or probability distribution regarding the pose of the robot is adjusted in a divergent form.

[0120] In the approach state of a decelerating driving environment toward a target speed, the probability distribution of the robot's pose or the frequency of the particle set expressing the probability distribution can be adjusted in a convergent manner.

[0121] For example, the above-mentioned computational unit adaptively adjusts the probability distribution regarding the pose of the robot according to the driving state of the robot.

[0122] In the above moving state, the probability distribution is adjusted in a diverging form so that the probability exists in a range that is symmetrically and balancedly extended along both directions along the driving direction of the robot, or the particle set is adjusted in a diverging form so that the particle sample exists in a range that is symmetrically and balancedly extended along both directions along the driving direction of the robot,

[0123] In the above approach state, the probability distribution can be adjusted in a convergent manner so that probabilities exist only in a range that is symmetrically and balancedly shortened along both one and opposite directions along the robot's driving direction, or the particle set can be adjusted in a convergent manner so that particle samples exist only in a range that is symmetrically and balancedly shortened along both one and opposite directions along the robot's driving direction.

[0124] For example, in the above moving state, the uncertainty of odometry may increase.

[0125] For example, in the above moving state,

[0126] The uncertainty of odometry may increase due to driving that follows a detour path that deviates from the initially set driving path according to the traction force applied to the wheels of the robot or the obstacle avoidance or collision avoidance maneuvers of the robot planned in advance according to the robot's behavior planning or local path planning to deal with unexpected situations.

[0127] For example, the above approach state is a state in which the robot approaches the target position, and may be a state in which the robot approaches the target position at least closer than the moving state.

[0128] For example, in the above approach state, as the robot's position approaches the target position, the uncertainty about the robot's pose can be reduced while the accuracy of inference about the robot's pose can be improved based on observations of the target position or landmarks of the target position at locations adjacent to the target position.

[0129] For example, the above operation unit,

[0130] In the above moving state and approach state, the probability distribution regarding the pose of the robot can be adjusted to diverge or converge symmetrically in one direction and the opposite direction along the driving direction of the robot, thereby increasing the probability of inference regarding the pose of the robot that is at a standstill or decelerated to a standstill from a mutual comparison of observations observed from opposite sides of the robot.

[0131] For example, in the moving state, the uncertainty of odometry may increase as the robot decelerates to a stationary state or close to a stationary state due to an obstacle blocking the rotation of the wheel of the robot.

[0132] For example, in the above approach state, the robot may decelerate to a stop state or close to a stop state as it approaches the target position.

[0133] For example, the above operation unit,

[0134] In the above-described robot's ideal state, inference regarding the robot's pose can be continuously performed according to the application of the particle filter.

[0135] For example, the above operation unit,

[0136] Depending on the abnormal state of the robot, the probability distribution can be adjusted to a divergent form so that the probability exists in a relatively wide first range, or the frequency of the particle set can be adjusted to a divergent form so that the particle samples are distributed in a relatively wide first range.

[0137] For example, the above operation unit,

[0138] Depending on the abnormal state of the robot, the probability distribution regarding the pose of the robot can be adjusted to a sparse form so that the probability distribution is distributed over a relatively wide first range with a relatively low probability, or the particle set expressing the probability distribution can be adjusted to a sparse form so that the particle set is distributed over a relatively wide first range with a relatively low frequency.

[0139] For example, the above operation unit,

[0140] By adjusting the probability distribution or the frequency of the particle set in a divergent form so that there is a probability or a particle sample of the particle set up to a relatively wide first range, the pose of the robot can be inferred as the robot returns from an abnormal state to a normal state.

[0141] An autonomous driving system of a robot according to another aspect of the present invention,

[0142] A computational unit that applies a particle filter for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment surrounding the robot, or for simultaneous localization and mapping (SLAM) to simultaneously implement localization and mapping to infer spatial information about the surrounding environment surrounding the robot.

[0143] Adaptively, depending on the classification of the surrounding environment, for a probability distribution or a particle set representing the probability distribution regarding the pose of the robot,

[0144] i) adjusting the probability distribution to be divergent so that the probability exists over a relatively wide first range, or adjusting the frequency of the particle set to be divergent so that the particle samples are distributed over a relatively wide first range; or

[0145] ii) Includes an operation unit that adjusts the probability distribution to converge so that the probability exists only up to a relatively narrow second range, or adjusts the frequency of the particle set to converge so that the particle samples are distributed only up to a relatively narrow second range.

[0146] For example, the above operation unit,

[0147] Depending on the classification of the surrounding environment,

[0148] a) For a dynamic environment in which there are relatively many obstacles, a relatively high collision probability, or a relatively large change over time, a probability distribution or a particle set expressing a probability distribution regarding the pose of the robot is adjusted to the above i) divergent form,

[0149] b) For a static environment with relatively few obstacles, a relatively low collision probability, or relatively little temporal change, adjust the probability distribution or particle set representing the probability distribution regarding the robot's pose to the convergence form mentioned above ii),

[0150] c) The number of obstacles, the probability of collision, or the temporal change may not be adjusted for the probability distribution or the particle set representing the probability distribution for the pose of the robot for an intermediate environment between a) a dynamic environment and b) a static environment.

[0151] 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.

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

[0153] 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.

[0154] 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.

[0155] 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.

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

[0157] 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.

[0158] 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.

[0159] 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.

[0160] 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).

[0161] Motion model

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

[0163] Observation model

[0164] Zt=CtXt+δt

[0165] 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).

[0166] 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)).

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

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

[0169] 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.

[0170] 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.

[0171] 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.

[0172] 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.

[0173] 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.

[0174] 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.

[0175] 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.

[0176] Kalman Filter Algorithm

[0177] Prediction

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

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

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

[0181] Correction

[0182] (μ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)

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

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

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

[0186] 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).

[0187] 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).

[0188] The following formula: Pose graph optimization

[0189] (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.

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

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

[0192] 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 centered on each node (or vertex) from the Taylor expansion.

[0193] non-linear least square optimization

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

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

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

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

[0198] 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.

[0199] 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).

[0200] 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.

[0201] 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.

[0202] ICP algorithm

[0203] (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)

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

[0205] 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.

[0206] 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.

[0207] 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.

[0208] 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.

[0209] 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.

[0210] The following formula: Pose graph optimization

[0211] (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.

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

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

[0214] Node optimization

[0215] 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.

[0216] 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.

[0217] 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.

[0218] 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.

[0219] 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.

[0220] 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 an excessive amount of 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). Observations can also cause distortions in maps that overlap or combine observations (e.g., global maps). For example, a mismatch can occur between local maps associated with long-term accumulated nodes (or vertices) and local maps associated with recently created nodes (or vertices).

[0221] 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.

[0222] 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.

[0223] 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.

[0224] 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.

[0225] 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.

[0226] 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.

[0227] 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.

[0228] 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. 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. 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, a first node selected as a target for elimination and all observations stored in connection with the first node can be deleted or removed, and by deleting or removing all of 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 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.

[0229] 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.

[0230] 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.

[0231] 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.

[0232] 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.

[0233] 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.

[0234] 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.

[0235] 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).

[0236] 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.

[0237] 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.

[0238] 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.

[0239] 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.

[0240] 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).

[0241] 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.

[0242] 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.

[0243] 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.

[0244] 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.

[0245] 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.

[0246] 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.

[0247] <Partial Mapping>

[0248] 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.

[0249] 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.

[0250] 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.

[0251] 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.

[0252] 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.

[0253] 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.

[0254] 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.

[0255] 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.

[0256] 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.

[0257] 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.

[0258] 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.

[0259] 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.

[0260] 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.

[0261] 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.

[0262] 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.

[0263] 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.

[0264] 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.

[0265] 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 that tracks the driving path of the robot, 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 the predicted observation predicted between the initiating node and the last node and the real observation stored in association with the initiating node and the last node.

[0266] 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.

[0267] 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.

[0268] 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.

[0269] Pose Graph Visualization

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

[0271] 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.

[0272] 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.

[0273] 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.

[0274] 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.

[0275] 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.

[0276] 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.

[0277] 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.

[0278] 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 with respect to 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 of 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 of an environment that delays or impedes the pose graph optimization can be provided.

[0279] 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.

[0280] 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.

[0281] 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.

[0282] 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.

[0283] 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.

[0284] 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.

[0285] 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 differential display elements compared to other nodes in the pose graph that have already been loop closed.

[0286] 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.

[0287] 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.

[0288] 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.

[0289] 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.

[0290] 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).

[0291] 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.

[0292] <observation 편집>

[0293] 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.

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

[0295] 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).

[0296] 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).

[0297] 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.

[0298] 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.

[0299] 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.

[0300] 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).

[0301] 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.

[0302] 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 a 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.

[0303] In one embodiment of the present invention, the matching rate between a recently generated observation (observed from a recently generated node) and a previously generated observation (observed from a previously generated node) is determined by predicting 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 calculating the matching rate between a group of point clouds and another group of point clouds 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 (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 (a third work area) and a temporal condition corresponding to an editing start condition, a recently generated node and a plurality of editing candidates are compared. 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).

[0304] 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 is a matching rate higher than this between previously generated nodes and recently generated nodes, which can be regarded as indicating that the surrounding environment surrounding the robot is substantially maintained as 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.

[0305] 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.

[0306] 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.

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

[0308] 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.

[0309] 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.

[0310] 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.

[0311] 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.

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

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

[0314] The setting of the editing range or the limitation of the editing range of 1) above 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, an editing range within the second distance scale may be set by applying a second distance scale centered on the editing reference node. 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.

[0315] 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.

[0316] 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.

[0317] Landmark

[0318] 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.

[0319] 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.

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

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

[0322] 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.

[0323] 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.

[0324] In one embodiment of the present invention, the sensor 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 the 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.

[0325] 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.

[0326] 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.

[0327] 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.

[0328] 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.

[0329] 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.

[0330] 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.

[0331] 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).

[0332] 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.

[0333] 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.

[0334] 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.

[0335] 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.

[0336] 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.

[0337] 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 the pose of the robot and the observation model for two nodes between two neighboring nodes on the pose graph, and a least square optimization can be applied to the Mahalanobis distance including the squared error and the uncertainty of the measurement between the predicted observation and the real observation from the real observation of the landmark from the pose of the robot for the two nodes, and additionally, a predicted measurement can be predicted from the pose of the landmark for the landmark node and the pose of the robot for the observation node and the observation model between the landmark node and the observation node, and a least square optimization can be applied to the Mahalanobis distance including the squared error and the uncertainty of the measurement (calculated from the covariance extracted from a plurality of data for predicting the position of the landmark, as described above) between the predicted observation and the real observation from the pose of the robot for the observation node, and for example, a Mahalanobis distance for an edge between nodes generated along the driving path of the 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.

[0338] <particle 필터>

[0339] 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.

[0340] 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.

[0341] 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.

[0342] 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.

[0343] 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)).

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

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

[0346] 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.

[0347] 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.

[0348] 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.

[0349] 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.

[0350] 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.

[0351] 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 with respect to a 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).

[0352] 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.

[0353] 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.

[0354] 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.

[0355] 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 balancedly 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.

[0356] 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.

[0357] 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).

[0358] 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, the probability distribution or probability distribution regarding the pose of the robot can be expressed, thereby adjusting the frequency or distribution of spatial particle samples in the work space or pose space of the robot, and for example, while extending the probability distribution regarding the pose of the robot in both directions along the driving 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.

[0359] 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.

[0360] 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, for example, a probability (substantially non-zero probability) can be generated even in a wider range on both sides outside the robot's driving direction.

[0361] 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.

[0362] 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.

[0363] 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.

[0364] 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.

[0365] 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.

[0366] 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.

[0367] 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.

[0368] 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.

[0369] 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.

[0370] 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 insufficient, 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.

[0371] 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.

[0372] 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.

[0373] 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, depending on the driving state (moving state and approach state) of the robot, the abnormal state of the robot, and the surrounding environment surrounding the robot.

[0374] 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.

[0375] 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.

[0376] 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.

[0377] 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

A computational unit that implements simultaneous localization and mapping (SLAM) for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment or mapping to infer spatial information about the surrounding environment together with the localization, An autonomous driving system for a robot, comprising a computational unit that infers a relative pose between a positioning code or positioning marker and the robot from observations of positioning codes or positioning markers visually observed from sensors of the robot, as landmarks installed around the driving path of the robot. In the first paragraph, An autonomous driving system for a robot, characterized in that an observation observed from the positioning code or positioning marker provides information about a relative pose between the positioning code or positioning marker and the robot. In the first paragraph, An autonomous navigation system for a robot, characterized in that an observation observed from the positioning code or positioning marker provides information about at least one of a two-axis translational position as a relative position between the positioning code or positioning marker and the robot and a rotation as a relative posture. In the first paragraph, The above operation unit, in the localization, An autonomous navigation system for a robot, characterized in that at least one of a relative position and a relative attitude of a pose of the robot taken at a first node and a second node, each of which observes different observations from different observations regarding the positioning code or positioning marker visually observed from a sensor of the robot at first and second nodes of different robots along the movement path of the robot, is inferred. In paragraph 4, The above operation unit, in the localization, A first image frame as an observation regarding the positioning code or positioning marker observed from the first node, Between the second image frames, as an observation regarding the positioning code or positioning marker observed from the second node, An autonomous driving system for a robot, characterized in that it produces at least one of a relative translational component and a relative rotational component as a relative rigid body transformation for mutual matching of the first and second image frames. In the first paragraph, The above operation unit is, An autonomous driving system for a robot, characterized by implementing graph-based SLAM that generates a pose graph including nodes related to the pose of the robot and edges connecting different neighboring nodes. In paragraph 6, The above operation unit is, According to loop closure detection for the above pose graph optimization (PGO), optimization is performed on nodes including the first and second nodes that observed the positioning code or positioning marker on the loop of the pose graph. Predict the predicted observation between the first and second nodes from the pose of the robot with respect to the first node and the pose of the robot with respect to the second node, Observations regarding the positioning code or positioning marker observed from the first and second nodes are considered real observations. An autonomous driving system for a robot, characterized in that pose graph optimization (PGO) is implemented by applying least square optimization to the Mahalanobis distance including the squared error between the predicted observation and the real observation and the uncertainty of measurement. In paragraph 6, The above operation unit is, According to loop closure detection for the above pose graph optimization (PGO), optimization is performed on nodes including the first and second nodes that observed the positioning code or positioning marker on the loop of the pose graph. Predict the predicted observation between the first and second nodes from the pose of the robot with respect to the first node and the pose of the robot with respect to the second node, As observations observed between the first and second nodes, the observations regarding the positioning code or positioning marker observed in the form of an image frame from the vision sensor of the robot and the observations regarding the surrounding environment observed in the form of a point cloud from the lidar sensor of the robot are different real observations. An autonomous driving system for a robot, characterized in that pose graph optimization (PGO) is implemented by applying least square optimization to a Mahalanobis distance including uncertainty of measurement for observation from a vision sensor of the robot and uncertainty of measurement for observation from a lidar sensor, as a Mahalanobis distance including a square error between the predicted observation and the real observation and uncertainty of measurement. In the first paragraph, An autonomous driving system of a robot, characterized in that the positioning code or positioning marker includes a QR code or April-Tag. In the first paragraph, The above operation unit is, An autonomous driving system for a robot, characterized in that a landmark node is created for the pose of the landmark in mapping for generating spatial information about the surrounding environment. In Article 10, The above operation unit is, An autonomous driving system for a robot, characterized in that it creates an observation node regarding the pose of the robot that observed the landmark node and the landmark node, and an edge connecting the landmark node and the observation node. In Article 10, The above operation unit is, An autonomous driving system for a robot, characterized in that the landmark node is generated by setting a generation condition that a number of observations of the same landmark from the robot are accumulated more than a threshold number of times along the driving path of the robot. In paragraph 12, The above operation unit is, An autonomous driving system for a robot, characterized in that clustering is applied to a plurality of data predicted as the pose of the landmark from the observation model regarding the observation of the robot, the poses of different robots that observed each of the plurality of accumulated observations, and the observation of the robot, and the mean and covariance are extracted from a cluster of data that is densely packed with relatively high density from the clustering, and the data scattered with relatively low density are removed as noise or outliers. In Article 13, The above operation unit is, An autonomous driving system for a robot, characterized in that it generates a landmark node from a mean extracted from a plurality of data predicted as the pose of the landmark, generates an observation node regarding the pose of a robot that observed the generated landmark node, and generates an edge between the landmark node and the observation node. In Article 13, The above operation unit is, An autonomous driving system for a robot, characterized in that it calculates the uncertainty of measurement for optimizing a pose graph for an edge connecting a landmark node regarding the pose of a landmark and an observation node regarding the pose of a robot that observed the corresponding landmark from covariance extracted from a plurality of data predicted as the pose of the landmark. In Article 10, The above operation unit is, An autonomous driving system for a robot, characterized in that it implements an edge connecting the landmark node and an observation node regarding the pose of the robot that observed the landmark node, and pose graph optimization for bundle adjustment of a plurality of nodes generated along the driving path of the robot. In Article 10, The above operation unit is, Generate a node for the pose of the robot at each discrete time step, and generate nodes at a first interval along the driving path of the robot. An autonomous driving system for a robot, characterized in that a landmark node is created based on observation of the landmark and an observation node that observes the landmark node is created, and a second interval is formed between the observation node and another neighboring node along the driving path of the robot, which is narrower than the first interval. In Article 10, The above operation unit is, In mapping for generating spatial information about the surrounding environment, a landmark node for the pose of the landmark and an observation node for the pose of the robot that observed the landmark node are generated from the observation that observed the landmark, and an edge connecting the landmark node and the observation node is generated. An autonomous driving system for a robot, characterized in that, in localization for predicting the pose of a robot on spatial information about a surrounding environment including the above landmark, the pose of a robot that has observed a landmark is predicted from an observation that has observed the landmark, or the predicted pose of the robot is corrected. A computational unit that applies a particle filter for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment or simultaneous localization and mapping (SLAM) to simultaneously implement localization and mapping to infer spatial information about the surrounding environment. For a particle set that adaptively represents a probability distribution or probability distributions about the pose of the robot, depending on whether the robot is in a driving state or an abnormal state, i) adjusting the probability distribution to be divergent so that the probability exists over a relatively wide first range, or adjusting the frequency of the particle set to be divergent so that the particle samples are distributed over a relatively wide first range; or ii) An autonomous driving system for a robot, comprising a computational unit that adjusts the probability distribution to a convergent form so that the probability exists only up to a relatively narrow second range, or adjusts the frequency of the particle set to a convergent form so that the particle samples are distributed only up to a relatively narrow second range. In Article 19, In the application of particle filters to localization for inferring the pose of a robot, The above operation unit is, Following the driving path of the robot, a posterior probability distribution for the pose of the robot generated in the previous step is used as a prior probability distribution for the next step, and a prediction for predicting a probability distribution for the pose of the robot from a motion model and odometry corresponding to the robot system is generated based on the prior probability distribution, and a correction for correcting the probability distribution of the prediction based on an observation (observation) observed from a sensor of the robot and an observation model corresponding to the robot system is generated to generate a posterior probability distribution, and particles are resampled from the generated posterior probability distribution. An autonomous driving system for a robot, characterized in that, with respect to the posterior probability distribution in which the particles are resampled, the frequency of a particle set expressing a probability distribution or a probability distribution about the pose of the robot is adaptively adjusted in a divergent manner or ii) the frequency of a particle set expressing a probability distribution or a probability distribution about the pose of the robot is adjusted in a convergent manner, depending on whether the robot is in a driving state or an abnormal state. In Article 19, The above operation unit is, Depending on the driving status of the above robot, In the moving state of an accelerated driving environment toward a target speed, the frequency of the particle set expressing the probability distribution or probability distribution regarding the pose of the robot is adjusted in a divergent form. An autonomous driving system for a robot, characterized in that the frequency of a particle set expressing a probability distribution or probability distribution regarding the pose of the robot is adjusted to converge in an approach state of a deceleration driving environment toward a target speed. In Article 21, The above operation unit adaptively adjusts the probability distribution regarding the pose of the robot according to the driving state of the robot. In the above moving state, the probability distribution is adjusted in a diverging form so that the probability exists in a range that is symmetrically and balancedly extended along both directions along the driving direction of the robot, or the particle set is adjusted in a diverging form so that the particle sample exists in a range that is symmetrically and balancedly extended along both directions along the driving direction of the robot, An autonomous driving system for a robot, characterized in that in the above approach state, the probability distribution is adjusted in a convergent manner so that the probability exists only in a range that is symmetrically and balancedly shortened along both directions of one direction and the opposite direction along the driving direction of the robot, or the particle set is adjusted in a convergent manner so that the particle sample exists only in a range that is symmetrically and balancedly shortened along both directions of one direction and the opposite direction along the driving direction of the robot. In Article 21, The above operation unit is, An autonomous driving system for a robot, characterized in that, in the moving state and the approach state, the probability distribution regarding the pose of the robot is adjusted to a divergent or convergent form symmetrically in one direction and the opposite direction along the driving direction of the robot, thereby increasing the probability of inference regarding the pose of the robot that is at a standstill or decelerated to a standstill from a mutual comparison of observations observed from opposite sides of the robot. In Article 19, The above operation unit is, An autonomous driving system for a robot, characterized in that in an abnormal state of the robot, inference regarding the pose of the robot is continuously performed according to the application of a particle filter. In Article 24, The above operation unit is, An autonomous driving system for a robot, characterized in that the probability distribution is adjusted in a divergent form so that the probability exists in a first range that is relatively widely extended according to the abnormal state of the robot, or the frequency of the particle set is adjusted in a divergent form so that the particle samples are distributed in a first range that is relatively wide. In Article 25, The above operation unit is, An autonomous driving system for a robot, characterized in that, depending on the abnormal state of the robot, the probability distribution regarding the pose of the robot is adjusted to a sparse form so that the probability distribution is distributed over a relatively wide first range with a relatively low probability, or the particle set expressing the probability distribution is adjusted to a sparse form so that the particle set is distributed over a relatively wide first range with a relatively low frequency. In Article 25, The above operation unit is, An autonomous driving system for a robot, characterized in that the pose of the robot is inferred as the robot returns from an abnormal state to a normal state by adjusting the probability distribution or the frequency of the particle set in a divergent form so that the probability exists up to a relatively wide first range or the particle sample of the particle set exists. A computational unit that applies a particle filter for localization to infer a pose of a robot on a map containing spatial information about the surrounding environment surrounding the robot, or for simultaneous localization and mapping (SLAM) to simultaneously implement localization and mapping to infer spatial information about the surrounding environment surrounding the robot. Adaptively, depending on the classification of the surrounding environment, for a probability distribution or a particle set representing the probability distribution regarding the pose of the robot, i) adjusting the probability distribution to be divergent so that the probability exists over a relatively wide first range, or adjusting the frequency of the particle set to be divergent so that the particle samples are distributed over a relatively wide first range; or ii) An autonomous driving system for a robot, comprising a computational unit that adjusts the probability distribution to a convergent form so that the probability exists only up to a relatively narrow second range, or adjusts the frequency of the particle set to a convergent form so that the particle samples are distributed only up to a relatively narrow second range. In Article 28, The above operation unit is, Depending on the classification of the surrounding environment, a) For a dynamic environment in which there are relatively many obstacles, a relatively high collision probability, or a relatively large change over time, a probability distribution or a particle set expressing a probability distribution regarding the pose of the robot is adjusted to the above i) divergent form, b) For a static environment with relatively few obstacles, a relatively low collision probability, or relatively little temporal change, adjust the probability distribution or particle set representing the probability distribution regarding the robot's pose to the convergence form mentioned above ii), c) An autonomous navigation system for a robot, characterized in that the number of obstacles, the probability of collision, or the temporal change do not adjust the probability distribution or the particle set representing the probability distribution for the pose of the robot for an intermediate environment between a) a dynamic environment and b) a static environment.

Images