Patent Description:
Significant research has been and is being devoted to the problem of developing perception mechanisms for supporting control systems in mobile industrial robots. A satisfactory perception mechanism should enable the robot to navigate constrained and populated environments efficiently, while at the same time avoiding destructive collisions. Objects mixed with other objects of different heights, objects with small dimensions, transparent or reflective objects, dark or black objects are all known to cause difficulties to perception mechanisms attempting to discover and correctly assess them.

So-called occupancy grids, or probability maps, are frequently used tools for modeling the navigable environment of a mobile robot. The cells of the occupancy grid are initialized with neutral probability values, which are subsequently refined based on acquired sensor data. The problem of updating such a probability map based on a sequence of sensor measurements made from a moving platform is discussed in a preprint by <NPL>.

Particularly addressing an indoor mobile robot application where the navigable environment is modeled by a two-dimensional occupancy grid, the present disclosure aims to improve the prior art techniques.

<CIT> discloses a machine-learning algorithm for finding a height map of an environment. A probabilistic generative appearance model, which is iteratively refined into agreement with an observed color map, is an optional further output of the algorithm. The observed color map is used to develop the appearance model. There height map does not direct depend on the color map.

Starting from a psychophysical definition of "saliency" as the ability to attract attention to a particular stimulus, <CIT> proposes computerized techniques for determining salient features in an image or video. An aim of these techniques is to enable the computer to perceive the image like a human.

One objective is to make available methods and devices for providing an occupancy grid to support the controlling of a mobile industrial robot. Another objective is to enable maintenance and successive refinement of such occupancy grid on the basis of continuing measurements by a color-depth sensor, notably a robot-mounted color-depth sensor. A particular objective is to propose efficient data processing techniques suited for use cases where RGB-D (red-green-blue plus depth, i.e., color image and depth map) measurements are available.

These and other objectives are achieved by the method and the robot controller according to the invention defined by the independent claims. The dependent claims are directed to embodiments of the invention.

In a first aspect, there is provided a method of controlling an industrial robot movable on a substrate with the technical features according to claim <NUM>.

The inventor has realized that, to judge whether a cell is free or occupied, it is expedient to identify globally dominant chromaticity values and search for cells in which, concurrently, the chromaticity value of the cell belongs to the globally dominant ones and the elevation value is comparatively small. A criterion based on this combination is therefore a useful tool for building an algorithm that converges in reasonable time, produces a highly accurate occupancy grid, or both. As a result, the potential productive clock time that the robot spends waiting for a fresh occupancy grid to be generated (or for an update of an existing occupancy grid) is reduced. Likewise, robot navigation may become safer and more precise.

In a second aspect, there is provided a robot controller with the technical features of claim <NUM>, which is configured for controlling at least one mobile industrial robot. The robot controller comprises an input interface, processing circuitry and output interface and it is configured to perform the above method.

The invention further relates to a computer program containing instructions for causing a computer, or the robot controller in particular, to carry out the above method. The computer program may be stored or distributed on a data carrier. As used herein, a "data carrier" may be a transitory data carrier, such as modulated electromagnetic or optical waves, or a non-transitory data carrier. Non-transitory data carriers include volatile and non-volatile memories, such as permanent and non-permanent storages of magnetic, optical or solid-state type. Still within the scope of "data carrier", such memories may be fixedly mounted or portable.

The aspects of the present disclosure will now be described more fully hereinafter with reference to the accompanying drawings, on which certain embodiments of the invention are shown. These aspects may, however, be embodied in many different forms and should not be construed as limiting; rather, the embodiments are provided by way of example so that this disclosure will be thorough and complete, and to fully convey the scope of all aspects of invention to those skilled in the art.

<FIG> depicts a setting where the present invention may be applied. Here, an industrial robot <NUM> is movable over a substrate <NUM> by means of wheels <NUM>, bands, claws, movable suction cups or other means of propulsion and/or attachment. The substrate <NUM> may be a planar or curved surface with a horizontal, slanted or vertical orientation; it may optionally be provided with rails or other movement guides adapted to cooperate with the robot <NUM>. Example substrates <NUM> include an indoor floor, outdoor ground, a surface preconditioned to facilitate driving etc. Present are furthermore physical objects which may act as obstacles of different seriousness to the robot's <NUM> movement over the substrate <NUM>. To exemplify, <FIG> shows a box-shaped object <NUM> comparable to the robot's <NUM> height, a puddle of liquid <NUM> and a moderately sized heap of powdery material <NUM>. From these, the puddle of liquid <NUM> will normally not impede the robot's <NUM> movement, and it is therefore an aim that it be treated as a non-obstacle that the algorithm to be described below.

The bookkeeping of obstacles relies on an occupancy grid <NUM>, a data structure which associates each of the multiple cells <NUM> of the grid with an occupancy probability p. An occupancy probability of a cell at grid indices (x, y) may be denoted p(x, y). The resolution of the grid may be expressed as a cell size parameter wres, which may be of the order of <NUM> to <NUM>, such as <NUM>, <NUM> or <NUM>. For navigation of relatively slower-moving and sensitive robots <NUM>, a smaller cell size is preferable. In the present disclosure, unless otherwise indicated, grid indices (x, y) refer to the discretized grid at resolution wres. The grid indices may for example be represented as integer pairs, and real-valued cartesian coordinates (X, Y, Z) (which may have been read from a sensor) may be converted into grid indices (x, y) by orthogonal projection on the Z = <NUM> plane (corresponding to the substrate <NUM>) combined with a rounding operation. The fact that grid indices (x, y) correspond to converted coordinates (X, Y, Z) may be written (X, Y, Z)~(x, y); it is noted that this is a rank-deficient operation which is in general not invertible. The grid indices may optionally be extended with a height index z into an integer triplet (x, y, z), each triplet referring to a cubic grid cell (voxel). In this disclosure, uppercase letters are preferred for coordinates and lowercase letters for grid and height indices.

The present disclosure will rely on ordinary terminology and the commonly embraced theoretical basics of occupancy grids. In particular, an occupancy probability of a cell may be understood as the posterior probability that a physical object is present in the cell. The highest likelihood that a physical object is present corresponds to the value p = <NUM>. Cells with occupancy probability values in the range (<NUM>,<NUM>] may be treated as occupied cells, and cells with values in [<NUM>,<NUM>) may be taken to be free. The left-out value <NUM> represents an unknown occupancy status. The occupancy grid <NUM> is a probabilistic representation that goes beyond the binary distinction occupied/free and thereby enables sensor fusion and the nuanced combining of multiple measurements by a single sensor. In some embodiments, an alternative occupancy grid is used where each cell is associated with a probability (a 'non-occupancy probability') p' that the cell is free, i.e., no physical object is present. This means that the highest likelihood that a physical object is present corresponds to the value p' = <NUM>.

The robot <NUM> is shown equipped with a movable arm <NUM>, end effector <NUM> and sensor <NUM>. The sensor may be elevated, i.e., located some distance Za above the level of the substrate <NUM> to make efficient use of its field of view. The sensor <NUM> may be configured to employ at least one measuring principle that includes sensing an incident electromagnetic wave reflected or emitted by the objects <NUM>, <NUM>, <NUM> on the substrate <NUM> or the substrate <NUM> itself. To exemplify, an incident ray from the box <NUM> is shown in <FIG>. In particular, the sensor <NUM> has the ability of providing an elevation map and a chromaticity map of the substrate <NUM> and any physical objects present on the substrate <NUM>. For instance, an RGB-D sensor may produce a two-dimensional color bitmap, where at least some pixels are annotated with depth values, representing an image plane orthogonal to the orientation of the angle of incidence S. Such depth-annotated pixels may be converted into a point cloud, where each point is represented in three-dimensional coordinates (e.g., cartesian, spherical, radial) and additionally carries a color or chromaticity value. Alternatively, the RGB-D sensor may produce a point cloud directly. Libraries with generically applicable software routines for analyzing point-cloud data and performing operations thereon are available, e.g., from The Point Cloud Library (https://pointclouds.

<FIG> is a schematic representation of a robot controller <NUM> suitable for controlling the robot <NUM> of <FIG>. The robot controller <NUM> may be integral with the robot <NUM> or may be implemented as a separate, stationary or mobile unit. It comprises an input interface <NUM> configured to receive occupancy-related measurements obtained by the sensor <NUM> on the robot, processing circuitry <NUM> configured to assign an occupancy probability on the basis of the received occupancy-related measurements, an output interface <NUM> for supplying commands to the robot <NUM>, as well as an operator interface <NUM>. The interfaces <NUM>, <NUM> and processing circuitry <NUM> may be local resources or networked (or cloud) resources; in particular, the processing circuitry <NUM> may include one processor or multiple processors. The operator interface <NUM> may be a text-based, graphical, auditory and/or haptic interface, by which the robot controller <NUM> may receive instructions directing on high level the commands to be fed to the robot <NUM>, and by which it may output human-intelligible information indicative of a state or condition of the robot <NUM>. <FIG> furthermore indicates an example set of communicative connections (data connections) among the internal functional components <NUM>, <NUM>, <NUM>, <NUM> of the robot controller <NUM>. Alternatively, the connections may be implemented as a central bus providing multipoint connectivity.

To establish and maintain the occupancy grid, the robot controller <NUM> may perform the method <NUM> depicted in <FIG>.

In a first step <NUM>, the occupancy grid is initiated. It may take into account the geometry of the substrate <NUM> and a configured value of the cell size wres. The initiation may further include assigning a neutral occupancy probability (e.g., p = <NUM>, corresponding to an unknown occupancy state) to all cells <NUM> of the occupancy grid <NUM>. Alternatively, a specific placeholder value may be assigned, so as to signify that the occupancy probability has not yet been updated in view of an occupancy-related measurement.

In a second step <NUM>, an elevation map and a chromaticity map of the substrate <NUM> are obtained.

As used herein, an elevation map associates each point of the substrate <NUM> with a value of the elevation (or height) above a reference level Z = Zmin. The reference level may coincide with the substrate <NUM> or may be a dedicated reference plane; the latter option may simplify calculations in cases where the substrate <NUM> is curved. The resolution of the elevation map may be one value per cell <NUM> of the occupancy grid <NUM> or a finer resolution; the elevation value may be continuous or discretized in agreement with the grid resolution wres. In the example illustrated in <FIG>, the physical objects <NUM>, <NUM>, <NUM> have respective elevations h<NUM>, h<NUM>, h<NUM>.

A chromaticity map associates points of the substrate <NUM> with respective chromaticity values. Similar to the elevation map, it is sufficient for the chromaticity map to have one chromaticity value per cell <NUM> of the occupancy grid <NUM>. The chromaticity may refer to any suitable color model, such as RGB, HSL, HSV, Munsell, NCS and the like. For purposes of illustration and not limitation, an RGB representation will be used in this description. If multiple points within one cell <NUM> carry chromaticity values, then their mean value or a value obtained by global smoothing (e.g., fitting a function similar to function h discussed below) may be applied as the chromaticity value of the cell. Alternatively, the multiple chromaticity values are left unchanged, the dominance criterion is assessed for each, and all outcomes of these assessments in the cell are merged into one.

The data source of the two maps may be the sensor <NUM> which, as noted, may be an RGB-D sensor. The sensor's <NUM> output may be (or be convertible into) point-cloud data items of the form (Xi, Yi, Zi, Ri, Gi, Bi), where the three pixel intensity components are separate, or of the form (Xi, Yi, Zi, RGBi), where the pixel intensity components are concatenated into one binary value. Then, starting from a tuple of this form, the second step <NUM> may include:.

The elevation extraction may optionally include a preprocessing operation <NUM>, which eliminates point-cloud points located in remote regions compared to the sensor and/or physically unattainable points. The remote regions in this sense, which may be poorly visible from the sensor <NUM>, may correspond to such points (Xi, Yi, Zi) which satisfy a criterion <MAT> where | · | denotes Euclidean distance, (Xa, Ya, Za) is the position of the sensor <NUM> and d<NUM> is a constant (cutoff point). The constant d<NUM> is dependent on the sensor hardware and the degree of difficulty of the navigable environment; it may for example be of the order of tens of meters in an indoor use case. While the above expression refers to the two-dimensional distance (i.e., measured parallel to the substrate <NUM>), variations of criterion C1 may refer to a three-dimensional distance. Examples of physically unattainable points (Xi, Yi, Zi), to be removed in the preprocessing operation, are those for which Zi < Zmin, which may be due to sensor artefacts.

The preprocessing operation <NUM> may further include eliminating point-cloud points which represent outlier values of the elevation. The fact that an elevation value differs significantly from other observations may suggest its correctness is doubtful, and it may thus be considered to be an outlier. Again, reference is made to the software routines available from The Point Cloud Library (https://pointclouds.

The chromaticity extraction may optionally include an intensity normalization, in which a pixel intensity triplet (R, G, B) is mapped to a pair (r, g) such that <MAT> Each of R, G, B may be a number in the range [<NUM>,<NUM>] which is in a linear relationship with the corresponding intensity of that color component. <FIG> is a graphical representation of the resulting two-dimensional color space. Reference colors green, blue and red have been indicated alongside with white, which corresponds to a flat spectrum where R = G = B. The intensity normalization may help eliminate measurement artefacts related to uneven illumination of different regions of the substrate <NUM>.

In a fourth step <NUM>, a mean elevation Zave(x, y) of each cell is computed and a first partial occupancy probability p<NUM> is derived. The execution of the fourth step <NUM> may be restricted to such cells <NUM> of the occupancy grid <NUM> for which the elevation map contains elevation data. The mean elevation may be computed as follows: <MAT> where N(x, y) is the number of sensor values (Xi, Yi, Zi) falling in the grid cell at (x, y). An alternative way of computing the mean elevation uses a function h such that the surface ε = {(X, Y, Z): Z = h(X, Y)} approximates the point-cloud data {(Xi, Yi, Zi)}. Here, the function h may have a predetermined smoothness (e.g., n times continuously differentiable), which is defined on the substrate <NUM> and fitted to the point-cloud data in such manner that said surface constitutes an Lq approximation for some q ≥ <NUM> or is 'close' to the point-cloud data in another suitable sense. The mean elevation is obtained by sampling the surface ε at each grid cell, as follows: <MAT> This alternative way of computing the mean elevation may make the data accuracy more uniform, as it reduces the random impact of the rounding/quantizing into grid cells of the sensor data.

From the mean elevation, the first partial probability may be computed as follows: <MAT> where Zmax is a maximum height value. The maximum height Zmax may be equal to the height at which the sensor <NUM> is arranged, as this may correspond to the height of the tallest objects that are visible to the sensor <NUM> without obscuration. Alternatively, the maximum height Zmax is set to a value corresponding to what is, for other reasons, considered to be the height of the tallest physical object on the substrate <NUM> that can be reliably discovered by the sensor <NUM> in normal operating conditions.

A subsequent step <NUM> of the method <NUM> comprises multiple substeps <NUM>, <NUM> where the chromaticity data is processed with the ultimate aim of determining a second partial occupancy probability p<NUM> for at least some cells <NUM> of the occupancy grid <NUM>.

In a first substep <NUM>, the distribution of the chromaticity is estimated to identify dominant chromaticity values. The estimation may include forming a histogram with respect to a set of predefined bins or color categories. <FIG> shows an example partition (mesh) of the (r, g) color space into squares of approximately uniform size, which have been numbered sequentially or have been provided with other identifiers. The definition of the partition is equivalent to a function C(r, g) which maps a pair of normalized pixel intensity values (r, g) to a sequential number of the bin. <FIG> shows an alternative partition reflecting the fact that the perceived chromaticity varies relatively slower in the vicinity of the corners (<NUM>,<NUM>), (<NUM>,<NUM>), (<NUM>,<NUM>) and therefore can be reliably captured by a smaller number of bins.

The partition illustrated in <FIG> and to some extent the one in <FIG> may be said to represent world-agnostic partitions, more precisely, in the sense that the density of bins is approximately uniform over the color space. In a use case where it is known or expected that certain chromaticity values are absent or very unusual, such a priori knowledge may be utilized to fashion a color space partition with locally reduced and/or locally increased density. <FIG> shows a histogram referring to the eleven-bin partition of <FIG> for an example chromaticity map. The vertical axis represents a frequency f of chromaticity of the respective bins. The frequency f may be an incidence, such as a number or normalized number of observations falling in each of the bins. For example, one may define <MAT> where Card denotes the number of elements in the set and cmax is the number of bins (here: <NUM>).

As an alternative to using a histogram, the first substep <NUM> may start from a parameterization of a probability density function (ansatz) and vary its parameters so that it matches the sensor data.

In the histogram plotted in <FIG>, the dominant chromaticity values correspond to those of bin <NUM>. A second tier of dominant chromaticity values may include bins <NUM> and <NUM>.

A second substep <NUM> makes use of the knowledge gained from the distribution of the chromaticity, as may be visualized in the form of a histogram. As explained, the inventor has realized that a cell-wise chromaticity value which is equal or close to a dominant chromaticity value (referring to the chromaticity map globally) and occurs in the same grid cell as a non-significant elevation value may suggest that the cell is non-occupied, i.e., free from physical objects. Therefore, for a cell at grid coordinates (x, y) which is associated with chromaticity (r, g) = (r(x, y), g(x, y)), the second partial occupancy probability may be assigned as per <MAT> Here, the relative dominance of the chromaticity bin, to which the cell belongs, is quantized as a fraction of the most populated chromaticity bin. If the chromaticity value belongs to the most populated (dominant) bin, then p<NUM>(x, y) = <NUM>. Alternate definitions of p<NUM> include <MAT> for some constant <NUM> < k < <NUM>, and <MAT>.

In a next step <NUM>, the first and second partial occupancy probabilities p<NUM>(x, y) and p<NUM>(x, y) for a cell at grid coordinates (x, y) are merged to obtain an occupancy probability p(x, y) to be assigned to the cell in the occupancy grid <NUM>. The first and second partial occupancy probabilities may be combined linearly or nonlinearly. One may for example set <MAT> where coefficients α<NUM>, α<NUM> > <NUM> are equal or different. Suitable coefficient values may be determined by empirical evaluation, wherein an initial guess may reflect the expected data quality of the elevation map and the chromaticity map, or its value as guidance for judging whether a physical object is present in the cell.

The fact that each one of the two partial occupancy probabilities contributes reflects the inventor's realization that the combination of a dominant chromaticity value and a low elevation is significant to suggest that a cell is likely free, i.e., that a low occupancy probability is to be assigned. It is recalled from the example of <FIG> that the puddle of liquid <NUM> has a color different than the substrate <NUM>, which taken alone suggests that it is an obstacle, but at the same time a negligible elevation h<NUM> ≈ <NUM>, which will tend to disqualify it as an obstacle.

In some embodiments, the first and second partial occupancy probabilities p<NUM>(x, y) and p<NUM>(x, y) are further merged with a pre-existing occupancy probability pold(x, y) of the cell. For this purpose, a recursive rule, as Bayes' rule, may be applied. The merging may proceed as follows: <MAT>.

As a final step <NUM> of the method <NUM>, the robot controller <NUM> may control movement of the industrial robot <NUM> on the basis of the occupancy grid. Such controlling may include local motion planning, tactical route planning, collision avoidance, speed regulation and similar tasks.

Claim 1:
A method (<NUM>) of controlling an industrial robot (<NUM>), which is movable on a substrate (<NUM>), comprising:
initializing (<NUM>) an occupancy grid (<NUM>) of cells (<NUM>) which each represents a portion of the substrate and is associated with an occupancy probability that some physical object (<NUM>) is present in the cell;
obtaining (<NUM>) an elevation map and a chromaticity map of the substrate;
computing (<NUM>), on the basis of the elevation map, a mean elevation of each cell and deriving a first partial occupancy probability, wherein a relatively higher value of the first partial occupancy probability corresponds to a relatively greater mean elevation and a relatively lower value of the first partial occupancy probability corresponds to a relatively smaller mean elevation;
on the basis of the chromaticity map,
- estimating (<NUM>) a distribution of the chromaticity,
- identifying dominant chromaticity values, and
- computing (<NUM>), on the basis of a chromaticity of each cell, a second partial occupancy probability, wherein a relatively lower value of the second partial occupancy probability is assigned to the cell if its chromaticity is one of the dominant chromaticity values and a relatively higher value of the second partial occupancy probability is assigned to the cell if its chromaticity is not one of the dominant chromaticity values;
merging (<NUM>), for each cell, the first and second partial occupancy probabilities into an occupancy probability for the cell; and
controlling (<NUM>) the industrial robot on the basis of the occupancy grid.