^{*}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

This paper describes a relative localization system used to achieve the navigation of a convoy of robotic units in indoor environments. This positioning system is carried out fusing two sensorial sources: (a) an odometric system and (b) a laser scanner together with artificial landmarks located on top of the units. The laser source allows one to compensate the cumulative error inherent to dead-reckoning; whereas the odometry source provides less pose uncertainty in short trajectories. A discrete Extended Kalman Filter, customized for this application, is used in order to accomplish this aim under real time constraints. Different experimental results with a convoy of Pioneer P3-DX units tracking non-linear trajectories are shown. The paper shows that a simple setup based on low cost laser range systems and robot built-in odometry sensors is able to give a high degree of robustness and accuracy to the relative localization problem of convoy units for indoor applications.

In the past, mobile robot cooperation has been widely studied in multiple application scenarios. Multi-robot systems exhibit advantages with respect to single-robot systems, in terms of flexibility, adaptability, scalability and affordability. However, localization, communication and control challenges are more significant in cooperative robotics.

From the point of view of localization, two kinds of scenarios can be considered: the first one requires a global localization of each unit independently (e.g., swarm applications [

The selection of the sensorial systems needed for robot localization is a crucial task that depends heavily on the application scenario (

Multi-sensory strategies are usually proposed to solve the relative localization problem, where odometry information (

This paper shows that a simple setup based on low cost laser range systems and built-in robot odometry sensors is able to provide a high degree of robustness and accuracy to the relative localization problem of convoy units in indoor environments. Aside from the localization problem, the design of a control strategy for each individual unit in the convoy presents important challenges. In order to follow the leader’s trajectory it is not enough to guarantee global stability. A movement coordination plan is also needed between at least each pair of consecutive units [

Among the published works regarding convoy guidance for indoor applications, the following should be mentioned: In [

In the light of the previous works the main contribution of this paper is to implement an innovative and low cost relative localization system for a convoy of robotics units in indoor transport scenarios. The LIDAR sensor proposed in this paper is a Hokuyo URG-04-LX [

As described in detail in [

The low level is based on a set of PID controllers that regulate the speed of each active wheel. The middle level includes a servo-controller in order to ensure reliable angular and linear speeds (Vo) of the robot. The robotic units are provided with optical encoders of 500 pulses per revolution linked to each active wheel, due to its 19 cm diameter the movement resolution is 1.2 mm. In this way, odometry permits closing both the low and middle control loops. Additionally, a discrete Kalman Filter (KF estimator) is included in order to filter the noise related to measurements provided by encoders and obtaining the filtered velocity vector (VE) for control purposes. The high level generates the inputs (UHL) for the middle level, such that each robot follows the previous one warranting a security distance, and approaches the discretized poses selected by the leader in its way. The follower pose, needed to accomplish the high control level objectives, is estimated by the discrete EKF combining data coming from the odometric system and the added laser scanner.

The paper is organized into five sections: after this introduction, the sensory sources involved in the fusion process are presented in Section 2, and the discrete EKF application to the data fusion data is described in Section 3. Experimental results obtained with a real robotic convoy demonstrator are shown in Section 4, and conclusions are revealed in Section 5. The mathematical component and algorithm description are included in an

The convoy consists of a group of robotic units that are only equipped with relative positioning sensors (

As already mentioned, global stability is achieved if each follower knows both its motion state and at least the one of the precedent unit. In order to accomplish this specification, as a first approximation, angular and linear speeds of each robot in the platoon can be estimated with a dead-reckoning process, using the encoders attached to active wheels already built on each robot. This estimation can be combined between each pair of robots, and sent through the wireless link, to obtain each unit’s relative distance and orientation to the precedent one. However, this first approach produces important drifts in the pose estimation, due to the accumulative error inherent to the dead-reckoning process, mainly in non-linear trajectories. A complementary sensorial system is therefore needed in order to better estimate the individual pose and to guarantee the reliability and stability in the guidance task performed in the convoy.

In this paper, authors propose to combine the odometric information with the laser sensor pose estimation. The laser sensor gives the relative pose (distance and orientation) from each robot to the preceding one (see

The laser contributes to measure the separation distance _{ri}_{ei}_{LTk}_{ri}_{ri}_{i}_{i − 1}

However, working with the proposed landmark has important advantages:

The inclusion of two planar elements minimizes the error when calculating the angle _{ri}

Although distance _{ri}_{1}, _{1}] and [_{2}, _{2}], the cylinder in the middle of the landmark eases its direct computation, improving the accuracy and computational time of the estimation.

Thanks to its three components, the artificial landmark can be easily identified in the robot structure and from the different elements in the environment, minimizing the fault detections.

Once the artificial landmark is detected by the laser scanner on top of a follower unit, the relative distance between this unit and the one in front of it is directly obtained from the laser measures to the cylindrical structure d_{ri}. From the two most external measures, detected in the landmark by the laser scanner (points _{1}_{2}

This way, the relative orientation respect to the precedent unit is obtained by the equation:

To better understand the data fusion process, the following nomenclature should be kept in mind:

Thanks to the wireless link between the units, the corrected pose _{i}_{−1,}_{k}_{−1} = [_{i}_{−1,}_{k}_{−1} _{i}_{−1,}_{k}_{−1} _{i}_{−1,}_{k}_{−1}]^{T}_{i}_{−1} unit at the _{i}_{i}_{,}_{k}_{i}_{,}_{k}_{i}_{,}_{k}_{i}_{,}_{k}^{T}_{i}

Finally, the EKF algorithm allows one to fuse the odometric information _{i}_{,}_{k}_{i}_{,}_{k}_{i}_{,}_{k}_{i}

The best pose estimation of each follower in the convoy is achieved through a discrete Extended Kalman Filter [

Prediction of the robot pose _{k}_{k}_{k}_{−1} in the previous sample time is also required.

Correction of the pose estimation _{k}_{k}

At the end of this paper, the

The implemented EKF has the standard structure of this filter, except for the factor _{k}_{k}_{k}_{k}

The different tasks developed by the filter at the EKF prediction step are summarized in the following paragraphs:

(p.1) Prediction of state _{i,k}_{i,k−1}, are required to obtain this predicted state.

(p.2) Estimation of measure _{i,k} from the corrected pose of the precedent unit _{i−1,k−1} and the measurement model based on the laser scanner (

(p.3) Prediction of the estimation error covariance matrix _{i,k}, using the corrected value of this matrix for the previous time step _{i,k−1}, as well as the noise covariance matrix _{W}_{f,X}_{f,W}

On the other hand, tasks developed by the discrete filter at the correction step, are the following:

(c.1) Updating the Kalman gain K_{i,k}. In order to obtain this gain, some matrices have to be previously computed: estimation of the matrix P̃_{i,k}, the noise covariance matrix Σ_{V} of the laser scanner measurements’ model, and the two jacobians J_{g,X̄} and J_{g.V} (see the

(c.2) Correction of the pose state predicted value X̂_{i,k}, only if the laser scanner updated measures are available, and thus Θ_{k} = 1. As it can be noticed in _{k}, and its prediction X̃_{k} through odometry, with the Kalman gain.

(c.3) Updating the estimation error covariance matrix P_{i,k}.

Among the contributions of this paper the standard discrete EKF adaptation for the pose estimation of robots in platooning guidance should be considered. Specifically the authors have developed:

Characterization of

Computation of the covariance matrices that model the noise related to both sensory systems: the one related to the odometric system _{W}_{V}

Calculation of the needed jacobians. The jacobians depending on odometry measurements: _{f,X}_{f,W}_{g,Z}_{g,V}

In the experimental tests developed to validate the described proposal, three robotic units adapted from the original P3-DX platform (see

Two types of tests are included in this section. The first one is dedicated to show the advantages of the implemented fusion technique, comparing the positioning results independently obtained with each of the two sensory systems under study. The second type focuses on the global results of control and sensorial fusion integration applied to a convoy of robots.

In the first set of results only two units are used: the leader, programmed to track a trajectory including straight and curve segments; and a single follower.

A new experiment is carried out with the same robot formation and with the same path reference. This time, the output of the fusion algorithm is applied to the high level control in the guiding architecture shown in

The second type of test is developed with a convoy of three units, as shown in

This work details how the proper combination of odometry and a low cost laser scanner provides the required accuracy and non-cumulative errors for indoor applications of a convoy of robotic units. First, it has been demonstrated that information coming from the proposed single sensors is not enough to accomplish the correct positioning of one or more units in cooperative guidance. In this context, the proposal presented in the paper calls for fusing odometric data (typical positioning system of a robot) with a laser scanner (added to the robotic platform together with a basic landmark structure) to achieve the guiding task of a convoy of P3-DX robots.

The contribution of the implemented discrete EKF is twofold. On one hand the inherent accumulative error due to dead-reckoning positioning is corrected by the laser measurements. On the other hand, the highest uncertainty related to the used low cost laser scanner is compensated by the lowest one of the P3-DX encoders.

As it has been demonstrated with the indoor experiments results, the sensorial fusion process is essential to maintain a safe distance between followers and to track the leader’s trajectory. The implemented solution allows one to achieve these objectives, even in situations where partial sensory information is lacking.

In summary, the paper details quantitatively how the performance of independent sensorial sources can be highly improved by means of a proper fusion algorithm, taking advantage of their best characteristics and minimizing their inherent limitations.

This work has been supported by the Spanish Ministry of Science and Innovation through the VISNU (Ref TIN2009-08984) Project.

The kinematical relation between the robot pose and the speed data of active wheels is not linear in a differential drive robot. Thus, in order to represent this relation in the state space, the transition and output equations respectively have to be expressed as follows:
_{i,k}^{3}, is the state vector representing the absolute pose of the follower unit, with its three components _{i−1,k} ∈ ^{3}, is the state vector of the precedent unit; _{i,k}^{2}, is the input vector, with its two components: angular speed _{R}, ω_{L})_{i,k}^{3}, is the pose estimation through the laser scanner measures (distance and angle); _{k}^{2}, is the state noise vector, therefore related to the odometric system; and _{k}^{6}, is the measurement noise vector related to the laser scanner perception system.

As defined in the previous paragraphs, nonlinear and stochastic functions

As explained in Section 3, and depicted in

In the case under study, function _{i,k}

To obtain _{i,k}_{f,Xi}_{f,W}_{W}_{f,W}

Besides, the noise covariance matrix related to the odometric information _{W}

Once _{i−1}_{i,k}_{i,k}_{i,k}_{i,k}^{T}

At this point, the correction step is tackled. The Kalman gain _{i,k}_{i,k}_{i,k}_{g,X̄}_{g,V}

Expressions from (A.29) to (A.46) allow one to achieve the 18 elements of _{g,V}

The noise covariance matrix regarding the laser scanner information _{V}_{d}^{−6} m^{2}, and _{θ}^{−5} rad^{2}.

All data generated as described in previous paragraphs are needed to correct _{i}_{i,k}_{i,k}_{i,k}_{k}

The correction step ends updating the error estimation covariance matrix, as follows:

One of the P3-DX robotic platforms equipped with encoders and laser scanner for the platoon guidance demonstrator.

Convoy of three robotic units as the one shown in

Organization of the control levels and their relation with the sensorial systems included in each follower unit.

Variables involved in the high level control of the platoon: points of the leader trajectory, pose of each follower and relative position information obtained through the laser scanner.

Description of the geometrical relation among the variables [_{1}, _{1}, _{2}, _{2}, _{ri}_{ci}

Processes and variables implied in the EKF to obtain the Fi movement state.

Block diagram of the implemented fusion algorithm, based on the standard EKF.

Movement developed by a P3-DX robotic unit following a leader. The red trace agrees with the odometric information of the robot. However, the laser scanner gives the more realistic blue path.

Linear and angular speed of the follower unit in the trajectory shown in

Path pursuit by the follower unit using the discrete EKF fusion proposal as part of the high level control. The blue plot shows the position information registered by the laser scanner, and the red one the location estimated by the EKF. The laser scanner is only available in intervals “

Trajectory followed by the platoon. The path described by the leader is plotted in red, the one described by the first follower is shown in green, and the one described by the second follower is plotted in black. The reference trajectory input to the leader is also shown in blue.