Abstract
In the mobile robotic systems, a precise estimate of the robot pose with the intention of the optimization in the path planning is essential for the correct performance, on the part of the robots, for tasks that are destined to it. This paper describes the use of RF digital signal interacting with beacons for computational triangulation in the way to provide a pose estimative at bidimensional indoor environment, where GPS system is out of range. This methodology takes advantage of high-performance multicore DSP processors to calculate ToF of the order about ns. Sensors data like odometry, compass, and the result of triangulation Cartesian estimative, are fused for better position estimative. It uses a mathematical and computational tool for nonlinear systems with time-discrete sampling for pose estimative calculation of mobile robots, with the utilization of extended Kalman filter (EKF). A mobile robot platform with differential drive and nonholonomic constraints is used as a base for state space, plants and measurements models that are used in the simulations and validation of the experiments.
1. Introduction
The planning of trajectory for the mobile robots, and consequently its better estimative of positioning, is the reason of intense scientific inquiry. A good path planning of trajectory is fundamental for optimization of the interrelation between the environment and the mobile robot. A great diversity of techniques based on different physical principles exists and different algorithms for the localization and the planning of the best possible trajectory.
The localization in structuralized environment is helped, in general, by external elements that are called of markers. It is possible to use natural markers that already existing in the environment for the localization. Another possibility is to add intensionally to the environment artificial markers to guide the localization of the robot.
This work uses an important mathematical and computational tool for the calculation of the data fusing collected by the sensors and the disturbances caused for the errors, with the purpose of estimate the mobile robot pose, that is the Extended Kalman filter (EKF). A mobile robot platform with differential traction and nonholonomics restrictions is used for experiments validation.
In the direct kinematics the system of mobile robot positioning is presented. A model for state space, plants and measurements are presented, that are needed for the development of the necessary attributes to the positioning estimates made with the Extended Kalman filter. Finally, we present the experimental and simulation results obtained from the models created.
2. Direct Kinematics for Differential Traction
This paper focuses on the study of the mobile robot platform, with differential driving wheels mounted on the same axis and a free castor front wheel, whose prototype used to validate the proposal system is depicted in Figures 11 and 1 which illustrate the elements of the platform.
We assume that the robot is in one certain point () directed for a position throughout a line making an angle with axis, as illustrated in Figure 2.
Through the manipulation the control parameters and , the robot can be led at different positionings. The determination of the possible positionings to be reached, once given the control parameters, is known as direct kinematics problem for the robot. As illustrated in Figure 2, in which the robot is located in position (), we have for the trigonometrical relations of the system where ICC is the robot instantaneous curvature center.
As and are time functions and if the robot is in the pose () in the time , and if the left and right wheel has ground contact speed and , respectively, then, in the time the position of the robot is given by
Equation (2.2) describes the motion of a robot rotating a distance about its ICC with an angular velocity given by [1]. Different classes of robots will provide different expressions for and [2].
The forward kinematics problem is solved by integrating (2.2) from some initial condition (), it is possible to compute where the robot will be at any time based on the control parameters and . For the special case of a differential drive vehicle, it is given by
A question more interesting, and at same time more difficult to answer, is the following how can the control parameters could be selected in a way the robot obtain a specific global position or follow a specific trajectory? This is known as the task of determining the vehicle's inverse kinematics: inverting the kinematic relationship between control inputs and behavior. It is also related to the problem of trajectory planning.
2.1. Inverse Kinematics for Differential Drive Robots
Equation (2.3) describe a constraint on the robot velocity that cannot be integrated into a positional constraint. This is known as a nonholonomic constraint and it is in general very difficult to solve, although solutions are straightforward for limited classes of the control functions and [3]. For example, if it is assumed that , and , then (2.3) yields where . Given a goal time and goal position (). Equation (2.4) solves for and but does not provide a solution for independent control of . There are, in fact, infinity solutions for and from (2.4), but all correspond to the robot moving about the same circle that passes through (0,0) at and at ; however, the robot goes around the circle different numbers of times and in different directions.
3. Position Estimation with RF Signal ToF
The communication system between the mobile robot and the beacons is follow described. The mobile robot, and each one of the beacons, have a module of control and reception of the address codes and a module of transmission. The communication protocol between the embedded control system, located in the mobile robot, and the beacons, that are located in strategical points in the environment, are composed of a frame formed for five quaternary codes.
3.1. Communication Protocol
The timing diagram shown in Figure 3 illustrates as each one of the codes in function of clock signal is formed.
Each half clock period correspond to a time about 896 μs. Each code has a time period composed of 8 clocks cycles, that is 14,336 ms. Table 1 depicts in a logic way the formation of the codes.
Each code is configured by a logic signals sequence, each one with a determined period. Table 2 shows how each logic signal of each code is composed.
The idea is to mount a quaternary codifier using binary logic levels, associates in such way that the logic levels alternate and the total period of each code is the same. The codification implemented was conceived here aiming the minimizing of the errors, such as the transmission of the one exactly signal level is transmitted without transitions of level for long time periods. In this case, the receiver tends to put out of the way itself and to perform the reading out the correct point, originating errors. In this way, RF transmission of the codes is sufficiently robust and trustworthy, practically extinguishing errors of signal decoding signal inside the area of system range.
3.2. The Communication Frame
The communication frames used between the mobile robot and the beacons are composed for five quaternary codes. The Figure 4 illustrates an example of a communication protocol frame. As each code has a period about 14,336 ms, the all frame has transmission time about 71,68 ms.
The maximum number of possible combination is given by
Each beacon has it own address, composed by five codes. In this way, the system is able to deal with up to 1024 beacons, with their own individual address.
3.3. The RF Link
The coded signal is transmitted in RF modulated by BASK-OOK technic. The carrier signal frequency is about 433,92 MHz (UHF band).
The RF link uses a half-duplex channel between mobile robot and beacons. The mobile robot control system is previously programmed with quantity and address of each beacon. Figure 5 depicts an example of environment configuration of the communication between the robot and beacons.
3.4. Beacon Transceiver System
The beacon embedded system is composed basically by two modules. One is responsible for RF signal receive and make all the concerned computation. This module has a 16F630 PIC microcontroller, operating at 4 MHz clock frequency. The other one is the RF signal transmitter. This module is equipped with 12F635 microcontroller and also operates at 4 MHz. The system is able to operate in autonomous way, been programmed with specific address. In other hand, the mobile robot must be programmed with the amount and the address of all operative beacon inside the navigating environment. Figure 6 depicts the block diagram of the RF transceiver at mobile robot and at beacons.
Figure 7 shows the beacons RF transceiver modules. The transmission module (Figure 7(b)) is able to function in asynchronous independent way, emitting a address code frame in a certain period of predetermined time, or synchronous way commanded by the reception and control module (Figure 7(a)). In the first case, a battery 12 V A23 model is used which allows autonomy of more than 3 months of continuous use, due to ultra low power energy consumption given by the embedded microcontroller with nanowatt technology. In second case the power supply and transmission command are made by the reception control module, illustrated in Figure 7(a). This second one is the mode utilized by this work.
(a) Implemented reception module
(b) Implemented transmission module
The mobile robot, as each one of the beacons, have a transceiver control system composed by reception module and transmission module. As the objective of our system is to provide a triangulation between the mobile robot position and the beacons, the transmission modules work in synchronous way. It is assumed that the module of control of reception-transmission of the mobile robot has been previously loaded with the amount of existing beacons in the environment and with its respective addresses codes. The functioning of the system goes to the following procedure. (1)The mobile robot emits a address code-frame for first beacon. In this instant it sends an interrupt control signal to the central processing unit for triggering and starts a timer counter. The robot then, waits the return of the signal. This return must occur in up to 100 ms. (2)If the signal returns, means that the beacon recognized the code and sent back the same code. In this instant is sent a signal to the robot embedded central processing unit for stops the timer and calculation of the signal return delay time, that could be about ns. (3)If the signal was not returned, means that the beacon is out of area reach or occurred some error in signal transmission-reception. (4)Increment the number of beacon and go to the loop first item.
The distance between the robot and a certain beacon is computed with base of the delay time in the reception of the same transmitted code. The total elapsed time between the code final transmission, sent by the robot, and the reception of the same code, sent back to the beacon, can be calculated by where is the travel signal time between leaves robot transmitter and reach beacon reception, is the processing signal time by the beacon, is the signal return elapse time, is the frame code period and is the processing time of the sent back signal received by robot.
It is well known that RF signal cover one meter in about 3,3 ns because its velocity is about 0,3 m/ns in air. We can considering that the linear speed of the robot is so small that the displacement of the robot could be considering as being zero during the time . In this way, the distance in meters between the mobile robot and the beacon can be given by where and are given in ns.
The elapsed time is computed with a 64 bits timer of the Texas Instrument TMS320C6474-1200 dualcore robot embedded processor. The instruction cycle time of it is about 0,83 ns (1,2 GHz clock Device), allowing timer calculations in order of ns, essential for our case of study. The times and are determined empirically and ms. In this way, the covered distance between the robot and beacon should be done by
Algorithm 1 depicts the computation method for distance calculation using RF ToF.
| ||||||||||||||||||||||||||||||||||
4. Triangulation
Triangulation refers to the solution of constraint equations relating the pose of an observer to the positions of a set of landmarks. Pose estimation using triangulation methods from known landmarks has been practiced since ancient times and was exploited by the ancient Romans in mapping and road construction during the Roman Empire.
The simplest and most familiar case that gives the technique its name is that of using bearings or distance measurements to two (or more) landmarks to solve a planar positioning task, thus solving for the parameters of a triangle given a combination of sides and angles. This type of position estimation method has its roots in antiquity in the context of architecture and cartography and is important today in several domains such as survey science. Although a triangular geometry is not the only possible configuration for using landmarks or beacons, it is the most natural [1].
Although landmarks, beacons and robots exist in a three-dimensional world, the limited accuracy associated with height information often results in a two-dimensional problem in practice; elevation information is sometimes used to validate the results. Thus, although the triangulation problem for a point robot should be considered as a problem with six unknown parameters (three position variables and three orientation variables), more commonly the task is posed as a two-dimensional (or three-dimensional) problem with two-dimensional (or three-dimensional) landmarks [4].
Depending on the combinations of sides (S) and angles (A) given, the triangulation problem is described as “side-angle-side” (SAS), and so forth. All cases permit a solution except for the AAA case in which the scale of the triangle is not constrained by the parameters. In practice, a given sensing technology often returns either an angular measurement or a distance measurement, and the landmark positions are typically known. Thus, the SAA and SSS cases are the most commonly encountered. More generally, the problem can involve some combination of algebraic constraints that relate the measurements to the pose parameters. These are typically nonlinear, and hence a solution may be dependent on an initial position estimate or constraint [5]. This can be formulated as where the vector expresses the pose variables to be estimated (normally, for 2D cases ), and is the vector of measurements to be used. In the specific case of estimating the position of an oriented robot in the plane, this becomes
If only the distance to a landmark is available, a single measurement constrains the robot's position to the arc of a circle. Figure 8 illustrates perhaps the simples triangulation case. A robot at an unknown location senses two beacons and by measuring the distances and to them. This corresponds to our case of study in which active beacons at known locations emit a signal and the robot obtains distances based on the time delay to arrive at the robot. The robot must lie at the intersection of the circle of radius with center at , and with the circle or radius with center at . Without loss of generality we can assume that is at the origin and that is at . Then we have A small amount of algebra results in resulting in two solutions and .
In a typical application, beacons are located on walls, and thus the spurious (in our example, the ) solution can be identified because it corresponds to the robot's being located on the wrong side of (inside) the wall.
Although distances to beacons provide a simple example of triangulation, most sensors and landmarks result in more complex situations [6]. The situation for two beacons is illustrated in Figure 9(a). The robot senses two known beacons and measures the bearing to each beacon relative to its own straightahead direction. This obtains the difference ins gearing between the directions to the two beacons and constrains the true position of the robot to lie on that portion of the circle shown in Figure 9(a). We can note that the mathematics admits two circular arcs, but one can be excluded based on the left-right ordering of the beacon directions. The loci of points that satisfy the bearing difference is given by where and are the distances from the robot's current position to beacons and , respectively. The visibility of a third beacon, as can be seen at Figure 9(b), gives rise to three nonlinear constraints on , and : which can be solved using standard techniques to obtain , , and . Knowledge of , and leads to the robot's position [7].
(a) Triangulation with two beacons
(b) Triangulation with three beacons
The geometric arrangement of beacons with respect to the robot observer is critical to the accuracy of the solution. A particular arrangement of beacons may provide high accuracy when observed from some locations and low accuracy when observed from others. For example, in two dimensions a set of three collinear beacons observed with a bearing measuring device can provide good positional accuracy for triangulation when viewed from a point away from the line joining the beacons (e.g., a point that forms an equilateral triangle with respect to the external beacons). On the other hand, if the robot is located on the line joining the beacons, the position can only be constrained to lie somewhere on this (infinite) line.
4.1. Triangulation with RF Beacons
In our case of study the beacons's position at 2D environment are known and thus, the distances between the beacons. If the beacons are positioned at points and the robot's position is given by , then, (4.6), that express the robot triangulation with three beacons, yield where the robot's position can be inferred by numerical methods.
5. Data Fusion
The question of how to combine data of different sources generates a great quantity of research in the academics ambients and at the research laboratories. In the context of the mobile robotic systems, the data fusing must be effected in at least three distinct fields: arranging measurements of different sensors, different positions, and different times.
5.1. State Space Models
A system for which the state vector can be fully determined from enough number of measurements is described as being perceivable. As used by Bar-Shalom et al. [8], to describe the estimate of state to be computed, is used to denote the estimate of the vector in the time step using collected data in a period of time where is including the time step . Using remarks until step , however abstaining , to form a prediction, this is in general expressed as which denotes prediction of state vector based in the information availability strict before the time . Being based on the availability of information until, and including themselves, the time , it forms an updated date state estimated , which denotes the estimative of the state vector in the time .
5.2. Plant Model
A plant model that describes how a state of the system , which in ours particular case represents the position of the mobile robot, changes in function of the time, control input and the noise can be expressed by where is the state transition function and is the noise function. One of most common and conventional way to represent the noise model is using the Gaussian noise model with zero average with covariance (Gelb also utilizes the notation to represent the Gaussian noise model with zero average with covariance ) [9].
A model of linear plant, from (5.1), can be written as where the matrix tells how the system evolves from a state to another one in the absence of entrances (that is frequently given as a identity matrix) and the matrix tells how the entrances of the control modifies the state of the system.
Considering a linear omnidirectional mobile robot with displacements restricted to the plan, a simple model of plant can be considered, as follow presented.
The robot state is given by where describes the robot pose at global coordinates system. As suppose that the robot is equipped with some omnidirectional locomotion system, then the control input can be described as an independent change in the robot and localization. If the error in the movement of the robot is independent in the directions and , and if this error can be modeled by some noise function and , then the robot plant model is given by (5.1), and at ours particular case becomes which the robot moves to where it will be commanded with each movement being corrupted by the noise process. Equation (5.3) describes a model of linear plant.
5.3. Measurement Model
The measurement model describes how the sensors data change in function of the system state. If the sensor model is inverting (hypothetical case) it will allows that the sensor data cam be used for state calculations. As explained for Leonard and Durrant-Whyte [10], which developed works with mobile robots equipped with ultrasonic sensors, the measurement model tells the sensor observation for robot position and the target geometry (bulkhead) that produces the observation, and it has the form of where is the noise function for so that represents the Gaussian noise model with zero average with covariance matrix , and is the target state vector and change accordingly the aim shape, that can be basically corners, edges, cylindrical surfaces or plain surfaces. The measurement function express a observation about the sensor for the target with a vehicle localization function and the target geometry.
As well as in the plant model, a linear measurement model is particularly interesting. From (5.4), this takes the form of where is the matrix which express how the measurements are derived with the state linear transformation. This simple case illustrate how a stare estimative cam be recover for measurement: if it is assumed the matrix is reversibly.
The majority of the mobile robots traction by wheels (MRTW) cannot be modeled in a linear way, and it is necessary to consider plant, model and not linear estimative processes, as ours case. Considering a nonlinear system, where the robot can be modeled as a punctual robot with independent control of the orientation and the speed, as the case of the synchronous transmission mobile robots. Then, for this type of robot, the control input that is, in the period until the robot moves from a distance forward, to direction that it is pointed, and then rotate itself . The system state is given by and the nonlinear plant model is given by
Each movement of the robot has a noise process parcel , which has a covariance matrix known or estimable . It is assumed for this process of noise that the same satisfies the necessary conditions that assures the use of the Kalman filter(presented in Section 5.4). If, in the practical way, the robot moves in distinct steps composites of pure rotations or pure translations (i.e., only one between and is different of zero), then only two versions of are necessary.
Assuming now that the robot is equipped with a sensor that can determine the distance from robot to one determined target marker in the environment. For example, the target can emit an only sound with a known frequency, and the robot is equipped with a receiver that captures the sound. If the robot and the sound emitted at the target have synchronized clocks, the distance between the target and the robot can be estimated. If the target is located at (), measurement model for this robot comes from (5.4) and is given by This model of measurement has the parcel of degradation given by the noise process with covariance matrix .
5.4. Kalman Filter
To control a mobile robot, frequently it is necessary to combine information of multiple sources. The information that comes from trustworthy sources must have grater importance about those one collected by less trustworthy sensors.
A general way to compute the sources that are more or less trustworthy and which weights must be given to the data of each source, making a weighed pounder addition of the measurements, are known with Kalman filter [11]. It is one of the methods more widely used for sensorial fusing in mobile robotics applications [12]. This filter is frequently used to combine data gotten from different sensors in a statistical optimal estimate. If a system can be described with a linear model and the uncertainties of the sensors and the system can be modeled as white Gaussian noises, then the Kalman filter gives a optimal estimate statistical for the casting data. This means that, under certain conditions, the Kalman filter is able to find the best estimative based on correction of each individual measure [13]. Figure 10 depicts the particular schematic for Kalman filter localization [14].
The Kalman filter consists of the stages follow presented in each time step, except for the initial step. It is assumed, for model simplification, that the state transition matrix and the observation function remain constant in function of the time. Using the plant model of (5.2) and computing a system state estimate in the time () based on robot position knowledge in the instant of time , we have how the system evolves in the time with the input control : In some practical equations the input is not used. It can also, to actualize the state certainty as expressed for the state covariance matrix through the displacement in the time, as: Equation (5.10) express the way which the system state knowledge gradually decays with passing of the time, in the absence of external corrections. The Kalman gain can be expressed as but, how it did not compute , this can be computed by
Using this matrix, a estimate of revised state can be calculated that includes the additional information gotten by the measurement. This involves the comparison of the current sensors data with the data of the foreseen sensors using it state estimate. The difference between the two terms or, at the linear case is related as the innovation. If the state estimate is perfect, the innovation must be not zero only which the sensor noise. Then, the state estimate actualized is given by and, the up-to-date state covariance matrix is given by where is the identity matrix.
5.5. Extended Kalman Filter
In many robotic applications with sensor data fusing, the system to be modeled fails for having a nonlinear Gaussian noise distribution. While the errors are approximately Gaussian, the Kalman filter can be used, even so, probably will not be optimal. For nonlinear systems, is used the Extended Kalman filter (EKF). This involves the linearization of the plant, (5.1) and, if necessary, the linearization of the measurement (5.4) cancelling high order terms of the Taylor expansion [15].
The model plant linearization involves the jacobian calculation of the plant model and to use it as a linear estimate of in the Kalman filter. The model measurement linearization involves the jacobian calculation of the measurement model and to use it as linear estimative .
For ours case of EKF use, takes the example of the plant model and nonlinear measurement model presented in Section 5.3. To simplify the exposition, it is assumed that and . For each robot movement, the next stages are follow. (1)For represent the robot displacement, it's used (5.1) and (5.7). The known control parameters are utilized for the robot pose estimation at the time , as: (2)A linearized plant model version is generated in the current estimative of the robot pose as: (3)The state uncertainty is generated by the state covariance matrix actualization, using measurements obtained until the time , including itself, through: which is the result of the linearization of (5.10).(4)The sensor model is linearized around current estimative of robot pose , as: If the sensor have a reference dot like target in , then (5)Using the value , the Kalman gain, that comes for (5.12), is computed like (6)The innovation, like presented in (5.13), in this way is compute as (7)Now, is possible to calculate the robot pose estimative, like shown at (5.15), like been (8)Finally, the actualized covariance matrix, like illustrated by (5.16), is now calculate as
After certain time interval, that is shortest possible, the derivatives used in the linearization model must be recalculated through the estimated current state. This backwards a deficiency in the EKF: if the estimated state is very far from the current state, the linear approach of the system behavior will not be enough precise.
6. Mobile Robot Rapid Prototyping
Figure 11 illustrates the mobile robot prototype developed by a research team led by authors and used for experiments validation and as base for the models and simulations.
The use of the rapid prototyping technique in mobile robotic systems differs from the traditional target used in mechanics engineering and enters in new field of research and development for projects of mobile robots mechatronics systems. In this way, the rapid prototyping of these systems is associated not only with the project of the physical system, but mainly with the experimental implementations in the fields of hardware and software of the robotic system. It is fundamental that the architecture of hardware of the considered system be opened and flexible in the way of effecting the necessary modifications for system optimization. A proposal of open architecture system was presented in [16].
The software of the embedded control system of the mobile robot, in the context of the rapid prototyping, can be elaborated in simulators and tested all the parameters for adjustments that makes necessary in accordance with the physical system to be implemented, the hardware architecture, the actuators and the sensors. In this way, in the context of this work, the rapid prototyping is then the methodology that allows the creation of a virtual environment of simulation for the project of a controller for mobile robots. After being tested and validated in the simulator, the control system is programmed in the control board memory of the mobile robot. In this way, a economy of time and material are obtained, sooner validating all the model virtually and later operating the physical implementation of the system.
6.1. HIL (Hardware-in-the-Loop)
The HIL technique is used in development and tests for real-time embedded systems. HIL provide a platform accomplish of development for adding the complexity of the plant under control to the tests platform. The control system is enclosed in the tests and developments through its mathematical models representations and all the respective dynamic model [17].
The Figure 12 illustrates the use of the HIL simulation technique for real-time simulation and experimental validation of the considered mobile robotic system. With the utilization of HIL it's possible to implement de Kalman filter methodology with others embedded control techniques for improve the mobile robot localization.
7. Experimental Results
Figures 13 and 14 depicts the result of EFK pose estimative applied in the trajectory of the mobile robot prototype (showed in Figure 11) accomplishing different routes.
As illustrated in Figure 13 it can seen that the mobile robot starts at point (10,10) moving itself with constant linear speed. The ellipses delimit the area of uncertainty in the estimates. It can be observed that these ellipses are bigger in the trajectory extremities, because in these points lass measurements date are computed. The average quadratic error varies depending on the chosen trajectory. It can be noticed that the pose estimative improves for more linear trajectories and with high frequency of on-board sensors measurements.
Figure 14 depicts another mobile robot trajectory example. In this case, the route is more irregular and curvilinear. The uncertainty of pose estimative are great than the first example (Figure 13). Thats because in this trajectory the on-board sensors measurements becomes more unprecise and with low-frequency samples.
8. Conclusions
Once given the nonlinearity of the system in question, the use of the EKF become necessary. It does not have therefore, at beginning, theoretical guarantees of optimality nor of convergence of this method. Therefore, it was implemented a model of simulation that allows, underneath of next conditions of the reality, to verify the performance of this technique. Among others parameters that were looked to realistic model the increasing error in the measure of the position of a marker (reference object in the environment) to the measure meets that in the distance between the robot and it increases. This effect also was introduced in the estimate of the observation covariance matrix to allow a more coherent performance of the filter. A factor extremely important is the characterization of the covariances matrices of the presents signals in the system.
The results of the simulations associated with experimental validations confirm that this technique is valid and promising so that the mobile robots, in autonomous way, can be able to correct its own trajectory. The consistency of the data fusing relative to the odometry of the mobile robot and the markers it is obtained even after inserted disturbances in the system.
The presented method does not make an instantaneous absolute localization, but successive measurements show that the estimative state converges for the real state of the robot.