314 A MATHEMATICAL MODEL OF A LEGO DIFFERENTIAL DRIVE ROBOT Ph.D. Stud. Eng. Gheorghe GÎLCĂ, Faculty of Automation, Computers and Electronics, University of Craiova, gigi@robotics.ucv.ro Prof. Ph.D. Eng. BÎZDOACĂ Nicu George, Faculty of Automation, Computers and Electronics, University of Craiova, nicu@robotics.ucv.ro Abstract: This paper details the development of a model for a mobile robot constructed from Lego Mindstorms. The equations representing the dynamics and kinematics of the robot are derived. In addition, the motors and wheels are represented in the model. The mobile robot is programmed in graphical programming language NXT-G and can follow a black line without problems, even if the route to achieve is difficult. Keywords: Mobile robots, Nonlinear model, Mechanical equation, Driving force. 1. Introduction Mobile robots are complex electromechanical devices that can be very difficult to construct and control efficiently. The control problem of Wheeled Mobile Robots (WMRs) is a topic of great research interest and has been studied extensively during the past few years. A WMR is a typical nonholonomic system characterized by kinematic constraints that are not integrable, i.e., the constraints cannot be written as time derivatives of some functions of the generalized coordinates. In the literature, the research for control of WMRs has been centered on three basic problems: trajectory tracking, path following and point stabilization. The goal of trajectory tracking is to control the mobile robot in order to follow a reference trajectory. Yu Hao et al. propose in their paper a fuzzy method for controlling the tracking of a wheeled mobile robot. Also using Lagrange equations to express kinematic and dynamic robot model [1]. Edison Orlando Cobos et al. develop in their work a model of traction based on the simple friction Coulumb. The linear velocities of the wheel are used in calculating these traction forces [2]. In the paper [3], the authors project a robot model equipped with a camera. The robot can detect and track the object through real-time processing of images from the camera. A Kalman filter is used for object tracking accuracy. Eka Maulana et al. presents in their work the inverse kinematic model for a mobile robot with differential driving on two wheels. A mobile robot orientation correction is made with closed loop control using a sensor matrix applied to the follower line in front of the robot chassis [4]. Abhishek Jha and Manoj Kumar have proposed in their work a method based on the odometry of two wheels in differential mode. Their method can estimate the estimated relative position of the mobile robot wheels in relation to the start position [5]. For the kinematic model, the authors use the Taylor series of second order. In the work [6] they present two algorithms based on non iterative linearization with application in the trajectory tracking of mobile robots. These two algorithms, extended Rauch Tung Striebel (ERTS) and unscented Rauch Tung Striebel (URTS), compare the nonlinear model predictive control with the iterative linear quadratic regulator controller and then approximates the inference approaches.
Sheelu Trees Mathewl et al. develop a control system for an inverted pendulum robot with two wheels using the LEGO Mindstorm kit [7]. This robot has a rotating encoder and a gyroscope sensor, the angular velocity of the body and the wheel rotation angle measurement is available as an output. In [8] it is presented a model of robot which has the control proportional with a variable model reference to the second order derivatives, in which the movement of the robot is adapted online. A trajectory learning algorithm based on sonar is experienced on the drive differential LEGO NXT robot, which is equipped with position sensors [9]. 2. Model of the Wheeled Mobile Robot The robot used is constructed from Lego and can be seen in Fig. 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 being driven at the same rate, turning right is achieved by driving the left wheel at a higher rate than the right wheel and viceversa for turning left. Fig. 1: Lego differential drive robot. 2.1. Kinematics and dynamics The WMR shown in Fig.2 is a typical example of a nonholonomic mechanical system. It consists of two rear driving wheels mounted on the same axis and a passive front wheel. The motion and orientation are achieved by the torques provided by the independent actuators, e.g., DC motors of the rear wheels. OXY is the reference coordinate system; PX 'Y ' is the coordinate system fixed to the mobile robot; P is the middle of the rear axis; P c is the center of mass of the robot body; d is the distance between P and P c ; 2b is the distance between the two driving wheels, e.g., length of the rear axis; r is the radius of the wheel. The configuration of the mobile robot can be described by five generalized coordinates:, where (x, y) are the coordinates of P in OXY. θ is the heading angle of the mobile robot. and denote the angles of the right and left driving wheels, respectively. Under the assumption that the wheels do not slip, there exist three constraints [10]: (1) (2), (3) These constraints can be written in matrix form: 315
, where (4) Fig. 2: Wheeled Mobile Robot and coordinate systems The assembly kinematic energy of the various components of the mobile robot is given by: (5), where m c is the mass of robot platforms without the driving wheels and the rotors of motors; m w is the mass of each driving wheel plus the rotor of its motor; I c, I m, I w are the moment of inertia of the body about the vertical axis through P c, the driving wheel with motor rotor about the wheel diameter, and the driving wheel with motor rotor about the wheel axis, respectively. (x c, y c ) are the coordinates of P c in OXY ; (x rw, y rw ) and (x lw, y lw ) are coordinates of right and left driving wheels in OXY, respectively. We apply the Lagrange s equations to derive the dynamic equations of the mobile robot. The constraint forces added as input terms are responsible for not allowing the wheels to slip sideways. The constrained dynamics can be written as: (6), where is the Lagrange multiplier vector corresponds to the constraint forces; represents the externally applied forces including the torques provided by the independent actuators and the viscous friction. Expressing (5) in terms of the generalized coordinates and substituting the result into (6), we obtain the system equations: (7), where is the inertia matrix; is the matrix of velocity-dependent forces. is the input transformation matrix; represents the torque input of the right and the left motor; is the viscous friction torque vector. 316
2.2. Motor and Wheel Model The robot model has two inputs: the force generated by each wheel. To calculate these forces the actuators, in this case two Lego 71427 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 (8) represents the electromechanics of the motor [11]:, (8), where R = 22Ω, L = 0.01H, K e = 0.2367, 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 friction equation is Equation (9):, (9) 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, ( ), has the effect of scaling the frictional term to suit the current wheel velocity. The mechanical equation for the motors is given in Equation (10): Where dω/dt is the angular acceleration (rad s -2 ), K t =0.2367, b S = 2.1975e -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 =2.8302e -4 kg m -2. The equation used to calculate the driving force from a rotating wheel is Equation (11):, (11) 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 (12) is used to calculate the torque:, (12) Where T is the torque of the wheel (Nm), T max is the current maximum torque that can be generated, Equation (13), T stall is the stall torque of the motor, ω abs max is the absolute maximum angular velocity the motor can run at and ω is the current angular velocity., (13) 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. 3. Programming and Implementation We have implemented a differential mobile robot using Lego Mindstorm kit. Its structure was presented in Fig. 1 and comprises: an intelligent brick, two engines, a light sensor, connection cables and components for construction. Brick intelligence is the operational heart of the system. It executes user programs and controls communication with sensors, actuators, with PC or other NXT units. The two engines are intended to make the robot walk, DC motors are (10) 317
powered from 9V or 12V (short periods). Light sensor is to see variations of light for the mobile robot to be able to move on the desired trajectory. The role of interconnection cables is to connect actuators and sensors to central processing unit, these can support analog and digital interface. For programming we used the minstorm lego nxt 2.0 software. In Fig. 3 shows the scheme for programming of a differential mobile robot so it can follow a route of black line. The functioning of the scheme is as follows: Light sensor is active. It works as a breaker in such a way that: if the light variation is less than 40 it goes one way and the robot is driven by one engine plugged in port B while the second engine is stopped; if the variation is greater than 40 it will go the second way and the robot is driven by the second engine is plugged in port C, while the first motor is stopped. The process is repeated indefinitely due to the repetition loop. Fig. 3: Diagram for programming of the differential mobile robot In Fig. 4 shows the results of mobile robot programmed by us: Fig. 4: Followed trajectory by mobile robot 4. CONCLUSIONS The differential mobile robot proposed by us can smoothly track any given route. The Kinematic and dynamic mathematical model can be applied to any robot in this category. The modeling engine and wheels are important in driving differential. The robot programming is made in the language of graphical programming NXT-G, which is simple and effective. In future we want to implement this robot a video camera and a system for the recognition of human emotions. 318
5. REFERENCES [1] Yu H., Tang, G.-Y., Su H., Tian, C.-P., Zhang J., Trajectory tracking control of wheeled mobile robots via fuzzy approach, Control Conference (CCC), 2014 33rd Chinese, vol., no., pp.8444,8449, 28-30 July 2014. [2] Torres, E.O.C., Konduri, S., Pagilla, P.R., Study of wheel slip and traction forces in differential drive robots and slip avoidance control strategy, American Control Conference (ACC), 2014, vol., no., pp.3231,3236, 4-6 June 2014. [3] Sefat, M.S., Sarker, D.K., Shahjahan, M., Design and implementation of a vision based intelligent object follower robot, Strategic Technology (IFOST), 2014 9th International Forum on, vol., no., pp.425,428, 21-23 Oct. 2014. [4] Maulana, E., Muslim, M.A., Zainuri, A., Inverse kinematics of a two-wheeled differential drive an autonomous mobile robot, Electrical Power, Electronics, Communications, Controls and Informatics Seminar (EECCIS), 2014, vol., no., pp.93,98, 27-28 Aug. 2014. [5] Jha, A., Kumar, M., Two wheels differential type odometry for mobile robots, Reliability, Infocom Technologies and Optimization (ICRITO) (Trends and Future Directions), 2014 3rd International Conference on, vol., no., pp.1,5, 8-10 Oct. 2014. [6] Armesto, L., Girbes, V., Sala, A., Zima, M., Smidl, V., Duality-Based Nonlinear Quadratic Control: Application to Mobile Robot Trajectory-Following, Control Systems Technology, IEEE Transactions on, vol.pp, no.99, pp.1,1. [7] Mathew, S.T., Mija, S.J., Design of H2 controller for stabilization of two-wheeled inverted pendulum, Advanced Communication Control and Computing Technologies (ICACCCT), 2014 International Conference on, vol., no., pp.174,179, 8-10 May 2014. [8] Ramirez-Martinez, O.L., Martinez-Garcia, E.A., Mohan, R.E., Sheba, J.K., Mobile robot adaptive trajectory control: Non-linear path model inverse transformation for model reference, Control Automation Robotics & Vision (ICARCV), 2014 13th International Conference on, vol., no., pp.877,881, 10-12 Dec. 2014. [9] Zaheer, S., Jayaraju, M., Gulrez, T., A trajectory learner for sonar based LEGO NXT differential drive robot, Electrical Engineering Congress (ieecon), 2014 International, vol., no., pp.1,4, 19-21 March 2014. [10] Fukao, T., Nakagawa, H., Adachi, N., Adaptive tracking control of a nonholonomic mobile robot, Robotics and Automation, IEEE Transactions on, vol.16, no.5, pp.609,615, Oct 2000. [11] Frankin, G.F., Powell, J.D. and Emami-Naeini, A., Feedback Control of Dynamic Systems, 2nd Edition, Addison Wesley, 1991. 319