9512.net

甜梦文库

甜梦文库

当前位置：首页 >> >> # Accurate Odometry and Error Modelling for a Mobile Robot

Accurate Odometry and Error Modelling for a Mobile Robot

Kok Seng CHONG

kok.seng.chong@eng.monash.edu.au

Lindsay KLEEMAN

lindsay.kleeman@eng.monash.edu.au

Intelligent Robotics Research Centre (IRRC) Department of Electrical and Computer Systems Engineering Monash University, Clayton Victoria 3168, Australia

http://calvin.eng.monash.edu.au/IRRC/

Abstract

This paper presents the key steps involved in the design, calibration and error modelling of a low cost odometry system capable of achieving high accuracy dead-reckoning. A consistent error model for estimating position and orientation errors has been developed. Previous work on propagating odometry error covariance relies on incrementally updating the covariance matrix in small time steps. The approach taken here sums the noise theoretically over the entire path length to produce simple closed form expressions, allowing efficient covariance matrix updating after the completion of path segments. Systematic errors due to wheel radius and wheel base measurement were first calibrated with UMBmark test [4]. Experimental results show that, despite its low cost, our system’s performance, with regard to dead-reckoning accuracy, is comparable to some of the best reported odometry vehicle.

1 Introduction One of the major tasks of autonomous robot navigation is localisation. In a typical indoor environment with a flat floorplan, localisation becomes a matter of determining the Cartesian coordinates (x,y) and the orientation θ of the robot on a two dimensional floorplan. For a wheeled robot, odometry (also known as dead-reckoning) is one of the most important means of achieving this task. In practice, optical encoders that are mounted on both drive wheels feed discretised wheel increment information to a processor, which continually updates the robot’s state using geometric equations. However, with time, odometric localisation accumulates errors in an unbounded fashion due to wheel slippage, floor roughness and discretised sampling of wheel increments. At the hardware level, gyroscopes and/or accelerometers [2] are used to determine the position of the robot based on inertial navigation, but this method is susceptible to drift. Blanche [6] uses a single steerable drive wheel with a pair of knife-edge unloaded bearing wheels solely for odometry. Dead-reckoning implementations at the University of Michigan include [3]: Cybermotion K2A utilises synchro-drive, making it insensitive to non-systematic errors; CLAPPER,

essentially two TRC LabMates connected by a compliant linkage, uses two rotary encoders to measure the rotation of the labmates relative to the compliant linkage, and a linear encoder to measure the relative distance between their centrepoints, giving it the unique ability to measure and correct non-systematic dead-reckoning errors during motion; Andros, a tracked vehicle, has a two-wheeled encoder trailer attached to its back which allows free horizontal rotation. The rotations of the trailer wheels and the trailer with respect to Andros are measured with the attached optical encoders. Theoretical research normally involves error modelling, so that a mathematical treatment is possible. For example, the Extended Kalman Filter requires that the odometry errors be expressed in the form of an error covariance matrix. Odometry errors can be classified as being systematic or non-systematic. In the work by Borenstein and Feng [3], a calibration technique called UMBmark test has been developed to calibrate for the systematic errors of a two wheel robot. The dominant systematic error sources are identified as the difference in wheel diameter and the uncertainty about the effective wheel base. In our work, this method has been used to calibrate systematic errors in our robot. For dealing with non-systematic errors, all classical models [4,8,9,12,13] either suffer from a lack of physical basis or inconsistency. To illustrate, suppose that at stage k-1, the state of the robot is Sk?1 = [ xk?1 yk?1 θk?1]T , which comprises Cartesian coordinates (xk-1, yk-1) and orientation θk-1 with respect to a global reference frame. A rotation α k followed by a translation Dk moves the robot to a new state S k .. ? Dk cos(θk ? 1 + αk )? (1) ? Sk = f ( Sk ?1 , u k ) = S k ? 1 + ? ? Dk sin(θk ?1 + αk ) ? ? ? αk ? ? To propagate the error covariance matrix of S k-1 to S k, the error incurred is assumed to be small so that first order Taylor’s expansion does not introduce significant higher order errors. By assuming that the error in stage k-1 is not correlated with the error introduced by the input,

the covariance matrix of stage k, Cov(S k ), can be evaluated as follows, (2) Cov(Sk ) = ?S fCov(Sk ?1 )?S f T + ? u fCov(u k ) ?u f T where ? Sk f , ? Uk f are the Jacobians of f with respect to S k and Uk , respectively, and

?σ 2 Cov( uk ) = ? D ? 0 0 ? ? σ2 α?

k k k k

(3)

In the authors’ opinion, the major problem with this treatment is that there is no physical basis in assuming that the translation error is uncorrelated with the rotation error [8,4,12,13]. Model parameters do not reflect the physical characteristics of the system, hence they are difficult to obtain experimentally. The model is also inconsistent. For the same path, if propagation of error is done in multiple parts, the model yields different solutions. To illustrate, suppose that T S k ?1 = [0 0 0 ] and Cov(S ) = 0 . Compare the k-1 following two scenarios : ?σ 2 T (i) uk 0 = [2 D 0] with Cov( u k 0 ) = ? D0

? 0 ?σ 2 D0 From equation (2), Cov(Sk ) = ? 0 ? ? ? 0 0 4 D2 σ 2 α0 2 Dσ 2 α0 0 ? ?. σ2 α0? 0 ? 2 ? 2 Dσ α 0? 2 ? σα 0 ?

T 0] , with

(ii) uk1 = [ D

Cov ( u

σ = ? ? 0

2 D1

0]

T

followed by u k 1 = [ D

0

2 α

twice, Cov(S k ) = ? 0 ?

? ? 0

? 2σ 2 D1

? . By applying equation (2) ? 0 0 ? ? 5 D 2σ α 1 3 Dσ 2 α1 ? 2 2 3 Dσ α 1 2σ α 1 ? ?

Even by setting Cov(uk0)=2?Cov(uk1), the two cases yield different final state error covariance, even though they lead to the same final state by following exactly the same path. This problem has been resolved [4] by performing error propagation for every time increment on the wheel encoders. This approach is conceptually similar to numerical integration but suffers a high computational cost. Also no physical justification for the error model has been offered. The theoretical work by [14] has introduced a more realistic, physically-based error model for an arbitrary circular arc motion. The result is a model which is very accurate for large wheel turn variance, but limited in its applicability to a range of rotation angles. For large rotation angles, the robot path has to be divided into small segments in which the total turning angle is within the limitation of the model. Other methods of representing position error includes the ‘circular-error probable’ (CPE) by [11] which is questionable because it is well known that the position

error is usually not equal in all directions. [10] has proposed the use of equal-error probability isoline and has outlined some ways of growing the isoline as the vehicle moves. It remains uncertain whether new and sound methods could be developed to make use of them. The new non-systematic error model developed by the authors has a strong physical basis which is closely related to the odometry design of our robot, Werrimbi. The model also generates error representation in the form of a error covariance matrix, which is the standard operating block for a multitude of noise filtering tools. Unlike the model illustrated earlier, the new model is consistent in a multiple path segments scenario. The computational load in incrementally updating the covariance matrix in small time steps, as done in [4] has been removed because simple close form formulae have been derived for three simple path types: (I) circular arc motion (II) straight line (III) rotation about the centre of the axle. Complex paths can be divided into a small number of sections which can be approximated by the aforementioned cases, hence the model can be applied on a section by section basis. Unlike the model in [14], it is valid for arbitrary distance and rotation angle. Even though the model by [14] is more accurate when the errors are large, compared to the first order accuracy of the new model, in the authors’ opinion, should the robot be operating in conditions likely to incur large errors, an accurate representation of these large errors is inconsequential. Instead, preventive measures should be adopted to avoid such errors, such as employing external referencing. Our odometry model can then be used to optimally combine other sensors with odometry. The remainder of this paper is organised into six sections: Section 2 presents the odometry system, and states, with justification, the key assumptions being incorporated into the model. Section 3 describes the UMBmark test used for reducing systematic errors and lists the key equations used. This is followed by section 4 on the derivation of the proposed non-systematic error model and its properties. The details and results of calibration of systematic errors and validation of the proposed error model constitute section 5. Lastly, section 6 is the conclusion which suggests possible future extensions of the model. 2 STEP ONE: Robot Design and Assumptions Werrimbi has two pairs of wheels: drive wheels and encoder wheels. The encoder wheels are as sharp-edged as practically possible to reduce the wheel base (B) uncertainty, and are unloaded because they are independently mounted on linear bearings which allow vertical motion, hence the problem of wheel distortion and slippage is minimised. The idea of unloaded wheels can

to assume that for a short unit of travel, the error incurred on both wheels are uncorrelated. because the two drive separate optical shaft encoders are used to gather odometry information. This assumption is also adopted in

xc. g., CW / CCW = yc .g .,CW / CCW =

1 n

∑ ex

i =1 n i =1

n

i,CW / CCW

1 n

∑ ey

i, CW / CCW

(5)

+

x

With the two pairs of centres of gravity, the tuning factors for the wheel base, the radius of left wheel and the radius of right wheel, cb, cl and cr, can be found by following this sequence of computations: + xc.g .,CCW yc .g.,CW ? yc.g .,CCW ? ?x (6) α = average? c.g .,CW , ?

? ?4 D ?4 D ? x c.g .,CW ? xc .g .,CCW yc .g .,CW + yc.g .,CCW ? β = average? , ? ? ? ?4 D ?4 D D + B sin(β / 2) Ed = D ? B sin(β / 2) ?

castor encoder wheel drive wheel

(7) (8) (9) (10)

y

motor B

cb = π / (π ? α)

castor

cl = 2 / ( Ed + 1)

optical shaft encoder

Figure 1 : (Left) Werrimbi, the sonar sensing robot (Right) Design of the accurate odometry system

Our work takes the assumption one step further. For a short unit of travel, the error is assumed to be zero mean, and white, that is, uncorrelated with the previous or next unit of travel. The variance of the cumulative error is then the sum of the variance of each statistically independent unit. This leads to a reasonable assumption that the variance of each unit of travel is proportional to the distance travelled

2 σ2 L = kL d L 2 σ2 R = kR d R

(11) cr = Ed cl and finally, the measure of dead-reckoning accuracy for systematic errors is defined in [3] as 2 2 2 (12) Emax, syst = max x 2 c. g ., CW + yc . g., CW , x c. g ., CCW + yc . g., CCW

(

)

4 STEP THREE: The New Non-systematic Error Model With the new non-systematic error model, the entire path travelled by the robot is treated as consisting of k small segments. Propagation of error covariance is required to be done k times to obtain the error covariance of the final state. This section shows that it is possible to obtain a closed form solution for this model, as k approaches infinity. The solution for a general circular arc motion is first developed. The solutions for two special cases, straight line motion and on-spot turn are then obtained by suitably taking limits.

(4)

where dL and dR are the distances travelled by each wheel, 2 2 and k L and k R are constants with unit m1/2. 3 STEP TWO: Calibration of Systematic Error Using UMBmark Test UMBmark test [3] is used to calibrate wheel base error and unequal wheel diameter error. Briefly, the robot is programmed to travel a square path of side D in the clockwise sense (CW) n times, and the offsets of the final Cartesian coordinates from the initial Cartesian coordinates, exi,CW, eyi,CW are recorded. The experiment is repeated for the counterclockwise sense (CCW) and exi,CCW, eyi,CCW are recorded. The ‘tuning factors’ required to be incorporated into the software to counteract the effect of the systematic errors are calculated from the weighted Cartesian offsets in both senses. In summary, the centres of gravity of the offsets can be computed from their averages

Figure 2 : Initial and final state of the robot after following a circular arc trajectory

Suppose that at segment k-1, the state of the robot is S k-1. It then propels its left wheel by Lk metre and its right wheel by Rk metre, to bring the robot to a new state

S k . Over an infinitesimal time increment, the speed of the wheels can be assumed constant, hence the path takes on a circular arc trajectory with constant radius of curvature rk. Refer to Figure 2. Based on the constant radius of curvature assumption, the following transition equation can be derived:

Lk ? rk sinθ k ?1 ? sin θ k ?1 + Rk ? ? B ? ? Rk ?Lk Sk = f (Sk ?1, uk ) = Sk ?1 + ?rk cos θ k ?1 + B ? cosθ k ?1 ? ? ? Rk ?Lk B ? ? ? ?

?2 rc1 r Cov(Uk )11 , = ( L?R) 2 (sin θ 0 ? sin θ k )cos θ k + ( B cosθ k ) c2 2 Bc3 ? 4( L 2(θ k ? θ 0 ) ? sin(2θ 0 ) + sin(2θ k )] ?R )3 [

1 2

2 rc Cov(Uk )2 ,2 = ( ? (cosθk ? cosθ0 )sinθk + ( r B sin θk ) c2 L?R) 2 Bc ? 4( L 2(θk ? θ0 ) + sin(2θ0 ) ? sin(2θk )] ? R) [

3 3

[ [ (

(

)

)]

Cov(Uk ) 3,3 = Cov(Uk )1, 2 =

c2 B2 ?rc1 ( L? R )

2

]

(13)

(cos(2θk ) ? cos(θ0 + θk )) + c2

3

2

( rB )2 sin(2θk )

Bc ? 4( L cos(2θ0 ) ? cos(2θk ) ] ? R) [

3

where rk is the instantaneous radius of curvature,

Cov (U k )1, 3 =

(14)

rk =

B 2

(

Lk + R k Lk ? R k

)

Cov(U k ) 2, 3 =

where

c1 ( L ? R )2 c1

(sin θ 0 ? sin θ k ) ? (cos θ k ? cosθ 0 ) ?

( L ? R) 2

rc2 B2 rc2 B2

cos θ k sin θ k

(17)

where

u k = [ Lk

T Rk ] .

The next step is to

2 c1 = k 2 R R L + kL R L 2 c2 = k 2 R R + kL L 2 2 2 2 c3 = k R L R + kL R L

propagate the error covariance matrix associated with (k1)th stage to the kth stage using equation (2). Now suppose that the arc segment is infinitesimally small, and the full path actually comprises k such segments being concatenated from end to end. The initial state of the robot is S 0 which is at the starting end of the first segment and the last segment is S k which is the destination of the last segment. The expression for covariance propagation can be recursively expanded (like a Markov process),

Cov( S k ) = ? Sk fCov ( S k ?1 ) ? Sk f T + ? u k fCov ( u k ) ? u k f T ? k ? ? k ? = ? ∏ ? S i f ? Cov ( S 0 ) ? ∏ ? S i f ? ? i =1 ? ? i =1 ?

T

(18)

These equations remove the need to incrementally update the covariance matrix in small time steps. As closed form expressions, they are applicable to any circular arc motion with constant radius of curvature. For simple paths, the matrix terms can be further simplified by suitably taking limits. 4.1 Special Cases : Straight Line Translation and Rotation about the Centre of Wheel Axle For a straight line path of length D, both wheels rotate by the same amount and the initial angle is approximately the same as the finally angle, the limits to be taken are L,R→D and θ0→θk. Contrary to popular assumptions [7,12], our model predicts that variance in the direction perpendicular to the direction of motion is proportional to the cube of the distance travelled, whereas the variance in the direction of motion is only proportional to the distance travelled. For rotation about the centre of wheel axle, both wheels still rotate by the same amount, but in opposite direction. The limits to be taken are L → B θ k ? θ 0 ) and 2 (

(15)

T k ?? k ? ? k ? ? ? ? + ∑ ? ? ∏ ?S j f ? Cov ( u k )? ∏ ? S j f ? ? ? ? ? ? i= 1 ? j = i+ 1 j =i +1 ? ? ?

Let Li , Ri denote the small increments in wheel turn at for the ith segment, and for circular arc motion, L = kLi , R = kRi. The Cov(S k ) can be further evaluated to

Cov( S k ) ?1 = ?0 ? ? ?0 0 1 0 r(cos θ0 ? cos θk ) ? ?1 r (sinθ0 ? sin θk ) ?Cov( S0 ) ?0 ? ? 1 ? ? ? ?0 0 ?? k ? ? ? ∏ ?S f ? 2 kR R i ? ? j = i + 1 j ?

T T

0 1 0 ? ? ? ? ?

r(cos θ0 ? cos θk ) ? r (sinθ0 ? sin θk ) ? ? 1 ? ?

R→ B 2 (θ 0 ? θ k ) . All simplified expressions can also be

found in [5]. This model is consistent unlike the models presented in the introduction of this paper [4,8,9,12,13]. That is, if propagation is done in multiple parts, the model generates exactly the same result. This is because the model itself is founded upon the concept of incrementally propagating error covariance from one infinitesimal section to the next. 5 Implementation and Results 5.1 Calibration for Systematic Error The experiments were conducted in a lab with a parquetry floor. The wheel encoder measurements were used to

k ?? k ? ?k 2 L ? +∑ ?? ∏ ?S j f ? ? L i ?? 0 i = 1 ?? j = i + 1 ?

(16) The sum of products part of equation (16) gives rise to an error covariance matrix henceforth known as Cov(Uk ). Let Cov(Uk )i,j be the ith row, jth column component of Cov(Uk ), after taking k→∞ (and considerable effort!), then

calculate the perceived final state of the robot, whereas a sonar array [15] mounted on top of the robot was to used to estimate its actual state by sensing some reference walls placed close to the initial position. Two reference walls were used to establish the robot’s coordinates and orientation.

0.04 0.02 0 0 -0.02

0 0.05

5.2 Computation of Non-systematic Error Parameters After calibration, Werrimbi was programmed such that it scanned two reference walls, moved forwards 10 metres, moved backward 10 metres, re-scanned the two reference walls and compared the position estimation from sonar sensing and odometry reading. The process was repeated 60 times (5 hours consumed) and the difference in the Cartesian coordinates, (ex, ey) and the difference in orientation et for all 60 runs are plotted against each other in Figure 4(a)-(c). The values of kL and kR have been obtained by fitting error ellipses to the data, and they are kL=0.00040m1/2, kR=0.00058m1/2. In Figure 4, both the 95% confidence error ellipses of the actual data and the 95% confidence error ellipses generated with kL and kR are overlayed on the plots for comparison. 6 Conclusion and Future Work This work is the preliminary stage of a robotics mapping project [5]. It draws together the key considerations and procedures involved in the calibration and error modelling for an odometry design. An accurate odometry system has been presented which is comparable to the best reported system but can be fabricated at low cost. A new first order odometry error model has been derived to propagate the state error covariance following a motion. The error model takes the form of a covariance matrix which is prevalent in statistics and filtering theory, hence could easily fit into many powerful tools such as the Kalman Filter. This model cannot account for unexpected errors such as hitting a bump on the floor, which violates the flat floorplan assumption. For certain applications such as mapping, external referencing should be deployed to detect such errors. 7 Acknowledgment The authors are grateful to Mr. Greg Curmi who took part in the design of the robot.

0 -0.001 0 -0.02 -0.04 -0.06 ey -0.08 -0.1 -0.12 -0.14 -0.16 ex

CW CCW

0.025

CCW CW

0.02

0.04

0.06

0.08

0.1

0.12

-0.04 -0.06 -0.08 -0.1

-0.05

-0.025

0

0.025

0.05

-0.025

-0.05

Figure 3 : Result of UMBmark test, before and after calibration. (metre units)

The distribution of Cartesian offsets after the completion of D=4m square path for 5 runs in each sense (clockwise and counterclockwise), before and after calibration, are shown in Figure 3. The value of D has been chosen as such in order to make benchmark comparison with the results presented in [3]. The calibration results are presented in Table 1. Comparison with other robot vehicles are made in Table 2. All except Cybermotion K2A are calibrated. It can be seen that the measure of dead-reckoning accuracy for systematic errors, Emax ,syst , of Werrimbi is comparable to those achievable by many advanced odometry systems. Further calibration has been carried out with the compensated parameters but no significant improvement has been made.

Table 1 : Key results before and after calibration

xc.g.,CCW (mm) yc.g.,CCW (mm) xc.g.,CW (mm) yc.g.,CW (mm) Emax,syst (mm)

Before 97 -94 32 31 135

After -26 16 1.5 11 30

Table 2 : Comparison of dead-reckoning accuracy and approximate cost of our design with four vehicles. The first four sets of figures are obtained from [3]

-0.003 -0.002

0.001

0.002

0.003

0.004

0.005

Vehicle TRC LabMate Cybermotion K2A CLAPPER Andros with Trailer Werrimbi

Emax,syst average 27 63 22 74 30

Cost (US$) 10K <50K 30K ? ~4K

(a) ex versus ey

1.00E-02 5.00E-03 0.00E+00 -0.001 -5.00E-03 -1.00E-02 et -1.50E-02 -2.00E-02 -2.50E-02 -3.00E-02 -3.50E-02 ex

-0.003 -0.002

0

0.001

0.002

0.003

0.004

0.005

(b) ex versus et

1.00E-02 5.00E-03 0.00E+00 -0.2 -0.15 -0.1 -0.05 -5.00E-03 -1.00E-02 et -1.50E-02 -2.00E-02 -2.50E-02 -3.00E-02 ey -3.50E-02 0

(c) ey versus et Figure 4 : Errors in the along-track direction ( ex), the off-track direction (ey) and orientation (et), for 60 runs, are plotted against each other. The solid ellipses belong to the real data whereas the dashed ellipses are generated with the kR and kL obtained by trial and error.

8 Reference

[1] Bar-Shalom, Y. and Li, X.R. "Estimation and Tracking: Principles, Techniques and software", Boston, London: Artech House Inc., 1993 [2] Barshan, B. and Durrant-Whyte, H. “An Inertial Navigation System for a Mobile Robot”, IEEE Transactions on Robotics and Automation, Vol 11, No. 3, June 1995, pp. 328-342. [3] Borenstein, J. and Feng, L. “UMBmark - A method for Measuring, Comparing, and Correcting Deadreckoning Errors in Mobile Robots.”, Technical Report UM-MEAM-94-22, University of Michigan [4] Crowley, J.L. and Chenavier, F. "Position Estimation for a Mobile Robot Using Vision and Odometry",

Proceedings 1992 IEEE International Conference on Robotics and Automation, pp.2588-2593. [5] Chong, K.S. and Kleeman, L, “Accurate Odometry and Error Modelling for a Mobile Robot”, Technical Report MECSE-1996-6, 1996, Intelligent Robotics Research Centre, Department of Electrical and Computer Systems Engineering, Monash University [6] Chong, K.S. and Kleeman, L. “Sonar Based Map Building for a Mobile Robot”, 1997 International Conference on Robotics and Automation (this issue). [7] Cox, I.J. "Blanche - An Experiment in Guidance and Navigation of an Autonomous Robot Vehicle", IEEE Transaction on Robotics and Automation, 1991, Vol 3, pp.193-204. [8] Durrant-Whyte, H.F. et al "Position Estimation And Tracking Using Optical Range Data", Proceedings 1993 IEEE International Conference On Robotics and Automation, Vol 3, pp.2172 - 2177. [9] Jenkin, M. et al "Global Navigation for ARK", Proceedings 1993 IEEE International Conference On Robotics and Automation, Vol 3, pp.2165-2171. [10] Krantz, D. “Non-Uniform Dead-Reckoning Position Estimate Updates”, IEEE International Conferencve on Robotics and Automation, April 1996, Vol 2, pp 20612066. [11] Leenhouts, P. “On the Computation of Bi-Normal Radial Error”, Navigation: Journal of The Institute of Navigation, Vol 32, No 1, Spring 1985, pp. 16-28. [12] Lu, F. and Milios, E.E. "Optimal Global Pose Estimation for Consistent Sensor Data Registration", IEEE International Conference on Robotics and Automation, 1995, pp.93-100. [13] Nishizawa, T. et al "An Implementation of On-board Position Estimation for a Mobile Robot", IEEE International Conference on Robotics and Automation, 1995, pp.395-400. [14] Wang, C.M., “Location Estimation and Uncertainty Analysis for Mobile Robots”, in IEEE Conference on Robotics and Automation, 1988, pp. 1230-1235 [15] Kleeman, L. and Kuc, R. "Mobile Robot Sonar for Target Localization and Classification", The International Journal of Robotics Research, Vol. 14, No 4, August 1995, pp.295-318.

赞助商链接

更多相关文章：
更多相关标签：