Predictive control applied to 5DPO - RoboCup middle-size omnidirectional robots

Predictive Control Applied to 5DPO - RoboCup Middle-size Omnidirectional Robots André Scolari Conceição*, A. Paulo Moreira, Paulo J. Costa Department of Electrical and Computer Engineering University of Porto Porto - Portugal. [scolari,amoreira,paco]@fe.up.pt Abstract— This paper presents a nonlinear model based predictive controller (NMPC) for trajectory tracking of the Middle-size Omnidirectional Robots from the 5DPO Robotic Soccer team. The strategy proposed uses methods of numerical optimization to perform real time nonlinear minimization of the cost function. The cost function penalizes the robot position error, the robot orientation angle error and the control effort. Experimental results of the trajectories following and the performance of the methods of optimization are presented. I. INTRODUCTION Nowadays, the prediction concept in the mobile robotics is very important, because the robots are inserted more and more in dynamic environments(for example: robotic soccer, manufacturing plants, agriculture). Above all, in Robotic soccer’s application the robots need to execute trajectories quickly and with a perfect position to the objective, for example, positioning to the ball, or to the goal, or to avoid dynamic obstacles. Therefore, the capacity to predict the control actions to follow a path or to avoid obstacles is very useful. Several applications of predictive control techniques can be found in the literature. The article [1] talks about the way of implementing a model-based predictive controller for mobile robot navigation using genetic algorithms to perform online nonlinear optimization. The cost function was defined as a quadratic function of sum of the future errors in reference tracking. Reference [2] presents a tracking method for a mobile robot which combines predictive control and fuzzy logic control, where the predictive control is used to predict the position and the orientation of the robot, while the fuzzy control is used to deal with the non-linear characteristics of the system. A path tracking scheme for mobile robot based on neural predictive control is presented in [3], where a multi-layer back-propagation neural network is employed to model non-linear kinematics of the robot. In [4], a neural network multilayer perceptron has been trained to reproduce the MBPC behaviour in a supervised way. In [5], the minimization of the cost function has to be carried out by a numerical optimization method, a neural network is used to solve the problem. Another strategy *The author is supported by the Program Alβan, the European Union Program of High Level Scholarships for Latin America, scholarship n.E04D028256BR using neural network is used in [6], where a neural-networkbased technique for developing nonlinear dynamic models from empirical data for an model predictive control (MPC) algorithm is presented. A genetic algorithm based predictive control strategy which allows the minimization of a nonlinear cost function in real time is presented in [7]. In this paper a new approach of predictive controller is presented, where methods of numeric optimization are used to obtain the minimization of the controller’s cost function. This approach became possible when we obtained good times of minimization of the controller’s cost function (<30 milliseconds). Nowadays with potent processors in personal computers, the use of optimization algorithms became viable. The paper is organized as follows. Section II presents a brief description of the omni-directional mobile robot. Section III presents the NMPC elements: NMPC scheme, prediction model, cost function and control law. In this section the optimization methods are presented. The experimental results of the NMPC are presented in section IV. Finally, the conclusions are drawn in section V. II. ROBOT DESCRIPTION The robot, Fig. 3(a), is equipped with four omnidirectional wheels connected to geared motors. Connected to each wheel there is a industrial encoder to measure its speed. Each pair wheel-encoder is connected to a controller board. This board has a microcontroller that measures the wheel speed and implements a local controller. This controller maintains the requested speed and is based on PID. This low level loop has a sampling frequency of 100Hz. The four controllers are connected to the PC by a RS-232 link running at 115200 baud. The robot has a standard PC motherboard with a 2GHz Celeron processor. Nevertheless, there is no hard disk and the Linux OS and the programs are stored in a 256 MBytes Compact Flash Card connected to the IDE. Another very important module is the one that deals with the image captured by the onmidirectional vision system and extracts the most important features. This information is used to construct an estimation of the robot position. The vision camera also provides the sample time control of the robot (ts=40 milliseconds). The mobile robot was built for the 5DPO Robotic Soccer team from the Department of Electrical and Computer Engineering at the University of Porto at Porto, Portugal. ROBÓTICA 2007 - 7th Conference on Mobile Robots and Competitions Centro Paroquial de Paderne, Albufeira, Portugal April 27, 2007 III. MODEL PREDICTIVE CONTROLLER The strategy of the MPC controller is characterized by scheme represented in Fig. 1. A process model is used to predict the future outputs, based on past and current values and on the proposed future control actions. These actions are calculated by optimization of a cost function. Take into account the trajectory tracking problem, the cost function J of the predictive controller is defined as follow: J(N1, N2, Nu) = N2 X j=N1 λ1[ˆx(k+j|k)−xc(k+j)]2+ ˆy(k+j|k)−yc(k+j)]2+ N2 X j=N1 λ2[ˆ θ(k+j|k)−θc(k+j)]2+ Nu X j=1 λ3[∆U(k+j−1)]2(1) where the first term in Jpenalizes the position error, the second term penalizes the orientation angle error and the third penalizes the control effort(∆U). N1and N2ate the minimum and maximum prediction horizons and Nuis the control horizon. The coefficients λ1,λ2and λ3are penalty factors, which are usually chosen to be constant along the time. ˆ Y(k+j|k) = [ˆx(k+j|k) ˆy(k+j|k)ˆ θ(k+j|k)]T, is an jstep prediction of the robot position and orientation angle made at instant k, and W(k+j) = [xc(k+j)yc(k+ j)θc(k+j)]Tis desired robot position and orientation angle. ∆U(k+j−1) = [∆v(k+j−1) ∆vn(k+j−1) ∆w(k+ j−1)]Tis the control effort, where vand vn are the robot linear velocities and wis robot angular velocity, which are the control variables. A. Reference trajectory of the controller, W The predictive controller needs the desired positions and orientation angles of the robot for the next Nperiods of time, see Fig. 2. The trajectory Ris defined as points in the world frame(OXY ):R(i)=[xr(i)yr(i)θr(i)]T,i= 0,1, ..., M, where Mis the total number of the trajectory points. The initial position (xc, yc)kis located at the intersection between the desired trajectory and its perpendicular, traced from the robot position (x, y). The next Npoints are spaced equally on the trajectory Rby ∆S(meters), which is a design parameter. The desired orientation (θc) is the linear variation between the reference angular position θr(i)and θr(i+ 1) of the trajectory R: θc=θr(i)(1 −d) + θr(i+ 1)d, (2) where dis the projection from robot position(x, y)to line segment (xr, yr)iand (xr, yr)i+1, it is normalized to length of the line segment. B. Prediction model The use of the process model is determined by the necessity to predict the future positions and orientation angle of the robot at future instants, ˆ Y(k+j|k) = [ˆx(k+j|k) ˆy(k+ Fig. 2. Controller reference trajectory, W. (a) Mobile robot. (b) Geometric parameters and coordinate frames. Fig. 3. Omni-Directional robot j|k)ˆ θ(k+j|k)]T. The omni-directional mobile robot model is developed based on the dynamics, kinematics and DC motors of the robot. The World frame(OXY ), the robot’s body frame and the geometric parameters is shown in Fig. 3(b). The following symbols, in SI unit system, are used to modelling: •b(m)→distance between the point P and robot’s wheels •M(kg)→robot mass •r(m)→wheel radius •l→motor reduction •v, vn (m/s)→linear velocities of the robot •w(rad/s)→angular velocity of the robot •θ(rad)→orientation angle of the robot •J(kg.m2)→robot inertia moment •Bv, Bvn (N/(m/s)) →viscous friction related to vand vn •Bw(N/(rad/s)) →viscous friction related to w •Cv, Cvn (N)→coulomb friction related to vand vn •Cw(N.m)→coulomb friction related to w •Fv, Fvn (N)→traction forces of the robot •Γ (N.m)→rotation torque of the robot •v1, v2, v3, v4(m/s)→wheels linear velocities •f1, f2, f3, f4(N)→wheels traction forces •T1, T2, T3, T4(N.m)→wheels rotation torque 1) Robot Dynamics: By Newton’s law of motion and the robot’s body frame, in Fig. 3(b), we have Fv(t) = Mdv(t) dt +Bvv(t) + Cvsgn(v(t)) (3) Fvn(t) = Mdvn(t) dt +Bvnvn(t) + Cvnsgn(vn(t)) (4) Fig. 1. Scheme of the predictive controller. Γ(t) = Jdw(t) dt +Bww(t) + Cwsgn(w(t)) (5) where, sgn(α) =    1, α > 0, 0, α = 0, −1, α < 0. The relationships between the robot’s traction forces and the wheel’s traction forces are, Fv(t) = f4(t)−f2(t)(6) Fvn(t) = f1(t)−f3(t)(7) Γ(t)=(f1(t) + f2(t) + f3(t) + f4(t))b(8) The wheel’s traction force(f) and the wheel’s torque(T), for of each DC motor, is as follow: f(t) = T(t) r(9) T(t) = l.Kt.ia(t)(10) where Ktis motor torque constant and ia(t)is the armature current. The armature current is limited to save battery, 0≤ia(t)≤imax where imax is a design parameter. The dynamics of each DC motor can be described using the following equations, u(t) = La dia(t) dt +Raia(t) + Kvwm(t)(11) T(t) = Ktia(t)(12) where Lais the armature inductance, Rais the armature resistance, wm(t)is the rotor angular velocity in rad/sec, kvis the emf constant. In SI unit system, the values of Ktand Kvare identical, see [8]: Kt(N.m/A) = Kv(V olts/(rad/sec)). The armature voltage is u(t), with input signal constraints, −24 ≤u(t)≤24 for t≥0. 2) Robot Kinematics: By geometric parameters of the robot and the robot’s body frame, in Fig. 3(b), is possible to derive the motion equations, dx(t) dt =v(t)cos(θ(t)) −vn(t)sen(θ(t)) dy(t) dt =v(t)sen(θ(t)) + vn(t)cos(θ(t)) dθ(t) dt =w(t) (13) The relationships between wheel’s linear velocities (v1,v2, v3and v4) and robot velocities (v,vn and w) are, v1(t) = vn(t) + bw(t) v2(t) = −v(t) + bw(t) v3(t) = −vn(t) + bw(t) v4(t) = v(t) + bw(t) (14) where x(t)and y(t)is the localization of the point P, and θ(t)the orientation angle of the robot. C. Control law The robot model is nonlinear, so an analytical solution cannot be obtained and an iterative method of optimization in real time should be used. In order to obtain control values U(k+j|k)it is necessary to minimize the cost function J, in 1. Between many different methods of numerical optimization, in [9]-[14], methods with low computational time and simple implementation was chosen for test in the controller. Three optimization methods had been tested: •The method of steepest descent, also known as the gradient method (G); •Nonlinear Conjugate Gradient method - Fletcher Reeves(GCFR); •Nonlinear Conjugate Gradient method - Polak Robiere(GCPR). The method of steepest descent is the simplest example of a gradient based method for minimizing a function of several variables. It has a simple implementation, but with slow convergence. The conjugate gradient method is slightly more complicated than steepest descent, but it converges faster than steepest descent. This methods make good optimization progress because it is based on gradients. The algorithm 1 presents the method of steepest descent for minimizing f(x). The algorithm 2 presents the Fletcher Reeves and Polak Robiere conjugate gradient method for minimizing f(x). The methods Flecher Reeves and Polak Robiere differ only in the Algorithm 1 - Steepest descent 1. Given x0, set k= 0. 2. Compute dk=−∇f(xk), where ∇fdenotes the gradient of f. 3. If dk≤, stop. Else, find the new point xk+1 =xk+αdk, αis the size of the step in the direction of the travel; xkand xk+1 are the variables values in the kand k+ 1; is the stop criterion. 4. Increment k=k+ 1, go to step 2. choice of β, where βF R denotes the Flecher Reeves method and βP R denotes the Polak Robiere method. Algorithm 2 - Conjugate Gradient methods - Fletcher Reeves and Polak Robiere 1. Given x0, compute d0=−∇f(xd),k= 0; ∇f(x)is the vector of gradients of the cost function at point f(x); 2. Using dk, compute xk+1 =xk+αdk, where αis the step size that minimizes f(xk+αdk). 3. Compute ∇f(xk+1), if ∇f(xk+1)≤, stop. Else βF R k+1 =∇f(xk+1)T∇f(xk+1) ∇f(xk)T∇f(xk) or βP R k+1 =∇f(xk+1)T(∇f(xk+1)− ∇f(xk)) ∇f(xk)T∇f(xk), xkand xk+1 are the variables values in the kand k+ 1; is the stop criterion. 4. Compute new direction dk+1 =−∇f(xk+1) + βk+1dk. 5. Increment k=k+ 1, go to step 2. IV. EXPERIMENTAL RESULTS The proposed control strategy has been tested with the mobile robot following two trajectories, and it was repeated for the 3 algorithms of the optimization (G,GCFR and GCPR). The first trajectory, Fig. 4(a), is a movement of rotation and translation in the same time. The second trajectory has special features, as sudden change of direction and orientation to the robot, in order to test the controller in hard condition, see Fig. 4(b). (a) "Diagonal" trajectory. (b) "Pulse" trajectory. Fig. 4. Trajectories of test. Firstly, we made simulations with the controller using the robot’s model. The first objective of these simulations was to define a first calibration in the controller’s parameters, as prediction and control horizons(N1,N2and Nu), penalty factors (λ1,λ2and λ3) and stop criterion of the optimization algorithms(). Another objective was to verify the computational time to calculate the minimization of the cost function. Informations about the computational time are extremely important to define the controller’s parameters. The robot uses a sample time equal to 40 milliseconds, then it is necessary to project the controller to respect this restriction of time. In the experimental tests with the robot was necessary to insert a restriction in the time used by the optimizator of the predictive controller. It was defined as 20 the maximum number of iterations of the algorithms. That means that the minimization of the cost function will be truncated in the iteration 20, in case it has not found a minimum. A. Experimental test 1 For the first experimental test with the robot and after the simulations results, the controller parameters have been chosen as follows in table I. In this test, we used a prediction N1N2Nuλ1λ2λ3∆S[m] 0 10 1 2 1 1 1e−3 0.04 TABLE I PARAMETERS OF THE CONTROLLER,TEST 1. horizon of 10 samples (N=N2−N1= 10), because it was verified in the simulation results that the optimization algorithms get a good minimization of the cost function in these conditions. The table II and the figures 5 and 6, show the results of the trajectory’s following, for the three methods of optimization. For the two trajectories, the method of the conjugated gradients GC_PR is the fastest in the minimization of the cost function, as in the simulated tests, see the table II. This method (GC_PR) presents robot position errors slightly larger than the other methods, but this difference in the errors is not significant. It was verified that the time for an iteration of the cost function minimization is approximately 1 millisecond for all methods, due to the algorithms almost have the same complexity for implementation. Usually the algorithms possess a larger contribution for the solution in the first iterations and a small contribution in you finish, as shown in the last iterations in the Fig. 5(d) and 6(d), therefore the truncation in the algorithms did not harm in a preoccupying way the trajectory’s following. Table III shows the amount of truncations and the number of samples along the robot navigation. Diagonal trajectory Iteration numbers Optimization time Method Maximum Minimum mean mean[milisec] G 20 4 9.13 7.42 GC_FR 20 4 7.98 6.65 GC_PR 16 4 7.31 6.21 Pulso trajectory Iteration numbers Optimization time Method Maximum Minimum mean mean[milisec] G 20 4 13.15 11.19 GC_FR 20 4 10.21 8.57 GC_PR 20 4 8.75 7.25 TABLE II DIAGONAL AND PULSE TRAJECTORY -TEST 1. Diagonal trajectory Number of Total of Method truncation samples G 4 115 GC_FR 1 89 GC_PR 0 69 Pulse trajectory Number of Total of Method truncation samples G 48 213 GC_FR 10 196 GC_PR 1 184 TABLE III TRUNCATION -TEST 1. (a) Positions (x,y). (b) Angular positions θ. (c) Errors (x,y,θ). (d) Optimization in the sample time = 1.52 sec. Fig. 5. Diagonal trajectory - test 1. B. Experimental test 2 In the experimental test 2 the controller parameters in the table IV were used. In this test a larger horizon of the N1N2Nuλ1λ2λ3∆S[m] 0 12 1 2 1 1 1e−3 0.04 TABLE IV PARAMETERS OF THE CONTROLLER,TEST 2. prediction was used (N= 12), to verify the performance of the optimizator and the behavior of the robot in the following of the trajectories. Fig. 7 and Fig. 8 show the results of the trajectory’s following, for the three methods of optimization. Fig. 7(c) and Fig. 8(c) show the errors of the position (x, y) and posture (θ)of the robot. The linear(v,vn) and angular(w) velocities of the robot are shown in the figures 7(d) and 8(d). The table V shows information about the number of iteration of the optimizator. The method GC_PR was the fastest again, and any truncation did not happen, see the table VI. In the experimental test 2 the number of iterations reduced comparing with the test 1. With the increase of the prediction (a) Positions (x,y). (b) Angular positions θ. (c) Errors (x,y,θ). (d) Minimization - the only sample truncated with GC_PR algorithm. Fig. 6. Pulse trajectory - test 1. horizon (N= 12), it is probable that the errors in the trajectory following have an increase, mainly in trajectories with abrupt changes of direction, as the pulse trajectory. The behavior of the robot tends to predict the control actions, avoiding the points of abrupt changes, for example the position x= 1 and y= 0 of the pulse trajectory, see Fig. 8(a). Another important characteristic is that the quantity of iterations of the optimization algorithms increases with the decrease of the prediction horizon, consequently it increases the time of calculation of the signs of control of the robot in each sampling. A good prediction horizon range is larger than 8 and smaller than 12 (8≤N≤12), it is a good commitment between the number of iterations of the optimization algorithms and the behavior of the mobile robot. In both experimental tests, the conjugated gradient methods are faster than the steepest descent method. Diagonal trajectory Iteration numbers Optimization time Method Maximum Minimum mean mean[mseg] G 20 4 9.37 8.23 GC_FR 16 4 7.32 6.53 GC_PR 14 4 6.80 6.13 Pulse trajectory Iteration numbers Optimization time Method Maximum Minimum mean mean[mseg] G 20 4 12.01 10.76 GC_FR 20 4 8.45 7.63 GC_PR 14 4 6.93 6.27 TABLE V DIAGONAL AND PULSE TRAJECTORY -TEST 2. Diagonal trajectory Number of Total of Method truncation samples G 2 97 GC_FR 0 89 GC_PR 0 76 Pulse trajectory Number of Total of Method truncation samples G 16 189 GC_FR 1 182 GC_PR 0 180 TABLE VI TRUNCATION -TEST 2. (a) Positions (x,y). (b) Angular positions θ. (c) Errors (x,y,θ). (d) Robot velocities. Fig. 7. Diagonal trajectory - test 2. V. CONCLUSIONS A nonlinear model based predictive controller has been proposed for a mobile robot path tracking problem. A new approach of predictive controller is presented, where methods of numeric optimization are used to obtain the minimization of the cost function of the predictive controller. The structure of the controller allows a vast capacity of calibration. The optimization algorithms, mainly the methods based on conjugate gradients, present good times of minimization of the cost function, allowing its use in the predictive controller. Some experimental results have shown the good performance of the strategy of the control proposed. (a) Positions (x,y). (b) Angular positions θ. (c) Errors (x,y,θ). (d) Robot velocities. Fig. 8. Pulse trajectory - test 2. REFERENCES [1] D.R. Ramirez, D. Limon, J. Gomez-Ortega and E.F. Camacho, Nonlinear MBPC for mobile robot navigation using genetic algorithms, International Conference on Robotics & Automation, vol. 3, pp. 24522457, 1999. [2] X. Jiang, Y. Motai and Xingquan Zhu, Predictive fuzzy control for a mobile robot with nonholonomic constraints, 12th International Conference on Advanced Robotics, ICAR ’05, pp.58-63, 2005. [3] D. Gu and H. Hu, Neural predictive control for a car-like mobile robot, Robotics and Autonomous Systems Journal, vol.39(2), pp.73-86, 2002. [4] J. Gomez-Ortega and E.F. Camacho, Neural Network MBPC for Mobile Robots Path Tracking, Robotics and Computer Integrated Manufacturing Journal, vol. 11(4), pp.271-278, 1994. [5] E.F. Camacho and C. Bordons, Model Predictive Control, SpringerVerlag, 2004. [6] S. Piche, B. Sayyar-Rodsari, D. Johnson and M. Gerules, Nonlinear model predictive control using neural networks, IEEE Control Systems Magazine, vol. 20(3), pp.53-62, 2000. [7] J.E. Normey-Rico, J. Gomez-Ortega, I. Alcalá-Torrego and E.F. Camacho, Low time-consuming implementation of predictive path-tracking control for a "Synchro-drive" mobile robot, 5th International Workshop on Advanced Motion Control, pp.350-355, 1998. [8] Benjamin C. Kuo, Automatic Control Systems, John Wiley & Sons, Inc, 1995. [9] R. Fletcher, Practical methods of optimization, John Wiley & Sons, 1987. [10] R. Fletcher and C.M. Reeves, Function minimization by conjugate gradients, The Computer Journal,vol 7(2), pp.149-154, 1964. [11] L.M. Gra˜na Drummond and B.F. Svaiter, A steepest descent method for vector optimization, Journal of Computational and Applied Mathematics,vol 175(2), pp.395-414, 2005. [12] W.H. William and Z. Hongchao, A New Conjugate Gradient Method with Guaranteed Descent and an Efficient Line Search, SIAM J. on Optimization, vol.16(1), pp. 170-192, 2005. [13] W.H. William and Z. Hongchao, Algorithm 851: CG_DESCENT, a conjugate gradient method with guaranteed descent, ACM Transactions on Mathematical Software, vol.32(1), pp. 113-137, 2006. [14] J. Shewchuk, An introduction to the conjugate gradient method without the agonizing pain, Technical report, School of Computer Science, Carnegie Mellon University, 1994.