Apparatus and method for performing localization of a robotic device

Abstract

An aspect of a method for performing localization of a robotic device includes subdividing an environment of the robotic device into a plurality of voxels, assigning a hash value to each voxel, and assigning to each hash value a list of map points of a map of the environment of the robotic device that are located within the voxel assigned the hash value. The method further includes, for each of a plurality of localization iterations, obtaining observations via a point cloud representing surface points of an object in the environment associated with the robotic device, and evaluating candidate locations using the hash values ​​of the points and the list of map points assigned to the hash values ​​of the points.

Description

[Technical Field]

[0001] Various aspects of the present disclosure relate to apparatus and methods for performing localization of a robotic device. [Background technology]

[0002] In an autonomous mobile robotic device, the robotic device must be able to determine its location within the environment. This can be done based on map data about the environment and data from sensors by matching points corresponding to obstacles indicated by the map data with points corresponding to obstacles detected by the sensors (i.e., indicated in the sensor data). Such localization approaches, such as Monte-Carlo localization, require searching among the points indicated by the map data for nearest neighbors to points from the sensor data. Because the map data may contain a large number of points, it is desirable for the approach to be able to find nearest neighbors and perform localization within a timeframe that makes the localization applicable to real-time control of the robotic device. Summary of the Invention

[0003] Various embodiments relate to a method for performing localization of a robotic device, including subdividing an environment of the robotic device into a plurality of voxels, assigning a hash value to each voxel, and assigning to each hash value a list of map points of a map of the environment of the robotic device that are located within the voxel to which the hash value is assigned. The method further includes, for each of a plurality of iterations of the localization, obtaining observations via a point cloud indicative of surface points of an object in an environment associated with the robotic device; For each of a plurality of candidate configurations of the robotic device in the environment, for each point in the point cloud, determining a hash of a voxel to which the point would belong if the robotic device were in the candidate location, linearly searching through a list of map points assigned to the determined voxel for a map point closest to the point cloud point, and evaluating the candidate locations based on the distance of the point cloud point to the map point in the list closest to the point cloud point.

[0004] According to one embodiment, a method includes performing localization of a robotic device using Monte Carlo localization, where each candidate configuration corresponds to a particle, and weighting each particle according to the evaluation of the candidate location.

[0005] According to one embodiment, a method includes determining, for each point in the point cloud, a distance of the point in the point cloud to a map point in a list that is closest to the point in the point cloud, and evaluating candidate locations based on a combination of the determined distances.

[0006] According to one embodiment, each candidate configuration includes a candidate location.

[0007] According to one embodiment, each candidate configuration includes a candidate direction.

[0008] According to one embodiment, the environment is a 3D environment and the map is a 3D map.

[0009] According to one embodiment, the observations are obtained from sensor data obtained by sensors of the robotic device.

[0010] According to one embodiment, the sensor data includes lidar sensor data and / or radar sensor data.

[0011] According to one embodiment, the method includes determining a configuration of the robotic device from the evaluated candidate positions.

[0012] According to one embodiment, there is provided a method for controlling a robotic device, the method including performing localization of the robotic device of any one of the above embodiments and controlling the robotic device according to the determined configuration.

[0013] According to one embodiment, there is provided a robotic control system including one or more sensors, a memory, and a processing unit configured to perform the method of any one of the above embodiments.

[0014] According to one embodiment, there is provided a computer program element comprising program instructions that, when executed by one or more processors, cause the one or more processors to perform the method of any one of the above embodiments.

[0015] According to one embodiment, a computer-readable medium is provided that includes program instructions that, when executed by one or more processors, cause the one or more processors to perform the method of any one of the above embodiments. [Brief explanation of the drawings]

[0016] The invention will be better understood by reference to the detailed description when considered in conjunction with the non-limiting examples and the accompanying drawings. [Figure 1] Show the robot. [Figure 2] Illustrates the robot's position as a probability distribution. [Figure 3] Illustrates the iterative process of Monte Carlo localization. [Figure 4] 1 illustrates the generation of a sparse map hash table. [Figure 5] FIG. 1 shows a flow diagram illustrating a method for performing localization of a robotic device. [Figure 6] 1 illustrates a robotic control system according to one embodiment. Detailed Description of the Invention

[0017] The following detailed description refers to the accompanying drawings, which show, by way of example, specific details and embodiments in which the present disclosure may be practiced. These embodiments are described in sufficient detail to enable those skilled in the art to practice the present disclosure. Other embodiments may be utilized, and structural and logical changes may be made, without departing from the scope of the present disclosure. Various embodiments are not necessarily mutually exclusive, as some embodiments may be combined with one or more other embodiments to form new embodiments.

[0018] Embodiments described in the context of one device or method are equally valid for other devices or methods, and similarly, embodiments described in the context of a device are equally valid for a vehicle or method, and vice versa.

[0019] Features described in the context of one embodiment may also be applied to the same or similar features in other embodiments. Features described in the context of one embodiment may also be applied to other embodiments, even if other embodiments are not explicitly described. Furthermore, additions, combinations, and / or substitutions of features described in the context of one embodiment may also be applied to other embodiments.

[0020] In the context of various embodiments, the articles "a," "an," and "the" when used in reference to a feature or element include a reference to one or more of the feature or element.

[0021] As used herein, the term "and / or" includes any and all combinations of one or more of the associated listed items.

[0022] In the following, the embodiments are described in detail.

[0023] FIG. 1 shows a robot 100 .

[0024] 1 is a four-legged robot with four legs 101 for walking on a ground surface 102 and one lidar sensor 103 (or multiple lidar sensors) for observing its environment (i.e., its surroundings), in particular the ground surface 102 and objects 104 (typical obstacles). Instead of or in addition to lidar, the robot may acquire other types of sensor data, such as radar data, images of the robot's environment (e.g., RGB-D, i.e., color plus depth images).

[0025] Through the acquired sensor data, the robot 100 observes surface points of objects in the robot's environment, which may be represented as a point cloud, i.e., the robot's processing unit 105 can generate a point cloud 108 from sensor data delivered by one or more sensors 103, the sensor data comprising 3D position points representing surface points of the object 104 detected by the one or more sensors 103.

[0026] The robot 100 may further store a 3D map 106 of its environment in memory 107. The 3D map may also be represented in the form of a point cloud, i.e., an array of points, where each point defines the spatial location of a surface point of an object (i.e., a landmark) in 3D space represented by (x, y, z) coordinates. These points thus represent the objects observed by lidar (or potentially other sensors) when the map was constructed.

[0027] Using the point cloud 108 and map 106 generated from sensor data (received by the robot 100 at its current location), the data processing device 105 may perform localization, i.e., determine the position and / or orientation of the robot within the environment by matching the point cloud 108 with the map 106.

[0028] Mobile robot localization may be performed using probabilistic methods. One approach is the use of particle filters. Such approaches are typically also called Monte Carlo localization.

[0029] FIG. 2 illustrates a representation of a robot's position (also known as the robot's reliability) as a probability distribution 200.

[0030] The complete pose of the robot in 3D space may be represented by six values ​​(e.g., a tuple (x, y, z, θ, φ, ω)), and therefore the probability distribution of actual poses is in 6D space (rather than 1D space as shown in FIG. 2), giving for each 6D point the probability that the robot 100 has the pose given at that 6D point. Note that in the following, when the position of the robot 100 is referred to, this may generally be the pose (i.e., including the orientation) of the robot. This means that each particle may represent not only a position but also a complete pose. The term "configuration" may also be used to refer to a position, orientation, or pose.

[0031] According to various embodiments, a particle filter-based approach for localization is used (e.g., by processing unit 105), which means that instead of solving a mathematical equation for probability distribution 200, probability distribution 200 is sampled, i.e., represented by a number of samples, each of which is called a particle 201.

[0032] For localization at a time t (i.e., the current time), it is assumed that the probability distribution 200 of the robot 100 at the previous time (or time step) t−1 is known. The goal of localization is to determine the probability distribution at the current time step t. Furthermore, it is assumed that the robot has a means to estimate its movement (or, in general, its pose change) from t−1 to t, for example, by performing odometry based on sensor readings of the robot 100's motion sensors.

[0033] Figure 3 illustrates the iterative process of Monte Carlo localization.

[0034] The process begins at 301 with a known probability distribution at time t-1 represented by a set of particles 305.

[0035] In 302, an estimate of the robot's movement (obtained from odometry) from time t to time t-1 is applied to each particle, so that there is an updated particle set 306 containing an updated particle for each particle in set 305. The updated particle for a particle represents a position corresponding to the position represented by the (unupdated) particle adjusted by the estimated movement (e.g., shift, which for poses may also include rotation). This may be done noisily; for example, if a movement of 1 m to the right is estimated, then each particle is randomly shifted to the right according to a probability distribution with mean 1 m.

[0036] In 303, for each particle in the updated particle set 306, a score is calculated using the sensor data (observations). This is done as follows: A particle represents a location estimate. According to the map and its estimated location, the obstacle 104 is expected to be at a specific location (i.e., at a specific distance and direction) relative to the robot. These expected locations are matched with points in the point cloud obtained from the sensor data. An error can then be calculated using the distance of each point in the point cloud to the nearest map point (i.e., the nearest point where the map actually identifies the obstacle as being located). If the distance between that point and the nearest map point exceeds a threshold, the error for that point in the point cloud is discarded. The particle's error (which may be considered a candidate location or an estimate of a candidate location) is then the combination of all these minimum distances, for example their sum or average according to some averaging function.

[0037] The particle's score is determined based on the error (eg, as the inverse of the error).

[0038] In 304, the particles for the current position (i.e., at time t) are redetermined, where the smaller the error (i.e., the higher the score), the higher the probability that a particle will be sampled again (i.e., kept in the particle set). Particles that are not selected are simply removed. The same particle may be selected multiple times to form multiple copies of it in a new particle set (so that the number of particles remains the same even though some are removed).

[0039] In the above localization approach, as explained, in 303, the error for each particle is computed by projecting the point cloud generated from scan data (e.g., lidar scan) for time t onto each point and then performing a nearest neighbor search for each point in the point cloud with respect to map 106. This means that the positions of the obstacle surface points in the map reference frame are computed by assuming that the robot 100 is in the position (or, typically, has the pose) represented by the particle, computing the obstacle surface positions (also called observed or projected points) from their positions relative to the robot (as given by the scan data), and then using the displacement (distance) of the observed points from their corresponding nearest map points (i.e., the point of the obstacle surface position given by the map). This process is also known as scan-to-map matching. Since the closest map point (i.e., the surface point of the obstacle contained in the map) is used for the particle error, a straightforward approach would require the data processing unit 105 to look up, for each projection point, all possible obstacle surface points in the map and find the one that is closest to the projection point. However, the map 106 can potentially become very large, resulting in a very large amount of computation that may be so large that real-time localization is no longer possible.

[0040] The example in Figure 3 shows only 10 particles (one lidar scan point 307 and two obstacle points 308 from the map). This suggests that only 10 × 1 nearest-neighbor searches are required. In practice, however, a 16-channel lidar may have approximately 30,000 scan points, and Monte Carlo localization typically uses about 500 particles. To improve runtime performance, practical implementations typically subsample the lidar scan by about 500 scan points to perform scan-to-map matching. However, doing so still requires 500 × 500 nearest-neighbor queries. This is not scalable, as point cloud maps typically have millions of points. This results in scan-to-map matching being very slow for localization algorithms. For such localization to be realistically deployed on robots, real-time performance on the order of 10 Hz is required.

[0041] Number of subsampled lidar scan points used in the scan = K Number of particles used in Monte Carlo localization = N Number of points in the point cloud map = P Let's say. These parameters specify the problem size of the nearest neighbor search: the linear method of searching all points when doing scan matching scales as O(KNP).

[0042] Among the three variables, P can potentially scale to very large numbers when deployed on real-world point cloud maps. K and N can typically be kept constant at around 500 without significant degradation of localization. Therefore, it is desirable for localization performance to scale with P.

[0043] An approach to increase the use of KD trees in searches. This involves building a KD tree of P particles in a point cloud map, where each leaf of the tree represents a small subset of points, and using a query point to search the KD tree up to the leaf. The nearest neighbor is one of the points in the leaf. Thus, a time complexity of O(logP) and a space complexity of O(P) (memory) can be achieved. However, while this may be usable in certain scenarios, the time complexity of O(logP) may still be too high for large maps running on edge computing devices.

[0044] Another approach is to use an occupancy grid map and distance field lookup. This involves constructing a grid array of the boundary of the operating environment. For each cell in the grid, the distance to the nearest occupied cell is pre-computed. In nearest-neighbor lookup, the corresponding cell in the grid array is queried to find its distance from the nearest map obstacle. This results in constant time complexity during runtime, but O(LW−H) space complexity, where L, W, and H correspond to the length, width, and height of the environment, respectively. Therefore, the space complexity does not scale well with 3D maps and would quickly overwhelm a workstation PC. Therefore, this approach (due to the O(LW) space complexity) can only reasonably be applied to 2D localization in limited settings.

[0045] In view of the above, various embodiments provide a sparse map approach for mobile robot localization.

[0046] In this context, a sparse map is a representation of a 3D point cloud map 106 for nearest neighbor searches. O(1) distance field queries are desirable without the cost of O(LWH) space complexity as with occupancy grid maps and distance field lookups.

[0047] This is achieved by implementing a spatial hash table data structure (i.e., a sparse map hash table). This approach allows us to leverage this data structure as if it were a regular occupancy grid, without the constraints of the occupancy grid map and distance field lookups mentioned above.

[0048] The input to the spatial hash table data structure is a 3D map point, identified by, for example, (x,y,z) coordinates. Each 3D map point is a 3D point discretized into an (i,j,k) voxel grid of the environment and spatially hashed to a bucket index suitable for a hash table entry. By choosing an appropriate voxel size for the environment, the point closest to a query point (i.e., the point in the point cloud obtained from the scan data for which the closest map point is to be determined) is likely to be in the same voxel (and therefore correspond to the same row in the hash table) as its closest map point (i.e., its nearest neighbor). This significantly reduces the number of possible nearest-neighbor checks (i.e., map points that need to be checked for proximity to the query point). The expected runtime performance is approximately <10 due to potential collisions in the hash table.

[0049] Table 1 below describes the sparse map approach according to one embodiment in pseudocode. [Table 1]

[0050] In preprocessing, a sparse map representation (a sparse map hash table) is constructed from the map.

[0051] FIG. 4 illustrates the generation of a sparse map table.

[0052] To construct the sparse map hash table 400, a hash function 402 is used to map (i.e., hash) the coordinates of each map point 401 and output a hash table key 403 for the map point 401. For each hash value 403 (used as the table key), the sparse map hash table has a list 404 of the map points 401 that are mapped to that hash value.

[0053] During runtime, i.e., when performing localization for time t, for each point in the point cloud obtained from the sensor data at time t (e.g., for each lidar scan point), the coordinates of that point are hashed, and the list of map points associated with that hash value (i.e., contained within the voxel having that hash value) is looked up in the sparse map hash table. Next, a linear nearest-neighbor check (i.e., a linear search through the list of map points associated with hash values ​​obtained by hashing the coordinates of the point in the point cloud) is performed for that point for all points retrieved from the hash table in this manner.

[0054] Hashing the coordinates of a point (which can be a map point, but also a point from a point cloud obtained from sensor data) means finding in which voxel (in the i,j,k coordinate system of voxels) this point is located and using its (x,y,z) coordinate to hash the voxel coordinate (i,j,k) (i.e., basically hashing the voxel).

[0055] This way, all points within the same voxel will hash to the same hash value (and therefore table key). The hash function takes as input the (i,j,k) coordinates of the voxel to generate the hash key. Since the map boundaries are discovered during map construction, the (x,y,z) coordinates can be easily converted to (i,j,k) coordinates. Therefore, the minimum of coordinates x_min, y_min, z_min is known for the map. To get the (i,j,k) coordinates of the voxel where point (x, y, z) is located, simply use i = floor( (x - x_min) / x_resolution), j = floor( (y - y_min) / y_resolution), k = floor( (z - z_min) / z_resolution). In this case, the physical interpretation of the i,j,k coordinates is basically that the voxel is the ith voxel from the left, the jth voxel from the top, and the kth voxel from the front of the entire voxel grid.

[0056] Hashing using the (i,j,k) coordinates of the voxel is such that every voxel has a different input (due to collisions in the hash table, two different voxels may still map to the same key; this is unavoidable, but a good hash function will minimize this).

[0057] A nearest neighbor sparse lookup is used for each point in the point cloud obtained from the scan data to determine an error according to 303 in FIG.

[0058] The sparse map approach described above (e.g., in an implementation according to Table 1) can achieve a time complexity (during runtime) of O(1), i.e., constant with respect to problem size, and a space complexity of O(P).

[0059] Table 2 provides a comparison of the performance of the sparse map approach (also called Sparse Lookup) and the KD-Tree based approach for the three maps.

[0060] [Table 2]

[0061] In summary, according to various embodiments, a method is provided as illustrated in FIG.

[0062] FIG. 5 shows a flow diagram illustrating a method for performing localization of a robotic device.

[0063] At 501, the environment of the robotic device is subdivided into a number of voxels.

[0064] At 502, a hash value is assigned to each voxel.

[0065] At 503, each hash value is assigned a list of map points in a map of the robotic device's environment that are located in the voxel to which the hash value is assigned.

[0066] At 504, for each of the multiple iterations of localization, Observations are obtained via a point cloud indicative of surface points of objects in an environment relevant to the robotic device; For each of a plurality of candidate configurations of the robotic device in the environment, for each point in the point cloud, a hash of the voxel to which the point would belong if the robotic device were in the candidate location is determined, a linear search is performed through the list of map points assigned to the determined voxel for the map point closest to the point cloud point, and candidate locations are evaluated based on the distance of the point cloud point to the map point in the list closest to the point cloud point.

[0067] In other words, according to various embodiments, a Monte Carlo localization is performed, where for each point, a nearest neighbor search is performed among the map points that have the same hash value as that point according to a hash function.

[0068] The method of FIG. 5 may be performed, for example, by a robotic control system such as that illustrated in FIG.

[0069] FIG. 6 illustrates a robotic control system 600 according to one embodiment.

[0070] The robot control system 600 includes a sensor 601 (attached to the robot and configured to store sensor data, e.g., lidar data). The robot control system 600 further includes a processing unit 602 and a memory 603. The memory 603 may be used to store, for example, a map, a list of map points associated with each hash value (e.g., a sparse map representation, and a point cloud according to a (current) observation). It should be noted that the localization may be performed by a remote device, i.e., the processing unit 602 and / or the memory 603 do not necessarily have to be part of the robot. The robot control system 600 is configured to execute the method of Fig. 5. The robot is generally a robotic device, which may include any mobile device, such as a vehicle, etc. (particularly a mobile device whose operation may be actively controlled).

[0071] The methods described herein may be performed, and the various processing or computing units, devices, or computational entities described herein may be implemented with one or more circuits. In one embodiment, a "circuit" may be interpreted as any type of logic implementation entity, which may be hardware, software, firmware, or any combination thereof. Thus, in one embodiment, a "circuit" may be a hardwired logic circuit or a programmable logic circuit, such as a programmable processor, e.g., a microprocessor. A "circuit" may also be software implemented or executed by a processor, e.g., any type of computer program, e.g., a computer program using virtual machine code. Any other type of implementation of each function described herein may also be interpreted as a "circuit" according to alternative embodiments.

[0072] Although the present disclosure has been particularly shown and described with reference to particular embodiments, it should be understood by those skilled in the art that various changes in form and detail can be made therein without departing from the spirit and scope of the invention as defined by the appended claims. The scope of the invention is therefore indicated by the appended claims, and all changes that come within the meaning and range of equivalents of the claims are therefore intended to be embraced therein.

Claims

1. 1. A method for performing localization of a robotic device, comprising: subdividing an environment of the robotic device into a plurality of voxels; assigning a hash value to each voxel; assigning to each hash value a list of map points of a map of the environment of the robotic device that are located within the voxel to which the hash value is assigned; During each of the multiple localization iterations, obtaining observations via a point cloud representing surface points of an object in the environment associated with the robotic device; for each of a plurality of candidate configurations of the robotic device in the environment, determining, for each point in the point cloud, the hash of the voxel to which the point would belong if the robotic device were in the candidate location, linearly searching through the list of map points assigned to the determined voxels for the map point closest to the point in the point cloud, and evaluating the candidate location based on the distance of the point in the point cloud to the map point in the list closest to the point in the point cloud. method.

2. 10. The method of claim 1, 10. A method comprising: performing localization of the robotic device using Monte Carlo localization, wherein each candidate configuration corresponds to a particle; and weighting each particle according to the evaluation of the candidate location.

3. A method according to claim 1, determining, for each point in the point cloud, the distance of the point in the point cloud to the map point in the list that is closest to the point in the point cloud; and evaluating the candidate locations based on a combination of the determined distances.

4. The method of claim 1, A method wherein each candidate configuration comprises a candidate location.

5. The method of claim 1, A method wherein each candidate configuration comprises a candidate direction.

6. The method of claim 1, The method, wherein the environment is a 3D environment and the map is a 3D map.

7. The method of claim 1, A method wherein the observations are obtained from sensor data obtained by sensors on a robotic device.

8. 8. The method of claim 7, The method, wherein the sensor data comprises lidar sensor data and / or radar sensor data.

9. The method of claim 1, determining a configuration of the robotic device from the evaluated candidate positions.

10. 10. A method of controlling a robotic device, comprising: performing localization of the robotic device according to the method of claim 9; and controlling the robotic device according to the determined configuration.

11. A robot control system comprising one or more sensors, a memory and a processing unit configured to perform the method of any one of claims 1 to 10.

12. A computer program element comprising program instructions which, when executed by one or more processors, cause said one or more processors to carry out the method of any one of claims 1 to 10.

13. A computer readable medium comprising program instructions that, when executed by one or more processors, cause the one or more processors to perform the method of any one of claims 1 to 10.

Images