A MATHEMATICAL MODEL OF A LEGO DIFFERENTIAL DRIVE ROBOT. University of Glasgow o Department of Aerospace Engineering, University of Glasgow

Transcription

1 A MATHEMATICAL MODEL OF A LEGO DIFFERENTIAL DRIVE ROBOT Kevin J Worrall + and Euan W McGookin o + Department of Electronic and Electrical Engineering, University of Glasgow o Department of Aerospace Engineering, University of Glasgow {K.Worrall; E.McGookin}@elec.gla.ac.uk Abstract: This paper details the development of a model for a mobile robot constructed from Lego Mindstorms. Equations representing the dynamics and kinematics of the robot are derived. In addition, the motors and wheels are represented in the model. The development of a multi rate simulation is described. Data from an IMU attached to the robot enables validation of the model using analogue matching and integral least squares. The result of the validation is that the model is a suitable representation of the robot. A method of controlling the robot is developed, using PID controllers and a Line Of Sight Autopilot. Copyright 2002 USTRATH Keywords: Mobile robots, Nonlinear model, Simulation, Validation, PID 1. INTRODUCTION Within the field of control engineering mathematical models of systems are commonplace, (Frankin et al, 1991). Mathematical models allow testing of controllers and algorithms in controlled circumstances, (Murray-Smith, 1995). Many different systems are chosen to be modelled for reasons of expense or safety, (Murray-Smith, 1995). Mobile robots are complex electromechanical devices that can be very difficult to construct and control efficiently. The associated complexities of altering code or testing ancillary systems in situ can be time consuming and potentially dangerous. It is widely recognised that simulations of robots can be used to develop designs (e.g. controllers), (Nehmzow, 2003; Lune, et al., 2005; Michel, 2004), quickly and test prototypes repeatedly without the difficulty of practical installation. For a mathematical model to be used there has to be trust in it and knowledge that it has been validated. Many companies that produce robots provide simulation packages (MobileRobots, 2006; Michel, 2004), though well documented and extremely versatile packages, no validation evidence is present. There is also literature concerned with the development of mobile robots; Liu et al, (2003), and Williams et al, (2002), develop models for omnidirectional robots; Albagul and Wahyudi, (2004), Bisgaard et al, (2005), and Campion et al (1996) develop models for standard wheeled robots. However none of these papers present validation data with the exception of Williams et al (2002) which shows experimental evidence of the friction term they have derived. This paper presents the development of a model and of a multi rate simulator for a wheeled mobile robot. Validation of the model using experimental data is described with results. The mobile robot is

2 constructed using Lego Mindstorms (Lego, 2006). Lego Mindstorms is designed to allow children develop and program their own creations made from Lego by using the supplied sensors, motors and programmable brick, the RCX. Though initially developed for use in the home it has found its way into schools and universities, as a valuable teaching and research aid, within the fields of robotics, control and mechatronics, as it enables fast prototyping. The outline of the paper is as follows; Section 2 provides a description of the robot that this paper is based on. The model of the robot and the model of the wheels are also defined. This section concludes by providing a complete state space model. The need for a multi rate simulation is explained in Section 3. Section 4 explains and shows a subset of the results from the validation procedure and Section 5 describes the control of the robot. Section 6 concludes the paper. 2. MOBILE ROBOT The robot used is constructed from Lego and can be seen in Figure 1. Lego has been chosen as it enables quick construction of the mobile robot. The robot is a two wheeled differential drive robot, where each wheel is driven independently. Forward motion is produced by both wheels been driven at the same rate, turning right is achieved by driving the left wheel at a higher rate than the right wheel and vice versa for turning left. This type of robot can spin on the spot by driving one wheel forward and the second wheel in the opposite direction at the same rate. It should be noted that a ball wheel is attached at the front of the robot to supply balance. An Inertia Measurement Unit, IMU, is used to sense the surge and sway accelerations and the yaw rate. This data is read from the sensors by the RCX which logs the data. The data is then transferred via the Lego Mindstorms Infra red tower to a PC. Fig. 2. References frames associated with the mobile robot. This rest of this section defines the mathematical model of the mobile robot. The development is split into three distinct steps; Dynamics, Kinematics and the Wheel model. 2.1 Dynamics The full body fixed dynamics equation of any 3 degree of freedom system can be stated as Equation (1), (Fossen 1994), with the origin of the body fixed frame coinciding with the centre of gravity of the robot, Figure 2. M. ν& + C(ν).ν + D(ν). ν = τ (1) Where M is the mass and inertia matrix, C(ν) is the coriolis terms, D(ν) is the Damping Matrix, ν is the velocities vector and τ is the force and moment vector. Since this model only considers surge, sway and yaw motion, τ represents forces X and Y and torque N. Rigid Body Dynamics ( M. ν& + C(ν). ν ); These two terms combined provide the rigid body equations of motion (Fossen, 1994). The equations are shown in Equations (2)-(4) and the derivations can be found in (Fossen, 1994). m (u& vr ) = X (2) m (v& + ur) = Y (3) J r& = N (4) Where m is the mass of the robot (kg), u and v are velocities (ms -1 ) in the x and y directions, r is the rotational velocity (rad -1 ) about the z axis and J is the moment of inertia (kg m -2 ) of the robot. Damping (D); This term describes the forces and moments that are acting in the opposite direction from the direction of motion. The robot is a ground vehicle with damping forces caused by air drag and friction. Fig. 1. Lego differential drive robot. The IMU can be seen on the top of the robot. The batteries seen on the top are used to power the IMU. Air resistance acting upon the robot causes air drag. The equation for this is shown in Equation (5), (Hoerner, 1965). F = 0.5 C A ρ V (5) ar d 2

3 Where F ar is the Force generated by air drag, C d represents the Drag Coefficient, A is the surface area presented to current direction of travel, ρ is the density of the material the robot is travelling through, in this case air, and V is the velocity which must be the velocity acting in the opposite direction from F ar. The drag coefficient is a number based on the shape of the robot. Assuming the robot to be a box shape the drag coefficient is determined to be 0.89, (Hoerner, 1965). Friction; is not included in the dynamic section of the robot. Instead friction is included as part of the wheel equations, Section 2.3. Force and Moment Matrix; This term is made up of the following [X, Y, N] T. These terms are the input forces and moments to the model and are given by the following Equations (6)-(8). X = (F + F ) cos ( β) (6) left right Y = (F + F ) sin ( β) (7) left right N = (F left F right ) moment_arm (8) Where β is the slip angle, explained below, F left and F right are the forces generated by the left and right wheels and the moment_arm (0.007m) is the distance between the centre of the wheel and the line of action that the centre of gravity lies on. Slip angle; The slip angle in the model of mobile robots has been the topic of many publications with Shekhar (1997), Balakrishna and Ghosal (1995), Williams et al (2002) and Bisgaard et al (2005), some of the more major papers. This paper takes a more simplicity look at the problem of slip. Slip is calculated using Equation (9) and is the relationship between the forward velocity and the sway velocity. This has been found, though experimental evidence, to be an acceptable method of calculating slip. -1 v β = sin (9) 2 2 ( (u v ) Where v is the sway velocity and u is the surge velocity. When the denominator is zero, β is assumed to be zero. 2.2 Kinematics The kinematics are concerned with the geometric aspects of motion, with regards to linear and angular velocities of the robot relative to the earth fixed frame, (Fossen, 1994). In order to map the bodyfixed velocities to the inertially fixed earth frame, a transformation matrix is required. This is represented in Equation (10). x& p cosψ = y& p sinψ ψ& 0 sinψ cosψ 0 0 u 0 v 1 r (10) This can also be represented by Equation (11). 2.3 Motor and Wheel Model & η = R( η). ν (11) The robot model has two inputs: the force generated by each wheel. To calculate these forces the actuators, in this case two Lego DC motors, need to be modelled along with the tires that are being used. The motors are standard DC motors and as such the standard equations of DC motors can be used to represent them. Equation (12) represents the electromechanics of the motor, (Frankin et al, 1991). di ( R I) (Ke ω) + Va = dt L (12) Where R = 22Ω, L = 0.01H, K e = , I is the current (A), ω is the wheel angular velocity (rad s -1 ) and V a = input voltage (V). The mechanical equation for the motor contains the friction term for the robot. The robot runs on wheels and as a result the friction is classed as rolling friction, (Jellet, 1872). The coefficient for rolling friction is obtained from tables in various books Jellet (1872), and Young & Freedman (2000), are two examples. For the simulation the value chosen is that of rubber tires on a concrete surface, a value in the range of , (Young & Freedman, 2000). The friction equation is Equation (13). F f = m g σ (wheel _ r ω) (13) Where F f is the frictional force generated, m is the mass of robot, σ, represents the Friction Coefficient, g is gravity, wheel_r is the radius of the wheel (m) and ω is the wheel angular velocity (rad s -1 ). The final term, ( wheel_ r ω), has the effect of scaling the frictional term to suit the current wheel velocity. This term was included in the equation after experimental evidence indicated that a scaling factor was necessary. Jellet (1872) suggests that the friction coefficient is related to the current velocity. The mechanical equation for the motors is given in Equation (14). dω (Kt I) (bs ω) (sign( ω) Ff wheel _r) = (14) dt J Where dω/dt is the angular acceleration (rad s -2 ), K t = , b s = e -5, I is the current (A), ω is the wheel angular velocity (rad s -1 ), F f is the frictional force, wheel_r is the radius of the wheel (m) and J = e -4 kg m Forces Generated The equation used to calculate the driving force from a rotating wheel is Equation (15).

4 F = T / wheel _ r (15) Where F is the force generated (N), T is the torque of the wheel (Nm) and wheel_r the radius of the wheel (m). Since the wheel radius is known the torque needs to be calculated. Equation (16) is used to calculate the torque. T T = T (( stall max ) ω) (16) ω abs max Where T is the torque of the wheel (Nm), T max is the current maximum torque that can be generated, Equation (17), T stall is the stall torque of the motor, ω absmax is the absolute maximum angular velocity the motor can run at and ω is the current angular velocity. V T in max = ( ) T V stall (17) max Where V in = input voltage (V), V max is the maximum voltage that can be applied to the motor and T stall is the stall torque of the motor. 2.4 Full State Space Model The full state space model can be seen in Equation (18). ν& M(D + C) 0 τ( ξ) ν 0 x& = R( ) 0 0. η & = η η + 0.Va 0 0 F( ) B ' (18) ξ ξ ξ & Where M(D+C) represents the rigid body dynamics and the damping, F(ξ) represents the equations of the motors and wheels, B is the input distribution matrix for the motors, Equation (19), and Va is the input to the model, i.e. the voltages to the motors. 1 0 B ' = L (19) 0 1 L 3. MULTI-RATE SIMULATION Two mathematical models have been derived; the robot and the motors. The robot model inputs are generated by the motor model. The need for two models became apparent during initial testing of a model that contained both the motors and the robot, as the dynamics of the motors respond extremely quickly to an input as compared to that of the robot. If the motors and the robot were modelled separately they could have different time periods, hence the development of multi-rate simulation. The motor step size was chosen to be s. The robot step size is 0.003s as this is the smallest time period that the RCX can log the sensor data at. This allows a direct comparison of the data between the simulation results and the physical robot. Since the two models are running at different rates the efficiency of the simulation is increased as only a small part of the Fig. 3. Structure of multi rate simulator code is run per iteration of the simulation. The structure of the multi rate simulator can be seen in Figure (3). 4. VALIDATION Validation is required to prove that the model is a good representation of the physical robot, without accurate validation the model is of little use (Murray- Smith, 1995) and the results produced will have no value. Two validation methods are introduced. 4.1 Analogue Matching Analogue matching, also termed visual inspection, is an established method of model validation (Gray, 1992), where the output from the model is compared to experimental data graphically by superimposing the plots (Gray, 1992). As the model is altered the output is compared to that of the results from the physical robot and the data that best fits the experimental data indicates the model and parameters that represent the physical robot best. 4.2 Integral Least Squares A quantitative measure of the models accuracy is required. This will provide further evidence that the model is a suitable representation of the physical robot. The measure is achieved by using Equation (20). This method is based on a suggestion in Murray-Smith (1995). = 2 Qm ( error ) (20) Where error is the difference between the data from the robot and the data from the simulator. Each output can be quantitative measured in this way. 4.3 Manoeuvres A set of simple manoeuvres were carried out on the robot. The manoeuvres chosen are: Straight line for a set length of time with motors at half power (7s)

5 1m forward Spin on spot both motors half power (The rotational velocity at full power is out with the range of the sensor) Spin on spot one motor on, second motor off These manoeuvres provide the experimental data, through the IMU, required to validate the model. 4.4 Results A subset of the validation results from the analogue matching can be seen in Figures (4) and (5). The figures show the experimental data, the simulation data and the error between them. The value from the integral least square is also shown on the Figures. It can be seen from the results that the model is a good representation of the physical robot. However some discrepancies exist, this is because all the parameters require low values of ILS and the ILS values shown are the best case for these parameters taking into consideration all the other parameters. Fig. 5. r over 7 seconds with motors running on half power and in opposite directions. 5. CONTROL OF SIMULATED ROBOT To show that the model can be used as a base for the development of controllers a controller is developed. With the forward velocity and the heading controlled, the robot can head in any direction and at any velocity up to the maximum velocity of the robot. 5.1 Controller development PID; Two PID controllers have been developed, one to control the forward velocity of the robot and a second to control the heading of the robot. The combined output from the PID s is used to control the voltages to the motors. The general continuous equation for the PID controller is shown in Equation (21). t de(t) Y (t) = Kp.e(t) + Ki. e(t)dt + Kd. (21) dt Where e(t) is the current error (difference between desired output and actual output), Kp, Ki, Kd are constants and Y(t) is the output. o Line Of Sight Autopilot; A method of directing the robot is required and the Line of Sight, LOS, Autopilot was chosen, (M c Gookin et al, 2000). This method uses waypoints to direct the robot and when the robot reaches one waypoint the next waypoint is selected. To detect if the robot has reached a waypoint each waypoint has an acceptance radius, which has a value equal to half the length of the robot. When the robot is within the acceptance radius of a waypoint the next waypoint is selected and the new heading calculated. To calculate the heading for the next waypoint Equation (22), (M c Gookin et al, 2000), is used. Figure (6) shows the waypoint concept and Figure (7) shows the final controller structure. Fig. 4. u over 7 seconds with motors set at half power. y wp y 1 pos ψ = tan (22) x wp x pos Fig. 6. Waypoint Concept

6 Fig. 7. Controller structure Fig. 8. Simulation run of the robot. It can be seen that the simulated robot navigates the waypoints. 5.2 Simulation Results A simulation was run with the following parameters, u = 1ms -1, t=60s, starting point (0, 0) and waypoints at [(1, 1) (1,-1) (0,-0.5) (-1.3,-0.7) (0, 0.2) (-0.1, 1.1) (-0.3, 1.5) (1.5, -1)]. The results of the run can be seen in Figures (8). It can be seen in that the robot navigates the waypoints in the correct order. 6. CONCLUSION This paper has presented a mathematical model of a differential drive robot built from Lego Mindstorms. The dynamics and kinematics of the mathematical model are described. With a complete mathematical model and a multi-rate simulation validation has been carried out using analogue matching and integral least square. The results from the validation show the model is a suitable representation of the physical robot. Using two PID controllers and a LOS Autopilot the simulation shows that the robot can be controlled easily. The model provides a basis for the development of control methodologies and navigation heuristics that can be applied to a mobile robot of this type. REFERENCES Albagul, A. and Wahyudi, (2004), Dynamic Modelling and Adpative Traction Control for Mobile Robots, International Journal of Advanced Robotic Systems, 1-3, pp Balakrishna, R., and Ghosal, A., (1995), Modeling of Slip for Wheeled Mobile Robots, IEEE Trans Robotics and Automation, 11-1, pp Bisgaard, M., Vinther, D., Ostergaard, K., Bendtsen, Jan. and Izadi-Zamanabadi, R., (2005), Sensor Fusion and Model Verification for a Mobile Robot, Proc 16 th IASTED Intl Conf Modelling and Simulation, 1, pp Campion, G., Bastin, G., and D Andrea-Novel, B., (1996), Structural Properties and Classification of Kinematic and Dynamic Models of Wheeled Mobile Robots, In IEEE Trans Robotics and Automation, 12-1, pp47-62 Frankin, G.F., Powell, J.D. and Emami-Naeini, A., (1991). Feedback Control of Dynamic Systems, 2 nd Edition, Addison Wesley Fossen, T.I., (1994). Guidance and Control of Ocean Vehicles, Wiley & Sons Ltd Gray, G., (1992) Development and Validation of Nonlinear Models for Helicoptor Dynamics, PhD Thesis, Department of Electronic and Electrical Engineering, University of Glasgow Hoerner, S.F., (1965), Fluid-dynamic drag: practical information on aerodynamic drag and hydrodynamic resistance, Brick Town, New Jersey Jellet, J., (1872). Theory of Friction, M c Millian & Co M c Gookin, E., Murray-Smith, D.J., Li, Y. and Fossen, T.I., (2000). The Optimization of a Tanker Autopilot Control System Using Genetic Algorithms, Trans Institute of Measurement and Control, 22-2, pp MobileRobots, (2006), Website: mobilerobots.com/, 15/2/2006 Lego, (2006). Official Lego Mindstorms Web Page: 9/2/2006 Liu Y., Wu X., Zhu J.J. and Lew J., (2003). Omni-Directional Mobile Robot Controller Design by Trajectory Linearization, In Proceedings 2003 American Control Conference, 4, pp Lune, T., Spiess, K., and Röfer, T., (2005). SimRobot A General Physical Robot Simulator and its Application in RoboCup, In Robocup 2005: Robot Soccer World Cup IX, Lectures in Artificial Intelligence, Springer Michel, O., (2004), WebotsTM: Professional Mobile Robot Simulation, International Journal of Advanced Robotic Systems, 1-1, pp Murray-Smith, DJ., (1995). Continuous System Simulation, Chapman & Hall, Cambridge Shekhar, S., (1997), Wheel Rolling Constrints and Slip in Mobile Robots, In Proceddings IEEE Int Conf Robotics and Automation, 3, pp Nehmzow, U., (2003). Mobile Robotics: A Practical Introduction, 2nd Edition, Springer, London Williams, R.L., Carter, B., Gallina, P. and Rosati, G., (2002). Dynamic Model with Slip for Wheeled Omnidirectional Robots, IEEE Transactions on Robotics and Automation, 18-3, pp Young, H.D. and Freedman, R.A., (2000). University Physics, 10 th Edition, Addison Wesley