WANG Jing， JIANG Gang

School of Manufacturing Science and Engineering， Southwest University of Science and Technology， Mianyang 621010， China

TheMobileRobotTrajectoryTrackingControlBasedontheEKFandtheLyapunovFunction

WANG Jing*， JIANG Gang

SchoolofManufacturingScienceandEngineering，SouthwestUniversityofScienceandTechnology，Mianyang621010，China

Accordingtothefactthatthewheeledmobilerobotsareinfluencedbytheenvironmentalfactorinpractice,theinformationofreferencetrajectoryoftherobotwascorrectedbyusingtheextendedKalmanfilter(EKF)algorithmfusionodometryandultrasonicobservationdata.Basedontherobotdynamicmodel,aglobalasymptoticalstabletrackingcontrollerwasconstructedbyusingtheLyapunovdirectmethod,andtheglobalstabilityofthesystemwasprovedbyusingLyapunovstabilitytheorem.Thesimulationresultsofthispapershowedthatthetrackingcontroller,whichcombinedboththedatafilteringandLyapunovmethod,hasbetterefficiency.

wheeledmobilerobot，extendedkalmanfiltering，Lyapunovmethod，trajectorytracking

1.Introduction

With the Wheeled Mobile Robot (WMR) used more and more widely, the demand of robot automation degree is getting higher and higher. The control of robot movement includes navigation path, speed and acceleration. The movement control methods mainly have the PID control, variable structure control and adaptive control, the fuzzy control, etc. Variable structure control is applicable to all kinds of linear and nonlinear system and it also has good adaptability to the system of interference and perturbation. Literature[1] used Lyapunov function variable structure design trajectory tracking control law, but only its linear system in the control law has asymptotic stability. Literature[2] took the back of thought design the tracking controller which possesses global convergence property .But the sliding mode controller design structure is complex.

The motion process of robot must gather sensor data, provide reference trajectory for the robot. There will be noise in the acquisition process; therefore, it is especially important to choose an appropriate filtering method.

Aimed at above problems, this paper used the extended Kalman filter (EKF) algorithm for optimal estimation of the reference trajectory, at the same time, based on control Lyapunov function (CLF) designed sliding mode controller for speed tracking control rates and nonlinear switching function. This method not only had simple design, but also the system had global asymptotic stability. For the mobile robot, the simulation results showed that this method has good trajectory tracking effect and could meet WMR trajectory tracking requirements.

2.Mobile robot kinematic model

Two rounds of the mobile robot model (as shown in Fig .1), including two rounds about, they are the drive wheels. Among them:XOYfor the global coordinate system, the state of the WMR by two driving wheels axis midpointMin the coordinate system location and heading angle of the coordinate system to represent, order, speed instructions, WMR’s kinematics equations.

Fig.1 Robot pose error coordinates

(1)

R(pr-p)

(2)

Where,Ris the transfer matrix.(xe,ye) is the vector coordinates of the local coordinate system. The robot pose error of the differential equation[3]:

(3)

3.Extend Kalman Filtering

Robot in athletic process gets the reference trajectory information by the sensors. Because of the noise, the robot reference trajectory measurement leads to a windage in the measurement results. Additionally, because the sensor and robot movement system is nonlinear, so when the state variable is small, the nonlinear function which is used to describe system and measurement can be considered asymptotically linear.The Extend Kalman Filtering is actually a special state estimation device which through the linear to achieve the asymptotic optimal bayesian decision-making[4]. Ultrasonic data and calculated the fusion process schematic position are shown in Fig.2.

Fig.2 Data integration flow chart

The noise of the real-time will influence the robot state measurement, includingX，Yandθvariables. Therefore, there is a need to filter these three parameters, respectively. Here in theXaxis for example, which gives extended kalman specific algorithm[5]. Hypothesis of the nonlinear systems for random difference equations are as follows:

xk=f(xk-1,uk-1,wk-1)

zk=h(xk,vk)

(4)

Among them:xkis the system state vector,zkis system observation vector,his the measurement function of the system,wk-1andvkare system noise and measurement noise, respectively, anduk-1is control function. In practice, we don’t know every moment of noise, i.e., the value ofwk-1andvk. Assume they are all zero, the state vector and observation vector are as follows:

(5)

Among them,xkis relative to a time before the process of the posteriori estimation ofk. Specific EKF algorithm procedure is as follows:

A.Time update equations:

(6)

(7)

B.Status update equations:

(8)

(9)

(10)

HkAndVkwere Eq. (5)htox,vpartial derivative of Jacob matrix at timekvalue. This is measurement noise at timek. A key part of the algorithm is that the Kalman gain expression of the Jacobi matrix can correctly pass or “weighted” observations useful information[6-7]. After the above steps to push the calculation, the estimated value of theX-axis direction of the whole process, similarly you can count theYdirection and the angleθvalue, therefore, the algorithm can estimate the reference trajectory in real time.

4.Sliding mode controller design

(11)

Lyapunov function of structure is:

(12)

ObviouslyV≥0, whenpe=0,V=0; whenpe≠0,V>0. Plug Eq. (3) and (11) into equation (12), it comes that:

k2xe(vrcosθe-v+wye)+k2ye(vrsinθe-xew)+

k2k3xeyevr·sinθe+k2yevr·sinθe-k2wrxeye-

Therefore, the sliding mode control Eq. (11) about the control law of the system asymptotic converge to a stable equilibrium pointpe=0.

5.The simulation experiment and analysis

According to the EKF and Lyapunov function, mobile robot sports system is simulated. The simulation procedure is as follows: the first step, with extended kalman filtering method to estimate the experimental data of the track reference, the effect could be obtained as shown in Fig. 3 and Fig.4. The second step, in order to verify the error of the global convergence properties, experimental reference input were divided into two kinds i.e., circular arc and linear. Under the different initial conditions and reference inputs, the simulations were conducted. Fig.5～7 were the trajectory tracking of the experimental results of robot in the circular arc and the Fig. 8～10 were the trajectory tracking of the results of robot in linear.

More graphics showed that, according to estimates of the reference trajectory EKF, robot can be used. The proposed controllers in the initial system conditions change, in sliding mode controller will be under the action of convergence. Mobile robot with these two methods has better performance under the control of the tracking performance.

Fig.3 Robot straight-line movement pose estimation

Fig.4 Robot curve movement pose estimation

Fig.6 obot circular trajectory parameter v, w

Fig.7 Robot circular trajectory theory and the actual contrast result

Fig.9 Robot Straight-line trajectory parameter v, w

Fig.10 Robot Straight-line trajectory theory and the actual contrast result

ThesimulationmodelInitialerrorxe,ye,θe()Expectspeedvr,wr()Controlparametersk1,k2,k3()ThesimulationresultsCirculararc(1,-1,0.8)(0.8,0.4)(4,8,6)Fig.5～7Straightline(2,-2,0.6)(2,0)(2,7,5)Fig.8～10

6.The experimental conclusions

This paper in view of the actual wheeled robot autonomous movement problems, used EKF as the first of the robot reference to track the optimum estimate, Fig. 3～4, the state of motion in straight lines and curves to estimate the robot trajectory closer to the theoretical, the amount of data is not big enough, hoped to get a better effect in a large amount of data. Therefore, this method has better effect in the general linear and curvilinear motion prediction. Sliding mode controller to optimize the control parameters of the robot in a straight line and curve motion control, with strong robustness, strong anti-interference performance, and the control of the robot to meet the stable and fast requirements.

[1] WANG Yanqing,YE Yanhui,GAO Yanfeng .A stable tracking control method for an Autonomous Welding Mobile Robot[J].Applied Mechanics and Materials,2011(79):264-269.

[2] Divya Aneesh.Tracking Controller of Mobile Robot[C]//2012 International Conference on Computing,Electronics and Electrical Technologies[ICCEET] .Tamil Nadu,India:[s.n.],2012:343-349.

[3] WU Weiguo,CHEN Huitang,WANG Yuejuan.Global trajectory tracking control of mobile robots[J].ACTA AUTOMATICA SINICA,2001,27(3):326-331.

[4] Bolognani S,Tubiana L,Zigliotto M.Extended kalman filter tuning in sensorless PMSM drives[J].IEEE Trans.on Industry Applications (S0093-9994),2003,39(6):1741-1747.

[5] Greg Welch,Gary Bishop.An introduction to the Kalman filter [EB/OL].[2001-09-21] .http://info.acm.org/pubs/toc/CRnotice.html.

[6] WEI Guo,WANG Xin,SUN Jinwei.Method for ultrasonic time-of-flight estimation based on extended Kalman filter[J].Journal of Jilin University (Engineering and Technology Edition),2011,41(3):832-837.

[7] SHEN Wenbin,PEI Hailong.Extended Kalman Filter Based on Support Vector Regression[J].Computer Simulation,2011,28(4):156-159.

[8] Kazem Dastgerdi,Hadi Bidokhti,Assef Zare.Adaptive Sliding Mode Control of Nonlinear Gyro Chaotic Vibration[C]//2012 IEEE Students’ Conference on Electrical,Electronics and Computer Science.Bhopal:[s.n.],2012:1-4.

[9] PAN Yaodong.Variable structure control by switching among Feedback Control Laws[C]//45th IEEE Conference on Decision&Control.San Diego,CA:[s.n.],2006:789-794.

[10] Farzad P,Mattias P K.Adaptive control of dynamic mobile robots with nonholonomic constrains[J].Computers and Electrical Engineering,2002(28):241-253.

[11] XU Jianxin.A quasi-optimal sliding mode control scheme based on control Lyapunov function [J].Journal of the Franklin Institute,2012,349(4):1445-1458.

[12] Kamal S.Arbitrary higher order sliding mode control based on control Lyapunov approach[C]//2012 12th International Workshop on Variable Structure Systems (VSS).Mumbai,Maharashtra.[s.n.],2012:446-451.

[13] Ali Gholipour,Yazdanpanah M J.sliding mode Trajectory tracking control of dynamic nonholonomic systems with unknown dynamics[J].Int J of Robust and Nonlinear Control,1999,9(13):905-922.

[14] Nersesov S G.On the stability and control of nonlinear dynamical systems via vector Lyapunov functions[J].Automatic Control,2006,51(2):203-215.

[15] ZO Huichao,LEI Junwei,YU Hongyun.Extended Lyapunov Stability Theorem and Its Applications in Control System with Constrained Input[C]//International Symposium on Computer Network and Multimedia Technology (CNMT 2009).Yantai:[s.n.],2009:1-4.

基于EKF和Lyapunov函數(shù)的移動機(jī)器人軌跡跟蹤控制

王 靜*，蔣 剛

西南科技大學(xué) 制造科學(xué)與工程學(xué)院，四川 綿陽 621010

針對輪式移動機(jī)器人在實際運行中受環(huán)境因數(shù)影響的情況，采用擴(kuò)展卡爾曼濾波 (EKF) 算法融合里程計與超聲波的觀測數(shù)據(jù)，對機(jī)器人的參考軌跡信息進(jìn)行校正。在機(jī)器人動力學(xué)模型的基礎(chǔ)上，運用Lyapunov直接法，構(gòu)造具有全局漸近穩(wěn)定的跟蹤控制器，對機(jī)器人進(jìn)行軌跡跟蹤。根據(jù)Lyapunov穩(wěn)定性定理證明了系統(tǒng)的全局穩(wěn)定性。仿真結(jié)果表明，數(shù)據(jù)濾波與Lyapunov方法結(jié)合的跟蹤控制器效果良好。

輪式移動機(jī)器人；擴(kuò)展卡爾曼濾波；Lyapunov方法；軌跡跟蹤

TP242

2013-01-10

Postgraduate Innovation Fund sponsored by Southwest University of Science and Technology (12ycjj37);National Natural Science Foundation of China and China Academy of Engineering Physics Mutual Funds Under Grant (NSAF:11176027)*WANG Jing.E-mail：wangjing7840@foxmail.com

10.3969/j.issn.1001-3881.2013.06.013