Real-Time Inverse Optimal Neural Control for Image Based Visual Servoing with Nonholonomic Mobile Robots

We present an inverse optimal neural controller for a nonholonomic mobile robot with parameter uncertainties and unknown external disturbances. The neural controller is based on a discrete-time recurrent high order neural network (RHONN) trained with an extended Kalman filter.The reference velocities for the neural controller are obtained with a visual sensor.The effectiveness of the proposed approach is tested by simulations and real-time experiments.


Introduction
Traditionally, robot motion control approaches have feedback provided by a taco-meter or encoder, whose advantages are its easy implementation and its low cost.However, in mobile robotics such information is not accurate due to the appearance of slip phenomenon.One sensor that can be used to overcome these problems is the visual sensor.In this work, we use computer vision techniques to overcome such disadvantages.
Although a visual sensor is more accurate, it can suffer from unknown external disturbances due to the robot motion.In addition, the robot model is inaccurate and it suffers from parameter uncertainties.To overcome such problems, we propose the use of a discrete-time recurrent high order neural network (RHONN) trained with an extended Kalman filter with visual feedback.
The main goal of optimal control theory is to determine the control signals that will force a process to satisfy physical constraints and minimize a performance criterion simultaneously [1].In optimal control theory, a cost functional is defined as function of the state and the control variables.Unfortunately it requires solving the Hamilton-Jacobi-Bellman (HTB) equation, which is not an easy task.To avoid the solution of a HTB equation an inverse optimal control can be used [2].In inverse optimal control, we start with the definition of a stabilizing feedback control, and then we have to show that it optimizes a cost functional.
In this work, the input of the inverse optimal control is determined by visual feedback.The visual sensor is responsible of tracking the target and the estimation of the robot velocities to achieve the desired task.In our case the task consists in moving the robot from an initial pose to a desired pose with respect to a target object.
1.1.State of the Art.An extensive class of controllers have been proposed for mobile robots [3][4][5][6][7][8][9].Most of these references present only simulation results and the controllers are implemented in continuous time.A common problem when applying standard control theory is that the required parameters are often either unknown at time or are subject to change during operation.For example, the inertia of a robot as seen at the drive motor has many components, which might include the rotational inertia of the motor rotor, the inertia of gears and shafts, rotational inertia of its tires, the robot's empty weight, and its payload.Worse yet, there are elements between these components such as bearings, shafts, and belts which may have spring constants and friction loads [10].

Main Contribution.
The paper main contributions are as follows: (1) presenting a controller for mobile robots which includes the robot dynamics and does not need the previous knowledge of robot parameters or model; (2) computing the trajectory references for the controller on real-time using visual data, acquired from a camera mounted on the robot; using visual data the controller drives the nonholonomic robot from its current pose toward a desired one; (3) real-time integration of visual servoing and an inverse optimal neural controller to allow nonholonomic mobile robots to perform autonomous navigation.
The rest of this work is organized as follows.First the problem formulation is presented in Section 2. Then the model of the mobile robot and framework setup is described in Section 3.After that the camera model is presented in Section 4. Later, the visual feedback algorithm is introduced in Section 5. Section 6 provides an introduction to the neural identification.The inverse optimal control approach is presented in Section 7. The neural identification and control of the mobile robot is presented in Section 8.The simulations results are presented in Section 9.The experimental results are presented in Section 10.Finally, the conclusions are given in Section 11.

Problem Formulation
The main focus of this work is the navigation of a differential drive robot from its current pose to a desired pose by using visual feedback and a neural controller, Figure 1.The camera is mounted on the robot, and therefore the robot motion induces camera motion.Before the task begins the desired feature  * is estimated applying a segmentation algorithm to the image of the target.The same process is done in real-time to compute the current future .When the algorithm begins the desired feature  * and the current feature  are compared to compute the error.Then, using this error, the controller estimates the velocities to complete the task.

Model Formulation
In this paper we consider a two-wheel differential drive mobile robot that moves on a plane.The motion of the mobile robot is defined with respect to an inertial frame F  fixed in the world.Let F  be a frame attached to the current camera pose, F *  a frame attached to the desired camera pose, and F  a frame attached to the mobile robot pose, Figure 2.
The mobile robot used in this work is a differential drive mobile robot; its kinematic model is where the inputs V and  represent the driving velocity and the steering velocity, respectively.
The mobile robot has two actuated wheels, and its dynamics can be expressed in the following state-space model [4,11,12]:  where each subsystem is defined as

Camera Model
In this paper the camera used is a traditional perspective camera.In this section we describe the perspective model; this model describes the relationship between a 3D point  X and its projection , which can be computed from where  T  is a rigid transformation that relates the world coordinate frame with respect to the camera coordinate frame and is defined as where  R  is a rotation matrix and  t is a translation vector, and both terms represent the extrinsic camera parameters.The point  x can be projected into the image plane as where the matrix K represents the intrinsic camera parameters and is defined as where  is the focal length in pixels,   and  V are the coordinates of the principal point (in pixels), and  represents the skew factor.

Visual Based Feedback
The use of visual feedback to control a robot is commonly termed visual servoing or visual control [13][14][15][16].In this work we assume that a monocular camera is mounted directly on the mobile robot, in which case motion of the robot induces camera motion.
The objective of the visual feedback is the minimization of where () denote the features extracted from the current pose and  * denote the features extracted from the desired pose.In our case the features of error ( 9) are two-dimensional vectors and are defined as  = ( log()) ⊤ and  * = ( * log * ()) ⊤ ; note that log() and log * () are supplementary normalized coordinates [17].
During the motion of the robot the camera suffers a motion k  = (V  ,   ), where V  is the linear velocity and   is the angular velocity.The relationship between this velocity and ṡ is The relationship between the camera velocity and the time variation error can be determined with ( 9) and (10); that is, where the interaction matrix L can be defined as [15] where the matrix  V  is a motion transformation matrix defined as [18] where  R  is the rotation matrix that relates the camera and robot frameworks and [] × represents the skew symmetric matrix associated with the vector .
The interaction matrix L  can be defined as [13] where the  value represents the depth of the feature relative to the camera frame.The values ,  are computed from the pixels coordinates   ,   of the tracked feature and the camera calibration matrix as where We can note from ( 14) that each feature point provides two equations; therefore for a 6-DOF problem we need at least three features.However, since we are dealing with a mobile robot with only 2 controllable DOF, then a single feature is enough.
From (11) and ( 12) we can define the velocity input to the robot as [15] k  = −L + e, (16) where L + is the pseudoinverse of the matrix L.
From the above expression, we can observe that velocities (16) can be used as reference for the neural controller.

Neural Identification
In this section, we present the neural identification process.First, let us consider a MIMO nonlinear system where   ∈ R  is the state of the system,   ∈ R  is the control input, and 17) is identified by a discrete-time recurrent high order neural network (RHONN), defined as where   is the state of the th neuron,   is the respective online adapted weight vector,  is the state dimension, and   (  ,   ) is given by where   is the respective number of high-order connections, { 1 ,  2 , . . .,    } is a collection of nonordered subsets of dimension {1, 2, . . ., +},  is the number of external inputs,    () is nonnegative integers, and   is defined as follows: In (20),  = [ 1 ,  2 , . . .,   ] ⊤ is the input vector of the neural network, and (•) is defined by where  is any real value variable.
Based on the structure of discrete-time RHONN series parallel representation [19], we propose the following neural networks model: where  , are the adjustable weight matrices,   , are matrices with fixed parameters, and  denotes a linear function of  or  corresponding to the plant structure or external inputs to the network, respectively.
The EKF-based training algorithm is described by [20]  ,+1 = with where  , is the identification error,  ,+1 is the state estimation prediction error covariance matrix,  , is the th weight vector at step ,   is a design parameter such that 0 ≤   ≤ 1,  , is the th plant state,  , is the th neural network state,  is the number of states,  , is the Kalman gain matrix,  , is the measurement noise covariance matrix,  , is the state noise covariance matrix, and  , is a matrix, in which each entry of ( , ) is the derivative of th neural network state ( , ), with respect to all adjustable weights ( , ), as follows: Usually   ,   , and   are initialized as diagonal matrices, with entries   (0),   (0), and   (0), respectively [21].It is important to note that  , ,  , , and  , for the EKF are bounded [22].
It is possible to identify ( 17) by ( 18) due to the following theorem.

Inverse Optimal Control
Let us consider a nonlinear affine system where   ∈ R  is the state of the system at time  ∈ N and  ∈ R   : R  → R  ,  : R  → R × are smooth and bounded mappings.We assume (0) = 0. N denotes the set of nonnegative integers.The following meaningful cost functional is associated with the trajectory tracking problem for system (26): where   =   −  , with  , as the desired trajectory for   ;   ∈ R  ; L(  ) : R  → R + ; (  ) : R  → R + is a positive semidefinite function and (  ) : R  → R  ×  is a real symmetric positive definite weighting matrix.The entries of (  ) can be fixed or can be functions of the system state in order to vary the weighting on control efforts according to the state value [1].
For solving the trajectory tracking optimal control problem, it is necessary to solve the following HJB equation: which is a challenging task.To overcome this problem, we propose to solve the inverse optimal control problem.
Definition 2. Consider the tracking error as   =   − , ,  , being the desired trajectory for   .Let one define the control law It will be inverse optimal (globally) stabilizing along the desired trajectory  , if (i) it achieves (global) asymptotic stability of   = 0 for system (26) along reference  , ; (ii) (  ) is (radially unbounded) positive definite function such that inequality is satisfied.
As established in Definition 2, the inverse optimal control law for trajectory tracking is based on knowledge of (  ).Then, a CLF (  ) is proposed, such that (i) and (ii) are guaranteed.Hence, instead of solving (28) for (  ) a quadratic candidate CLF (  ) for (29) is proposed with the form where in order to ensure stability of the tracking error   , where The control law (29) with (31), which is referred to as the inverse optimal control law, optimizes the meaningful cost functional of the form (27). Consequently, by considering (  ) as in (31), control law (29) takes the following form: and (  ) are positive definite and symmetric matrices; thus, the existence of the inverse in (34) is ensured.

Neural Identification and Control of the Mobile Robot
In this section we describe the neural identification and neural control for the nonholonomic mobile robot; in the next section we will describe the reference for this controller.The reference velocities will be estimated using visual feedback.It is important to note that these parameters are considered unknown for the controller design and are only included for simulation purposes.Then, we apply the neural identifier, developed in Section 6, to obtain a discrete-time neural model for the electrically driven nonholonomic mobile robot (2), with  = 7 trained with the EKF [20], as follows: where  1 and  2 identify the  and  coordinates, respectively;  3 identifies the robot angle;  4 and  5 identify the angular velocities of right and left wheels, respectively; finally,  6 and  7 identify the motor currents, respectively.The NN training is performed online, and all of its states are initialized in a random way.The RHONN parameters are heuristically selected as It is important to consider that for the EKF-learning algorithm the covariances are used as design parameters [21,24].

Control Synthesis.
In order to facilitate the controller synthesis, we rewrite neural network (36) in a block structure form as The goal is to force the state  1, to track a desired reference signal  1, .This is achieved by designing a control law as described in Section 7. First the tracking error is defined as Then using (38) and introducing the desired dynamics for  1, result in where The desired value  2, for the pseudocontrol input  2, is calculated from (40) as At the second step, we introduce a new variable as Then using (38) and introducing the desired dynamics for  2, result in where The desired value  3, for the pseudocontrol input  3, is calculated from (43) as At the third step, we introduce a new variable as Taking one step ahead, we have where   is defined as where the controllers parameters are selected heuristically as

Simulation Results
This section presents the simulation results of the proposed approach.The simulations have been performed using Simulink.In the simulation the robot moves under the action of the proposed controller; the controller uses as references the velocities provided by the visual feedback.In the simulation the initial pose of the robot was [0 0 0] ⊤ , and the desired pose was a rotation about the -axis   (16.5757 ∘ ) and a translation [2.5197 −0.4180] ⊤ .The camera frame is related with the robot frame with a rotation   (/2)  (/2) and a translation [0 0 −0.13] ⊤ .The sampling time of the simulation was  = 0.04 s.
In Figure 3 we present the camera motion in the 3D space.Figure 4 shows the identification performance for -axis, -axis, and  angle.Figure 5 shows the trajectory tracking results.In Figure 6 we present the tracking errors.In Figure 7 we show the applied control signal for the left and right wheels.Figure 8 presents the current identification for simulation in left and right wheels.Finally, Figure 9 shows the angular velocity identification for simulation in left and right wheels.9.1.Comparison.In order to compare the proposed control scheme with respect to previous works as presented ones in [3], Table 1 is included, which is described as follows: the controllers used in this comparison are (1) Neural Backstepping Controller (NBC) [3], (2) High Order Sliding Mode (HOSM) controller [3], and (3) inverse optimal neural controller (IONC) and are proposed in this paper.

Experimental Results
In this section, we present the experimental results obtained by applying the proposed approach.The mobile robot and the visual sensor used in the tests are shown in Figure 10.The sensor used in this work was a Kinect sensor; this sensor has a perspective camera, an IR emitter, and IR depth sensor which are used to estimate the depth of the feature.
The camera calibration matrix of our Kinect perspective camera is The visual feature used in this work is the center of a circle.The target object contains one circle which is segmented using an hsv segmentation; then its centroid is used as features for the task, Figure 11.
The initial and desired pose of the robot are shown in Figure 12. Figure 13 shows the identification performance for -axis, -axis, and  angle.In Figure 14 we present the tracking errors.We have to note that the target is at a distance of approximately 3 m and the linear velocity of the robot is 0.2 m/s, and therefore the algorithm takes approximately 15 s to converge to zero.
In Figure 15 we show the applied control signal to the left and right wheels.Figure 16 presents the current identification of the left and right wheels.Finally, Figure 17 shows the angular velocity identification of the left and right wheels.It is important to note that differences between simulation and experimental results are due to some reasons found in real-time implementations as parametric uncertainties, external disturbances, unmodeled dynamics, limits on actuators, delays, and differences on processing times, among other circumstances that difficult real-time implementations, making them a truly difficult task, especially integrating different techniques like visual servoing, automatic control, and neural identification.

Conclusions
In this work, an inverse optimal neural controller for model identification and control of mobile robots with nonholonomic constraints and visual feedback has been presented.The reference velocities for the neural controller were computed from a desired target acquired from visual sensor and tracked by a visual algorithm.From the simulations and realtime experiments we can observe that the proposed approach can effectively drive the nonholonomic mobile robot from its current pose to the desired pose.In addition, the neural identifier is able to couple with unknown external disturbances and parameter uncertainties.In the future we want to test the proposed approach with different computer vision algorithms.We will like to extend the proposed approach to omnidirectional vision systems due to its large field of view.We also want to extend the proposed approach to different robotic platforms, like UAVs.

Figure 2 :
Figure 2: Coordinate frames of the robot F  , camera F  , and object F  .

Figure 5 :Figure 6 :
Figure 5: Trajectory tracking result for simulation (plant signal in solid line and neural signal in dashed line).

Figure 7 :Figure 8 :
Figure 7: Applied control signal for the left and right wheels, respectively.

Figure 9 :
Figure 9: Angular velocity identification for simulation in left and right wheels, respectively (plant signal in solid line and neural signal in dashed line).

Figure 13 :
Figure 13: -axis identification (a), -axis identification (b), and  angle (c); plant signal in solid line and neural signal in dashed line.

Figure 16 :Figure 17 :
Figure 16: Current identification of the left and right wheels, respectively (plant signal in solid line and neural signal in dashed line).
11,  12 ,  13 ]   2 + 2   2 +   + 2  ,   = [ 1 ,  2 ]  represents the half of the width of the mobile robot and  is the radius of the wheel and  is the distance from the center of mass   of the mobile robot to the middle point  0 between the right and left driving wheels.The values  11 = ,  12 =  are the coordinates of  0 and  13 =  is the heading angle of the mobile robot,  21 = V 1 ,  22 = V 2 represent the angular velocities of right and left wheels, respectively, and  31 =  1 ,  32 =  2 represent motor currents of right and left wheels, respectively.The values   and   are the mass of the body and the wheel with a motor, respectively.,   , and   are the moment of inertia of the body about the vertical axis through   , the wheel with a motor about the wheel axis, and the wheel with a motor about the wheel diameter, respectively.The positive terms   ,  = 1, 2, are the damping coefficients.The value   ∈ R 2 is a vector of disturbances including unmodeled dynamics.= diag[  1 ,   2 ] is the motor torque constant,  ∈ R 2 is the input voltage,   = diag[  1 ,   2 ] is the resistance,   = diag[  1 ,   2 ] is the inductance,   = diag[  1 ,   2 ]is the back electromotive force coefficient, and  = diag[ 1 ,  2 ] is the gear ratio.Here, diag[⋅] denotes the diagonal matrix.Model (2) is discretized using the Euler methodology.

Table 1 :
Comparison between inverse optimal neural controller (IONC) with respect to Neural Backstepping Controller (NBC) and High Order Sliding Mode (HOSM) controller.