WO2014091043A1

WO2014091043A1 - Simultaneous localization and mapping method for robotic devices

Info

Publication number: WO2014091043A1
Application number: PCT/ES2013/070846
Authority: WO
Inventors: Tomás MARTÍNEZ MARÍN; Eduardo LOPEZ REDONDO
Original assignee: Universidad De Alicante
Priority date: 2012-12-14
Filing date: 2013-12-05
Publication date: 2014-06-19
Also published as: ES2476565A1; ES2476565B2

Abstract

The invention relates to a simultaneous localization and mapping method for robotic devices, comprising at least steps of association, localization and map updating or mapping, characterised in that the modeling of the map in the mapping step is based on entities known as objects, defined as a sequence of moving points of a size that can be dynamically adjusted in order to represent the shape of the contours of real obstacles detected by at least one sensor of the robotic device. Moreover, each point in the sequence representing the objects is associated with a position and a weight indicating the degree of mobility thereof. The basic idea is to create a model of the environment through which a vehicle is moving while it is localized.

Description

SIMULTANEOUS LOCATION AND MAPPING METHOD FOR DEVICES

ROBOTICS

DESCRIPTION

The object of the invention is a simultaneous location and mapping system for robotic devices, known in the field of robotics by its acronym SLAM. The basic idea is to create a model of the environment through which the vehicle moves at the same time that it is located. The way to locate from the observations of the sensor, as well as the way in which the information of the environment is modeled generate the various techniques of the SLAM, technical field where the present invention is integrated.

State of the art

The purpose of the SLAM methods and systems is to use the sensor data of a robotic vehicle (which will be referred to herein as a robot or vehicle) to update its position, as well as the information of the environment stored in a structure called Map. Normally, a laser device is usually used as a sensor that provides the information of the environment. With it, the robot must be able to correct its position and that of the elements contained in the map. This is achieved by extracting different characteristics of the sensor data.

In the case of a laser sensor, characteristic is understood as any consecutive region of points, in the sequence of data provided by the sensor, that belong to the same real object. Each of these characteristics has a position uncertainty, as does the vehicle, which has this uncertainty through the information provided by its odometry system (consisting of the angle of rotation of the wheels and the distance traveled). The update of the position of the vehicle and that of the map elements is carried out taking into account the uncertainty of the observations coming from the sensor and that of the odometry.

In the process of locating, initially, the vehicle detects obstacles through its sensor. Subsequently, the vehicle moves, while its odometry system provides the distance traveled and the angle of rotation of the wheels. With this information a first location hypothesis is established. After that, the robot measures the location of the objects of the environment and based on that location establishes a second hypothesis of location that does not have to coincide with that calculated by odometry. Finally, the robot merges the location hypothesis provided by the odometry and the sensors to, depending on the uncertainty of both, establish a definitive location hypothesis.

Our SLAM proposal consists of three inputs and two outputs. The inputs consist of the last known position of the vehicle (X _t ), the data acquired by external sensors, such as a laser and an image sensor, and finally, the state of the map at the current time (M _t ).

In SLAM, one of the most used external sensors is the laser. It collects the metric information of the environment by performing an angular scan of its laser beam so that, for each angle, the distance or range to the nearest obstacle is known. All the range information acquired for each position of the beam in the same scan is defined herein as "be". On the other hand, odometry information has to do with the distance traveled by the vehicle in a given period of time, as well as with the turning position of its wheels. Normally, this information is usually collected using encoders associated with the drive and rotation motors of the vehicle's wheels.

The map of any SLAM process has the task of saving the information provided by the external sensor. The way in which this information is stored is one of the keys that gives any SLAM strategy its decisive advantage over the rest. Therefore, one of the technical problems to be solved by the present invention is the optimization of the information management provided by the sensor, that is, the establishment of an environment map.

One of the determining factors in the location of any SLAM process is the association of the information received in a moment by a sensor with that already existing on the map. By identifying this information captured by the sensor at one time with another already stored and assuming the invariability of the map, the location of the vehicle can be resolved. This form of location is based on observation, since it requires information from an external sensor for its calculation. So that the association between the sensor information and the information contained in the map is possible, all SLAM processes perform a segmentation of the son. This segmentation consists in the extraction of significant units of information (characteristics) from the set of ranges provided by the sensor. The characteristics are an aggregation of consecutive sensor data that facilitates the association with the information stored in the map at an abstraction level higher than the minimum possible based on simple points.

On the other hand, odometry provides the information necessary to calculate a vehicle location based on its dynamics. This location will be referred to below as location by prediction. Once the location has been calculated using all the vehicle's sensory information: (a) laser (observation location) and (b) encoders (prediction), any SLAM strategy merges both location information to obtain a unique location (X _{t +} i). From this, the map is updated using the information of the laser sensor, obtaining a new state (M _{t +} i).

SLAM techniques can be grouped into two classes: topological SLAM and metric SLAM.

The first of these (topological SLAM) tries to maintain a map of the environment based on relative positions of the most significant places in the environment in which the vehicle moves (rooms, corridors, corridor crossings, etc.). Maps in this category [Adrien Angelí, StéphaneDoncieux, Jean A Arcady Meyer, David Filliat. Visual topological SLAM and global localization. In ICRA'09 Proceedings of the 2009 IEEE internationalconferenceon Robotics and Automation, pages 2029 to 2034] are modeled using graphs of connected nodes, so that each of these represents a specific environment scenario. When the robot arrives at one place on the map and wishes to go to another, check its topological map and find out the distance and direction of the new destination.

In the second case (metric SLAM), the vehicle maintains a map of the most relevant characteristics of the environment, which serve to locate it. The characteristics are stored on the map keeping their most probable position within an absolute metric coordinate system; The probability of the position is modified as the vehicle moves around the environment, using the new observations of the characteristics to reinforce its certainty of position. Next, the different location and mapping techniques used in the metric SLAM techniques in the state of the art are established, since this is where the technical problems solved by the present invention are found and, therefore, where the approximations are proposed more novel of the present invention.

The metric SLAM location techniques are mainly grouped into three basic categories: EKF (Kalman extended filter), RBPF (particle filter) and Sean - Matching.

Mapping techniques are classified into three other categories: brands, grid (grid), patterns and scans alignment. In accordance with this classification, the following SLAM techniques can be considered taking into account all possible relationships between location and mapping methods:

For each of the alternative techniques we have the following references:

[1] R. Smith, M.Self, and P. Cheeseman.Estimating uncertain spatial relationships in robotics.ln Autonomous Robot Vehicles, I. Cox and G. Wilfong, Eds. Springer Verlag, New York, 1988, pp. 167-193.

[2] G. Grisetti, C. Stachniss, and W. Burgard. Improving grid A based slam with rao- blackwellized particle filters by adaptive proposals and selective resampling. In IEEE International Conference on Robotics and Automation (ICRA, 2005).

[3] Juan I. Nieto and José E. Guivant and Eduardo M.Nebot, The Hybrid Metric Maps (HYMMS): A novel map representation for denseSLAM. In IEEE International Conference on Robotics and Automation (ICRA 2004, pp. 391-396).

[4] Chieh-Chih Wang and Charles Thorpe and Sebastian Thrun. Online Simultaneous Localization And Mapping with Detection And Tracking of Moving Objects: Theory and Results from a Ground Vehicle in Crowded Urban Areas. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2003). [5] Tim Bailey and Juan Nieto. Sean- SLAM: Recursive Mapping and Localisation with Arbitrary - Shaped Landmarks. Workshop on Quantitative Performance Evaluation of Navigation Solutions for Mobile Robots, Robotics: Science and Systems Conference (RSS), 2008].

[6] P. Newman, D.Cole and K. Ho. Outdoor SLAM using visual appearance and laser ranging. In IEEE International (Conference on Robotics and Automation, 2006)

Explanation of the invention The object of the present invention is a simultaneous location and mapping system for robotic devices that provides improvements in SLAM processes with respect to the association, location and process of updating the map. As the first two (association and location) result of the update process, the different advantages and contributions of the present invention are shown below.

The contribution in the mapping stage that is proposed is related to the modeling of the map. As mentioned in the state of the art, all the map models used in the existing SLAM techniques are of the type based on: (a) marks; (b) grid of probabilistic cells; or (c) based on patterns, this is a set of consecutive ranges of all received by the sensor (be) and that represent a fragment of a real object.

The first (brands) are the simplest, but also the ones that have the most limitations when it comes to capturing environments with arbitrary forms. They are designed to represent structured environments based on points and lines. Each brand is represented by a position and an associated uncertainty probability. The seconds (the grid) model the environment based on cells with an associated occupancy probability. Third parties (employers) can be considered an extension of the former, in the sense that they try to expand the restricted set of points and lines with which they work in order to take advantage of the information in the form of any obstacle to establish more robust locations in Not too structured environments.

In order to avoid the limitations of the marks and take advantage of the characteristics of the other methods in a simplified manner, the present invention proposes to replace the two-dimensional grid and the patterns by entities called objects, which consist of a sequence of moving points dynamically adjustable in size in order to represent the shape of the contours of the real obstacles being detected. Each point in the sequence that represents the objects has an associated position and weight that indicates its degree of mobility. In the case of brand / pattern and grid maps, there are location probabilities associated with the position of each brand / pattern and cell, which allows the map to be updated to achieve a global consistency. In the case of the invention, each weight associated with a point of the object represents the probability that its position is more or less close to what it actually represents. The advantages of this map model approach are several. Thus, in relation to the brand-based strategy, it allows you to model any contour accurately and in detail without substantial loss of the memory used.

In relation to grid-based models, although both strategies allow to capture contour information in any way, in our case the memory required to store the information of objects grows linearly with their number and length and not quadratically according to the surface of map explored so far.

In relation to pattern-based models, it allows you to incrementally update their shape and size and, in addition, allows you to establish a probability of local consistency for each point of the object, rather than a single probability associated with the entire pattern. In addition, map objects can be adapted to local deformations of the contours of obstacles that may appear in real time. In contrast, patterns are not averaged to reduce or eliminate uncertainty.

Another advantage to note is that the grid-based method does not provide information on the connectivity of the cells that are occupied (FIG. 17). However, the present invention stores the information of the objects as independent entities from each other, which allows establishing association strategies based on the relative positions of the objects. These objects, therefore, are defined as an orderly sequence of points with information on their position (X, Y) and a weight (P) proportional to the degree of uncertainty of that position. The ordering of the points makes it easy to introduce concepts of computational geometry, such as occlusion and the analysis of the shape of objects. Thus, according to what is stated in the state of the art, the maps based on marks only model points, lines or patterns (defined as the set of measurements of the sensor). The overall consistency of the map is achieved by maintaining a covariance matrix that reflects the uncertainties in the positions of the state vector used, that is, the state of the vehicle (position (X, Y) and orientation) and position of the rest of the marks.

The grid-based map achieves the consistency of the map by maintaining a probability map for the cell. The map can be modified as the probability associated with its cells varies.

However, object-based maps use the weights associated with each point of them as a measure of their overall consistency. When a point is added to an object, its associated weight is the smallest possible (for example, 1). As its position is updated over time based on the measurements received from the sensor, its weight increases, so that it is proportional to the number of updates of its position using the sensor information (FIG. 17). One of the fundamental differences observed between the object map and the grid is that the first stores the contour information classified naturally by physical objects detected by the sensor. In addition, the weighting of the points of the objects allows, as in the case of maps based on marks / patterns and grid, to ensure the overall consistency of the map as the system evolves over time.

In the case of grid and objects, in the first there is no information on the connectivity of the cells, while in the second, it simply appears naturally. As illustrated in FIG. 17, the occupancy map does not take advantage of the potential advantage of knowing that all cells with an occupancy probability equal to 0.6 form a single related grouping, just as cells that have a probability of 0.7 may also be representing The same physical contour. This way of representing the physical reality internally provided by the sensor makes it easy for us to introduce concepts of computational geometry of the type occlusion between objects and object shape analysis to know if they are closed on themselves or to associate, based on shape, sensor characteristics With existing objects. The way of storing sensor information in objects allows us to associate the sensor segmentation (related groupings of sensor points that refer to the same physical object and that we will call "features") with the map information. In the present invention, the association process consists in the use of the object concept to robustly associate map entities using the geometric criteria of shape and occlusion.

Finally, in the localization process, the techniques known as Scan-Matching are based, in general, on the point adjustment algorithm called ICP [Lu and E. E. Milios. Robot pose estimation in unknown environments by matching 2D range scans. In Proc. IEEE Comp. Soc. Conf. On Computer Vision and Pattern Recognition, Seattle, WA, pp. 935-938, June 1994]. Basically, this procedure tries to find the relative displacement between two collections of points: one from a characteristic extracted from the sensor and the other from an entity stored on the map. The basic idea of ICP is to match both collections of points using a transformation based on a shift and a rotation. To do this, a priori associations are established between the points of both sequences and the resulting transformation is resolved. The process is repeated until one collection of points fits with the other. Unfortunately, one of the main disadvantages of this algorithm is its degree of convergence. The ICP technique does not converge to the desired result when the collections of points are very distant (this occurs when the accumulated error of vehicle position is large and an area is revisited: closed loop) or when the relative rotation of both is remarkable.

At this point, the present invention solves the problem by using a transformation to achieve relative displacement and rotation between the points of the feature and the object as described below. To do this, information is used on the form of the collections of points to fit. This information is summarized in another sequence of data called a signature that is formed by calculating the relative angle between two consecutive points in the collection of original points. The advantage of working with the signature is that it is invariant to translations and rotations of the original sequence. Unlike the ICP algorithm, where you have to establish associations between the points of the original collections, the proposal presented is to establish these relationships using exclusively the information contained in the signature, where the relevant information as corners and in general , angled points, is clearly reflected. In this way, hypotheses of association between points of both signatures can be established that correspond to translation and rotation transformations. Throughout the description and the claims the word "comprises" and its variants are not intended to exclude other technical characteristics, additives, components or steps. For those skilled in the art, other objects, advantages and features of the invention will be derived partly from the description and partly from the practice of the invention. The following examples and drawings are provided by way of illustration, and are not intended to restrict the present invention. In addition, the present invention covers all possible combinations of particular and preferred embodiments indicated herein. Brief description of the figures

A series of drawings that help to better understand the invention and that expressly relate to an embodiment of said invention which is presented as a non-limiting example thereof is described very briefly below.

FIG. 1 shows a graph with the object and observation signatures, including the relative displacement between them.

FIG. 2 shows a scheme with the association of objects for a particular case in which three signatures of observations (k, k + 1, k + 2) and N signatures of objects are based.

FIG. 3 shows a graph with the relative displacement between objects and observations, where the maximum is produced for a displacement of eleven points.

FIG. 4 shows a graph with the construction of the signature in the practical case of FIG. 3, the homologous reference points are calculated.

FIG. 5 shows the scheme of the location process that is part of the object of the invention.

FIG. 6 shows an object-based map.

FIG. 7 shows the object-based map of Figure 6 during the update process.

FIG. 8 shows the update of the object map for the case where there is an overlap between the observation points and those of the associated object.

FIG. 9 shows the update of the object map for the case where there are points of an observation that do not overlap with points of the associated object.

- FIG. 10 shows the update of the object map for the case where they exist points of an object that do not overlap with points of any observation taken from the sensor.

FIG. 1 shows a diagram of the hardware solution for the simultaneous location and mapping method for robotic devices object of the present invention.

FIG. 12 schematically shows the realization of segmentation in the hardware solution of the invention.

FIG. 13 schematically shows the hardware implementation of the cross correlation between the signature of an object and that of a characteristic.

- FIG. 14 shows the general scheme of association implemented in an FPGA according to the method object of the present invention.

FIG. 15 shows the outputs of the cross correlators as a function of the viewing angle of the sensor (a).

FIG. 16 shows the final location scheme that is obtained by merging the location assumptions based on the observation and the only prediction one.

FIG. 17 shows a comparison of the mapping and grid-based mapping techniques together with the mapping proposal using weights on the objects.

Detailed presentation of embodiments and example

As indicated, the main contributions of the invention are detailed in the mapping process, the association process and the localization process. In order to explain the invention in detail, first, the association process based on the signature of the characteristics (one-dimensional) and of the objects (sequences based on the form) is described. Subsequently, the localization process based on associative hypotheses between notable points of the signatures of the characteristics and their associated object to achieve a transformation in displacement and rotation that achieves a good alignment of the points of the characteristics and those of the objects is explained. . Finally, it will be described how points move, delete or add to map objects.

Objects are defined as an ordered sequence of points. Each of them contains position information in an absolute reference system as well as a probability measure associated with the position information of the point being in a near environment of the actual position it represents. So, given any ordered set CO of N ordered pairs, CO - {(X _Í , ¥ i) V 1 <i <N i ¾, yi e R}

We define your signature as,

SCO - {arelan ((γ, ₊ ι-νΜκ ₊ ι-κ)} V 2 <i <Nl}

The association between the observations (characteristics) coming from the sensor and the map objects is made trying to fit the observation signature (SOBV) in some region of the object signature (SOBJ). To do this, we calculate the mean square error of both signatures, so that for each displacement of the observation signature over that of the object, a measure of similarity is obtained between the two:

The minimum of the mean square error function indicates the relative displacement between the signatures for which there is a better fit between the two. Figure 1 shows both signatures and the relative displacement for which the best fit is produced.

The association between the regions of the segmented (characteristics) and those of the map objects is carried out in parallel by calculating the mean square error of each of the observations signatures (SOBV) with all those signatures of the map objects ( SOBJ) that are compatible with your position. The point of the mean square error function where a minimum occurs will generate a hypothesis of association between a characteristic and an object on the map.

Figure 2 illustrates the process described for a particular case in which we start from three observation signatures (k, k + 1, k + 2) and N object signatures.

The general scheme of association previously seen generates an association hypothesis, that is, characteristic - object pairs of the map, where each of them, in turn, establishes a location hypothesis that is solved as indicated below.

The relative displacement for which a minimum of the mean square error function (ECM (SOBJ, SOBV) (i)) allows to establish an association between the points of the observation signature with those of its object associate; Figure 3, identical to Figure 1, shows how the relative displacement for which the minimum occurs is eleven points. A point of the signature of a characteristic of the observation (3) and more specifically, the maximum value is represented in Figure 3. The homologous point (3 ') in the object signature is represented taking into account the relative displacement.

By construction of the signature, from the pair of points established in Figure 4 another pair of homologous reference points is calculated in the original sequences of characteristic and object (31, 31 ', 32, 32', 33, 33 ' , 34, 34 ', 35, 35').

A two-dimensional shift and rotation transformation is defined by at least two pairs of points. One of the pairs is determined by the homologous points calculated above and the other can be established considering as associated those points of the original sequences that are equidistant from their respective reference points. Once the point associations have been established in an environment of the reference points in both sequences, the shift and rotation transformation is determined. Figure 5 shows the proposed location scheme based on the form.

As can be seen in Figure 5, the location hypothesis (51) is established in the following stages: i. Definition of the associated OBJ object (52) and the OBV characteristic (53).

II Definition of the signature of the SOBJ object (54) and the SOBV characteristic (55).

III Establishment of the correlation between object and characteristic signatures (54,55) according to:

A stage of relative displacement of signatures and establishment of homologous points in the signatures (56). v. A stage of establishing homologous points in the characteristic and its associated object (57.57 '). saw. A stage of establishing associations between the points of the sequence of the characteristic and that of the object in an environment centered on the homologous reference points (58). vii. A final stage of location resolution (59).

Once the vehicle is located, the map is updated based on its current status and information from the sensor. The map update is done locally, taking into account that the map objects are defined by a sequence of one-dimensional points. The points that define the objects are characterized by their absolute position on the map (X, Y) and by their weight (P), as seen in Figure 6.

Updating an object on the map requires the location of the vehicle calculated at the current time (X _t ) as well as the state of the map at the previous time (Mu). In this way, the local information of the observations extracted from the sensor is converted to absolute coordinates of the map using the location of the vehicle already estimated (X _t ) which allows to integrate in the same absolute reference system the sensor data and the points of Map objects.

An example of a map object (71) together with the sensor observations (72) converted to the absolute reference system, according to the position of the sensor (73), and including the sensor points outside, is illustrated in Figure 7 of rank (74). When updating a region of an object using sensory information, we can distinguish several cases depending on the degree of overlap of the sensor points with the object points from the sensor position:

CASE 1) There is an overlap between the points of the observation and those of the associated object.

In this case, the updating of a region of an object is done by overlapping the updated points of the observation with those of the object. The update of one point of the observation is carried out considering the local information of the weights of the two closest points of the object, as shown in Figure 8. The observation point update is performed as follows:

Xobvt

(Pobv¡ / (Pobvi ⁺ 1)) ⁺ Xobvt-1 (1 / Pobvi ⁺ 1))

Where X _0bV t are the updated absolute coordinates of the observation point; X _0bV n are the absolute coordinates without updating the observation point; X _0bV ¡are the absolute coordinates of the point of intersection between the observation ray that joins the sensor with the observation point without updating (X _0b v ti) and the segment defined by the two points of the map closest to the latter. Finally P _0bV ¡is the weight of the point X _0bV ¡defined as P _0bV ¡= _P¡ (a - a¡) / a) + P _j (a¡ / a) where a¡ is the angle between the point X _0bV ¡ and the i-th point of the object seen from the sensor. Finally, α is the angle between the ith point of the object and the next one seen from the sensor. CASE 2) There are observation points that do not overlap with points of the associated object.

In this case the update of the region of the object is done by adding the points of the observation that do not overlap the object. The new points that are introduced in the object are of weight 1. All this, as it is observed in figure 9.

CASE 3) There are points of an object that do not overlap with points of any observation taken from the sensor.

As can be seen in Figure 10, the update of the area of the object that does not overlap with any observation is done by subtracting one unit from the weights of the points in that region. When the weight of the points is negative, the object to which it belongs is deleted.

Example of practical embodiment of the invention In order to take advantage of all the advantages of the described innovations, a possible practical embodiment (hardware) of the same is presented using an FPGA. This is characterized in that it is a general purpose programmable logic device. FPGAs consist of configurable logic blocks (BLC) that communicate with each other via programmable connections. Thus, any FPGA consists of a two-dimensional matrix of these blocks, surrounded by modifiable connections between them. Further, With the purpose of communicating the FPGA with the outside, it has a set of user-configurable input and output ports.

Next, and following the recommendations of the present invention, a hardware embodiment implemented in an FPGA is described. Since the most remarkable feature of an FPGA is that it allows different tasks to be executed in parallel, different blocks are shown in Figure 1 1 to correctly execute the invention. Thus each of the blocks represents the reserve of the part of the configurable logical resources of the global FPGA to achieve a specific purpose.

Thus, for example, we have as the sensor data acquisition signal in an instant S _t is the data input to the system set (100) configured in the FPGA and which results in the object map Mt + 1 and Xt + 1 position updated. The system comprises a first segmentation module (101) that represents all those FPGA resources configured to execute the segmentation of the input sensor (signal St) for the purpose of generating characteristics. Similarly, the prediction module (102), the association and location module (103), the fusion module (104) and the object map generator module (105) are implemented in the system of the invention (100). ).

Each of the previous modules is implemented by logical blocks, which together allow defining the functionality of each module within the system programmed in the FPGA. Thus we have the following basic configurable logic blocks: a) configurable logical block type point are real (10) configured to represent the information associated with a point of the sensor (range and angle). b) Configurable logical block type point simulated (20), configured to represent the information associated with a point simulated by the SLAM process (range and angle). Thus, from a vehicle location, the propagation of the rays on the map objects is simulated, in the same way as with the sensor rays on the real objects in the environment. c) Configurable logical block type particle (30), which is configured to represent the information of a particle or point associated with an object. Therefore this block will treat information related to the position (X, Y) of the particle, as well as its weight. d) Configurable logical block type vehicle location (40), configured to represent the position of the vehicle at a specific time in the SLAM process. Therefore, the information it handles is that of a position (X, Y) and an orientation

(Ψ)

The segmentation process, implemented in the segmentation module (101) is configured to extract the characteristics of the sensor's sound. The criterion by which it is decided that a region of points of the sea is raised to the characteristic range is usually the local proximity that exists between two consecutive points of that region. If two of them are relatively close, it is assumed that they belong to the same characteristic. Otherwise, one characteristic ends and another begins.

Taking advantage of the parallel processing capacity of the FPGA, the segmentation module (101) comprises as many configurable logic blocks as real point (10) as measures contain the sean; All configurable logic blocks are connected one after the other, so that except for the extreme logic blocks, each has two neighbors, as shown in Figure 12. From the initial information (range and angle of a point of the sea) that is loaded in each logical block are real (10), each of these performs in parallel a comparison of its range with that of its neighbor on the left (for example), storing the difference in the block itself. Next, a sequential tour of the logical blocks from right to left is made by comparing the range difference of a block with that stored in the block on its left. If the comparison does not exceed a certain threshold (which will depend on the application), both points belong to the same characteristic; otherwise, a counter of the number of detected characteristics is increased and the comparison between neighbors is continued. On the other hand, the object map generator module (105) reserves the necessary logical blocks of particle type (configurable logic block particle type (30)) to be able to store the information of each of the objects on the map. Each map object is composed of a set of these configurable logic blocks type particle (30) connected to each other. Each time a point is added to an object, memory is reserved for the corresponding logic block and is initialized with the appropriate position values (X, Y) and weight. Similarly, the removal of a point from an object results in the corresponding deletion of the block associated with the point. The association and location module (103) is configured to establish the association between the features extracted from the sea and the map objects. As previously stated, it is based on the cross correlation of two signatures. The mean square error of two unidimensional sequences is given by the expression:

This equation requires a large number of multiplications, subtractions and sums. In a general purpose microprocessor, the calculation of this function involves a high computational cost, since it cannot be executed in parallel. However, in an FPGA, it can be done via hardware in an efficient way using ad hoc hardware that performs sums and multiplications simultaneously. Most FPGAs contain logical blocks that perform these types of atomic multiplication and accumulation operations. Normally, these types of operations are called MAC operations (multiple accumulation or multiplyaccumulation). Specifically, the FPGA manufacturer Xilinx ® names these dedicated MAC hardware units, DSP Slice. In the present invention they are called MAC blocks (50).

The hardware of the mean square error between the signature of an object and that of a characteristic is shown in Figure 13. As seen in this figure, the necessary hardware comprises a shift register (51) or delay line (this is a memory array that shifts the contents of each position one place to the right or left in each clock cycle) as well as as many MAC blocks (50) as elements have the signature of the object with which we want to establish the mean square error. The operation is described below.

First of all the data of the object signature is loaded into a memory array. In the example shown in the figure, data line B.

I. Next, the signature data of the characteristic is loaded into a delay line. In each clock cycle, the delay line shifts the contents of each position to the right. iv. The contents of lines A and B are subtracted from record to record and the results obtained are entered in the MAC blocks to square the error. v. Finally, the MAC blocks (50) are executed in order from left to right by multiplying the contents of their inputs (A and B) and then adding the result of the MAC block (50) on their left. saw. The final result of the mean square error for a specific offset of the delay line is returned at the rightmost MAC block output.

The described element configures an ECM logic block that performs the mean quadratic error of two signatures (60) and comprises three inputs: the signatures of the characteristic (SCAR) and of the object (SOBJ) as well as the relative displacement between them. The output of the block is the average square error of the input data for a particular offset.

The advantage of working with an FPGA over a general purpose processor is that it is possible to reserve the necessary hardware to replicate the logical blocks we want and, thus, speed up the execution of the processes. Thus, the number of ECM logical blocks (60) that will be necessary to accelerate the association to the maximum will be equal to the number of object segments with which we want to establish the association of the characteristics extracted in the segmentation stage (101).

The general association scheme is shown in Figure 14, where first of all those sections of objects that are compatible with the last known position of the vehicle are taken into account. Secondly, an initial association is established by angle of view of the sensor between the segmented characteristics with the object segments considered. Third, the signatures of the characteristics are calculated, as well as that of the object segments. Fourth, the ECM logic block (60) is used to calculate the mean square error function between the associated signatures. Fifth, the maximums of the outputs of the blocks (ECM) (60) that are compatible are selected, that is, that they maintain an angular relationship between them that is consistent with that between the viewing angles of the characteristics of the be. Figure 15 shows the outputs of the ECM modules as a function of the viewing angle of the sensor (a). In bliss The different minimums for the calculated mean square error (ECM) functions have been represented. Thus, the minimums identified by a point are maximum compatible with each other because they maintain the same angular relationship that exists between the characteristics extracted from the sea. However, the minimum identified by a cross is not a compatible minimum since it is not located in any of the compatibility windows a and therefore will not be taken into account when generating a location hypothesis from it.

Finally, one of the minimums found in any of the regions is selected to establish a pair of reference points in the corresponding signatures. Each of the possible maximums establishes a location hypothesis. Continuing with the example in Figure 15, of all the minimum of the ECM mean square error functions, only those identified as ECM1 and ECM5 indicate the best location hypothesis because it is where the ECM function grows and decreases most rapidly. However, at the points identified in ECM4 in windows 2, 3 and 4, the function is almost constant in an environment. Therefore, the most promising or reliable location hypothesis will be chosen as those minimum of the ECM function where its slope variation is as large as possible. Once the location is resolved, a figure of merit of the fit between the sea and the map is calculated

(for example, the mean square error). In the event that this is not as good as expected, an attempt is made to return to the beginning of the process, that is, by reassigning the characteristics to the map segments until a better fit is achieved based on the established figure of merit.

Finally, and after resolving the location from the fit of the sea with the map (location based on the observation) it remains to be determined how the fusion of this with that from the information provided by the odometry of the vehicle is established. Each location hypothesis based on the observation contemplated in the process described in the previous section has an associated Gaussian probability density function whose mean (μ) and covariance (∑) are established based on the multiple embodiments of the ICP algorithm from different points initials. In turn, each Gaussian is weighted according to the figure of merit (mean square error) obtained by its location hypothesis as described above. Similarly, the location hypothesis from the prediction is also modeled as a Gaussian parameter (μ _ρ , ∑ _ρ ) established according to vehicle dynamics. Thus, the final location is obtained by merging the location assumptions based on the observation and the only prediction, each with an associated weight that indicates the reliability of the hypothesis, as shown in Figure 16.

The weights of each of the location assumptions obtained from the observation are normalized, so that their sum is equal to 1. In this way, the probability density function of the final location of the vehicle turns out to be the product of all functions of density discussed above: N probability density functions, which are associated with N observation hypotheses considered and a density function associated with prediction location.

Claims

1. Simultaneous location and mapping method for robotic devices that includes at least the stages of association, location and updating of the map or mapping and that is characterized by the fact that the modeling of the map in the mapping stage is based on entities called objects, defined as a sequence of dynamically adjustable moving points in size in order to represent the shape of the contours of the real obstacles detected by at least one sensor of the robotic device; and where each point of the sequence that represents the objects has associated a position and a weight that indicates the degree of mobility of the same; and where the information provided by the sensor is stored on the objects, in such a way that it allows its association with a related grouping of sensor points that refer to the same physical object by establishing a stage of association between said objects and the related grouping of points or characteristic using the geometric criteria of form and occlusion; and where the localization stage comprises a transformation that uses the information of two associated initial points using the signature information of the Sean and the map, achieving a relative displacement and rotation between the points of the object and the characteristic.

2. A method according to claim 1 wherein each object contains position information in an absolute reference system as well as a probability measure associated with the position information of the point being in a near environment of the actual position it represents. .

3. Method according to claims 1 and 2 wherein the association between the characteristic, which are the observations from the sensor and the objects of the map is carried out by fitting the observation signature (SOBV) in some region of the signature of the object (SOBJ) calculating the mean square error of both signatures so that for each displacement of the observation signature over that of the object a similarity measure is obtained between the two, and where the minimum of the ECM function indicates the relative displacement between the signatures for which there is a better fit between the two.

4. Method according to any of claims 1 to 3 wherein the association between the characteristic regions and the regions of the map objects is carried out in parallel by calculating the mean square error function of each of the observations' signatures (SOBV ) with all those signatures of the map objects (SOBJ) that they are compatible with their position; and where the ECM function that produces a lower minimum generates a hypothesis of association between a characteristic and an object on the map.

5. Method according to any one of claims 1 to 4 wherein the association generates an association hypothesis, that is pairs of features - map objects, where each of them establishes a location hypothesis (51) such that it comprises the steps for defining the associated object OBJ (52) and the OBV characteristic (53); definition of the signature of the SOBJ object (54) and the SOBV characteristic (55); establishment of the mean square error function between object and characteristic signatures (54,55); a stage of relative displacements between the signatures to find out which is the optimum based on a criterion of mean square error and establishment of homologous points in the signatures (56); a stage of establishing homologous points on the characteristic and its associated object (57, 57 '); a stage of establishing associations between the points of the sequence of the characteristic and that of the object in an environment centered on the homologous reference points (58); and a final stage of location resolution (59).

6. Method according to any of the preceding claims where once the vehicle location is made, the map is updated from its current state and from the information coming from the sensor; and where said map update is performed locally, taking into account that the map objects are defined by a sequence of one-dimensional points; and where said updating of a map object requires the location of the vehicle calculated at the current instant (X _t ) as well as the state of the map at the previous instant (Mu).

7. Simultaneous location and mapping device for robotic devices comprising means for executing the method according to any one of claims 1 to 6.

8. Mobile robot comprising means for executing the method according to any of claims 1 to 6.