A Novel Control-navigation System-based Adaptive Optimal Controller & Ekf Localization of Ddmr

—This paper presents a newly developed approach for Differential Drive Mobile Robot (DDMR). The main goal is to provide a high dynamic system response in the joint space level, the low level control, as well as to enhance the DDMR localization. The proposed approach depends on a Linear Quadratic Regulator (LQR) for the low level control and an Adaptive LQR for the high level control. The investigated DDMR is considered highly nonlinear system due to uncertainty exhibited by the mobile robot incorporated with actuators nonlinearity. DDMR's uncertainty leads to erroneous localization. An Extended Kalman Filter (EKF)-based approach with fusion sensors is used to enhance the robot degree of belief for its posture. Intensive simulation results obtained from the developed uncertain model and the proposed approach have shown very good dynamic performance on the low level control and very good convergence to the desired posture of the mobile robot path with the presence of robot uncertainty. I. INTRODUCTION The question of " where am I? " exhibited by mobile robots, in general, remains challenging and incompletely covered in academics. The topic of mobile robot localization has been paid wide attention by academics and industry to enhance the robot performance in different aspects for which the robot can perform its motion towards any desired posture with as minimum error as possible. Different techniques proposed to enhance trajectory tracking of wheeled mobile robots. [1, 2, 3, 4] utilize EKF algorithm with fusion sensors and gyroscope to localize the robot system as close as possible to the desired posture. The disadvantages of the work presented in [1, 2] do not pay attention to the dynamic performance of the system due to robot uncertainty but they considered only the noise affecting the proprioceptive and the exteroceptive sensors.


INTRODUCTION
The question of "where am I?" exhibited by mobile robots, in general, remains challenging and incompletely covered in academics.The topic of mobile robot localization has been paid wide attention by academics and industry to enhance the robot performance in different aspects for which the robot can perform its motion towards any desired posture with as minimum error as possible.Different techniques proposed to enhance trajectory tracking of wheeled mobile robots.[1,2,3,4] utilize EKF algorithm with fusion sensors and gyroscope to localize the robot system as close as possible to the desired posture.The disadvantages of the work presented in [1,2] do not pay attention to the dynamic performance of the system due to robot uncertainty but they considered only the noise affecting the proprioceptive and the exteroceptive sensors.
However, in [2,3] researchers use fusion sensors and vision sensor to help the system to be located correctly in a specific location.People in [3] implement EKF-based algorithm assisted with a gyroscope sensor to enhance the robot localization.Researchers in [3,4] consider the proprioceptive sensors provide exact information about the robot motion in which the robot"s posture is considered correct but noisy.Thus [3,4] use EKF to purify the information about the robot"s localization.[6,8] have proposed trajectory tracking algorithm but considered the information comes from the proprioceptive sensors is correct enough to determine the robot"s posture with no lack of accuracy.The uncertainty of the mobile robot due to inaccuracy in the mechanical robot design and due to joint space inaccuracy exhibited by the mobile robot actuators lead to accumulated drift and divergence from the desired robot"s posture.
This paper focuses on a novel control approach utilizing Linear Quadratic Regulator for joint space control of robot actuators as well as proposing EKF-assisted optimal controller to overcome the problem of robot uncertainty, which may lead to robot posture divergence.The proposed approach is supported by fusion sensors consisted of a gyroscope sensor (rate & accelerometer) which is fixed to the robot"s center of gravity and robot"s on board sensors (odometry sensors).The onboard sensors (proprioceptive sensors) assumed noisy, in addition to the robot"s mechanical parameters also considered highly uncertain.
It is presumed, as well, that the mobile robot is due to some random disturbance represented by τ d which is very common in the system control areas [3].The remaining sections of the paper are as follows: Section II provides details about the dynamic model of the mobile robot incorporating the actuators dynamics; section III explains the design of the proposed controller for the joint space control system i.e., low level control; section IV discusses mobile robot navigation and localization; section V exhibits intensive simulation results with different system uncertainty and section VI provides a conclusion for the presented work.

A. Mobile Robot Motion Description:
The HBE-RoboCar wheeled mobile Robot (WMR) has four driving wheels, which determine the moving direction of the robot through the rotating direction and speed of wheels.This WMR uses 4-DC geared motors for operation, and each wheel has one motor mounted.The wheels in the same side (left or right) are operated together as shown in Fig. 1 to follow a certain robot trajectory.Such wheeled robots called Differential Drive Mobile robot (DDMR).In this section a mathematical description of DDMR moving on a planar surface is presented.www.ijarai.thesai.orgAs shown in Fig. 2, a x and a y coordinate denote the position of DDMR in the global frame.The angle θ between the moving direction of the DDMR and the positive direction of the x-axis of the global coordinate frame denotes the orientation.Symbols used in this section are listed in Table1:   The study of DDMR motion is divided into three parts including kinematics, dynamics and drivers [8,9].

B. Kinematic Model of the DDMR:
Kinematics is the most basic study of how mechanical systems behave in order to model, analyze and simulate any control system design.The motion of this type of wheeled robot is classified as non-holonomic, which means, the motion constraint equations are needed to introduce into the DDMR"s motion equations based on two main assumptions [9]:  The robot can"t move sideward.These non-holonomic constraints are taken into account by defining the velocity of the center point A in the local frame and forcing it to be zero.This constraint introduces in the global frame related to the velocity of point A by the following equation (1): is the coordinate of point A in the global frame.
 To simplify the model, it is assumed that each wheel has one contact point with the ground and there is no slippage in its longitudinal axis and lateral axis.The velocities of the contact points in the local frame are related to the wheel velocities by (2): The linear and angular velocities of the DDMR related to the wheels velocities and the geometric parameters of the robot are given as follows (3): ) and the new orientation of DDMR given by (4): (4) Considering that the increment of distance and orientation is small then and the DDMR kinematic equation is given by (5): Equation ( 5) is called Navigation Equation also.The navigation system analysis and design are based on equation (5).Another form for the kinematic model describes the robot behavior relative to the linear and angular velocities of the DDMR which can be written as follows ( 6): The velocities of the center of mass C represented in the global inertial frame are given by ( 7): Equation ( 7) is the relation between the velocities in local and global frames named as Guidance matrix G .

C. Dynamic Model of the DDMR:
In this subsection, the dynamic behavior of DDMR mechanisms based on Newton-Euler approach is presented [9].
By considering the free body diagram of DDMR; Fig. 4 shows the forces acting on the DDMR [9].From Fig. 4 the DDMR position, velocity and acceleration represented in the global frame using polar coordinate system and the equations (8) are divided using Newton"s laws of motion in the robot frame: where is linearly dependent on the wheel control input as follows: Equations ( 8) represent the dynamic model of DDMR taking into account the non-holonomic constraints [9].

III. CONTROLLER SYSTEM DESIGN:
In this section, a controller design based on solving quadratic optimal control problem has been presented to improve the dynamic performance of the DDMR for accurate trajectory tracking.
Classical control methods are first designed and then their stability is examined.In optimal control, based on Liapunov approach the conditions for stability are formulated first and then the system is designed within these limitations.Thus, the designed system has a configuration with inherent stability characteristics [12].The proposed control scheme shown in Fig. 5 has two main controllers; high level control based on kinematic model of DDMR, that; correct the robot position and orientation to follow the commanded trajectory; and low level control based on dynamic model of DDMR which follows the velocities commands given by the high level controller.Both controllers employ quadratic optimal control approach.

A. Low Level Controller:
It is presumed in subsection (II, B) that, the linear and angular velocity of the DDMR related to the wheels velocities are given by (3) then the accelerations terms of the robot velocity and orientation angular speed are given by ( 9): By substituting (9) equations in the dynamic equation of DDMR (8): www.ijarai.thesai.org Where 2 1 ,  are the coupling terms between the left and the right wheels.An integral optimal control is proposed to control right and left wheels velocities by inserting an integrator in the feed forward path between the error comparator and the plant.Fig. 6 shows the block diagram of the DDMR with optimal low level control.Fig. 6.DDMR with optimal low level control.
The vector control op u which minimizes a selected cost function J given in ( 11) is determined as follow to solve the quadratic optimal control problem for the system given in ( 9): (11) Where: 1 4 x  : System state vector.The optimal control law is given as follows (12): P  is the state covariance matrix which can be obtained from Reccati equation which is given as follows (13): By designing the low level control system to have exponentially stable dynamic response with high dynamic performance response, the high level controller can be designed, considering linear dynamic response in the low level.

B. High Level Controller:
A proposed Adaptive Discrete Quadratic Optimal Controller formula based on DDMR kinematic system is described in this subsection.To derive the high level controller, the mathematical model of DDMR kinematic will be used.As seen the velocities of the center of mass C represented in the global frame are given by (7).
It should be noted that the kinematic model of this type of wheeled robot is classified as nonlinear and involves nonholonomic constraints.To apply the optimal controller, a linear model of DDMR kinematic needs to be obtained.But if the linearization was about a stationary operating point the system become not controllable (In case that 0   and the DDMR move straightforward the information about y axes is lost.A novel technique is developed and applied on DDMR) by Adapting the discrete quadratic optimal algorithm to include this type of nonlinear systems using linear error model system along a desired trajectory [7].
First, equation ( 7) is discretized using the backward Euler method with sampling time s T .The discretized form of DDMR kinematic model is obtained as shown in ( 14): ) , ( : where   is the system state vector and is the input vector.Second, defining the same equations for a desired trajectory generated by Guidance System as follows (15): The posture error vector k x e is given by ( 16): The velocities error vector k u e is given by (17): The linear error model is given by (18): are the Jacobian matrices of the system with respect to respectively which was derived around a desired trajectory.Now, considering the following discrete cost function: The closed loop system state space will be (21): www.ijarai.thesai.org 1 ) ( Where K is the optimal control gain vector.These errors in position and orientation of DDMR are represented in the global frame, then the errors in the local frame have been found as follows (22): The velocities commands which tracked by the low level control given by (23): IV. NAVIGATION SYSTEM AND LOCALIZATION: The robot navigation is the task of an autonomous robot to move safely from one location to another [13].To make a truly autonomous robot an accurate localization is a key problem for successful navigation systems [4,11].
The objective is to accurately determine DDMR"s posture involving sensor noise uncertainties and potential failures in an optimal way with respect to a global or local frame of reference by integrating (fusing) kinetic information received from proprioceptive sensors (odometry and gyroscope) which give the robot feedback about its driving actions and obtaining knowledge about DDMR"s environment [13].
In mobile robot navigation systems, onboard navigation sensors based on dead-reckoning are widely used.Dead reckoning is the process of calculating DDMR's current position by using a previously determined position and estimated speeds over the elapsed time [2,5].
In the present section, EKF sensor fusion method is used for the estimation of DDMR"s accurate posture and eliminates the effect of uncertainty associated with the system.
The Extended Kalman filter (EKF) is a recursive optimum stochastic state estimator which can be used for parameter estimation of a non-linear dynamic system in real time by using noisy monitored signals that are disturbed by random noise [14,15,16].
The EKF has been widely used for mobile robot navigation and system integration to address the nonlinearity in the system kinematic.
The goal of the EKF is to estimate the unmeasurable state (e.g.DDMR"s posture) by using measured states, and also statistics of the noise and measurement (i.e.covariance matrices P , R , Q of the system noise vector, measurement noise vector, and system noise vector respectively) [15,16,17].So the problem of mobile robot localization can usually not be sensed directly.The robot has to integrate data over time to determine its pose as accurate as possible using EKF fusion sensors method [4].
In order to apply the EKF algorithm [15], first, a discrete time Navigation Model for the DDMR based on equation ( 6) using the backward Euler method with sampling time s T has been obtained (24): Second, note that equation ( 24) is nonlinear.So, in order to apply EKF algorithm, a linear approximation of Navigation Equation using Taylor series have to be obtained as follows (25): The terms u x J , J are the Jacobian which are obtained by differentiating equation ( 24) with respect to the state vector and input vector respectively (26): The EKF algorithm contains basically two main stages [10], Fig. 7:

1) Action (or prediction) update:
Having a priori knowledge implies that (27) The covariance matrix after moving is given by (28): (28) During this step, the robot uncertainty grows.

2) Perception (or measurements) update:
The robot makes an observation using its uncertain model and corrects its position by opportunely combining its belief before the observation with the probability of making exactly www.ijarai.thesai.org that observation.The Kalman gain is chosen to minimize the estimation error variance of the states to be estimated.
The predicted state estimate k x ˆ (and also its covariance matrix k P ) is corrected recursively through a feedback correction scheme which is the product of the Kalman gain K and the deviation of the estimated measurement output vector and the actual output vector ( y ŷ  ) that makes use of the actual measured quantities.
During this step, the DDMR uncertainty shrinks.If one looks at the problem of probabilistically, one can say that the robot has a degree of belief (bel) about where it is [5].
The goal of localization is to make this belief get as close as possible to the real distribution of the robot location.The robot incorporates these measurements into its belief to form a new belief about where it is [5].
The determinant of k P provides a good measure of uncertainty as it is proportional to the volume of the deviation error ellipsoid.[5] The degree of belief is given by (31): The higher ) P det( k is the less degree of belief there is in the measurements.

V. SIMULATION RESULTS:
This section, several test results are demonstrated for the proposed control-navigation system based DDMR using MATLAB/ SIMULINK and C code.
The kinematics and dynamics model of the DDMR described in section II are used.The simulation is carried out by tracking different 3 DOFs desired paths with the high and low level control system of the DDMR.The proposed controlnavigation system is implemented based on the structures shown in Fig. 5.
The DDMR with optimal low level control has been tested.Fig. 8.a shows the time response of the right and left motor wheels linear velocities and their corresponding control law i.e., actuator voltage control respectively to a step input command simulated using MATLAB/ Simulink.As seen from Fig. 8(a, b) the dynamic system response for the right and left wheels is high, and they converge to the desired set value exponentially with no overshot in the system response i.e., the equivalent damping ratio of the system is ζ=1.It is noted that the low level dynamic system is exponentially stable and shows high dynamic performance response with the limitation of Two experiments were performed and compared for DDMR with adaptive optimal high level control, with several trajectories to examine the robot dynamic performance, one was implemented utilizing only the onboard sensors data, and the other used EKF assisted fusion sensors method as navigation system considering different system uncertainty.

A. Square Trajectory Tracking:
This section demonstrates the mobile robot control performance to a square path with a side length of 1 meter.Fig. 9 shows the system dynamic response for the high-level control.As seen in Fig 9 that the system exhibited advancement trajectory tracking with a high degree of localization belief has explained by Fig. 10.
As observed in Fig. 10 that the mobile robot system control in the high-level is very certain in its location i.e., where I am, since the degree of belief is high which is also very convincing indicator since the trajectory tracking error is negligible.

B. Eight Shape Trajectory Tracking:
This section provides two case simulation results for 8shape.These are simulation without system uncertainty and with system uncertainty.As seen in Fig. 12, the robot system tracks correctly the system the desired path (blue) and (red) real trajectory.
The result, in this case, of Fig. 12, was obtained assuming the system is certain and there is no need to use any correction technique to correct the trajectory tracking.
It is seen that the system exhibited a very good convergence to the desired posture of DDMR.The same system is now tested presuming such uncertainty the robot model.It is to kindly remind the reader that the system is to work with no correction technique.
As seen in Fig. 13 the robot dynamics showed a remarkable divergence in the system trajectory tracking (see "black" reference trajectory and "blue" real trajectory).Fig. 14 shows the robot"s dynamic performance for an 8shape with presence of uncertainty in the mobile robot, but the control system was supported with a correction technique for posture correction ("blue" is the reference trajectory, and "cyan" the real trajectory).Fig. 15 shows that the proposed Control-Navigation system enhances the robot"s belief at different points in time.The solid line displays the actions, and the ellipsoids represent the uncertainty effects on the robot"s dynamic performance for the trajectory.

C. Flower Shape Trajectory Tracking:
More complex trajectory is used to examine the robot dynamic performance without the presence of uncertainty and with uncertainty.The system dynamic performance is shown in Figures 16, 17, 18 and 19.As noticed in Fig. 19 that the system showed high dynamic performance control for which is the system is capable to correctly track the desired path and even ensure high degree of belief for the robot posture.It can be observed that better performance control in real time has been obtained by integrating the control system (Low and high levels) with EKF fusion sensors approach to make the control system sensitive to the effects of the environment and able to eliminate the uncertainty effect.

VI. CONCLUSION
This paper has proposed a newly developed approach to enhance the dynamic response of a differential drive mobile robot with the presence of huge system uncertainty.The developed approach has considered a low-level system control developed on the joint space level (the actuators control level) and on the Cartesian space level control i.e., high-level control.For low-level control optimal type controllers (optimal and integral optimal) have been proposed for the actuator velocity control.It has been shown that the dynamic control response of the actuators is high and robust to system uncertainty.
In the high-level control, the system has been incorporated with Extended Kalman Filter (EKF) to estimate, as accurate as possible, the mobile robot posture.Simulation results obtained from the developed control system have shown that a large divergence was due to occur because of system uncertainty, which may lead to erroneous system localization as well as exhibited low degree of belief in the robot location.The proposed control approach in high-level domain overcomes the problem of robot"s posture mismatch and tries to correct and compensate the trajectory tracking which may happen due to uncertainty.
The interactive navigation process with the degree of belief, which reflects how the present robot"s posture is close to the desired one.The validation of the proposed approach for lowlevel and high-level control has been confirmed through intensive simulation results obtained for different cases in which the robot has been imposed with uncertainty.As explained the proposed technique has given very encouraging results and has provided very good target tracking with high degree of belief for the robot localization.This work presented in this paper is basis for future.It is to be used for real time implementation for Mobile robot localization system control on the high and low levels identification e.g.UKF in real time utilizing dSPACE system board.

Fig. 1 .
Fig.1.Block diagram of DDMR with actuators.Usually, the DDMR"s posture is determined in its environment based on two coordinate frames: the Global frame {G} and Local Frame {L}.The global frame is fixed in the environment in which the robot moves in.The local frame attached to the DDMR at the middle point A between two back wheels.The movement of point A represents the movement of the robot [9].

J
of the wheels (rad.s - ) v The linear velocity of DDMR (m.s -1 ) ω The angular velocity of DDMR (rad.s - ) Robot moment of inertia (kg.m 2 ) uL uR F F , Tangential forces exerted on DDMR by the wheels.
forces exerted on DDMR by the wheels.

Fig. 3 .
Fig.3.The DDMR new position and orientation To determine the new DDMR"s position and orientation it is considered that the robot moves from the point A with a position ( 1 1 y , x   ), and orientation ( NAC 1    

4 4 Q
 : Positive semi-definite symmetric matrix determines the relative importance of the error.

2 2 R
 : Positive definite symmetric matrix determines the relative importance of the expenditure of the energy of the control signals.

Fig. 8 .
Fig.8.Time responses of the right and left wheels velocity

Fig. 11 Fig. 9 .
Fig.11shows the mobile robot speed response, as seen in Fig.11the robot moved with maximum speed value 0.14 m.sec-1

Fig. 17
Fig.17.The accumulated divergence in the DDMR"s posture with time.
Fig.17.The accumulated divergence in the DDMR"s posture with time.

TABLE I .
KINEMATIC & DYNAMIC MODEL VARIABLES: