Dead Reckoning Navigation for Autonomous Mobile Robots

Copyright © IFAC Intelligent Autonomous Vehicles, Madrid, Spain, 1998

DEAD RECKONING NAVIGATION FOR AUTONOMOUS MOBILE ROBOTS KyuCheol Park 1

1 ,

Hakyoung Chung

2 ,

and Jang Gyu Lee

1

Automatic Control Research Center, School of Electrical Engineering Seoul National University, Seoul , 151-742, Korea 2 Department of Control and Instrumentation Engineering Seoul National Polytechnic University, Seoul , 139-743, Korea

Abstract - A dead reckoning navigation system for autonomous mobile robot localization is presented. The navigation system was implemented by novel sensor fusion using a Kalman filter. A differential encoder and the gyroscope error models are developed for the filter. An indirect Kalman filter scheme is adopted to reduce the computational burden and to enhance the navigation system reliability. The filter, which feeds back the estimated errors to the main navigation system, mutually compensates the encoder errors and the gyroscope errors. To evaluate the filter performance, the observability of the filter was analyzed. The characteristics of unbounded position error growth in dead reckoning navigation system has been shown by the observability analysis. The experimental results show that the proposed mobile robot navigation algorithm provides the reliable position and heading angle of the mobile robot without any help of external positioning system. Copyright © 1998 1FAC Keywords: Mobile robot, Navigation, Gyroscope, Encoder, Kalman filter

1.

Introduction

into deterministic and stochastic errors [Borenstein, Soiuris) . The deterministic errors can be compensated by the suitable function, but the stochastic errors must be compensated by filtering with error models. The extended Kalman filter has been widely used for the error compensation of the navigation system, and the deterministic error characteristics of the inertial sensors are reflected in previous research [Barshan et ai, Komoriya et all. The purpose of this work is to develop a dead reckoning navigation system using a differential encoder and a gyroscope. Previous researches on the dead reckoning navigation have not considered the systematic errors of the encoder and the stochastic errors of the gyroscope explicitly. The Kalman filter of the previous mobile robot navigation system estimates the position and heading angle, but our Kalman filter estimates the position error, heading angle error, the encoder errors, and inertial sensor errors. In addition, the encoder systematic errors and gyroscope stochastic sensor errors are considered explicitly to enhance the navigation system performance. The estimated errors are fed back into the mobile robot navigation system to compensate the navigation errors. For the performance analysis of the designed Kalman filter, the observability of the filter has been examined. The previous research did not explain the cause for the growth of the position and heading error of the dead reckoning navigation system. The increasing characteristics of the position

The dead reckoning navigation system determines the current location by utilizing some a position, trajectory, and velocity information over a given period of time. The dead reckoning navigation system is totally selfcontained, and it can always provide the mobile robot with an estimated position. The encoder navigation system is widely used for the mobile robot since the encoders are inexpensive and easy to use . As the encoder basically measures the moving distance, it is apparent that the errors of the encoder have an effect on both the heading and the position of the mobile robot. The errors are accumulated without a bound while the mobile robot moves. The heading error can result a serious position error. One of the refined dead reckoning systems is the inertial navigation system. The inertial navigation system consists with inertial sensors such as gyroscopes, accelerometers, and some types of filters. To compute the position and attitude, the gyroscope and the accelerometer data must be integrated. Thus, even very small errors in the inertial sensors will cause an unbounded growth in the position and attitude. Therefore, the navigation errors of the dead reckoning navigation system using encoders and inertial sensors should be compensated by other external mechanisms. Encoder errors are classified into systematic and nonsystematic errors, and inertial sensors errors are divided

?1Q

and heading angle error of the dead reckoning navigation system are analyzed in this paper. In the Section 2, the dead reckoning navigation system with using encoders, a gyroscope and a Kalman filter is designed. The observability of the designed filter is analyzed in the Section 3. Experimental result is shown in the Section 4 . In the final Section 5, the advantages of the designed navigation system are discussed .

c5X (k + 1)

=c5X(k) + sin lJf(k)B- S. (k) + sin lJf(k).!:.S L(k) 2

2

R+L + cos lJf(k )--c5IJf(k ) 2

R

L

2

2

c5Y(k + 1) =c5Y(k) + cos lJf (k ) -S . (k ) + cos lJf(k )-S L(k)

.

(I)

R+L

-sm lJf(k)--c5IJf(k)

2 R L L-R c51Jf (k + 1) = c51Jf(k)+OS . (k ) - 0 SL(k ) + c5D(k )

OZ

2. The mobile robot dead reckoning navigation system

Equation (1) expresses the error propagation of the position and heading angle of the mobile robot. The encoder scale factor errors, and the wheel base error can be regarded as the random constant due to their slow time-varying characteristics. Thus, those error can be expressed by

The dead reckoning navigation for a mobile robot usually uses encoders, accelerometers, and gyroscopes. The encoder, which is directly coupled to the motors armatures, measures the displacement along the travel path. The differential drive mobile robot measures not only the displacement but also the heading angle with two encoders. As the encoder errors are proportional to the moving distance, the position and the heading angle error increases without a bound. It is well known that encoder error sources fit into one of two categories ; systematic errors and non-systematic errors[Borenstein] . Systematic Errors I. Unequal wheel diameters which can be defined as encoder scale factor errors 2. Difference between the actual wheel base and the nominal wheel base 3. Misalignment of wheels Non - Systematic Errors 1. Travel over uneven floors 2. Travel over unexpected objects on the floor 3. Wheel slippage The systematic errors are accumulated consistently, and they are dominant in smooth indoor environment surface. However, the non-systematic errors may be dominant when a mobile robot moves on the rough surface. Our model considers only systematic errors, because the non-systematic errors are unpredictable. The mobile robot is controlled by two drive motors and the position and heading angle can be computed by the information of the two incremental encoders. It is assumed that the mobile robot moves in two dimensional smooth indoor environment. Then, the position error and the heading angle error are expressed by (I), where 8X(k),8Y(k),8ljf(k),c5D(k) represent X and Y axis position error, heading angle error, and the wheel base error, respectively. S. (k) and SL(k) are the scale factor errors of the right and left encoder, respectively. Rand L are the right and left encoder increments. D is the distance between the wheels[K. Park et all The navigation error equations can be derived by subtracting the true value from the calculated value. In equation. (I), it is assumed that sin 81Jf(k) =81Jf(k) cos 8ljf(k)

=1 ,

81Jf(k)8S L(k)

D(k) » c5D(k) ,

8ljf(k)8S. (k)

=0 ,

5. (k + 1) = 5. (k)

SL(k+1)=SL(k)

(2)

5D(k + 1) = 5D(k)

The heading information can also be acquired by using the gyroscope. A gyroscope is independent of the encoder errors, even the non-systematic errors. However, a small error in angular velocity of the gyroscope can cause large heading angle error because the heading angle is obtained by integrating the angular velocity. The dominant gyroscope errors are the bias and the scale factor error. The gyro bias is some offset caused by manufacturing imperfections. The gyro bias has the deterministic and stochastic error characteristics. The deterministic errors can be compensated by some predetermined function. However, the stochastic errors need feasible modeling and filtering scheme to be eliminated. The gyroscope scale factor is the ratio of the measured gyroscope data to the angular rate. The ideal gyroscope scale factor is set to be a constant, but in the real world the gyroscope scale factor is not a constant it varies due to the temperature and operating environment. Typical random error models of the inertial sensors are the random bias. The random bias is described by the differential equation dx(t)/dt = 0 with an unknown initial value. The heading angle error equation of the gyroscope is developed in (3). The bias error and the scale factor error can be modeled as a random constant [K. Park et all. In equations (3) Q is the angular rate, &p is the gyroscope heading angle error, 813, is the gyroscope scale factor error, and 813,. is the gyroscope random bias error. &P(k + 1) = &P(k) + Q813, (k) + 813 •• (k) 813, (k+1)=813, (k)

(3)

813 •• (k + 1) = 813•• (k)

We proposed an indirect Kalman filter and the details are omitted for the simplicity[K. Park et al) . In the indirect Kalman filter, the filter estimates the position error, heading error, and sensor error, and the navigation system computes the position and heading. There is no need to model the mobile robot dynamics explicitly in the filter, since the filter is based on the navigation error propagation equations, which are

and

=0 .

220

represented in (l - 3). Once the errors are estimated by the designed Kalman filter, they are used to compensate the position and heading angle of the mobile robot. For the Kalman filter design, the system matrix x= Ax + IV is given by (4), where w is the process noise . The measurement equation Z = Hx + v is, Z =

vi -

~ = 8", - 81/! = 8X 8Y 8", SR

[0

0

0

0

0

- 1

0

0] SL 8D 8 I/! 8B 8B "

(5)

+v

Fig. I . Mobile robot navigation system block diagram If the system is not completely observable, we can not estimate some states even if the noise level is negligible. We can only reduce the error increasing rate. The observability sets a lower limit of the estimation error. If a time varying system can be approximated to a piece-wise time invariant system, the observability analysis can be performed as follows[l . Lee et aI, G. Menskin et all . Assume that the measurements are obtained as shown in Fig. 2. At each segment, the system matrix Fj and

where t/t is the calculated encoder heading angle, ~ is the calculated heading angle using the gyroscope and v is the measurement noise. The proposed navigation system is shown in Fig. 1. The Kalman filter estimates the errors in the mobile robot navigation, but they are fed back into the navigation system and each sensors to compensate the errors. The sample rate of the Kalman filter can be much lower than that of the direct filter. As the filter is out of the navigation system, the unfiltered position and heading angle are still available even if the filter fails . The encoder and the gyroscope errors can be compensated each other systematically by the Kalman filter. The accuracy of the navigation system is improved as well. The developed system expressed in (4) and (5) is a linear time varying system. The Kalman filter using (4) and (5) estimates the sensors errors, position error and heading angle error. Those errors are fed back into each sensors and the navigation system to compensate the errors (see Fig. 1). As the angle difference between the differential encoder and the gyroscope is used for the filter measurement, the designed navigation system can compensate the encoder error and the gyroscope error mutually through the proposed Kalman filter.

3.

the measurement matrix H j are assumed time invariant. In each segment, the system can be represented as (6) x(k + 1) = Fjx(k) , z J(k) = H Jx(k) where

x(k)

E

':R ' ,FJ E :R "" ,z/k) E :R m , H I

E

:R ",., and

j=I ,2,oo., r.

2 Z, (1 )

2n·1

n-1

z, (2)

2n

2n+1

z, ( 11 - 1) z,(n) z, ( n )

z,( n+ 1) .. .. z , (2n -1 ) z,( 2n ) z, (2 n ) z ,( 2n + 1)

Fig. 2. Measurement sequences in the piece-wise time invariant system. The measurement is a function of xCI) as shown in (7).

Observability analysis of the navigation system

The observability of a given system indicates that how much the navigation system can exclude the errors and estimate the states such as position and heading angle.

1

OX(k + 1) OY(k+ 1) 8'1'(k + 1) SR(k+1) SL(k+1) 8D(k + 1) 8I/J(k+1) 8B,(k + 1) 8B,, (k+1)

0

R+L cos 'I'(k)--

2

R+L - sin'l'(k)-2

0 0

1

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0

0

. k)LSIn'l'( 2 L cos 'I'(k)2 R

sin'l'(k)~

0

0

0

0

R cos'I'(k)-

0

0

0

0

0

0

0

0 0 0 1 0 0

0 0 0

0 0 0 1 0 1

2

2

D 1 0 0 0 0 0

L

L-R

D

D'

0 1 0 0 0 0

221

0 0 1 0 0 0

n 1 0

OX(k) OY(k) 8'1'(k) SR(k) Seek) +w 8D(k) 8I/!(k) 8B, (k) 8B,, (k)

(4)

unobservable states, such as X, Y position error and heading angle error. Since those states are not observable, the designed Kalman filter does not completely eliminate the X, Y position and heading angle error of the mobile robot. On the contrary, other states such as the left and right encoder scale factor error, the wheel base error, the gyro bias, and the gyro scale factor error are observable. Thus, those errors can be reduced, and they can be estimated precisely by filtering . However, the filter only reduces the growth rate of the position and heading angle error.

H, H ,F,

z, (1) z , (2)

r

H, ~,f;:~I----------HF ":

z, (n) = Q(r)x(1) = H z, (n) __ z, (n+ 1)

zJ)

r

~"-,

2_~_

F"-' , _ __________ _

x(1)

(7)

-~:-J----------":

4.

F"-' F"-' ... F1,,-, ,-1 r-2

The mobile robot used in the navigation experiment is Seoul National University Mobile Robot. Experiments have been performed in a typical indoor environment. The mobile robot travels 490m for 20 minutes in the experiment. The average velocity is about 0.4 m1sec. In experiment, the proposed navigation result and the encoder only navigation result are compared.

H,F; -'

The matrix of each segment j is defined as

Q ~ = [H ~ I(H,F,)'

I··· I(H,F/'-' r]

Experimental Results

(8)

(9)

Positicn

the total observability matrix(TOM) of the given discrete system. By the definition of observability, the system is completely observable if and only if Q(r) has full rank[G . Menskin) . As mentioned in Section 2, the developed mobile robot navigation system is time varying. Thus, the navigation system is approximated as a piece-wise time invariant system, and the rank of the TOM is investigated. The rank of the total observability matrix is six ; i.e. at least the three states of the navigation system is unobservable. Since the null space of the TOM contains the un observable state information, the orthonormal basis of the null space of the TOM is followed by the matrix (equation (10)); The third column of the basis indicates that the X axis position error (the first state of the system is the X axis error) is not observable, since the first element in the third column is not zero. From the first and the second column, we can recognize that the combination of the Y axis position error (the second state of the system), heading angle error from the encoder (the third state of the system), and the heading angle error from the gyroscope (the seventh state of the system) are not observable. More detailed explanation is referred in the Appendix. Q(r)

IS

o

o

1

.fi .fi

0

1

1 2

o o o

1 2

o o

1

20

o -10

o

-20

Fig. 2. Trajectories in Experiment

Fig. 3. Position and heading angle error variances of the navigation system

I

1 2

o o o o o o 1 2

20

tvETER

1500

:&

_ &.-~."o~ ,,~

(l0) °0

o

o o o o o

1000

300

600

iOO

I

"""

1500

Fig. 4. Sensor error variance of the navigation system

As a consequence, The dead reckoning navigation system using the encoder and the gyroscope has

222

The solid line is the result of our proposed navigation algorithm, and the dotted line is the result of the encoder only navigation. In the experiment (Fig. 2), the mobile robot starts from (0, 0, 0°) which is marked by '0 ' and comes back to the start position after 20 minutes. The final position from the proposed navigation algorithm is marked by '*' , and the encoder only navigation is marked by 'x'. The final position of the proposed navigation algorithm is (0.1202m , 0.869Im, _2.6°) and the encoder only navigation is (l .0480m, 11.l423m, -8l.l 0). The encoder only navigation system results in a wrong position and heading angle after 20 minute run . The experimental result shows that the proposed navigation system estimates the position and heading angle more precisely than the encoder only navigation system does. The accumulated heading angle error produces the large position error in the encoder only navigation system. However, the proposed navigation algorithm successfully combines the encoder and gyroscope information to estimate the position and heading angle of the mobile robot. The state variances of the Kalman filter are shown in Fig. 3 and Fig. 4. The position and heading error variances have increasing characteristics. However, the error variance of other states of the system are converged as discussed in section 3.

5.

divergent characteristics of the position and heading angle in the dead reckoning navigation system. The observability analysis of the dead reckoning navigation indicates that the designed system can provide position and heading angle during a certain period of time. However, the navigation error should be compensated by other system such as an absolute positioning system to prolong the navigation time and distance. The experimental result shows that the proposed mobile robot navigation system can give reliable position and heading angle without any help of other external positioning aids for certain period of time.

Appendix The observability analysis of a dynamics system indicates the efficiency of a Kalman filter. Thus, it is necessary to check the observability of the designed Kalman filter. If the given system is completely observable, the filter estimates all states precisely, and the variances of the estimated states converge. However, if the system is not completely observable, the unobservable system states are to be identified for the filter performance analysis. In our problem, the unobservable states are at least three since the rank of the TOM is six. The null space basis constructed in Section 3 indicates the un observable states. Thus, the unobservable states are the X position error, the combination of the Y position error, and the heading angle error from the encoder and the gyroscope. To investigate the unobservable state more exactly, the state of the system is changed. Let 88(k) '" 8,!,(k) - ~(k) , then the (4) and (5) can be expressed as (A I). In equation (A I) w is the process noise and the measurement equation can be expressed as,

Conclusion

A dead reckoning navigation system using a differential encoder and a gyroscope have developed. As the accurate dead reckoning navigation requires the careful compensation of encoder and gyroscope measurements, the systematic errors of the encoder and the stochastic errors of the gyroscope have been estimated explicitly using the Kalman filter. By augmenting the encoder errors and the gyroscope errors in the Kalman filter states, the Kalman filter is able to estimate and compensate both the sensor errors. The main advantage of our navigation system is that the proposed Kalman filter can compensate the encoder and the gyroscope mutually by combining the encoder and the gyroscope measurements. Also, the navigation system still produce the unfiltered position and heading angle of the mobile robot even if the filter fails. The observability analysis is performed to explain the 0 8X(k+ 1) 8Y(k+ 1) 8e(k+ 1) 5, (k+1) 5,(k+1) 8D(k+ 1) 88, (k + 1) 8B,,(k + 1)

0 0 0 0 0 0 0 0

0 0 0 0 0

R+L cos,!,(k)--

sin,!,(k).!:: 2 2 L . R+L -sm,!,(k)-- cos,!,(k)2 2 R 1 0 1 0 0 0 0 0 0 0 0 0

Z

= ,!,(k) -

~(k)

= 8,!,(k) -

~(k)

+ v = 88(k) + v 8X(k) 8Y(k) 88(k)

= [0

0 1 0 0 0 0

oJ

5, (k) + v 5,(k) lSD(k) 88, (k) 8B,,(k)

(A2)

The rank of the total observability matrix is also six and the orthonormal basis for the null space of the TOM is 1 0 0 0 0 0 0 O]T [o 1 000 000

The first and the second column of the basis indicate . k )R sm'!'(

2 R cos,!,(k)2 L

0

0

0

L-R

-Q

0 8X(k) 8Y(k) 88(k) 5,(k) +w 5,(k) 0 lSD(k) 0 88, (k) 0 88" (k)

0

1

0' 0 0

0 0 0

1

0 0 0

0 0

1

0

0

1

0 0

223

0

(AI)

that the X axis and Y axis position error (the first and the second state of the system) are not observable . As those errors are not observable, the X , Y position including those error of the mobile robot are not observable. It means that the error of the position of the mobile robot can not be reduced . The filter only reduces the growth rate of the those errors. This fact illustrates that the divergent characteristics of the position of the dead reckoning navigation system using the encoder, the gyroscope and the accelerometer. However, the difference between the encoder heading error and the gyroscope heading error is still observable. To examine heading error from the encoder and the gyroscope, the X and the Y position errors are excluded from the system. Thus, the (A I) is changed to (A3), o'l'(k+1) S,(k+1) S, (k+l) 8D(k+l) liqI(k + 1) OB. (k+l) OB.. (k+1)

0 0 0 0

R

L

O

0

1 0 0 0

~ ~

0 1 0 0

L-R , 00 0 1 0

~

~

0

0

0 0 0 1

0 0 0

n

References Borentein, J. and Feng, L. 1995, "Correction of Systematic Dead-reckoning Errors in Mobile Robots," 1995 International Conference on Intelligent Robots and Systems (IROS'95), Pittsburgh, PA, Aug. 5-9, pp.569-574. Barshan, B. and Durrant-Whyte, H. F., 1995, "Inertial Navigation Systems Mobile Robots," IEEE Transaction on Robotics and Automation, Vol. 11 , No. 3, June, pp. 328-342. Goshin-Menskin and I. Y. Bar-Itzhack, "Observability Analysis of Piece-Wise Constant Systems - Part I : Theory," IEEE Transaction on Aerospace and Electronics Systems, Vol. 28, NO. 4, October 1992, pp. 1056 - 1067. Jang Gyu Lee, Chan Gook Park and Heung Won Park, "Multiposition Alignment of Strapdown Inertial Navigation System," IEEE Transaction on Aerospace and Electronics Systems, Vol. 29, NO. 4, October 1993, pp. 1323 - 1328. Komoriya, K. and Oyama, E., 1994, "Position Estimation of a Mobile Robot Using Optical Fiber Gyroscope(OFG)," 1994 International Conference on Intelligent Robots and Systems( IROS '94). Munich, Germany, Sept. 12-16, pp. 143-149. KyuCheol Park, Hakyoung Chung and Jang Gyu Lee, 1997, "Dead Reckoning Navigation for an Autonomous Mobile Robot Using a Differential Encoder and a Gyroscope," 8th International Conference on Advanced Robotics, Monterey, California, USA, July 7-9. Siouris G. M. Aerospace Avionics Systems - A Modern Synthesis, Academic Press 1993.

0 o'l'(k) S,(k) 0 S, (k) 0 oD(k) +w (A3) 0 liqI(k) 1 OB(k)

~ ~ ~

OB,: (k)

where w is the process noise. The measurement equation Z = Hx + v is , Z = 'I'(k)-tfJ(k) = O'l'(k)-8t/J(k) + v

= [1

0 0 0 -1

8'1'(k) S, (k) S, (k) 0 0] 8D(k) + v 8t/J(k) OB. (k) OB,, (k)

(A4)

where v is the measurement noise. The rank of the total observability matrix is six and the orthonormal basis of the null space of the TOM is [

_1_

.fi

0 0 0 _1_

.fi

0

O]T

From the null space basis of the TOM, we can conclude that the combination of the heading error from the encoder and the gyroscope is not observable. The fact with the null space basis of the TOM from (AI) and (A2) indicates that the heading error of the mobile robot is not observable, but the difference between the encoder heading angle error and the gyroscope heading angle error is observable. The observability analysis results are validated by observing the variance computed by the Kalman filter. The variance of the states have convergent characteristic if the states are observable. On the contrary, the variance of the unobservable states are divergent. The variances of the X position error, Y position error, and the heading angle error are increasing as shown in Fig. 3 . Meanwhile, the encoder scale factor errors, the wheel base error, the gyro bias, and the gyro scale factor error converge. By the observability analysis, the increasing position error characteristics of the dead reckoning navigation system are explained.

224