Visual servo optimization stabilization of nonholonomic mobile robots based on control Lyapunov functions

This study considers the visual stabilization problem of nonholonomic mobile robots and proposes a novel optimization stabilization method for visual servo control of nonholonomic mobile robots with monocular cameras fixed onboard. The main idea of the method is to utilize control Lyapunov functions of discrete-time nonlinear systems to design a family of explicit stabilization control laws of the visual servo error system. The parameters of the control laws can indirectly reflect the performance of the visual servo controllers. Then taking account of visibility constraints and actuator limitations, a set of optimal parameters of the control laws is calculated by offline solving a constrained finite horizon optimal control problem. Moreover, the stabilization results on the optimal visual servo controller are established based on the properties of control Lyapunov functions. Finally, some simulation experiments are used to illustrate and evaluate the performance of the visual servo control scheme proposed here.


Introduction
Recently, there have been increasing interest in visual feedback-based servo control of mobile robots that commonly focus on nonholonomic kinematic constraints, that is, nonholonomic mobile robots (NMRs). In the study on the visual feedback-based servo control of NMRs, there exist two basic visual servo control problems: visual servo tracking control and visual servo stabilization control of NMRs. 1,2 These visual servo control problems can be formulated as the Cartesian motion control problems of NMRs in the framework of position-based visual servoing of NMRs. This paper works on the visual servo stabilization control of NMRs with monocular vision systems.
Visual stabilization control of NMRs is to use a visual information feedback to drive an NMR from an initial position and pose to a desired position and pose. 1,3 Due to the nonholonomic constraints of NMRs, 3,4 the visual servo stabilization problem of NMRs is more difficult than the visual servo tracking problem of NMRs. Namely, from the viewpoints of control, any time-invariant continuous state feedback control law cannot stabilize a nonholonomic system asymptotically. 4 To this end, several efforts have been made for the visual servo stabilization control of NMRs.
In early studies, visual servo stabilization of NMRs need position information of the feature points relative to the camera-robot platform. [5][6][7] Image-based visual servo methods can obtain a priori knowledge from the feature points of a three-dimensional (3D) scene which can directly describe the control objective. 1 In general, the early image-based visual servo controllers calculate the inverse of the image Jacobian of an NMR, which will suffer from the singular in some desired poses of NMRs. 8,9 Lately, several advanced control approaches have been used to design visual servo controllers for stabilization of NMRs. For example, the epipolar geometry technique was exploited to design the two-step visual servo control strategies for stabilization of NMRs. 10,11 Based on the state estimation technique, Lopez-Nicolas et al. 12 presented a hybrid visual servo control strategy for stabilization of NMRs in planar and nonplanar scenes. Without having accurate visual parameters and depth information, Lopez-Nicolas et al. 13 proposed a two-phase visual servo stabilization controller to stabilize NMRs to the origin. Moreover, a self-adaptive sliding mode controller was considered for stabilization of NMRs 14 and a position-based backstepping visual servo controller was presented for steering control of NMRs. 15 Due to the object occlusion and actuator limitations, visual data obtained from cameras may be lost, which leads to the failure of servoing of NMRs 16 if visual verso control strategies do not take these constraints into account during the controller design. Hence, model predictive control (MPC) has been used to develop the visual servo control schemes of NMRs in recent years, as MPC has advantages to explicitly deal with constraints and optimization control problems. [17][18][19] For instance, Cao et al. 20 proposed an image-based MPC for visual servo stabilization of NMRs with the constraints on visibility and actuators. In order to improve the computational efficiency of visual servo MPC of nonlinear NMRs, Li et al. 21 employed the neurodynamic optimization technique and He et al. 22 adopted the linear matrix inequality technique to design the visual servo MPC of nonlinear NMRs. Nevertheless, the recursive solution of the nonlinear optimization problem of the visual servo MPC generally leads to a heavy computational load for the control system of an NMR and hence, may limit the response speed of the NMR.
In this paper, we propose a novel optimization stabilization method for visual servo stabilization control of NMRs with monocular cameras fixed onboard. First, using control Lyapunov functions (CLF) of discretetime nonlinear systems, 23,24 a family of explicit feedback control laws is designed to stabilize the visual servo error system of the NMR, where the parameters of the control laws can indirectly reflect the performance of the visual servo controllers. Then in order to take account of the visibility constraints and actuator limitations, the parameters of the control laws are optimized by offline solving a constrained optimal control problem. By the properties of CLFs, 23,24 the stabilization results on the optimal visual servo controller of the NMR are established. With respect to the available visual servoing stabilization strategies, the main contribution of the work is to present an optimal control scheme for visual servo stabilization and constraint satisfaction of the NMR. Note that the nonlinear constrained optimization problem is solved offline and hence will increase the online computational burden of the proposed visual servo controller. Finally, the simulation results evaluate the performance of the visual servo control scheme proposed here.
The rest of the paper is organized as follows. Section ''Modeling and problem formulation'' presents the visual servoing system of the NMR model and the visual servo control problem. In Section ''Visual servo controller design,'' the visual servo stabilization controllers of the mobile robot are first designed via CLFs and then the optimal visual servo controller is redesigned by taking the system constraints into account. Some simulation results are given and discussed in Section ''Numerical simulations'' and Section ''Conclusion'' concludes the paper.

Modeling and problem formulation
Consider the NMR shown in Figure 1, which is driven independently by two wheels. In the visual servo system of the robot, a monocular camera is used and fixed onboard. Let O-XYZ be the world coordinate system, R-X r Y r be the coordinate system fixed on the robot and C-X c Y c Z c be the camera frame. C is the mass center of the robot. The camera is fixed along the Y r axis, and the distance between C and the middle of the drive wheels is l. r is the radius of the back wheel and the distance between the back wheels is 2L.
Consider the mobile robot with a monocular camera as shown in Figure 1. It is assumed that there is no lateral slip for the mobile robot. Then, the kinematic model of the mobile robot can be expressed as where q = [x, y, u] T is the status of the robot with x and y being the coordinates of the center of the robot and u being the orientation angle of the mobile robot, and the linear and angular velocities of the mobile robot are represented by v and v, respectively. Here, v and v are selected to be the control inputs of the mobile robot. Assume that the four static feature points shown in Figure 1 are selected to be the target. According to the perspective model, 20 the two-dimensional (2D) image coordinate p =[P x , P y ] T is derived as where f is the camera focal length and invariant. Using equation (2), the target point in the 3D Euclidean space is projected to its corresponding point in the image plane.
In visual servo stabilization, visual information is used to drive the mobile robot to a desired image target. When the mobile robot is stabilized to the desired target, the errors of the current and desired images converge to zero. Define the target feature point P as (P xd , P yd ) and u d as the angle at which the mobile robot reaches the target position. Then, the new coordinate is denoted by the image coordinate (s 1 , s 2 , s 3 ) as where u e =u2u d is the angle error of visual servo stabilization for the mobile robot. Now consider that the relationship between the robot with camera and target point is shown as in Figure 1. Due to the orthogonal transformation between the error vector e and the correlation coordinate system, the differential equation of the error system is obtained by combining equation (1) as 20 where h = 1/z c is a nonzero constant as the height of camera is not zero, that is, z c 6 ¼ 0. According to equations (4) and (5), the mobile robot will arrive at the target states shown as equation (7) when the error e ! 0 Moreover, it is observed from equations (3) and (7) that the image coordinate P x ! P xd , P y ! P yd , and u e ! 0. Hence, the goal of the paper is to design a visual servo stabilizing controller u(t) = u(e(t)) of the mobile robot such that lim t ! N e(t) = 0 and lim t ! N u(t) = 0. Here, the tool of CLFs of discrete-time version of equation (6) will be used to find the stabilization controller.

Visual servo controller design
Since visual servo controllers are generally implemented in digital-computer control systems, this paper designs the visual servo stabilizing controller of the NMR based on the discrete-time analog of continuous-time visual servo system (6). By the Euler approximation to equation (6), the discrete-time nonlinear visual servo error model of the NMR can be obtained as the following equation where T s . 0 is a sufficiently small sampling interval, the control input u =[v v] T , and the input matrix If there exists a feedback control law K(x) such that the resulting system is asymptotically stable at the origin, then the system is said to be asymptotically stabilized at the origin by the controller u = K(x). 23 Definition 2. Consider a smooth, proper, and definite positive function V: R n !R + , and the system x + = f(x) + g(x)u, where x + is the successor of x. 23,24 If the function V(x) satisfies that then V(x) is a CLF of the system. Note that Definition 2 is an extension of Sontag's result of continuous-time systems to the corresponding discrete-time systems. According to Sontag's result of continuous-time systems, the control law for the continuous-time system can be obtained. 25,26 Similarly, under some assumptions, there exists a discrete-time feedback control law to stabilize the discrete-time system x + = f(x) + g(x)u if a CLF of the system can be found in prior. 23 However, it is not easy to find a generic, systematic method to construct a CLF of the nonlinear system x + = f(x) + g(x)u. In general, it is assumed that there exists a positive definite matrix P such that V(x) = x T Px, namely, there exists a quadratic CLF for the nonlinear system.
Consider the discrete-time visual servoing system (8) and define a candidate of CLFs of the system as V(e) = l 1 e 1 2 + l 2 e 2 2 + l 3 e 3 2 = e T Pe where diagonal matrix P = diag{l 1 , l 2 , l 3 } with l 1 , l 2 , l 3 . 0. For simplicity, let e + is the successor of the current error e, that is, e + = e(k + 1) = e(k) + g(k)u(k). Then, we have the stability result on the discrete-time visual servoing system (8) as follows.
Theorem 1. Consider the discrete-time visual servoing system (8) and the function V(e) in equation (10). There exists a nonempty, invariant set Sˆand the feedback control law u = [u 1 , u 2 ] T with u 2 = l 1 lhe 1 + (l 2 À l 1 )e 1 e 2 + l 3 e 3 l 1 (lh À e 2 ) 2 + l 2 e 2 1 + l 3 such that V(e) is the local CLF of the system in Sâ nd the system (8) is asymptotically stabilized by equation (11) to the origin for any 0 \ m 1 , m 2 \ 1.
Proof. Consider the discrete-time visual servoing system (8) and the function V(e) in equation (10). Taking differential operation of V(e) along the closed-loop trajectories of equation (8), it is derived that l i e i g i1 + 2u 2 where g ij is the element in row i and column j of matrix g for i, j = 1, 2, 3. To clarify, let Substituting equation (13) into equation (12), it is obtained that V(e + ) À V(e) = 2u 1 l 2 hT s e 2 +u 2 1 l 2 h 2 T 2 s + 2u 2 a 2 + u 2 2 b 2 + 2u 1 u 2 l 2 he 1 T 2 s = l 2 (u 1 hT s + e 2 ) 2 À l 2 e 2 2 + Now let us substitute equation (11) into equation (14) and obtain that V(e + ) À V(e) = À l 2 m 1 e 2 2 À m 2 a 2 2 =b 2 + 2u 1 u 2 l 2 he 1 T 2 s For the discrete-time visual servoing system (8), define a set Note that the origin is the interior of set S and hence, S is nonempty. Pick a number r . 0 such that the set Sˆ={e2R 3 : V(e)4r}4S approaches to the set S as close as possible. Then for any e(k)2Sˆat time k50, we have that e(k + 1)2Sˆsince V(e(k + 1))2V(e(k)) \ 0. Hence, Sˆis an invariant set of the discrete-time visual servoing system (8) in closed-loop with equation (11). Moreover, since V(e) is a quadratic CLF of equation (8), the closed-loop system (8) with equation (11) is asymptotically stable to the origin for any 0 \ m 1 , m 2 \ 1. This competes the proof of Theorem 1.
Note that from equations (15) and (13), it is observed that the decreasing speed of V(e) in Sˆrelies on the values of l = (l 1 , l 2 , l 3 ) and m = (m 1 , m 2 ). If V(e) is regarded as the energy function of the visual servo system (8) of the mobile robot, then its decreasing speed can reflect the convergence rate of the stabilization error signal e(k). Hence, in principle we can find a set of the optimal parameters l and m such that the stabilization error signal e(k) converges in the sense of maximal satisfaction. However, the interplay between these parameter values and the decreasing speed of V(e) is too complex to derive an explicit condition due to strong nonlinearity among them. Moreover, to ensure the target feature points within the camera range and that the mobile robot drives safely and quickly during the servoing process, some constraints must be considered during the design of the controller.
One constraint is the visibility constraint, which keeps the visual features always to be visible. Here the limitations in the errors of e 1 and e 2 are used to formulate the visibility constraint, that is e 1, min 4e 1 (k)4e 1, max e 2, min 4e 2 (k)4e 2, max 04e 3 where e 1,min and e 2,min are the lower limits of e 1 and e 2 , respectively, and e 1,max and e 2,max are the upper limits of e 1 and e 2 , respectively. The other important constraint is the constraint on the actuator limitations in the velocity and torque variables v min 4v(k)4v max where v min and w min are the lower limits of velocity v and torque w, respectively, and v max and w max are the upper limits of v and w, respectively. In order to find a set of the optimal parameters l and m and to satisfy the constraints (17) and (18), one efficient way is to optimize these parameters according to a predefined cost function. 27 To this end, consider the discrete-time visual servoing system (8) and define the cost function over a sufficient large time horizon window N as where R . 0 is a weighted matrix on the control input and P = diag{l 1 , l 2 , l 3 } that can be viewed as a weighted matrix on the error. Then for any e(0), define the following optimal control problem to determine the parameters m where function u(e, l, m) is given by equation (11), set U is identified by equation (18), and set D = (0.1) 3 (0.1). From Theorem 1, it is known that there exists at least a set of m satisfying the constraints in problem (20), that is, the problem is always feasible for any initial state e(0) 2 Sˆ. Using numerical algorithms, for example, sequence quadratic program (SQP), mesh segmentation, we can derive some optimal solutions to the problem (20). Let m * be the optimal solution to equation (20). Then, the visual feedback-based servo optimal control law of the mobile robot is defined as u(k) = u(e(k), l, m Ã (k)), k50 = e 2 (k) hT s m Ã 1 l 1 lhe 1 (k) + (l 2 À l 1 )e 1 (k)e 2 (k) + l 3 e 3 (k) l 1 (lh À e 2 (k)) 2 + l 2 e 2 1 (k) + l 3 where m Ã i = À 1 + ffiffiffiffiffiffiffiffiffiffiffiffiffi ffi 1 À m Ã i p for i = 1 and 2. Note that the optimal controller (21) is a closed-loop feedback control system since it is a function on the current state e(k). Moreover, the computational burden of solving equation (20) may be expensive if the horizon window N is selected to be a large value in order to enhance the performance of the visual servo controller (21). However, the computational burden of online calculating of the control action (21) is not increased during this computing process as it is operated offline.
In what follows, the optimization stabilization result of the visual servoing system (8) with the optimal controller (21) is summarized as Theorem 2.
Theorem 2. Consider the discrete-time visual servoing system (8) and the function V(e) in equation (10). Then, the closed-loop system (8) with the optimal controller (21) is asymptotically stable in the domain of attraction Sˆ.
Proof. The conclusion of Theorem 2 is obtained by directly applying Theorem 1 and the proof of Theorem 2 is omitted here. j

Numerical simulations
In order to demonstrate the effectiveness of the control scheme, in this section the simulations are executed in MATLAB7.1V. In this study, the physical parameters of the visual servoing system (8)  , that is, P = diag{1, 1, 1}. The simulation is implemented for the controller in equation (11) without optimizing the parameters (l,m) and considering the constraints (17) and (18). The solid lines in Figures 2 and 3 show the simulation results in this experiment. From the solid lines in Figures 2 and  3, one can see that the visual servoing system (8) can converge to the origin, namely, the controller in equation (11) can asymptotically stabilize the visual servoing system. This result shows the feasibility of the proposed approach. Although the convergence velocity of the stabilization errors is slow, the control actions do not satisfy the constraints for all times. In the real application, once some target feature points move out of the camera range during the servoing process, the visual servo controller will be interrupted. Therefore, some constraints must be considered during the controller design.
To this end, in the second simulation experiment, the truncation method is used to truncate the violence of the control actions computed from Theorem 1. The dash-dotted lines in Figures 2 and 3 show the simulation results in this experiment. From the dashed-dotted lines in Figures 2 and 3, one can see that although the visual servoing system (8) can converge to the origin, the convergence velocity of the stabilization errors is slower than that of stabilization errors in the first experiment. The main reason is that the truncation of the control actions in this experiment decrease the overall control input of the visual servoing system by comparison with the control input in the first experiment.
In order to optimize the visual servo controller (11) and demonstrate the capability of handling constraints (17) and (18), the nonlinear constrained optimization problem (20) is initialized with the parameters N = 20, P = diag{1, 0.1, 1} and R = diag{0.1, 0.1}. Solving the problem (20), it is derived to be one local optimal solution m * = (0.697, 0.897). In order to improve the computational efficiency of solving (20), the Lagrange multiplier method is employed in the MATLAB function ''fmincon'' as well as the truncation method. Then implementing the optimal visual servo controller (21), we have the simulation results shown as the dashed lines in Figures 2 and 3. It is shown from Figures 2 and  3 that both the stability goal and the visibility constraints are satisfied using this optimal controller. Moreover, the stabilization errors also can be corrected more quickly and tend to zero asymptotically than those obtained by applying the other controllers. These results further illustrate the validity of the proposed control method.

Conclusion
Based on the notion of CLFs of discrete-time nonlinear systems, a novel optimization stabilization method was proposed for visual servo control of NMRs with onboard monocular cameras. By adopting discrete-time CLF of the visual servoing system, a family of explicit stabilization control laws was first designed and then a set of optimal parameters of the control laws was  computed by offline solving an optimal control problem by taking account of visibility constraints and actuator limitations. From the simulation results, it was verified that the proposed algorithm was feasible and effective for visual servo control with the NMR. The issues of robust visual servo control and receding horizon optimization visual servo control of constrained NMRs will be pursued along this work in the future work.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) received no financial support for the research, authorship, and/or publication of this article.