Infrared Beacons based Localization of Mobile Robot

A number of methods solving mobile robot localization problem exist, mostly based on sensor information of various kinds fused together with an estimator [1]. While advanced types of sensors bring data of high precision and in high rates, the cost of such sensors is high either financially (laser rangefinders [2, 3]) or computationally (vision based approaches [4–6]). In order to decrease the cost the approaches based on beacons can be utilized with the drawback of necessity to install the beacons in the environment. The information from the beacons (usually the range and/or bearing) is commonly processed with Bayesian filters to obtain correct estimate of robot pose. The beacons can be of various physical principles, from laser pointer emitter [7], through ultrasonic [8–10] to chirp-spread-spectrum (CSS) ranging based on the time of flight of radio frequency signals [11]. Our goal was to find the way to localize the robot in indoor environment with as low cost as possible and as few sensors as possible in order to be able to reduce the dimensions of the robot in future development. This goal can be met with the proposed localization system based on the infrared beacons. Infrared beacons cost is very low, but on the contrary to commonly used beacons the infrared beacons provide only the information about the bearing, not the ranging. However, bearing information is proved sufficient for localization using Extended Kalman filter (EKF).


Introduction
A number of methods solving mobile robot localization problem exist, mostly based on sensor information of various kinds fused together with an estimator [1].While advanced types of sensors bring data of high precision and in high rates, the cost of such sensors is high either financially (laser rangefinders [2,3]) or computationally (vision based approaches [4][5][6]).In order to decrease the cost the approaches based on beacons can be utilized with the drawback of necessity to install the beacons in the environment.The information from the beacons (usually the range and/or bearing) is commonly processed with Bayesian filters to obtain correct estimate of robot pose.
The beacons can be of various physical principles, from laser pointer emitter [7], through ultrasonic [8][9][10] to chirp-spread-spectrum (CSS) ranging based on the time of flight of radio frequency signals [11].
Our goal was to find the way to localize the robot in indoor environment with as low cost as possible and as few sensors as possible in order to be able to reduce the dimensions of the robot in future development.This goal can be met with the proposed localization system based on the infrared beacons.Infrared beacons cost is very low, but on the contrary to commonly used beacons the infrared beacons provide only the information about the bearing, not the ranging.However, bearing information is proved sufficient for localization using Extended Kalman filter (EKF).

Localization using bearing only measurements
Localization problem is stated as determination of the robot position , , coordinate system in given time step k, as depicted in Fig. 1.The inputs to the localization algorithm are: 1) The motion model of the robot describing the change of the position due to actions , -the translational and rotational velocities of the robot.The actions can either be extracted from motion controller or recalculated from odometry readings when available; 2) The measurement.In the case of infrared beacons it is the measurement of relative angles between the robot and the beacons placed in known fixed positions; 3) The initial position of the robot.

 
Fig. 1.The state of the robot is given by its position and orientation in the 2D plane with the respect to global coordinate system.The beacons are in known fixed positions (given by xy coordinates), measurement is represented by the relative angle between the robot and the beacon Due to the imperfections of the motion model and noise in the measurement the position is represented in a probabilistic manner, using simple unimodal Gaussian approximation.The description of the underlying model can therefore be defined as: where k v is white Gaussian process noise (representing the imperfections of motion model) with zero mean and covariance matrix k V , k y is system output, k w is white , , Gaussian measurement noise with zero mean and covariance matrix k W and f and h are continuously differentiable nonlinear functions.The state transition function f defines how the state (robot position) changes when action is applied Regarding the measurements, the output k y is the set of relative angles between the robot and beacons, as only the bearing information is available.The length of this set depends on number of beacons detected.There is a maximum of N beacons generally available (however, there is no guarantee, that all the beacons will be detected), their positions are (Fig. 1) For a single beacon the output equation is   (5) gives the system output (relative angle between the robot and beacon depending on robot position, orientation and position of the beacon in environment.For N beacons the measurement equations are simply added one by one depending on what beacon was measured.In general In (5) all the beacons are considered, when only some beacons are detected, appropriate rows are omitted.In order to use correct beacons positions, the identification of what beacon belongs to what measurement must be provided.
The position of the robot can not be determined exactly, instead the state estimate with mean ˆk x and covariance matrix k P is produced by Bayesian filter [12][13][14].There is a number of Bayesian filters suitable for the problem in hand, however when considering computational cost, the simplest one -Extended Kalman Filter (EKF)gives the best performance/cost ration.EKF represents the estimate by random vector with Gaussian distribution, producing the estimate with mean ˆk x and covariance matrix k P .It uses the Taylor Expansion to cope with nonlinear functions f and h from (1).EKF therefore requires partial derivatives of the functions.
The EKF estimator works in two stages.First the motion model is applied (estimate changes when action is performed) and new position estimate is predicted.In second stage the measurement is taken into account (beacons relative angles) and position estimate is corrected.Therefore the input is the current estimate (at the beginning of the whole process this estimate is equal to initial estimate of robot position), the action and the measurement.Necessary relations are given by following equations: Prediction (state estimate change when action is applied): Update (correction of the estimate using measured data):

H
x .
EKF requires the partial derivatives of f and h functions.For the motion model, the partial derivatives forming Jacobian k F are given by Jacobian for a single beacon is (the matrix is transposed to fit within the text) As the output equation for N beacons is formed according to (7), the Jacobains for more beacons are added the same way

Infrared beacons
The beacons are placed in environment in known positions according to (3).The measurement provides the , .
, , relative angle between the robot and the beacon (5).Such measurement is performed by beacons scanner device, that consist of a ring of M receivers, each i-th receiver returns Boolean value D i indicating whether the beacon is detected or not.Such set of Boolean values is used to calculate the measured angle rel  in a following way.The angles between robot heading direction and position of the i-th receiver on scanner circle ri  are transformed to Cartesian coordinates, the mean is calculated individually for each axis (12) and then resulting angle is recalculated back (13).
The validity val is calculated as the length of the resulting angle vector ( 14), as illustrated on Fig. 2: The measurement noise estimate, required by the estimator, is set according to measurement validity coefficient in (14).The beacons used for localization are based on IrDA (Infrared Data Association) principle.Each beacon has its own power-supply and the infrared light is emitted via six infrared LEDs giving the flaring angle of approximately 120 degrees.These beacons can be placed anywhere in the robots environment because of small size/weight.The reliable range of the beacons is about 10m.The signal from the beacons is detected by beacon scanner device placed on the top of the robot chassis.The scanner, shown uncovered in Fig. 5., consists of 16 IrDA receivers, placed on a circle, covering whole 360 degrees range.The scanner detects every beacon within the range which is not hidden behind an obstacle.
In order to reduce the power consumption the beacons are not in operation continuously, but the light is emitted only upon request from the scanner.During beacon detection the ID of the beacon is also retrieved in the following way.The beacon scanner and the emitters communicate with each other using combined radio and infrared communication protocols.The low power consumption radio modules with free 433MHz modulation are used for one way data transmission, in the direction from the scanner to the beacons.This way the proper beacon is selected for transmission and thus beacon identification problem is solved.For further power consumption reduction only the beacons within the operating range from estimated position are requested to respond.The beacon itself is shown on Fig. 3. with AA size battery to illustrate its dimensions, while in reality it is powered by single LiPol 1100mAh cell that can last for 55 hours of continuous emitting.As the beacon is used approximately twice per second, the total beacon endurance for fully charged battery is about 14 days.The beacon starts with data transmission immediately after the proper identification number is received.The transmission from selected beacon to the scanner is also one way and it is based on infrared principle.The beacon uses infrared LED (880nm light-wave) with carrier frequency 38kHz.This signal is detected by infrared receivers on the scanner and resulting measured angle rel  and validity is calculated according to ( 12)- (14).

Robot navigation
The actions as one of the estimator inputs can be taken directly from the motion controller, or from robot odometry measurements.In later case the action must be recalculated to generalized velocities, depending on particular chassis type, so the estimator can remain intact and can be used with different types of robot chassis, from common to special types [15].The overall simplified view of robot navigation scheme is shown on Fig.The motion planner/controller produces desirable values of translational and rotational component of velocity, the robot performs the motion and provides the set of measurements (relative angles towards the beacons together with the identification of the beacons).The data are fed to EKF estimator that produces the estimate of robot position, further used by the planner (together with obstacle detectors, goal position, etc. -those planner inputs are omitted on the figure).The planner can work in reaction mode or it can incorporate additional data from the surrounding, see e.g.[16].

Simulation
The localization system was first verified using simulations in Matlab.The motion model of the robot necessary for the correct settings of process noise in EKF in the simulations was identified experimentally on real test robot.The response of the robot to the set of actions was repeatedly measured and those data were used to obtain probabilistic model of the motion.The simulations included other implementations of Bayesian filter, Unscented Kalman filtr (UKF) and Particle filter (PF) with 50 particles in particular, to compare the EKF based localization with filters.All implementations exhibited similar results with UKF and PF slightly surpassing EKF regarding the localization precision for the cost of higher computational expenses, see Table 1.showing the values obtained for 4 beacons visible.Further simulations were performed to determine the influence of beacons measurement character.Beacons providing bearing only information, distance only and both distance and bearing were compared.For those simulations only EKF estimator was considered.As can be seen on Table 2. the bearing only requires higher number of beacons to obtain the same precision of localization.However, the cost of additional beacon does not overpass the cost of beacons capable of providing the distance information.Further more the error in robot orientation is of higher importance as it essentially influences the robot position in the long term.

Verification experiment with real robot
The algorithm was first implemented into test robot Leela, shown on Fig. 5. Leela has common differential drive chassis and is equipped with beacons receiver and obstacle detectors.Obstacle detectors are used solely for the path planning and not for localization.The robot is also equipped with odometry readings (IRC sensors on both drives), however during the tests the odometry was ignored, or used for comparison purposes only, as our goal was to reduce the number of necessary sensor to bare minimum.The estimator was implemented in ANSI C and all the code runs on 8-bit AVR family processor.ATmega128 is a low cost 8-bit processor with programmable 128kB flash memory and two serial communication interfaces.The processor acts as the main control unit and it runs both the localization algorithm and path planner.As the processor is a low cost, it has some limitations that must be overcome in implementation, e.g.goniometric functions are approximated using look-up table, whenever possible the calculations are transferred into integers, etc.While the processor runs on low frequency (16 MHz), it can operate the localization system with sufficient speed.
The tests were performed on a number of locations, Fig. 6. shows an example: entrance halls of our University.In this particular experiment 4 beacons were used, placed on the ground, with the view often blocked by passing people.The true trajectory was extracted from the videofeed and is shown compared to estimated position (and orientation) determined by EKF.One can see that there is a difference between real trajectory and estimated one, however the localization is stable.
Number of other tests was performed with different number of beacons, different number of students moving in the hall (representing dynamic obstacle) and different planned trajectories.This can be illustrated on Fig 7 .,where planned trajectory goes beyond the range of most of the beacons.While the certainty of the position is reduced (larger confidence ellipses), the localization is still stable.The influence of reflections and therefore the robustness of the beacon scanner filter was also examined.As we are interested in the use of the system in real environment, the exact dependencies on number of obstacles, reflections, etc. are impossible to measure and the only criteria is whether the robot looses its position or not.The tests approved that the method successfully localized the robot in real environment with the presence of dynamic obstacles (people) blocking the beacons for extended period of time, when the maximum time depends on precision of motion control / odometry readings.The recovery of correct position after blocked beacons operation is fast and takes about 5 steps of the algorithm (approx. 1 second).

Real world operation
As the proposed localization system proved to be capable of succesfull localization, it was used in larger scale robot -the prototype of commercial robot Advee (Fig. 8).The robot chassis differs from previously described robot as it uses Ackerman steering principle [17], resulting in more reliable odometry.Therefore the odometry readings from IRC sensors on front wheels were used instead of controllers commands (velocities) as the inputs to the estimator.The odometry readings were recalculated into velocities from simple trigonometry and measurements of given time interval.The implementation of the estimator was ported from AVR processor to TS-7800 embedded computer based on ARM family processor running OS Linux, that handles the lower levels of robot software architecture (interaction with hardware devices, path and position estimator).
The beacon scanner is installed on top of the head of the robot.As can be seen from the figure, the robot height is sufficient to keep the scanner high enough for most of dynamic obstacles not to block the direct view between the scanner and the beacons installed close to the ceiling.The tests performed confirmed the usability of proposed solution, as the robot was capable of wondering in a crowded hall for 5 hours without loosing its position.Currently the robot has over 300 hours of operation in real environment with no issues regarding the localization.

Conclusions
When selecting the proper localization system for mobile robot, several matters must be considered.The key factors are the application of the robot, computational power available and the characteristics of its environment, such as the accessibility.Proposed localization technique is focused on low cost both in financial, spatial and computational domains.Both simulations and real robots verifications proved, that localization using bearing only beacons as the only source of the information from the outside world can be done with low cost sensors and computational unit.The overall cost of sensor equipment for the robot falls dramatically and the possibility to use the beacons as the only sensors for localization enables to reduce the dimensions of the robot.The prototype cost for the beacons is about 30,-EUR each, the scanner price is about 100,-EUR.Reduction of the cost may seem not as important as the performance.However, there are numerous possibilities of application of the localization system when its cost is on reasonable levels.What is more of an issue is the essential drawback of any localization based on beacons, no matter the principle, and that is the necessity to install and service the beacons.Presented beacons cope with the service necessity by prolonged live of the beacons.As the beacons are operating only upon request, the power consumption is low and the beacon can run on battery power for sufficiently long.Thus the installment does not require any wiring and can be done relatively easily.Beacons are reliable and battery replacement/charging is the only service necessary.Further reduction of power consumption by selection of the beacons to respond based on the position estimate of the robot reduces the time required for processing the beacons data in larger areas where higher total number of beacons placed in environment is expected.
Regarding the estimation method, certainly the EKF is not the best available estimator in the terms of precision, mainly when highly nonlinear functions are considered.However, the computational requirements are extremely low and therefore the estimator can run on microcontroller with sufficiently low estimation times that allow the localization procedure to be run in reasonably short intervals.When computational power is sufficient, the Unscented Kalman filter or Particle filters can be used for estimation, but according to our experiences the improvement of localization is not substantial.

Fig. 2 .
Fig. 2. Beacon relative angle determination.The length of resulting vector determines the validity of measurement.Left case shows typical valid measurement, right case shows the influence of reflections

Fig. 4 .
Fig. 4. Robot navigation scheme.Actions are taken either from the controller (I.) or transformed robot odometry measurements (II.)

Fig. 5 .
Fig. 5. Test robot Leela with detail of beacons receivers ring around the neck and the uncovered beacons scanner

Fig. 6 .Fig. 7 .
Fig. 6.Evaluation experiment example.Dotted line represents real trajectory, solid line the estimate.Beacons are denoted by diamond symbols.Ellipses denote 95 % confidence interval for the estimate.Path traveled started on the left side of the figure

Table 1 .
Comparison of EKF, UKF and PF estimators

Table 2 .
Influence of measurement character.The values in the table state localization error [m, degrees]