Quadratic Programming Algorithm of Approximate Nonlinear Model Predictive Control for Tracking Trajectory of Wheeled Mobile Robots

Nonlinear model predictive control (NMPC) has proved its competency in controlling constrained nonlinear processes. Although NMPC can achieve exemplary tracking performance, the related computation effort as well as guaranteeing tracking convergence are its main drawbacks. Indeed, constrained NMPC is a nonlinear and nonconvex optimization problem where it is difficult to find a feasible solution within a reasonable time. Motivated by these difficulties, this paper proposes a procedure, using Euler approximation, to transform the nonlinear optimization problem of NMPC into a constrained quadratic optimization problem. The proposed tracking controller is applied to the autonomous navigation problem of a wheeled mobile robot (WMR) in a constrained space. Under certain assumptions, we prove the closed-loop system stability and the boundedness of the tracking error. Further, we show the recursive feasibility of the solution. Simulations are performed, first to determine the adequate control parameters, and secondly to show the effectiveness of the proposed algorithm, while its real-time implementation is experimentally verified using a differential drive mobile robot.


I. INTRODUCTION
Over the last few decades, mobile robots have been widely studied due to their inherent applications in different fields. Indeed, mobile robots are being used for a simple exploration of the unknown region for search and rescue military applications and surveillance [1]. Hence, many researchers have focused on the autonomous navigation of mobile robots in an unknown environment with static and dynamic obstacles. Therefore, the desired trajectory is generated online by a motion planner, based on the information acquired from the environment, in order to avoid obstacles and minimize the distance to the target. Control methods utilized to control the under-actuated systems are mainly nonlinear. Therefore, many methods based on nonlinear techniques, have been developed to solve the tracking problem and allow the mobile robot to track the desired trajectory [2,3,4,5]. While the authors in [6,7] have focused on a more complex environment (outdoor) where the wheels are subject to deformation. However, mobile robots have physical limits, and space constraints that affect the achievable closed-performance in the indoor and outdoor environment.
It is a known fact that model predictive control is the best technique used to cope with hard constraints on controls and states. For this reason, nonlinear model predictive control has been widely used in academic research and industry due to its simplicity to include different constraints in the optimization problem [8,9]. To solve the NMPC problem variety of approaches, based on sequential quadratic programming (SQP) algorithms, have been developed. This direct method is based on converting the NMPC problem to an optimal control problem. This nonlinear programming problem (NLP) requires an online solution to a receding horizon optimization problem in order to be implementable in practice. Thus, this is a nonlinearly constrained optimization problem with the following challenges: 1-The processing time of the algorithm. 2-Nonconvexity of the optimization problem.
3-Stability of the closed-loop system and feasibility of the solution. For this reason, most of the NMPC applications that have been reported in the literature have been applied to systems with slow dynamics like, for instance, chemical processes. For instance, in [10], the authors have designed the NMPC to a continuous stirred tank reactor (CSTR) using a multi-stage control approach. While the authors in [11], presented a linear mismatched model-based offset-free model-predictive control approach for CSTR nonlinear system. In [12], the authors have applied the robust nonlinear predictive control to the CSTR process using the radial basis function network (RBF-ARX) model. In [13], the authors have formulated the nonlinear optimization problem as a quadratic cost function with inequality constraints and applied it to the CSTR plant. In recent years significant effort has been made to cope with the real-time implementation of the NMPC to be applied to high dynamic systems with a fast-sampling rate. The real-time iteration (RTI) scheme has been proposed in [14], and the authors have combined ideas of the RTI with Multi-Level Iteration (MLI) that ensures high computational efficiency for real-time implementation of the NMPC. The authors in [15] have proposed a homotopy-based nonlinear interior-point method for an efficient real-time implementation of NMPC. In [16], the authors have developed a highly parallelizable Newton-type method to solve efficiently the NMPC. In [17], the authors have presented a collection of embedded optimization algorithms under a modular structure written in C language for better real-time optimization for nonlinear optimization. A parallel optimization toolkit for a real-time implementation of NMPC has been proposed in [18]. Indeed, the authors have used a highly parallelizable method for an efficient real-time implementation of NMPC. The authors in [19] have used the RPROP algorithm based on a faster backpropagation approach for the gradient-descent optimizer. To this end, the authors in [20] have used the highperformance framework ACADO software for a computationally efficient implementation of the NMPC to reluctance synchronous machines. Consequently, to overcome the computational burden of NMPC, a huge amount of NMPC schemes have been developed during the last years (see the excellent reviews [39]).
Despite previous challenges, several applications of NMPC to a mobile robot can be found in the literature. First applications have focused on tracking the desired trajectory [19,21,22]. In [23], the authors have proposed NMPC used with a nonlinear (nonconvex) optimization algorithm to track the centerline of the roadway while avoiding obstacles. However, the iterative method used to determine the optimal solution is a high time-consuming algorithm. The authors in [24] have combined path following controller with obstacle avoidance using nonlinear model predictive control. Even though the authors have provided the stability and feasibility analysis of the algorithm, the optimization problem is nonlinear and nonconvex with a high computational burden. The nonlinear model predictive control has been also used in [25] to steer the WMR to the desired pose. Despite of the rigorous asymptotic stability analysis, the proposed solution needs to solve a nonlinear optimization problem. To avoid the local minimum of the optimization problem, using receding horizon control (RHC), the authors in [26] have proposed a localminima-free navigation function with the global minimum at the target state. The proposed algorithm solves the navigation problem with control constraints. The authors in [27] have used a genetic algorithm (GA) to solve the NMPC algorithm to avoid the computational difficulties of the nonlinear programmingbased MPC. The algorithm has been successfully applied to solve the steering problem of an autonomous vehicle. To reduce the processing time due to the online optimization, the authors in [28] proposed a convex quadratic programmingbased model predictive scheme for collision-free collision navigation of autonomous vehicles.
Consequently, the processing time of the NMPC algorithm is one of the most commonly encountered problems when NMPC is used to control a highly dynamic nonlinear system. Indeed, the time required to reach the optimal solution is high with regard to the dynamic of the process. This is due to the nonlinear optimization problem that is solved at each sample time (online) [29]. Further, the optimization problem is, in general, nonconvex and the solution can converge to the local minimum [30,31]. Hence, the convexity of the optimization problem plays a central role in the practical implementation of the constrained NMPC.
In this paper, we propose a simple algorithm, based on approximated model predictive control, for safe autonomous navigation of the WMR in a constrained space. This algorithm permits the WMR to track accurately the feasible predefined trajectory. To avoid the computational burden of the NMPC, the Euler approximation is utilized to convert the NLP to constrained quadratic programming (QP). Figure 1 describes the constrained QP implementation of the proposed algorithm. The stability analysis of the closed-loop system is investigated using the Lyapunov method and the positive invariance principle in constrained cases. Therefore, the contribution of this work resides in the transformation of the tracking problem, which is a nonconvex optimization problem, to a constrained QP problem. The restricted space and constraints on the control input are written as convex constraints (inequality constraints). Thus, the heavy computation burden of the original optimization problem can be reduced considerably using the fast-available algorithms [32,33].
The proposed algorithm can be used efficiently in applications where the navigation space is limited, like for instance, robot vacuum cleaners and landmine detection robots. Further, undetectable obstacles, like for instance downstairs in an indoor environment, can also be included as space constraints to avoid the robot falling downstairs. Therefore, the proposed control algorithm is simple, practical, and efficient with the feasible solution and asymptotic stability of the equilibrium point of the closed-loop system.
The paper is organized as follows. In section 2, the kinematic model of the unicycle mobile robot is derived with some considered assumptions and properties. Section 3 deals with the trajectory tracking algorithm based on approximated nonlinear model predictive control (ANMPC). The stability issue of the closed-loop system and the feasibility of the solution are discussed in section 4. To see the effectiveness of the proposed scheme, simulations and experimental tests have been performed. The obtained results are shown in section 5. The paper ends with a conclusion and future work.

II. PRELIMINARIES
This section deals with some preliminaries required for this work.

A. MATHEMATICAL MODELING
In this work we consider a differential drive mobile robot. It contains two independent rear wheels and a front castor wheel. The linear speed of the WMR is given by the combination of the left speed and right speed of each DC motor: = + 2 and the angular speed is given by: where is the wheelbase. The transformation between these variables is given in (Fig.1): The Kinematic model of the mobile robot is given by [1] (2) is where Ts is the sampling time.
The tracking error dynamic can be written as

B. ASSUMPTIONS
In order to design the ANMPC algorithm, some assumptions are required.
1-Both reference position ( ) and its time derivative

2-The first-order Taylor approximation (Euler method)
is used to convert the constrained nonconvex optimization problem to a constrained quadratic optimization problem. It is assumed that the approximation is valid for a small step-time h. 3-The state vector ( ) is available.

C. DEFINITIONS
Here, we define some properties that will be used in the stability and feasibility analysis.
1-The column vectors of ( ) are linearly independent.

D. Notation
In addition to the nonholonomic constraint, the position of the while the restricted cartesian space is defined as compact (nonempty) set by: We can also define the constrained compact set related to the predicted tracking error as Then, for an initial feasible state error ( ), the sequence of the control vector is is an admissible control vector, if the state error trajectories ( ), generated by the dynamic system (3), satisfies ( ) ∈ .

III. RECEDING HORIZON CONTROL OF A WMR
The formulation of the NMPC problem for trajectory tracking is to find the control vector ( ) such that the predicted tracking error ( + ) = ( + ) − ( + ) converges to zero along the interval ∈ [ , + ]. Note that ( ) is the safe trajectory generated by the planner (Figure 1) in order to move the WMR to the desired target without colliding with obstacles. The above problem is reformulated as an optimization problem [8,9]: subject to: where > 0 is the prediction horizon, > 0 is a positive definite matrix (dim ( ) = 3 × 3), and ≥ 0 is a positive semidefinite matrix dim ( ) = 2 × 2).
Problem (4) is a nonconvex optimization problem and must be solved online, with computational complexity, at each sampling time. Further, most of the existing algorithms may converge to a local solution. In this work, the optimization problem (4) will be converted to a convex quadratic optimization problem using the first-order Taylor approximation to solve it efficiently with a global minimum. Therefore, to obtain an explicit solution of NMPC, the prediction horizon T is divided into m subintervals ( ≥ 2) of equal width h such that = ℎ. The discrete-time version of the cost function (4) is given by where ( ) is the initial tracking error and ( + ℎ) = To ensure the stability of the closed-loop system, the terminal cost should be included in the objective function (5) as follows where the adequate selection of the symmetric definite-positive matrix ∈ ℜ 3×3 will be given later. So, the cost function (5) can be written as Note that, to avoid the cross product of control signals in (6), the following approximation is used: Hence, the cost function (6) becomes where ( ( ),̇, ℎ) represents terms that are independent of the control vector U(t), ̅ = ( , , , … … . , ).
To evaluate the effect of the control effort on the predicted tracking error, the Euler approximation is used as follows Using (7), the cost function can be rewritten under the quadratic form Since ̅ > 0, > 0, and ̅ ≥ 0, so the matrix ( ̅ + ℎ 2 Π) is a positive definite matrix. The optimization problem (8) can be written under the following quadratic form such that -

CONSTRAINTS HANDLING
Constrained spaces are being utilized mainly in the motion planning of robot manipulators. Hence, the contribution of this work is to formulate the motion planning problem of the mobile robot in the constrained environment as a constrained quadratic optimization problem. So, two kinds of constraints are considered:

A. CONSTRAINTS ON CONTROL VECTOR
All real-world control problems are subject to actuator constraints. Thus, the control signals ( ) and ( ) of the mobile robot (the speed and the steering angle) are bounded and cannot exceed some threshold levels [2]. In model predictive control, the predicted control signals should be constrained inside the admissible set. Thus, the constraint ( ) ∈ ℧ can be expressed as linear constraints

STATE CONSTRAINTS
The idea used in this work, which allows the mobile robot to navigate safely inside the restricted space, has been used in the handling of contacts in crowd motion simulations [34] and extended to mobile robot navigation [35]. The admissible subspace can be seen as an intersection of finitely many halfplanes in the space ℜ 2 and can be defined by a set of nonlinear inequalities ( ) such that It is desired to keep the predicted Cartesian position of the mobile robot inside this feasible region . Therefore, the problem to be solved is to determine the adequate control vector ( ) that satisfies the following constraints Or under the following inequalities The m-steps predicted Cartesian position of the WMR is given by: , and Constraints given in equation (11) can be written as: Consequently, the state constraints ( + ℎ) ∈ ℤ are approximated by the linear (with regards to ( )) inequality Therefore, if assumption 2 holds, the optimization problem can be formulated as a constrained QP problem as follows * ( ) = min ].
The optimization problem (12) is a convex quadratic problem; this is due to the quadratic cost and linear (with regards to the vector U(t)) inequality constraints. Further, the optimization problem is feasible since the feasible set, defined by ℧ = { ( ) ∈ ℜ 2× / ( ) < } is nonempty (the matrix has a full rank). Hence, many algorithms exist that solve this convex optimization problem within a few steps [31,32,33].

IV. STABILITY AND FEASIBILITY ANALYSIS
The stability of the constrained model predictive control of nonlinear systems has been a main subject of research for many years. The closed-loop stability of the plant under the cost function with state and input constraints (12), can be guaranteed independently of the nonlinear mathematical model of the plant [36,37]. This fact is realized by adding an equality constraint to the optimization problem that forces the tracking error to lay inside a terminal region at the end of the finite horizon. This terminal region should be a region of attraction for the nonlinear system controlled by a local controller.

A. STABILITY ANALYSIS
In this work, the Lyapunov method is used to prove the stability, and feasibility of the system closed by the model predictive control. The proposed controller will drive the state error to a terminal set. Inside this terminal set, a local controller will be used to drive the error state to the equilibrium point. Lemma For the system (3) Proof: In order to find the current control ( ) that reduces tracking error along with the interval ∈ [ + ℎ], we consider a dynamic performance index, that penalizes the tracking error under the form where = + . The minimization of ( , ) with respect to ( ), using the first-order Taylor approximation of the onestep-ahead predicted tracking error, and by setting = 0 yields to: where ( ( ) ( )) −1 is a positive-definite matrix.
Let's take the following Lyapunov function Given that the planned trajectory can be considered as a path or a set of points with ̇= 0, the time derivative of the Lyapunov function using the local controller (13) yields, Theorem 1 Assume that the initial solution of the optimization problem is feasible, ( + ℎ) ∈ , and ( + ( − 1)ℎ) ∈ ∪ = 1, . Suppose that the following inequality holds: Then, under the terminal controller (13), the optimal cost function ( , ( ), ℎ) is a control Lyapunov function (CLF) for the system (3). Consequently, the equilibrium point is asymptotically stable.
Proof: See the appendix.

B. FEASIBILITY ANALYSIS
To guarantee the closed-loop stability and convergence derived previously, we assumed that the predictions by the tail lie inside the constrained space. That is, the optimal solution at the time t is feasible: where ( + ℎ) ∈ ∪, = 1, ⋯ , .
We have to ensure the feasibility of the solution at the next step, i.e at the time t+ h [36,37]. Hence, the next solution to the optimization problem has to be feasible (not necessarily optimal). The solution at the time t + h is: Therefore, necessary and sufficient conditions for the predictions generated by ̃( + ℎ) and ̃( + ℎ) to be feasible at time t+ h, whenever the solution of the optimization problem at time t is feasible, are: For the solution given by (i), it is assumed that the predicted tracking error at time t + mh is feasible, then there exists a feasible subset of , named Λ such that * ( + ℎ) ∈ Λ ⊆ . Hence, using the Lemma, the tracking error at the next time is also feasible * ( + ℎ + ℎ) ∈ Λ ⊆ . Thus, the optimal solution will drive the state error from the set to the terminal set Λ . To constraint the control signal inside its limits (ii) when the tracking error lies inside the terminal region Λ , the following inequality should be satisfied using assumption 6, the inequality becomes where Ψ = ( ( ) ( )) −1 ( ) , ̅ = ( , | | ).

Hence, the condition (16) is satisfied if the terminal region is chosen as
Consequently, the feasibility can be achieved provided that the control problem admits a feasible solution at the initial time , in order to maintain the feasibility of all solutions. Indeed, it is a well-known fact that the feasibility at one time instant leads to a feasible solution at the next instant, and the value of the cost function decreases [36,37].
Theorem 2 Suppose that the assumptions 1-3 hold, the matrix P is chosen such that the inequality (15) holds. Starting from a feasible initial position ( ) ∈ ℤ and a feasible control prediction sequence ( ) ∈ ℧ , then the solution * ( ) of the quadratic problem (12) steers any initial tracking error from to Λ . Further, the terminal control signal ( ) will steer the tracking error to the equilibrium point * for ≥ ( + + ℎ).
Proof: Assume that the initial control sequence ( ) and the initial state are feasible at time t. Using the constrained QP problem (12), the optimal solution * ( ) is feasible. From theorem 1, the control Lyapunov function is a decreasing function, then the polyhedron set ℤ is a positively invariant set and the tracking error will converge to the terminal region within the interval [ , + ]. For ≥ ( + + ℎ), the terminal controller ( ) will steer the tracking error to the equilibrium point * . Thus, the equilibrium point of the closedloop system is asymptotically stable.
According to the feasibility or unfeasibility of the reference trajectory ( ), we have two cases:

Case 1:
( ) ∈ ℤ (feasible trajectory) In this case, the terminal region is an attractive region of the equilibrium point which is the origin. Indeed, the solution provided by the QP algorithm (12)  In this case, Z( ) ∈ ℤ and ( ) ∉ ℤ, the equilibrium point is * = ( + ) − ( + ) = − = ≠ 0, where is the value of ( ) on the border of the feasible set ℤ. Thus, the solution will converge to an equilibrium point defined by ≠ 0.
In conclusion, the solution to the convex optimization problem (12) steers asymptotically the tracking error to the equilibrium point * defined by

V. SIMULATION AND EXPERIMENTAL RESULTS
This part illustrates the effectiveness of the proposed control algorithm, given by the QP (12), through simulation using MATLAB software (2021b) and a real-time implementation using the Khepera III platform [39]. This section deals with the different performances achieved by the proposed algorithm in terms of: 1-Achieved control performances (tracking performance and feasibility of the solution). 2-Processing time comparison. 3-Real-time implementation of the approximated NMPC.
Note that, for the simulation part, the control parameters Q, R, and h have been tuned in order to determine the optimal values that achieve reasonable tracking performances. It is noticed that the step time h is a key parameter that should be chosen carefully to validate the Euler approximation used in this work. Therefore, many simulations have been performed to find the adequate control parameters that achieve admissible tracking performances.

A. CONTROL PERFORMANCE RESULTS
For this part, we have stated two different scenarios [19]: I. Tracking eight-shaped trajectory. In this case, constraints on y-axis limits and control signals are included in the optimization problem. II.
Tracking a circular-shaped trajectory where the admissible region is a square-shaped space. In this case, both state constraints and input constraints are included in the optimization problem.
For simplicity, we assume that the matrices and R are diagonals. After some tuning, the control parameters are chosen as Because of the bounded velocity of each wheel, constraints on the control signals should be included in the optimization problem. In this simulation part, the same bounds provided by the authors in [2] are used, which are The constrained space is delimited over the y-axis, where = { ( ) ∈ ℜ; 1.5 ≤ ( ) ≤ 4.5 , ∈ [ , + ]}. Figure 3 shows the good tracking performance achieved by the proposed controller. In this scenario, the mobile robot is represented by the black triangle and the moving target by a green ball. One can see that the desired eight-shaped trajectory is precisely tracked inside the constrained space. The applied control signals, which are inside the saturation limits, are depicted in Figure 4 for t ∈ [0, 75s]. Small oscillations of the steering angle are noticed during the start-up.  b. Second scenario: In this case, the WMR is required to follow a circle-shaped trajectory generated by the planner. The circleshaped trajectory is defined as follows The mobile robot has to navigate inside the restrained region; therefore, it is required to find the control vector ( ) to track the desired trajectory only inside the free space. Thus, the following constraints have been included in the optimization problem The tracking performance of the WMR under the control algorithm (12) is shown in Figure 5. It is observed that when the desired trajectory is inside the feasible region, the robot follows it precisely. However, when the reference trajectory is outside the desired region (unfeasible), the mobile robot navigates close to the border of the admissible region. Figure 6 illustrates the applied control signals. So, the linear speed and the steering angle are inside the saturation limits.
Here also oscillations are noticed at the beginning of the movement. It is shown in Figure 7 the tracking error converges to zero when the reference trajectory is inside the desired region and is different from zero when it is outside the desired region. Further, to show the effect of the feasibility of the desired trajectory, Figure 8 shows the tracking performance for two different cases: The desired trajectory is feasible, completely inside the restricted region (R=1m) and for the second case the desired trajectory is unfeasible, completely outside the restricted region (R=3m), but the state constraints are satisfied. Figure 8-a shows that the equilibrium point is the origin, and the tracking error is zero ( * = 0), whereas, in Figure 8-b, the equilibrium point is different from zero ( * ≠ 0).

B. EXECUTION TIME COMPARISON
The proposed NMPC formulation based on the approximation of NMPC (ANMPC) is compared against the processing time of the NLP formulation (4) and the algorithm that uses the CasADI tool to fasten the operation of NMPC [40]. The NLP problem, termed by NMPC in this comparison part, is solved using MATLAB "fmincon" solver with SQP. To perform a valid comparison, we have used these three algorithms, with the same sampling time, prediction horizon, and control parameters. Further, regulation to the same target in free space has been conducted for ∈ [0, 2 ]. During simulation, we have noticed that the initial guess of the algorithm affects the processing time for the first steps of the simulation. To this end, Monte Carlo simulation is utilized in these simulations with ten different runs. The initial vector guess is sampled from the normal distribution ℕ(0,1).   The simulation results are shown in Figure 9. As it is shown clearly that the proposed algorithm (ANMPC) achieves a better processing time (2.9 ms) per iteration. Table 1 resumes the mean and the standard deviation of the processing time per iteration for these three algorithms.
Observing this comparison demonstrates that the ANMPCbased approach is an attractive alternative algorithm to solve the NMPC problem efficiently.

C. EXPERIMENTAL RESULTS
In this second part, we consider the real-time implementation of the proposed algorithm on a real mobile robot platform. The experiment has been conducted using a TOSHIBA laptop with Intel Core i7 processors. The platform is the mobile robot Khepera III (Figure 10), which is a differential drive mobile robot [39]. Khepera platform is an automated differential drive guided mobile robot designed and equipped for autonomous tasks. It is provided with infrared, and Ultrasonic sensors for navigation, and encoders for localization. The distance between the left and right wheels is equal to L= 88.4mm. The diameter of the wheel is 41mm. The connection between the platform and the Laptop is established via Bluetooth communication technology. Since MATLAB functions support communication with devices via Bluetooth, the MATLAB software has been also used in this work to implement the proposed algorithm. The scenario of tracking a circular-shaped trajectory in constrained space is adopted in this experimental part. The reference trajectory is the same as the one used in the simulation part, with the following parameters  Because of the presence of noisy measurements, and also due to the inaccuracies that come from residual errors (quantization of the velocity controls), the constrained space used by the control algorithm is diminished with regards to the real constrained region (hard constraints). Consequently, for more safety, the soft constraints used in the algorithm are as follows:   Figure 11 displays the desired trajectory (circle), the restricted space region (square), and the trajectory of the mobile robot. The tracking of the reference trajectory inside the restricted space is accurate and when the desired trajectory is outside the restricted space, the mobile robot stays close to the border of the square. Figure 11 shows the obtained solution from the constrained optimization problem. Both controls signal, linear velocity, and steering velocity are inside the saturation limits. The command signals sent to WMR are the velocities of the right and left wheels ( , ), which are deduced from the following equation  Figure 12 illustrates the Cartesian positions of the mobile robot along the x-axis and y-axis. One can see clearly that the mobile robot navigates practically inside the restricted region and that the constrained optimization problem has forced the mobile robot to move inside the tolerated space, which is a square for this case.
Finally, we note that the main drawback of the proposed control algorithm is to find adequate tuning control parameters (h, Q, R, etc.) that meets the desired tracking performance of different tracking trajectories.

VI.CONCLUSION
In this work, the approximated nonlinear model predictive control has been applied to the autonomous navigation of a differential drive mobile robot in a constrained region. Euler approximation was used to overcome the nonconvex and nonlinear optimization problem in the control design. The stability of the closed-loop system has been provided using the Lyapunov method. The autonomous navigation of the mobile robot in the constrained region was formulated as a quadratic optimization problem or QP where both physical constraints (control vector) and space constraints were included in the design of the controller. Therefore, all constraints have been explicitly included in the optimization problem. Thus, the obtained constrained optimization problem is convex with the fast update and many numerical solutions exist that solve this kind of optimization problem efficiently. It was proven that the solution is feasible and the trajectory tracking error was bounded and may converge to the origin when the desired trajectory lies inside the feasible region.
The proposed method offered many advantages over existent nonlinear methods: simplicity, fast computation, and its ability to include different constraints in the controller design. Simulations and experimental results demonstrated the effectiveness of the proposed control algorithm and good tracking performances were achieved.
Future work will focus on the application of the proposed approach to three-dimensional vehicles for instance aerial unmanned vehicles.

Proof of Theorem 1
Let the initial solution * ( ) be feasible, and the equivalent optimal cost function (9) is given under the form of ( , ( ), * ( ), ℎ) at the time t. For the next step, we have The following equality is obtained It follows from the definition (1), the following inequality is obtained Consequently, the function ( , ) is negative. This inequality can be simplified if we take diagonal matrices as = 3 ; = 3 , and = 2 , then the previous inequality becomes > + ℎ 2 ( 2 +1) .