# INTRODUCTION utonomous mobile robots have various applications in the field of industry, military and security environment. The problem of autonomous motion planning and control of wheeled mobile robots have attracted lot of research interest in the field of robotics. Consequently engineers working on design of mobile robots have proposed various drive mechanisms to drive such robots. However the most common way to build a mobile robot is to use two-wheel drive with differential steering and a free balancing wheel (castor). Controlling the two motors independently make such robots to have good manoeuvring and work well in indoor environment. Mobile robots with such drive systems are a typical example of non-holonomic mechanisms due to the perfect rolling constraints on a wheel motion (no longitudinal or lateral slipping). An asymptotic stable controller using Backstepping method for posture tracking and its stability has been validated [1] [7]. An Adaptive Controller [2] to compensate errors can further improve its stability. A non-linear control design [6] using feedback linearization can provide better performance than conventional linearized controller. Many authors [2] [3] have proposed methods to reduce odometry error caused by kinematic imperfection. A different approach of localization using RFID technology [5] is efficient, fast and cheap. A neural network [4] based reactive navigation algorithm for mobile robot in unstructured environment while avoiding obstacles is found to be optimized in computation. In the present paper, kinematics model and localization using optical encoder coupled with the DC motor of a differential drive robot is presented. This model itself is used as a motion controller in a closed loop control scheme. In the absence of workspace obstacles, the basic motion tasks assigned to wheeled mobile robots may be reduced to moving between two robot postures and following a given trajectory. The paper is organized as follows: In section II, basic equations of Kinematics and Motion Model of the robot are reported. The Localization in indoor environment using optical encoder coupled with the motor is described in brief. In section III, Control law is proposed and its stability analysis is carried out based on Lyapunov theory. In section IV, PID speed control of motor is presented. Section V includes some simulation results. Section VI highlights implementation strategies of control and localization using an 8-bit ATmega 32 microcontroller while Section VI contains conclusion and future work. # II. KINEMATICS OF DIFFERENTIAL DRIVE ROBOT a) Motion Model Let Inertial Reference Frame is {X I , Y I } and Robot Frame is {X R , Y R }. The Robot position P [x y ?] is expressed in cartesian co-ordinate system of inertial frame. The relationship between Inertial and Robot frames is represented using basic transformation matrix as follows: ?? ?? ?= ??(??)?? ?? ? = ??(??). [???????? ]??(1) Where, The robot under consideration is a two wheeled differential drive robot, where each wheel is driven independently. Forward motion is achieved when both wheels are 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 mobile robot can turn on the spot by driving one wheel forward and second wheel in opposite direction at the same rate. Third wheel is a castor wheel needed for the stability of mobile robot. ??(??) = ? cos ?? sin ?? 0 ?sin ?? cos ?? 0 0 0 1 ? A Global Journal of Researches in Engineering Each individual wheel contributes to the robot's motion and at the same time, imposes constraints on robot motion. It is assumed that the wheels of the robot do not slide. It is expressed by Non Holonomic Constraint. ?? ?sin ?? ? ???cos ?? = 0 (2) Also the measure of the traveled distance travelled by each wheel is not sufficient to calculate the final position of the robot. One needs to know how this movement is executed as a function of time. This can be illustrated in Fig. 2. ?? 1?? = ?? 2?? ; ?? 1?? = ?? 2?? ?????? ?? 1 = ?? 2 ?? 1 ? ?? 2 ; ?? 1 ? ?? 2 Fig. 2 : Dependence of Robot position on its velocities as function of time velocities ?? and ?. First consider the contribution of each wheel's spinning speed to the translation speed at P in the direction of +X R . If one wheel spins (?? 1 = ???? ?? ) while the other wheel contributes nothing and is stationary (?? 2 = 0), Since P is halfway between the two wheels, it will move instantaneously with half the speed i.e. ?? ?? = (1/2)???? ?? . In a differential drive robot, these two contributions can simply be added to calculate the?? ?? . Consider, for example, a differential robot in which each wheel spins with equal speed but in opposite directions. The result is a stationary, spinning robot. As expected, ?? ?? will be zero in this case. The value of ?? ?? is even simpler to calculate. Neither wheel can contribute to sideways motion in the robot's reference frame, hence ?? ?? is always zero. Finally, we must compute the rotational component ?? of the robot. Once again the contributions of each wheel can be computed independently and just added. Consider the right wheel (we will call this wheel 1). Forward spin of this wheel results in counter clockwise rotation at point P. Recall that if wheel 1 spins alone, the robot pivots around wheel 2. The rotation velocity ?? 1 at P can be computed because the wheel is instantaneously moving along the arc of a circle of radius d. ?? 1 = ?? ?? ?? ?? . The same calculation applies to the left wheel, with the exception that forward spin results in clockwise-rotation at point P : ?? 2 = ? ?? ?? ?? ?? . Mapping between Robot velocities to wheel velocities is given as follows: ?? = ?? ?? = ?? 1 + ?? 2 = ??( ?? ?? +?? ?? 2 ) ;?? ?? = 0 ? = ?? 1 + ?? 2 = ??( ?? ?? ??? ?? ?? )(3) Where, r = radius of wheel and d = axial distance between wheels. # b) Kinematic Equations Kinematics is the most basic study of how mechanical systems behave. In mobile robotics, we need to understand the mechanical behaviour of the robot both in order to design appropriate mobile robots for desired tasks and to understand how to build control software. Consider a differential drive robot at some 5) is valid when ?? ? 0 and above set of equations will be employed in establishing feedback control law for robot manoeuvring as discussed later in section III. # c) Odometric Localization Localization is one of the most fundamental aspects of a mobile robot. All mobile robot system has to answer the fundamental question, which is "Where has to be obtained, so that the robot can easily move from source to destination. There are number of localization techniques with respect to mobile robot, however in the present work we have used dead reckoning method for localization. Dead reckoning method uses odometry to measure the movement of the robot. In the present work, we obtain the data from an incremental encoder (odometry), which is fitted along with a motor of the mobile robot. Incremental encoders measure the rotation of the wheels, which in turn, calculates robot position and orientation using integration approaches of kinematic model over [t k , t k+1 ]. Assuming Robot configuration q k [?? ?? ?? ?? ?? ?? ] and constant velocity inputs V k and w k are known at discrete time t k , then using Euler integration ?? ??+1 = ?? ?? + ?? ?? ?? ?? cos ?? ?? ?? ??+1 = ?? ?? + ?? ?? ?? ?? cos ?? ?? ?? ?? = ?? ?? + ?? ?? ?? ??(6) where, ?? ?? ?? ?? = ??? and ?? ?? ?? ?? = ??? ?? ?? = ?? ??+1 ? ?? ?? The reconstruction of the current robot configuration is based on the incremental encoder data (odometry). Let ??? ?? and ??? ?? be the no. of wheel rotations measured during the sampling time T s by the encoders. Linear and angular displacements of the robot is given as ??? = ?? 2 (?? ?? + ?? ?? ) , ??? = ?? ?? (?? ?? ? ?? ?? )(7) Where, r = radius of wheel and d = axial distance between wheels. For a differential-drive robot the position can be estimated starting from a known position by integrating the movement (summing the increment travel distances). The estimate of robot configuration at time t k is computed as: ? ?? ?? ?? ?? ?? ?? ? = ? ?? ?? ?1 ?? ???1 ?? ???1 ? +? cos ?? 0 sin ?? 0 0 1 ? ? ??? ??? ? ,(8) Robot localization using the above odometric prediction (commonly referred to as dead reckoning) is accurate enough in the absence of wheel slippage and backlash. III. # CONTROL LAW AND STABILITY The control algorithm must now be designed to drive the robot from its current configuration; say (?? ??, ?? ?? , ??) to the goal position (?? ð??"ð??", ?? ð??"ð??" , ??) . Here the objective is to find a control ?? = [?? ??] so that the robot's goal position is reached in finite interval of time. The proposed control law is state dependent i.e. [?? ??] = ð??"ð??"(??, ??, ??) which guarantees the state to be asymptotically driven to [0, 0, ?] without attaining ? = 0 in finite time. One of the most commonly used methods to study the asymptotic behaviour is based on the Lyapunov stability theory. Consider a simple positive definite quadratic form of Lyapunov function: ?? = ?? 1 + ?? 2 = 1 2 ?? 2 + 1 2 ?? 2(9) Where the parameters V 1 , V 2 represent one half of the squared weighted norms of the "distance error vector" ? and "orientation error vector" ? exhibited by the robot between its current position and goal position defined with respect to the Reference Inertial Frame. Its time derivative is given by: ?? ?= ?? 1 ?+ ?? 2 ?= ???? + ????? Using kinematics equation ( 5), ?? ?= ??(??? cos ??) + ??(??? + ?? ?? sin ??)(10) From the equation ( 10), the first term can be made non-positive by letting the linear velocity of the form: ?? = ?? ?? ?? cos ?? ?? ?? > 0 (11) then , ?? 1 ?= ?????? ?? ???????? 2 ??? = ??? ?? ?? 2 ?????? 2 ?? ? 0(12) This means that ?? 1 ? term is always nonincreasing in time and consequently, since it is asymptotically converges to non-negative finite limit. Similarly the second term ?? 2 ? can be made be non-positive by letting the angular velocity ?? take the form of: The result in (15) is a negative semi-definite form. By applying Barbalat's Lemma, it follows that ?? ? necessary converges to zero for increasing time; thus in turn implying convergence of the state vector [?, ?, ?] to [0, 0, ?]. Hence it can be concluded that control vectors expressed by ( 11) and ( 13 ?? = ?? ?? sin ?? cos ?? + ?? ?? ?? ?? ?? > 0( # LOW LEVEL PID SPEED CONTROL The speed of DC motor can be adjusted to a great extent so as to provide easy control and high performance. There are several conventional and numeric controller types intended for controlling the DC motor speed. Recently, many modern control methodologies such as nonlinear control [8], optimal control [9], variable structure control [10] and adaptive control [11] have been extensively proposed for DC motors. However, these approaches are either complex in theory or difficult to implement. PID controller algorithm involves three parameters denoted P, I and D interpreted in terms of time. P depends on present error, I on the accumulation of past errors, and D is a prediction of future errors. PID control with its three term functionality covering both transient and steady-states response, offers the simplest and yet most efficient solution for many real world control problems. In spite of the simple structure and robustness of this method, optimally tuning gains of PID controllers have been quite difficult. The electric equivalent circuit of the armature and the free-body diagram of the rotor are shown in the Fig. 5 below The PID control design criteria are (i) less settling time (<1 s) (ii) overshoot less than 5% (iii) steady state error less than 1%. The PID speed control design is incorporated into the system. The transfer function for a PID controller given is by: ??(??) = ?? ?? + ?? ?? ?? + ?? ?? ??(17) Where, K P , K I and K D are gains. PID is simulated in Matlab and its response is plotted. Further, gain values are tuned manually to obtain desired response. V. # SIMULATION RESULTS Simulation results of DC motor characteristics incorporating PID for unit step input at different gains are shown in figure 7: # IMPLEMENTATION The robot which we used for our experiments is fabricated in-house. It has a differential-drive mechanism consisting of high torque Dynaflux DC motors each of which is equipped with high resolution Jayashree-15S optical quadrature shaft encoder for precise position and speed feedback to controller. The robot uses two 12V DC batteries in series to power motors, while 5V for Microcontroller development board. The power electronics module used to drive the DC motor is Super Hercules 9V-24V, 15A Motor Driver from Nex-robotics. The algorithm of localization and control has been implemented on 8-bit Atmega32 Microcontroller. The output signal from one channel of encoder is fed to the rising edge enabled external interrupt pin. On interrupt, the status of other channel decides the direction and equivalent rotation counts of robot motion. These counts on accumulation at fixed sampling frequency (in our case 33 Hz) resolve robot position estimation using the expression for odometric localization explained in section II. Also these counts acts as feedback to PID speed control executing at 100 Hz frequency. The control algorithm runs at 33 Hz wherein distance error vector and orientation error vector between robot instantaneous position and goal (target) position are calculated and robot control vectors viz. Two approaches to reach the goal position have been implemented. In first one, robot is made to rotate until orientation error is eliminated and then translated to overcome distance error. While in other method, robot exhibits both linear and angular velocities to overcome distance and orientation error simultaneously. Using Kinematics motion models the linear and angular velocities of the robot are transformed to right and left wheel speeds and fed as reference speed to PID speed control. The upper limit of robot motion is set to 40 RPM. # CONCLUSION AND FUTURE WORK In this paper, Kinematics feedback path controller for a differential drive mobile robot has been presented. Both the approach of Go-to-Goal motion is implemented using 8-bit Atmega 32 microcontroller. The robot motion behaviour in real time is in line with the behaviour observed in simulation. Also the proposed control law is validated as per Lyapunov stability criterion. The control algorithm proposed in this paper will be extended for human following application, where set points (goal locations) are updated regularly through computer vision algorithm of detecting and tracking human. The present work will be extended to include obstacle avoidance in Go-to-Goal motion and dynamics consideration of robot for control which can enhance better stabilization. Further, Umbmark calibration needs to be performed to avoid localization error due to irregular wheel diameter and wheel axial alignation. Also Attitude compensation has to be incorporated in odometric localization whenever robot travels in irregular surface. VIII. 1![Fig. 1 : Representation of Robot on a Cartesian coordinate system](image-2.png "Fig. 1 :") ![arbitrary position P (q c ) [x c , y c , ? c ] which has a non-zero distance with the goal position R (q d) [x d , y d , ?] defined with respect to global inertial frame. Global Journal of Researches in Engineering ( ) Volume XIV Issue I Version I 2 Year 2014 © 2014 Global Journals Inc. (US) Kinematics, Localization and Control of Differential Drive Mobile Robot H The actual robot motion commands are the angular velocities ?? ?? and ?? ?? of the right and left wheel respectively, rather than robot driving and steering](image-3.png "") 3![Fig. 3 : Error vectors of robot's position and orientation The coordinate system of the robot is governed by combined action of both the linear velocity ?? and the angular velocity ?? . Using geometrical assumption the robot cartesian is given by: ???= ?? cos ?? ???= ?? sin ?? ?? ?= ?? (4) Where ?????????? and ?????????? are the components of ?? along its X and Y axes and x, y and orientation ? are measured with respect to the reference Inertial Frame. Same way position of the robot can also represented in terms of polar coordinates involving error distance ? > 0 as: ?? ?= ?????????(?? ? ??) = ??????????? ?? ?= ?? ???????? ?? ?? ?= ?? Now, ?? = ?? ? ?? be the angle measured between robot axes frame [X R , Y R ] and the distance vector frame ? also ?? = ?(?? ?? ? ?? ?? ) 2 + (?? ?? ? ?? ?? ) 2 as per distance formula between two coordinates system. Finally we conclude ???= ??????????? ???= ??? + ?????????? ??](image-4.png "Fig. 3 :") 2![?= ??(??? ?? sin ?? cos ?? ? ?? ?? ?? + ?? ?? ?? sin ?? cos ?? ?? ) = ??? ?? ?? 2 ? 0 (14) Finally leading to the following expression for the time derivative of the Lyapunov function ?? ?? ?= ?? 1 ?+ ?? 2 ?= ??? ?? ?? 2 ?????? 2 ?? -?? ?? ?? 2 ? 0 (15)](image-5.png "2") 4![Fig. 4 : Block diagram of proposed Kinematics based motion controller](image-6.png "Fig. 4 :") 56![Fig. 5 : Circuit Diagram of DC motor The dynamic equations and the open loop transfer function of the DC motors are: ??(???? + ??)??(??) = ????(??) (???? + ??)??(??) = ??(??) ? ??????(??) ??(??) = ????(??) ??(??)](image-7.png "Fig. 5 :Fig. 6 :") 7![Fig. 7 : Unit step response at a) K P =0.2, K I =39, K D =0.25; b) K P =5, K I =8, K D =1.5 Simulation results are performed to illustrate the effectiveness of the proposed control law. Here sets of five position coordinates [0, 0, 0], [5, 0, 90?], [5, 5, 180?], [0, 5, -90?] and [0, 0, 0] are considered to traverse a square path. Both the approaches of reaching the goal position are depicted below.](image-8.png "Fig. 7 :") 8![Fig. 8 : Posture tracking with simultaneous translation and rotation motion](image-9.png "Fig. 8 :") ![Journal of Researches in Engineering ( ) Volume XIV Issue I Version I Year 2014 linear velocity ?? and angular velocity ?? are obtained.](image-10.png "Global") 10![Fig. 10 : Experimental Differential Drive Robot VII.](image-11.png "Fig. 10 :") © 2014 Global Journals Inc. (US) Kinematics, Localization and Control of Differential Drive Mobile Robot © 2014 Global Journals Inc. (US) H ## ACKNOWLEDGEMENT The authors express their sincere gratitude to Prof. N R Shetty, Director and Dr. H C Nagaraj, Principal for their encouragement and support to undertake multidisciplinary research at NMIT. The authors thank the Mechanical Engineering team of NMIT led by Prof. Sunil Kumar H S for designing and fabricating the robot. The and Mr Srikanth A Rao, former Research Associates, Robotics Lab NMIT for their contribution in the work. ## Global * A Stable Target Tracking Control for Unicycle Robots Sung-OnLee Young-JoCho MyungHwang-Bo Bum-JaeYou San-RokOh proc. of IEEE Int. conf. on intelligent Robots and Systems 2000 * A model-reference adaptive motion controller for a differentialdrive mobile robot LFeng YKoren JBorenstein Proc. of IEEE International Conference on Robotics and Automation of IEEE International Conference on Robotics and Automation 1994 * Kinematic correction of a differential drive mobile robot and a design for velocity trajectory with acceleration constraints on motor controllers Jong-WooMoon Chong-KugPark FHarashima Proc. of IEEE International Conference on Intelligent Robots and Systems of IEEE International Conference on Intelligent Robots and Systems 1999 * Neural-based navigation of a differential-drive mobile robot MAl-Sagban RDhaouadi Proc. of IEEE International Conference on Control Automation Robotics & Vision p of IEEE International Conference on Control Automation Robotics & Vision p 2012 * An Efficient Localization Scheme for a Differential-Driving Mobile Robot Based on RFID System SoonshinHan HyungsooLim Jangmyung IEEE Transactions on Industrial Electronics 2007 * Feedback control design of differential-drive wheeled mobile robots ShoulingHe Proc. of IEEE International Conference on Advanced Robotics of IEEE International Conference on Advanced Robotics 2005 * Tracking Control of Differential-Drive Wheeled Mobile Robots Using a Backstepping-Like Feedback Linearization DongkyoungChwa IEEE Transactions on Systems, Man and Cybernetics 2010 * Identification and control of a DC motor using back propagation neural networks SWeerasooriya MAEl-Sharkawi IEEE Trans. on Energy Conversion 1991 * An Investigation into Modeling and Solution Strategies for Optimal Design and Control JAReyer PYPapalambros ASME Design Engineering Technical Conferences 2000 * Variable structure adaptive control for PM synchronous servo motor drive FJLin KKShyu YSLin IEEE Proc. on Elect. Power Application 1999 * Online identification and control of a DC motor using learning adaptation of neural networks ARubaai RKotaru IEEE Trans. On Industry Application 2000 * authors also wish to acknowledge Mr R Praveen Jain "Closed Loop Steering of unicycle-like Vehicles via H Lyapunov techniques MAicardi GCasalino ABicchi ABalestrino IEEE Robotics & Automation Magazine March 1995 * Introduction to Autonomous Mobile Robots RSiegwart INourbakhsh 2004 Cambridge MIT Press MA * Robotics Vision and Control, Fundamental Algorithms in MATLAB PeterCorke 2011 Springer