Application of Kalman filter on mobile robot self-localization

HUANG Liang-song,GUO Xiao-li,LI Yu-xia

(College of Electrical Engineering and Automation,Shandong University of Science and Technology,Qingdao 266590,China)

0 Introduction

In recent years,robots are used to perform various complicated and hazardous tasks,especially in the fields of military,security and service. If mobile robots want to obtain a fast and collision-free access to the target location,it is necessary to know where it is. Traditional localization methods generally use electromagnetic navigation,sonar or laser range finder,GPS and other methods to obtain location information of the robot[1,2]. These methods are widely used,and can get satisfactory accuracy and reliability in the structured environment. With the in-depth research on the problem of robot localization,the uncertainty of the localization process is paid to more and more attention. when configuration environment map comes to match with environment model that is built based on sensor information,it may appear that an object matches with a number of objects due to the sensor error and uncertainty factors. In this case,generally we adopt the method based on probability to eliminate the ambiguity of matching. Among the methods based on probability,Markov localization[3],Kalman filter and Monte Carlo[4]are very representative and have been successfully applied.

Kalman filter is commonly used in the dynamic estimation. It is easy to achieve real-time optimal recursive filtering algorithm on the computer,suitable to handle multivariable systems,time-varying systems and non-stationary random process. It supports forecast and correction of the states in the past,present and future,particularly it is suitable for the situation that could not accurately model. And it has the advantages of a high prediction accuracy,a small amount of data storage and less computation time. Kalman filter has a wide range of applications in motion analysis. In this paper,we research the kalman filter method applied on the robot self-localization.

1 Kalman filter

Kalman filter[5]was first proposed by Kalman (R E Kalman) in 1960,and the filtering algorithm uses the signal extraction observations to estimate the desired signal. The concept of state space is introduced into state estimation theory. Kalman filter uses signal process as output of a linear system with white noise,uses the state equation to describe the relationship of input-output,and uses the system observation equation and the statistical characteristics of white noise as filter algorithm in the process of estimation.

Kalman filter is an optimal linear recursive estimation method[6],and uses linear state equation and observation equation to get a global optimal state estimation regardness of whether the data is accurate.

2 Basic theory of Kalman filter

The linear discrete time dynamic system is described by state equation and measurement equation[7],and they are as follows:

Xk=FkXk-1+Gkwk,

(1)

Zk=HkXk-1+vk,

(2)

whereXkis the system state vector;Zkis the system observed series;wkis the system process noise series;vkis the system measurement noise series;Fkis the system state transfer matrix;Hkis the system observation matrix;Gkis the system noise input function with covarianceQk[8]; and assuming that the random variableswkandvkrepresent the process and measurement noise respectively. They are assumed to be independent of each other. They are defined as

wk～N(0,Qk),

(3)

vk～N(0,Rk),

(4)

whereQkis the process noise covariance;Rkis measurement noise covariance.

(5)

(6)

(7)

(8)

(9)

We can get two loops of Kalman filter from the steps,which are plus loop and filter loop. Meanwhile,after each time and measurement update pair,the process is repeated with the previous posteriori estimates to project(or predict) the new prioriestimates. The recursive nature is one of the very appealing features of Kalman filter,so the method is convenient for real-time processing,computer realization and time saving.

3 Model of mobile robot self-localiza-tion

Commonly,the classical Kalman filter used in robot localization requires a linear motion model. We assume that the mobile robot goes along a line with variable motion. Displacementsk,velocityvk,accelerationak,jerkjkat the timetk， their initial values are:s0=1,v0=0.5,a0=0.2. To measure the position of the mobile robot，the measured value isZk=sk+vk. Noise interference is zero-mean white noise.

The sampling period isT,andT=0.1. Loop equation is

vk=vk-1+ak-1T,

ak=ak-1+jk-1T.

(10)

For motion tracking,the jerkjkis a random quantity,so we can use white noise to describe it.

AndXk=[skvkak]T.

State equations are

Xk=FXk-1+Gjk-1,

(11)

(12)

Measurement equation is

(13)

SoH=[1 0 0].

The initial values are estimated as

(14)

4 Simulation

Kalman filter is simulated by MATLAB. Line of true position represents the ideal curve of the position of the moving target varied with time in Fig.1. And line of measurement position represents the actual curve,and line of KF position represents the predicted values. Based on the predictive values of the target position we can get the current position by Kalman filter. The simulation results of the homing bomb show that the position got by Kalman filter is very closed to the actual position,and errors decrease with time prolonged.

Fig.1 Figure of target position estimation varied with time

5 Conclusion

In this paper,we discuss that Kalman filter is used to estimate states and the initial values in the case of noise linear motion model. Compared with other positioning methods,Kalman filter has strong robusticity,which is useful for decision-making and cooperation of Robot. Moreover，the key advantage of Kalman filter algorithm is its effectiveness,and the recursive characteristic of Kalman filter algorithm makes that its data processing needs not mass data storage and compute. So it has been widely used in the field of mobile robot self-localization.

[1] CHEN Rong-bao,ZHAO He,XIAO Ben-xiao. Self-localization of mobile robot based on monocular and extended kalman filter. Electronic Measurement & Instruments,ICEMI '09. 9th International Conference,2009: 450-454.

[2] Chen G,Chui C K. Kalman filtering: with real-time applications. New York: Springer-Verlag Berlin and Heidelberg GmbH & Co. K,2nd edition,1991.

[3] LI Tie-zhu. Study on the method of obstacle avoidance of robot navigation and localization. Tiaojin: Northeast Dianli University,2006.

[4] WANG Xiao-dong,CHEN Rong,LIU Jun-s. Monte carlo signal processing for wireless communications/ Journal of VLSI Signal Processing,2002,30: 89-105.

[5] Kalman R E. A new approach to linear filtering and prediction problems. Journal of Basic Engineering,1960,82(1): 35-45.

[6] Mortari D,Markley F L,Singla P. Optimal linear attiude estimator. Journal of Guidance,Control and Dynamics,2007,30(6): 1619-1627.

[7] ZHOU Hua. Application of multi-sensor fusion technology in the localization of mobile robot. Wuhan: Wuhan University of Technology,2009.

[8] QIN Yong-yuan,ZHANG Hong-yue,WANG Shu-hua. Kalman filter and the principle of integrated navigation. Xi’an:Northwestern Polytechnical University Press,2004.

[9] CAO Qi-xin,ZHANG Lei. Wheeled autonomous mobile robot. Shanghai: Shanghai Jiao Tong University Press,2012.

[10] FU Meng-yin,DENG Zhi-hong,ZHANG Zhi-wei. Application of Kalman filtering theory and its application in navigation system. Beijing: Science Press,2003.