Real-Time Uncertainty Estimation of Autonomous Guided Vehicle Trajectory Taking Into Account...

696 IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, VOL. 56, NO. 3, JUNE 2007

Real-Time Uncertainty Estimation of AutonomousGuided Vehicle Trajectory Taking Into Account

Correlated and Uncorrelated EffectsMariolino De Cecco, Luca Baglivo, and Francesco Angrilli

Abstract—This paper presents the description of a noveluncertainty estimation method employed for the navigation ofautonomous guided vehicles. In the proposed algorithm, the uncer-tainty of the odometric navigation system is estimated as a functionof the actual maneuver being carried out, which is identified bynavigation data themselves. The result is a recursive method forestimating the evolution of spatial uncertainty, which takes intoaccount unknown systematic effects and uncorrelated effects dueto kinematic model uncertainty. The method is explained startingfrom the measurement models and its parameters as a functionof the actual maneuvers. A verification of covariance propagationestimate due to systematic effects was carried out by means of aMonte Carlo simulation method. Experimental verification wascarried out using an autonomous vehicle. Compatibility betweena reference environment-referred system and the uncertainty esti-mated by the proposed method was achieved in 95% of the trials.

Index Terms—Autonomous guided vehicle (AGV), Monte Carlo(MC) simulation, pose estimation, spatial covariance.

NOMENCLATURE

(x, y, δ) Sensor fusion estimated position and attitude withrespect to the fixed reference of the reference pointPr on the vehicle.

Xk (x, y, δ) position and attitude vector at step k.CV Covariance matrix of vector V .RR, RL Right and left driver wheel radii, respectively.nRk, nLk Number of counts from the driving right and left

encoders, respectively.n0 Number of counts of the driving encoder in

one turn.b Wheel base of the differential drive robot.σλ Standard uncertainty in parameter λ.ελ Uncertainty in parameter λ defined with an as-

signed coverage factor.�Φk Jacobian matrix of the nonlinear function Φk.Cwk Diagonal matrix with covariances of the parameters

of wk vector.

Manuscript received June 15, 2006; revised February 2, 2007.M. De Cecco is with the Department of Structural Mechanical Engineer-

ing, University of Trento, 38100 Trento, Italy (e-mail: [email protected]).

L. Baglivo and F. Angrilli are with the Department of Mechanical Engineer-ing, University of Padova, 35131 Padova, Italy (e-mail: [email protected];[email protected]).

Color versions of one or more of the figures in this paper are available onlineat http://ieeexplore.ieee.org.

Digital Object Identifier 10.1109/TIM.2007.894904

Swk Matrix whose elements are the square root of Cwk

elements.Cξk Diagonal matrix estimating uncorrelated model un-

certainty with covariances of pose increments.ξk Random error vector affecting pose increments.MC Monte Carlo.

I. INTRODUCTION

AUTONOMOUS guided vehicles (AGVs) are widely em-ployed in many situations, such as factories, ports, hospi-

tals, farms, etc., yet measuring a vehicle’s attitude and position(the pose) is still a challenging problem.

The measurement systems that are currently used can be di-vided into direct, incremental, and environment-referred guid-ance. Although the first category is made of systems that are themost reliable, such as wire guidance, etc., there are also systemsthat suffer from the considerable problem of path planning.If the path has to be changed, production must be stopped.The incremental or dead-reckoning methods, such as encoders,gyroscopes, ultrasound, etc., have the considerable advantagesof being totally self-contained inside the robot, relatively simpleto use, and able to guarantee a high data rate. On the other hand,since these systems integrate relative increments, errors growconsiderably over time [1], [3]–[8]. Environment-referred guid-ance makes use of external references to achieve a measurementwith respect to the environment where the robot is moving [2].These systems work at a slower rate and need visible artificialtargets. However, since they measure the robot’s position andattitude with respect to those environment references (targets),the error is always bounded and environment-referred repeata-bility guaranteed [2]–[7], [9].

To achieve flexibility in path planning and robustness in ac-curacy, a combination of environment-referred and incrementalguidance [3]–[5], [7], [14] must be used by means of fusingthe information coming from sensors of the aforementionedcategories. In this way, a high data rate and an uncertainty inpose estimation, which is always bounded, are guaranteed.

To gain good accuracy from the fusion algorithm, an ac-curate and real-time uncertainty estimation of both the incre-mental and the environment-referred measurement systems isfundamental. While, for the environment-referred system, theuncertainty is a direct function of some known parameters(the number of visible targets, the vehicle speed, etc.), for theincremental ones, it is a function of some kinematic parameters,

0018-9456/$25.00 © 2007 IEEE

DE CECCO et al.: REAL-TIME UNCERTAINTY ESTIMATION OF AUTONOMOUS GUIDED VEHICLE TRAJECTORY 697

kinematic model, and the state history (in particular, the tra-jectory attitude). Furthermore, the kinematic parameter uncer-tainty correlation must be considered. For the aforementionedreasons, the uncertainty estimation cannot be based on methodsof combined standard uncertainty [13] but must itself be arecursive state-dependent function.

Many methods of spatial uncertainty estimation based onKalman filtering data fusion consider covariance propagationthat is unaffected of parameter uncertainty correlation. Thisleads to uncertain regions (ellipses) that decrease as a functionof the trajectory instead of growing, as is actually the case (fortrajectories, basically in one direction, i.e., unclosed trajectoriesthat actually lead to partially uncertain compensation). To over-come this problem, some of them make use of augmented stateand noise modeling [3]–[7].

The covariance propagation method proposed in this paperis based on a recursive covariance estimation that embodiesthe actual trajectory and takes into account both correlationof kinematic parameters and uncorrelated effects due to kine-matic model uncertainty [16]. The correlation issue is achievedby adding an integral matrix that recourses together withthe covariance one. The kinematic model uncertainty issue isachieved by introducing a stochastic term to the model, whichleads to an additional covariance term that is proportional to thepose increments.

The propagation of uncertainty due to correlation (through-out a path) of parameters of robot nonlinear kinematic modelhas been verified by means of Monte Carlo (MC) simulation,as outlined in [15]. Instead, the effects of kinematic modeluncertainty cannot be verified with MC simulation; they havebeen verified experimentally.

The benefits of the proposed method are that the actual ma-neuver uncertainty and kinematic model uncertainty are takeninto account when fusing odometers with inertial sensors. Theproposed method allows avoiding the use of augmented stateand noise modeling, thus simplifying and fastening the real-time implementation.

The algorithm was implemented on a differential drive au-tonomous vehicle. A PXI (National Instruments) with an em-bedded real-time operating system was used to control the robotand implement the online uncertainty estimation.

II. ONLINE UNCERTAINTY ESTIMATION

The kinematics and navigation equations of the differentialdrive are explained in this section. An odometric system usingtwo encoders on the driving wheels were installed on thevehicle (Fig. 1).

The discrete form of the odometric navigation equations,neglecting the uncertainty factors, is as follows:

xk+1 = xk + π · nRk·RR+nLk·RL

n0· cos(δk)

yk+1 = yk + π · nRk·RR+nLk·RL

n0· sin(δk)

δk+1 = δk + 2π · nRk·RR−nLk·RL

b

. (1)

The definition of variables relating to the equations shown inthis paper can be found in the nomenclature.

Fig. 1. (a) Kinematic scheme of the differential drive AGV. The attitude δis the angle between the environment-referred reference system xOy and themobile reference system XPrY . The pose Pr is defined by the vector (x, y, δ),which takes into account the attitude of the mobile robot. (b) Experimentalautonomous vehicle.

To combine the incremental system measurements (mainlyaffected by drift) with an environment-referred system, theuncertainty estimation at each step [4], [6] must be known.Uncertainty is expressed in terms of the covariance matrix ofthe vector Xk = [xk, yk, δk]. To achieve the aforementionedcovariance estimation, (1) must be rewritten by taking intoaccount the pose increments noise, i.e.,

Xk+1 = Xk + Φwk + |Φwk| · ξk (2)

where Φ is a nonlinear function of [nRk, nLk, n0, RR, RL, b]that calculate the position and attitude increments at eachiteration step. Parameter n0 is obviously constant because it isthe number of pulses of the encoder for each revolution. Param-eters nRk and nLk are neglected as a source of uncertaintybecause their errors are commonly compensated for betweenthe present cycle and the ones that follow if the period Tc issmall enough, as compared with the system dynamics. From theaforementioned considerations, the vector wk of variables thataffect the accuracy of the estimation of the position and attitudeas for the systematic uncertainty component can be writtenas follows:

wk = [RR, RL, b, δk]. (3)

The vector ξκ is a stochastic variable that is related tokinematic model uncertainty. It defines a random vector, and itscovariance matrix is diagonal, with the first two main elementsbeing equal, thus defining an uncertainty circle in an x−yspace. It is used to describe the error that affects pose increment.This error is proportional to increment modulus; therefore, theresulting covariance is proportional to the covariance of ξκ bythe square of increment modulus factor.

While the vectorwk refers to the uncertainty due to the modelvariables’ correlation, the vector ξκ refers to a stochastic com-ponent of the uncertainty due to kinematic model uncertainty.

Equation (2) can be used to evaluate the standard uncertaintyof the estimation Xk+1 if the standard uncertainty of theestimates Xk, wk, and ξκ are known. Under the hypothesisthat will be explained later in the text, the uncertainty can be


expressed using the following covariance matrix (refer to thenomenclature for the definition of the various terms):

CXk+1 =CXk + �Φk · Cwk · �TΦk + �Φk

· Swk · ITk + Ik · Swk · �T

Φk + |Φk|2 · Cξk(4)

where the integral term Ik can be evaluated using the followingsynchronized recursion:

Ik = Ik−1 + �Φk−1 · Swk−1. (5)

– There are five terms (one additional term has been addedwith respect to the previous method in [16]) on theleft-hand side of (4). The first one is the covariancematrix of the preceding sample. The second term is thecontribution of the parameters’ uncertainty as if theywere uncorrelated between each other, i.e., E{εwk(i) ·εwk(j)} = 0 ∀I �= j, and as if the same parameter wasalso uncorrelated with itself over time (no systematiceffect), i.e., E{εwk(i) · εwh(i)} = 0 ∀k �= h. The stan-dard uncertainties expressed in the covariance matrixCwk depend on the actual maneuver that the robot isperforming and are assumed to be uncorrelated, thusleading to an orthogonal matrix. The third and fourthterms take into account full correlation of the sam-ples throughout the integration period, i.e., E{εwk(i) ·εwh(i)}/(σwk(i) · σwh(i)) = 1 ∀k,∀h. In the case ofclosed trajectories, the contributions of the second, third,and fourth terms are such that at the endpoint, they com-pensate between each other, and the covariance matrix,without considering the contribution of the fifth term,returns to its initial value. This happens because theseterms do not take into account part of uncertainty thataffects estimated increments and are due to the nonidealkinematic model (model uncertainty). The fifth term isa diagonal matrix that is proportional to the modulus ofthe increment vector that takes account of uncertaintyin the estimation of the instantaneous center of rotationof the robot. This will be better explained in Section IV.In other words, the last three terms estimate the drift as afunction of time. For long periods, these last three termsprevail. For short periods, the first two terms prevail.

The hypothesis that there is complete correlation of eachuncertainty factor wk(i) with itself over time but uncorrelationbetween the different factors needs to be explained for eachfactor separately: It is true for RL and RR if the effect ofthe load is considered constant throughout the path, whereasit is not true if the effect of the floor irregularities on thewheels’ radius is considered; moreover, load variations inducea correlation effect between the two wheels’ radial dimensions,and a temperature variation induces correlation between RL,RR, and b as well. The correlation holds for δk throughoutthe integration period, but it is not uncorrelated to the otheruncertainty factors so that the uncertainties related to thisparameter are supposed to be underestimated. This has beenverified by simulation results.

III. VERIFICATION OF COVARIANCE PROPAGATION

ESTIMATE DUE TO SYSTEMATIC EFFECTS

In this section, we describe two methods that are used forverification of the uncertainty propagation algorithm concern-ing systematic effects.

1) A deterministic method was employed to test covariancematrix calculation correctness. It consists of making acomparison between a nominal and a changed trajectory.

2) An MC numeric simulation technique for the propagationof distributions was employed to verify that the model,to which the propagation is applied, under supposedconditions (distribution type and standard uncertainty ofthe input parameters known) is correct. In other words,it was used to verify if at the endpoint (but also inany other point) of a trajectory, the covariance estimatedfor model’s outputs is actually representative, under aspecified level of confidence, of the robot’s attitude andposition estimate. These two verification methods donot concern the part of uncertainty propagation modelregarding the uncorrelated effects of kinematic modeluncertainty. It is important to note that this last partcan be separated from the others in the case of closedtrajectories.

A. Deterministic Verification

The algorithm of uncertainty propagation due to systematiceffects has been tested by means of simulations and experimen-tal verification carried out for a set of representative trajectories:straight path, a curve of 90◦, and unclosed trajectories.

The calculation correctness has been tested by reconstructinga changed trajectory with respect to the nominal one. Thechanged trajectories are obtained by changing each influenceparameter one at a time, assigning to each parameter its variate,i.e., one of the two extreme values of its assigned coverage in-terval, and leaving the other parameters to their nominal values.Each trajectory is reconstructed from the same odometric datathat are acquired during a real path. As expected, the simula-tions show that the changed trajectory is the envelope of theuncertainty ellipses that are calculated with the same coveragefactor [12] that is used to calculate the variate of the relativeinfluence parameter (see Figs. 2 and 3). The uncertainty ellipsesrepresent a section of the multivariate normal distribution [10]and are calculated with a given confidence level (95%).

B. MC Verification

An MC simulation was carried out to verify the hypothesisof Gaussian distribution for the output end position and attitudethat were estimated and to verify whether the orientation ofthe estimated uncertainty ellipse and the entity of its principalcomponents is reliable, i.e., if the estimation of the covariancematrix is correct. To implement this technique, we referred to[15], which outlines a “propagation of distribution” approachto deriving the distribution of a measurand for any nonlinearfunction and for any set of random inputs; the guide still doesnot propose a method for multiple output systems like that used


Fig. 2. Graphic verification that the path with two-sigma radius variate is theenvelop of two-sigma ellipses of the nominal path. Nominal: RR = 89.8 mm.Changed: RR + 2σRR = 90.2 mm.

Fig. 3. Graphic verification that the path with two-sigma wheelbase variateis the envelop of two-sigma ellipses of the nominal path. Nominal: b =754.3 mm. Changed: b − 2σb = 753.3 mm.

TABLE IPARAMETERS AND THEIR STANDARD UNCERTAINTY USED

FOR UNCERTAINTY PROPAGATION METHOD

AND FOR MC SIMULATIONS

here. The Supplement’s approach assumes that the distributionsof the random inputs are exactly known. We supposed thatinput parameters are all normally distributed and have a bestestimate and a standard uncertainty that has been calculated bya calibration procedure.

Table I shows the parameters used for MC simulations.

Note that attitude δ is considered as neither an input param-eter nor as its covariance, because in the MC simulation, it isan output value depending on the values of the other kinematicparameters indicated in Table I; thus, its distribution will be anoutput of the simulation, whereas in the propagation method,its covariance is recursively calculated and associated with theinfluence parameter covariance matrix Cwk.

All the MC simulations are referred to the end position oftrajectory. For each iteration, the input parameters, which areconsidered as influence parameters, have been all randomlydrawn within their assumed normal distribution, with eachiteration carrying out an endpoint (xi, yi, δi) that is recon-structed by the kinematic system model [see (1)]. Choosing asufficient number of iterations M , the result of simulation isa sample set that is representative of the distribution of endposition (including attitude), and its covariance matrix has beencalculated and compared with that of the estimate by means ofthe recursive propagation method. Equation (6) explains howthe ith normal variate parameter was calculated, with normrandas the function for randomly choosing, for example, the RR

variate within its normal distribution with mean RR0 (nominalvalue) and standard uncertainty σRR

, giving the ith valueRR,i, i.e.,

RR,i = normrand(RR0 , σRR)

RL,i = normrand(RL0 , σRL)

bi = normrand(b0, σb)(xi, yi, δi) = f(RR,i, RL,i, bi, nRk, nLk)

. (6)

For all simulations, a value M = 105 iterations have beenchosen.

Concerning the (x, y) endpoint position of tested trajectories,MC simulations show a good correspondence (Figs. 4–7) bothfor the orientation of the covariance matrix eigenvectors and forits eigenvalues (standard deviations). It was also verified that,assuming a Gaussian distribution for the influence parametersRL,RR, and b, the distribution of the final pose that is estimated(x, y, δ) is also Gaussian. A chi-square test for all the analyzedsample sets gave a positive result for normal distribution hy-pothesis at a 95% confidence level.

In addition, it has been found that the hypothesis of attitudeuncertainty that is underestimated, as explained in Section II,is verified; in fact, the mixed terms σxδ and σyδ in the MCcovariance matrix are larger than the correspondent terms ofthe covariance matrix that is estimated with the propagationmethod that is presented and discussed here. Equation (7)is a numerical example of the estimated covariance and MCcomputed covariance related to the endpoint of trajectory inFig. 4, i.e.,

Cxyδ_est =

0.001037 −0.001040 0.0000290.001040 0.001126 0.0000190.000029 0.000019 0.000521

Cxyδ_MC =

0.0009686 −0.001043 −0.000695−0.001043 0.001162 0.000775−0.000695 0.000775 0.000518

.

(7)


Fig. 4. Uncertainty propagation for a 90◦ curve.

Fig. 5. MC simulation for a 90◦ curve. M = 105 iterations, 95% confidenceellipses.

The mixed terms of the third column are different betweenthe two matrices (the matrices are symmetric). Instead, if wecompare only the covariance submatrix related to the (x, y)endpoint distribution and the term σδδ , then there is a goodcorrespondence between the two matrices. In the case consid-ered as an example, while the difference between the 2-norm ofdiagonalized 2 × 2 Cxy submatrices is 1%, the same differencebetween the complete 3 × 3 diagonalized Cxyδ covariancematrices is 27%.

An MC simulation is also used to study the effect of pos-sible factors that can give rise to a correlation between theinfluence factors, which have been not considered in the prop-agation method, such as varying temperature and varying loadeffect.

The load variation correlation effect is supposed to interestthe parameters RR and RL and behaves similarly to that oftemperature but in a slightly different principal direction ofcorrelation. This effect could be taken into account by introduc-

Fig. 6. Uncertainty propagation for a straight path.

Fig. 7. MC simulation for a straight path. M = 105, 95% confidence.

ing an estimate of the correlation coefficient in the parameters’covariance matrix.

The statistical model that is used to simulate the thermal andload correlation effects is reported here.1) Thermal Correlation Model That Is Used: If

∆RR,∆Tmax , ∆RL,∆Tmax , and ∆b∆Tmax are the parameters’variations caused by thermal expansion because of a ∆Tmax

variation with respect to the nominal environment temperatureand ∆Ti is a random variable drawn from a supposedrectangular distribution with a [−∆Tmax,∆Tmax] interval andrelative to iteration i, then

RR,i = RR0 + ∆RR,Tmax · ∆Ti/∆Tmax

RL,i = RL0 + ∆RL,Tmax · ∆Ti/∆Tmax

bi = b0 + ∆bT0 · ∆Ti/∆Tmax

(8)

where RR0 , RL0 , and b0 are the nominal values of theparameters.


Fig. 8. Correlation by temperature. The dot-line ellipse is that calculated withonly the propagation method. The MC simulation takes into account only of thethermal correlation effect. ∆Tmax = 10 C◦.

Fig. 9. Correlation by load. The dot-line ellipse is that calculated with onlythe propagation method. The MC simulation takes into account only of the loadcorrelation effect. ∆Lmax = 10 kg.

The model that is used for load correlation simulation (Fig. 9)is the same as that used for the thermal one (Fig. 8) but with a∆Lmax load variation and a ∆Li random load; only parametersRR and RL are included for this model.

IV. EXPERIMENTAL CALIBRATION AND VERIFICATION

OF COVARIANCE PROPAGATION ESTIMATE

DUE TO MODEL UNCERTAINTY

In the previous section, we verified the propagation of spatialuncertainty concerning the effect of kinematic parameter un-certainty (vector wk). As confirmed by simulation results, theuncertainty propagation algorithm for these parameters is thatfor a straight forward–backward path, the covariance matrixreturns to its initial value. This is due to the effect of therecursive state-dependent function, where the integral terms

Fig. 10. Comparison of uncertainty propagation. Last two plots: Kinematicmodel uncertainty taken into account. First two plots: Kinematic model uncer-tainty not taken into account. The ellipses are of 95% confidence level and areenlarged two times for major visibility.

take into account of the trajectory history, and the systematiceffects lead the covariance matrix to reduce itself to its initialvalue, thus exploiting a compensation effect.

This is a well-suited result if the kinematic model is con-sidered to be not affected by uncertainty. The last term in (4)is a contribution due to a random error, which is proportionalto the amount of the increment itself. This random uncertaintycan arise when the model is not fully representative of thetrue kinematic system because of the wheels’ slippage andwheels’ misalignment and because varying wheelbase causesa source of uncertainty in the instantaneous center of rotationof the robot. The effect of this uncorrelated uncertainty sourceis proportional to the actual pose increment and, therefore, isassumed to increase the whole pose uncertainty in a way thatis independent of the actual maneuver, except for the distancecovered by the robot wheels and attitude variations duringintegration time.

The estimate of model uncertainty cannot be performed bymeans of simulation; thus, a calibration procedure has to becarried out by means of the experimental vehicle to definethe three elements of the covariance matrix of vector ξκ. Agood test trajectory is a straight forward–backward path to theinitial position to reduce the systematic effect contribution touncertainty estimation.

A straight 8-m path has been tested both in forward andbackward directions, i.e., starting and arriving at the sameposition, to compare the uncertainty that is estimated in bothcases of taking into account (or not) the contribution of the termof kinematic model uncertainty (Fig. 10).

In the first case, when the robot returns to its starting position,the covariance matrix returns to its null initial value because ofthe systematic correlation effect of the kinematic parameters;in the second case, when moving backward, the uncertaintyellipses start decreasing by the systematic correlation effect, butat a certain point, they restart to increase by the prevailing effectof model uncertainty errors that are taken into account.


Fig. 11. Continuous line: Path estimation using odometers. ◦: Endpoint esti-mation using the reference system. ∗: Endpoint displacements estimated usingthe reference system with respect to the endpoint estimation by odometers.

A set of forward–backward straight paths of different lengths(2, 4, 6, and 8 m) was repeated with a nominal load and aconstant velocity of 0.3 m/s in three different initial attitudesin such a way that the robot met different floor irregularities.Fig. 11 shows the endpoint estimation differences between theodometric system and the reference system and the estimateduncertainty ellipses computed with a 95% confidence level.The calibration procedure has led to obtain compatibility withthe uncertainty ellipses in 95% of the trials. To verify thedistribution of endpoint measurements, the covariance matrixof the experimental data has been computed, which shows goodcorrespondence with the covariance matrix that is computedwith the propagation method.

V. EXPERIMENTAL RESULTS

To verify the proposed technique experimentally, a mock-upof the differential drive robot that is 1.1 m in diameter (seeFig. 1) was used. Two incremental encoders were mounted onthe vehicle.

The encoders are 1000 ppr. The control references weregenerated, and the navigation recursion was computed by anindustrial PXI embedded system with an analog board and adigital one.

To verify the odometric uncertainty propagation algorithm,a triangulation sensor that was based on an optical scannermounted on the robot and infrared transmitters fixed in theenvironment [2], [11] was used. The angular uncertainty is±47 arc-seconds, and the position accuracy in the center of a5-m square room is ±1 mm (confidence level ±2σ). Fig. 12shows the uncertainty ellipses that were caused by the system-atic and uncorrelated effects that were discussed in Section IIas a function of the path [the ellipses are generated by takinginto account the first two rows and the first two columns of thematrix CXk; see (4)].

To show the effectiveness of the proposed algorithm in eval-uating the uncertainty ellipses, a set of four paths was repeated,

Fig. 12. Path odometric with uncertainty ellipses computed as a function ofthe path.

Fig. 13. Continuous line: Path estimation using odometers. ◦: Final point esti-mation using the reference system. ∗: Final point displacements estimated usingthe reference system with respect to the endpoint estimation by odometers.

changing the vehicle load and velocity: On the first path, thevehicle had a nominal load and a velocity equal to 0.3 m/s;on the second, the vehicle had the nominal load plus 10 kgand a velocity equal to 0.3 m/s; on the third, the vehicle had anominal load and a velocity equal to 0.6 m/s; and on the fourth,the vehicle had the nominal load plus 10 kg and a velocityequal to 0.6 m/s. For each set, two initial attitudes (see θ inFig. 12) and two different positions of the load have been tested.Fig. 13 shows the endpoint estimation differences between theproposed algorithm and the reference system and the estimateduncertainty ellipse computed with a 95% confidence level.

As shown in the MC simulations, the correlation incrementgives, in general, a thin ellipse (i.e., a preferred direction), and


this is caused by the fact that the influence parameters giverise to the same direction in endpoint estimation errors. Theexperimental results show that the uncorrelated effect on thepose estimation uncertainty is to enlarge the ellipses in boththeir main directions.

There is a huge variation in the geometry and orientationof the ellipses as a function of path. This clearly shows theneed for the covariance estimation of uncertainty and not onlythe standard uncertainties σx and σy , which could lead toinaccurate fusion with an environment-referred system (forexample, with laser scanner position estimation).

VI. CONCLUSION

This paper presents a recursive method for the estimationof the evolution of spatial uncertainty that was developed bytaking into account the systematic and uncorrelated effects aswell. Calibration of the uncertainty parameters was carried outas a function of the different maneuvers.

The uncertainty ellipses were thus computed as a function ofthe path. The method’s calculations, as for systematic effects,were tested by means of MC simulations that were employedto verify the propagation of the input parameters’ distributionsand the effect of thermal and load correlations that were nottaken into account. A calibration of the model uncertainty thatgives rise to uncorrelated effect was carried out experimentally.

The estimated errors in the endpoint measurement wereexperimentally carried out using a reference system with dif-ferent loads and speed conditions. The compatibility with theuncertainty ellipses was verified in 95% of the trials.

REFERENCES

[1] J. Borenstein and L. Feng, “Measurement and correction of systematicodometry errors in mobile robots,” IEEE Trans. Robot. Autom., vol. 12,no. 6, pp. 869–880, Dec. 1996.

[2] A. Adam, E. Rivlin, and H. Rotstein, “Fusion of fixation and odometry forvehicle navigation,” in Proc. IEEE Int. Conf. Robot. Autom., Detroit, MI,May 1999, pp. 1638–1643.

[3] S. Shoval, I. Zeitoun, and E. Lenz, “Implementation of a Kalman filterin positioning for autonomous vehicles, and its sensitivity to the processparameters,” Int. J. Adv. Manuf. Technol., vol. 13, no. 10, pp. 738–746,1997.

[4] D. Bouvet, M. Froumentin, and G. Garcia, “A real-time localizationsystem for compactors,” Autom. Constr., vol. 10, no. 4, pp. 417–428,May 2001.

[5] S. I. Roumeliotis, G. S. Sukhatme, and G. A. Bekey, “Circumventingdynamic modeling: Evaluation of the error-state Kalman filter appliedto mobile robot localization,” in Proc. IEEE Int. Conf. Robot. Autom.,Detroit, MI, May 10–15, 1999, pp. 1656–1663.

[6] L. Jetto, S. Longhi, and D. Vitali, “Localization of a wheeled mobilerobot by sensor data fusion based on a fuzzy logic adapted Kalman filter,”Control Eng. Pract., vol. 7, no. 6, pp. 763–771, Jun. 1999.

[7] Y. K. Tham, H. Wang, and E. K. Teoh, “Multi-sensor fusion for steerablefour-wheeled industrial vehicles,” Control Eng. Pract., vol. 7, no. 10,pp. 1233–1248, Oct. 1999.

[8] G. Ippoliti, L. Jetto, and S. Longhi, “Localization mobile robots: Devel-opment and comparative evaluation of algorithms based on odometry andinertial sensors,” J. Robot. Syst., vol. 22, no. 12, pp. 725–735, 2005.

[9] S. Sukkarieh, E. M. Nebot, and H. F. Durrant-White, “A high integrityIMU/GPS navigation loop for autonomous land vehicle applications,”IEEE Trans. Robot. Autom., vol. 15, no. 3, pp. 572–578, Jun. 1999.

[10] R. C. Smith and P. Cheeseman, “On the representation and estimation ofspatial uncertainty,” J. Robot. Res., vol. 5, no. 4, pp. 56–68, 1986.

[11] M. De Cecco, “Self-calibration of AGV inertial–odometric navigationusing absolute–reference measurements,” in Proc. IEEE Instrum. Meas.Technol. Conf., Anchorage, AK, May 21–23, 2002, pp. 1513–1518.

[12] JCGM, Guide to the Expression of Uncertainty in Measurement (GUM),1995, Geneva, Switzerland: Int. Organization Standardization.

[13] E. M. Nebot and H. Durrant-Whyte, “A high integrity navigation architec-ture for outdoor autonomous vehicles,” Robot. Auton. Syst., vol. 26, no. 2,pp. 81–97, Feb. 1999.

[14] L. Baglivo, M. De Cecco, F. Angrilli, F. Tecchio, and A. Pivato, “Anintegrated hardware/software platform for both simulation and real-timeautonomous guided vehicles navigation,” in Proc. ESM, Riga, Latvia,Jun. 1–4, 2005, pp. 227–232.

[15] ISO, 1995 Guide to the Expression of Uncertainty in Measurement. Sup-plement 1, currently under review to the member organizations of theJCGM and the national measurement institutes (draft Gen. 2007).

[16] M. De Cecco, L. Baglivo, and M. Pertile, “Real-time uncertainty esti-mation of odometric trajectory as a function of the actual maneuvers ofautonomous guided vehicles,” in Proc. IEEE Int. Workshop AMUEM,Sardagna, TN, Apr. 20–21, 2006, pp. 80–85.

Mariolino De Cecco was born in Pescara, Italy, in1969. He received the degree in electronic engineer-ing (with first-class honors) from the University ofAncona, Ancona, Italy, in 1995 and the Ph.D. degreein mechanical measurement from the University ofPadova, Padova, Italy, in 1998.

He is currently an Associate Professor of “me-chanical measurements” and “robotics and sensorfusion” at the University of Trento, Trento, Italy. Hisinterests include mechanical measurements, sensorfusion, vision systems, signal processing, mobile

robotics, adaptive control, and space mechanisms and qualification.

Luca Baglivo was born in Tricase, Italy, in 1978. Hereceived the higher degree in mechanical engineering(automation) from the University of Padova, Padova,Italy, in 2003. He is currently working toward the Re-search Doctorate in space science, technology, andmeasurement with the Department of MechanicalEngineering, University of Padova.

His interests include AGVs, sensor fusion, simu-lation, and robotics.

Francesco Angrilli received the higher degreein electronic engineering from the University ofPadova, Padova, Italy.

He is a Full Professor of mechanical and thermicmeasurements with the Department of MechanicalEngineering, University of Padova, where he is alsothe Director of the Center of Studies and Activitiesfor Space (CISAS) “G. Colombo.” He is also theDirector of postgraduate courses for the ResearchDoctorate in Space Science and Technology and aPrincipal Investigator for a number of research works

sponsored by the Italian National Research Council (CNR), the Italian SpaceAgency (ASI), the European Space Agency (ESA), and the Italian Minister ofNational Education (PNI), and private industries. He has been a member ofTechnical Team and a Program Manager for a number of space missions. Heis the author of more than 170 papers published in Italian and internationaljournals.

Real-Time Uncertainty Estimation of Autonomous Guided Vehicle Trajectory Taking Into Account...

Documents

Transcript of Real-Time Uncertainty Estimation of Autonomous Guided Vehicle Trajectory Taking Into Account...