Closing the Loop – Predictive Lifted Newton Trajectory Tracking Algorithm

This paper introduces a predictive closed-loop trajectory tracking algorithm for nonlinear control systems that combines the Model Predictive Control (MPC) approach with the task priority Lifted Newton method. The optimal control problem within MPC is replaced by the open-loop trajectory tracking problem formulated as a constrained motion planning problem. Constraints reflect the distance in the task space between the system current output and the desired trajectory. The original constrained motion planning problem is replaced by an unconstrained one addressed in an extended control system representation, and solved with the task priority version of the Lifted Newton method. All other steps of the MPC scheme remains unchanged. Performance of the closed-loop predictive Lifted Newton trajectory tracking algorithm has been demonstrated with series of computer simulations for the kinematic car type platform.


Introduction
Typically, in the perspective of robotics, the trajectory tracking problem is considered as a feedback motion control task [21,23,24,36]. The trajectory tracking problem regarded as a planning problem, has been studied in [20] where the continuation method has been used for design of the so-called reproduction equation, and then plan a trajectory of the rolling ball in the task space. In [30], the Endogenous Configuration Space Approach (ECSA) has been applied to a continuous inverse problem, where the author has defined an instantaneous kinematics of a nonholonomic system equipped with a Jacobian inverse algorithm. The ECSA framework has been also used in [17] to formulate the trajectory tracking problem as an open-loop planning problem. The original motion planning through waypoints problem [18], has been expanded with the additional task responsible for dragging the tracking error to zero between wayponits. Formally, the trajectory tracking problem has been defined as a constrained motion planning problem [19], whereas constraints reflect the instantaneous distance between the system output and the desired trajectory. Constraints have been incorporated into the system through extending the system by extra state variables. As a result, the trajectory tracking problem has been made equivalent to an unconstrained motion planning problem formulated in the extended system, and solved with the task priority version of the Lifted Newton method [1,16]. In this context, the task priority Lifted Newton algorithm can be regarded as "multiple shooting" version of a ECSA Jacobian algorithm [31].
Due to lack of feedback in the approach presented in [17] , the evolution of the system's actual state is not considered in the solution of the tracking problem. As a consequence, in the presence of disturbances, model mismatch and uncertainties the quality of the resultant trajectory tracking may deteriorate dramatically. This makes the open-loop algorithm not suitable for a real-life scenarios. In order to "close the feedback loop", the well known Model Predictive Control (MPC) scheme [4,11,26,27] can be applied. In general, MPC refers to a class of iterative control algorithms that make use of the explicit process model to predict the future response of a system over a certain prediction horizon. At each iteration, based on the most recent measurements, the MPC attempts to solve a finite horizon open-loop optimal control problem subject to system dynamics and constraints involving states and controls. The first part of the optimized control trajectory defined over control horizon is implemented at the real system. Then, the whole procedure is repeated with a prediction and a control horizons moved one step forward. A good introduction to theoretical and practical issues associated with the MPC method can be found in [5,7,10,32,33]. Recently, thanks to increasing computation capabilities and novel efficient algorithms [15,29], the MPC is gaining popularity also in the field of robotics [9,13,14,22]. The combination of the predictive scheme along with ECSA has been discussed in [25], where the general algorithm of finding inverse kinematics for mobile manipulators has been presented.
An integration of the MPC scheme with the open-loop trajectory tracking algorithm [17] is straightforward. It consists of replacing a certain optimal control problem by a constrained motion planning problem which by definition is formulated over a finite time horizon [17]. At each MPC iteration, the most currently observed system state serves as an initial value in the planning problem. The problem is solved with the use of the task priority Lifted Newton algorithm. All other steps remain unchanged. Unfortunately, this modification entails the need to review a theoretical aspects of a closed-loop algorithm stability and robustness. These issues will be a subject of a further publication. In order to verify a theoretical concept, the closedloop algorithm will be tested by computer simulations. Nevertheless, we expect that the modified MPC will have similar properties as the standard moving horizon MPC.
The design of a robust stabilizing control laws for robotics systems is challenging mainly due to their complex nonlinear dynamics (eg. nonholonomic and underactuated systems), possible model uncertainties, disturbances, and physical constraints imposed on the system state and controls. Compared to the traditional techniques such as a Model-Based Control and an Adaptive Control, the Model Predictive Control can be considered as a general framework for a wide class of linear and nonlinear control systems, that offers robustness and good dynamic performance while ensuring operation within certain physical limits. Since the MPC tuning parameters are directly related to a certain cost function, it is relatively easy to achieve good performance, in contrast to the the traditional techniques for which the control laws are not intuitively obtained. The main drawback of MPC schemes is related to its computational complexity which limits application areas only to a sufficient slow or a simple dynamic systems.
To demonstrate differences between the optimizationbased MPC and the presented approach, let's consider as an example a problem of finding solution of systems of nonlinear equations. This problem can be solved using a class of the gradient based methods such as ECSA, or can be formulated as an unconstrained optimization problem and solved by applying optimization methods [12,28]. The ECSA is a Newton-type method, thus is local, assumes system of class C 1 , demonstrates a very fast local convergence rate, and a low computational complexity. Incorporation of constraints is intricate and involves an additional effort [19]. Optimization methods are global, but suffer from a local minima, assume system of class C 2 , demonstrate fast convergence rate at the expense of increased computational complexity. Incorporation of constraints of any type is straightforward. A more in-depth comparison of these two methods in the context of MPC, will be subject of further studies.
The organization of this paper is the following. Section 2 introduces the trajectory tracking problem formulation along with all necessary theoretical preliminaries. Section 3 presents derivation of the open-loop task priority Lifted Newton algorithm. The composition of the predictive scheme with open-loop trajectory tracking algorithm is discussed in Section 4. An application of the closedloop predictive algorithm to selected trajectory tracking problems is demonstrated in Section 5. The paper concludes with Section 6.

Problem Formulation
We shall examine a nonlinear control system with output q f (q, u), characterized by generalized coordinates q ∈ R n and velocitiesq ∈ R n , where f (q, u) and k(q) are continuously differentiable, y ∈ R r denotes a vector of task space coordinates, u ∈ R m denotes a control vector, and m ≤ n. Admissible control functions u(·) are chosen Lebesgue where t e > t b . It will be assumed that the state trajectory q(t) ϕ q 0 ,t u(·) exists for every initial state q 0 = q(0), and every control u(·) ∈ U .
The trajectory tracking problem consists in finding a control function u(·) driving the system (1) over a prescribed time interval [0, T t ], such that the system output trajectory y(t) is as close as possible to a given demanded task space trajectory y d (t) ∈ R r , thus where is a small positive number.
The formula (2) specifies a system trajectory, and can be regarded as a constraint imposed on the system behavior. Thus, the trajectory tracking problem can be defined as a constrained motion planning problem. To solve such a problem the idea presented in [19] can be adopted. This approach consists in replacing the constrained problem by an unconstrained one addressed in an extended control system representation. The constraint (2) can be included into the system (1) by adding a set of extra state variables x ∈ R r , where Theoretically, the proposed form of vectorial-constraints (4) can introduce unnecessary singularities at locations in the search space U , for which elements of the system task space trajectory y i (t) fully coincide with equivalent elements of the desired trajectory y di (t), resulting in This issue can be solved in two ways, either by including to g(q) only active constraints [8] or by the Jacobian regularization [19]. Practically, mainly because of control discretization, numerical integration and errors, such a situation is very rare. Now, assuming that x 0 x(t 0 ) = 0, the constrained motion planning problem becomes equivalent to the unconstrained problem of finding a control function u(·) in the extended system (3), such that where E q 0 ,x 0 ,T t u(·) is the end-point map of the extended system (3) and x(t) ψ x 0 ,t u(·) represents the flow of the extra state equation.

Open-Loop Algorithm
Following the idea presented in [17], the motion planning problem (5) will be solved using a task priority version of the Lifted Newton Method [1]. This method involves a number of intermediate variables that correspond to the original system's states s i q(t i ), i = 1 . . . N, defined at time grid spanned over a given time horizon In order to simplify an algorithm implementation, we assume fixed time intervals t i+1 − t i = τ , τ = T /N, but this assumption is not mandatory. Further on we will use the superscript to denote the interval to which a marked element belongs. Intermediate states can be grouped into a vector s (s 1 , . . . , s N ) ∈ R nN . Moreover, for a further notational simplicity, and only for this purpose, we define s 0 q(t 0 ) that refers to the original system's initial state, and should not be regarded as intermediate state. On the time grid (6) we perform a piecewise continuous control discretization denotes a collection of control parameters, and M is the length of each control series chosen in such a way that Mm ≥ n. As a result of discretization, the control space is represented by R mMN , thus u(t) depends only on a finite number of control parameters λ (λ 1 , . . . , λ N ) T .
The flow of the original system (1) is divided into a number of subsequent flows, calculated separately on each interval . This breaks the system flow and make it discontinuous -see Fig. 1. This is an inherent nature of all "multiple shooting" techniques including the Lifted Newton Method. The flow of the system preserves continuity, when the following relation holds where is a function that describes the evolution of states of the original system due to the applied controls. Each These functions define the distance of the system's output to selected waypoints defined along the desired trajectorysee Fig. 2.
To proceed further, similarly to Eq. 7, we divide the flow of the remaining part of the extended system (3) into a sequence of independent sub-flows Again, we can group these sub-flows, and define the function that describes the evolution of the extra state variables due to the applied controls where G 0,T : R mMN −→ R rN . This function reflects the constraints violation (4) at the end of each interval, as presented in Fig. 3. Any constraints violation g(q) = 0 drives the evolution of the flow G 0,T (λ) moving away extra state variables from 0.
The trajectory tracking problem consists in determining the control parameters λ and the intermediate states s so that the system's flow preserves continuity (8), task-space error (9) vanishes

and constraints are not violated
The original trajectory tracking problem has been transformed into two conjugated, finite dimensional, nonlinear root finding problems. The former, the primary task (11) reflects the motion through waypoints problem and reaching the terminal point, the later, the secondary task (12) maintains the trajectory tracking constraints. Task separation along with task priorities should ensure continuity of the system's flow even when the constraints are violated.
To solve these two conjugated problems (11) and (12) with distinct priorities the combination of the Newton method and the task priority approach [31] will be applied. A conventional Newton algorithm iterates from an initial guess s(0), λ(0) until either κ > κ max or a sum ||F s 0 ,T (s, λ)|| where κ max defines the maximum number of algorithm's iterations, T OL( ) is a total error tolerance, γ s and γ λ are damping factors, and ( s, λ) represents the Newton step. The algorithm's convergence can be improved when the dumping factors γ s and γ λ are calculated in each iteration basing on formulas presented in [3] where the tolerance γ > 0 is a design parameter of the algorithm. According to the task priority approach [31] calculation of the Newton step length involves using a Jacobian inverse along with the projection onto the Jacobian kernel where J F s 0 ,T (s, λ) represent the primary and the secondary task Jacobian, respectively, the symbol [·] # denotes a right Jacobian inverse, and X s 0 ,T (s, λ) is the projection onto ker J F s 0 ,T (s, λ), so , and I stands for the unit matrix. In accordance with [17], the part of the system (14) referring to the primary task can be decomposed into the following system of linear equations which after the transformation take the form where the index κ has been dropped for notational convenience, and the matrix M lower triangular, square and invertible [1]. Because the where W #P W T (WW T ) −1 stands for the Moore-Penrose pseudoinverse, X (I mMN − W #P W) denotes the projection onto ker W, and ζ ∈ R mMN is any element in the parametrized control space. This element will be determined by the secondary task (12), with the use of the Jacobian inverse For the secondary task, Eq. 16 needs to be satisfied only within the kernel of W #P , so by projecting (16) onto ker W, we end up with assuming that by definition the projection is idempotent XX = X, and XW #P = 0. Finally, the formula for calculating the Newton step, including two tasks with different priorities has the following form For more technical details on the structure of the matrices and ∂e(s) ∂s , please refer to [17]. Beside ∂e(s) ∂s , which can be calculated analytically, the derivatives of the remaining matrices have to be calculated numerically, using e.g. the forward sensitivity analysis method [2]. To this aim, two additional systems of differential equationṡ and The sensitivity of the system (3) to initial conditions is calculated as while the sensitivity to control parameters The algorithm presented in Fig. 4 summarizes the described Lifted Newton procedure with task priorities.
The most time-consuming part of the algorithm is related to solving at each algorithm's step, a set of ordinary differential Eqs. 3, 18, and 19. Fortunately, these equations are not dependent across the time intervals, thus can be solved concurrently. In computation it is recommended to set initial values of the intermediate state s i so that Typically, the initial values of the control parameters λ 0 should be chosen sufficiently small, not too far from the solution of the problem (5).

Closing the Loop
By design, the algorithm derived in the previous section is an open-loop trajectory tracking algorithm. It solves the tracking problem on entire time horizon without any feedback from the system. In the presence of any internal or external disturbances the final tracking quality may by significantly reduced, what makes the open-loop algorithm unsuitable for practical applications. In order to overcome these difficulties, the model predictive control method will be adopted. Originally, it is an iterative, a feedback control technique based on the online solution of a finite horizon open-loop optimal control problem subject to system dynamics and constraints. The basic principle of the predictive scheme is illustrated in Fig. 5, it may be summarized as follows. At each control step, the most currently observed system state serves as the initial value in the optimization problem. The problem is solved on a predefined interval called the prediction horizon T p . Only a part of the resulting solution is applied to the system over a chosen one-step control horizon T c . In the next step, the whole procedure is repeated with the one-step control and prediction horizons moving forward and new measured initial state.
In the proposed approach, instead of solving a certain optimization problem, the root finding problem (5) will be solved using the open-loop trajectory tracking algorithm presented in Section 3. All other steps remain unchanged. The general procedure of the closed-loop predictive control algorithm equipped with the Lifted Newton method is summarized in Fig. 6.
The algorithm iterates over an interval [0, T t ] on which the desired trajectory y d (t) is defined. An acquisition process of the most recent system state q 0 is represented Fig. 5 Principle of the predictive scheme by the getMeasure() function (line 5). Typically, such a function returns a state estimate with a noise, not a real state of the system. In the next step, the Lifted Newton algorithm solves a finite time trajectory tracking problem defined on the prediction horizon T p (line 6). Using the resulting control parameters λ, only the first part covering the control horizon T c of the control signals u(·) is calculated and applied to the original system (line 7). Finally, the algorithm repeats the whole procedure moving the time horizon one step forward (line 8), and using the new values of λ and s. In case of the piecewise constant control discretication, a control parameters vector λ may by updated in each iteration, in such a way to discard parameters associated with first interval T c , switch others λ one interval T c left, and fill the last missing interval with values taken from a previous one.
The procedure presented in Fig. 6 does not include any optimizations which are required for a real-time application [6]. Mainly, it turns out that the computations of the new values of the control signals can largely be prepared without knowledge of the value of actual system state. Thus assumption can be made that the approximation of the feedback control is instantly available at the time that actual state is known. However, after feedback has been delivered, the full computing time is needed to prepare the next iteration. The procedure presented in the Fig. 6 will be used only to verify a theoretical idea. The performance of the closed-loop algorithm will be tested with computer simulations. The formal proof of the algorithm stability and robustness will be a subject of a further research.

Computer Simulations
In this section we shall present results of a series of computer simulations that have been performed in order to verify the performance of the closed-loop predictive trajectory tracking algorithm based on the task priority Lifted Newton method. For comparison, along with the presented algorithm we have tested a traditional model  predictive controller provided by the ACADO toolkit [15]. Both algorithms have been applied to several trajectory tracking problems of the kinematic car type platform. The kinematic car is shown in Fig. 7, its motion can be described by the following control system reflecting the exclusion of the side-slip of the platform ⎧ ⎨ ⎩q 1 = u 1 cos q 3 cos q 4 ,q 2 = u 1 sin q 3 cos q 4 , where (q 1 , q 2 ) denote the robot position in the XY plane, q 3 is the platform orientation, and q 4 represents the front wheels direction angle. The output function selects the part of the state that reflects the platform position and orientation on the plane, therefore the discussed problems will be defined with respect to these platform coordinates. The desired trajectory is defined over the horizon T t = 2π s as a Lissajous curve with ratio 0.5 ⎧ ⎨ ⎩ y d1 = 5 sin(t + π 2 ), y d2 = 5 sin(2t), y d3 = arctan 2 10 cos(2t), 5 cos(t + π 2 ) + r(t), where r(t) represents a regularization function that makes orientation component continous  Simulation results in which a uniform random noise q noise has been added to the state feedback signal are presented in Figs. 14 and 15.
The order of the plots is the following. The first plot presents the resulting platform paths corresponding to the solution of the tracking problem. The continuous line represents the real path of the platform, the dashed line represents the desired trajectory. The neighbor plot, illustrates the tracking error components: the platform Obtained results have shown that the derived predictive algorithm is able to track a trajectory in task space very efficiently. This is manifested by a low tracking error, as  shows that the moving horizon of the predictive algorithm significantly disturbs convergence of the primary task that is responsible for maintaining continuity of the system flow.
In each algorithm step, waypoints are moving forward along with the desired trajectory, which results in an increase in the distance between waypoints and respective intermediate states. Therefore, above a certain number of intermediate states, the increase in task space error may exceed the rate of convergence of the control algorithm, resulting in losing stability. In this case the only remedy is to perform more solver steps in each iteration. It is worth stressing that the trajectory tracking task has a lower priority, thus according to the task priority approach it operates inside the Jacobian kernel of the primary task. The kernel dimension depends directly on the dimension of the control parametrization M. For greater M, the root finding algorithm with priorities has more degrees of freedom whereby provides a better solution.
In case of the NMPC algorithm, the trajectory tracking problem is formulated in terms of the following optimal control problem min u(·) (22) subject to Eq. 21, where weight matrices P = 100 · I 3 , and Q = 0.005 · I 2 . Control signals have the same form of piecewise constant signals, with the same initial values, and the control interval T c as in the ECSA algorithm. The problem (22) has been implemented in C++ language [35] with use of the master branch of the ACADO Toolkit (the short hash commit 21be39c), and the g++ v5 In each case, the following condition holds T c = T p N . The plots layout is the same as before.
Comparison of the ECSA and the NMPC algorithms for the same simulation conditions (T p , T c , κ max ) in the ideal case without state disturbances, shows that the NMPC behaves better than the ECSA algorithm. The NMPC algorithm convergences faster, and generates smother trajectories (ECSA 1 vs NMPC 1, and ECSA 2 vs NMPC 2). Due to the nature of the ECSA algorithm, that defines the trajectory tracking task as a low priority, secondary task, its resulting performance may be slightly worse than the NMPC algorithm. The deterioration of the trajectory tracking quality in the initial simulation phase of the NMPC 2, when the control algorithm performs two optimization steps in each iteration, is unexpected and counterintuitive. In this case the resulted trajectory starts to resemble the ECSA 2 trajectory, what raise the question about the optimality of the ECSA solution. This will be the subject of further investigation. In the initial phase of simulation, the NMPC control signals have a larger amplitude than the ECSA, mainly due to a faster convergence and lack of a predefined control constraints. Both algorithms, when reach the desired trajectory, behave similar and generate a comparable control signals. Interesting results are given by simulations that include feedback noise. A relatively low, uniform random noise q noise = ±(0.1m, 0.1m, 5 • , 0 • ) only slightly affects the resulted trajectories and control signals generated by both algorithms (ECSA 7 and NMPC 3). The noise propagation can be observed on the error and control plots. Both algorithms are stable, and convergent fast, however ECSA algorithm seems to produce less noisy signals, especially e 2 and u 2 . In the presence of larger noise q noise = ±(0.5m, 0.5m, 10 • , 0 • ), this observation becomes more evident -compare ECSA 8 and NMPC 4. The ECSA behaves much better than NMPC, the trajectory is smother, and the error and control signals are much less noisy in the amplitude and frequency range. The ECSA algorithm can be considered as a low pass filter for feedback noise, due to its slower convergence rate than the NMPC algorithm.
Since all simulations have been carried out without any control constraints, the control signals have significant amplitudes in the initial phase of the platform movement, when tracking error is relatively large. Such controls can be difficult to implement in a real object. To make resulting controls closer to practical applications, the constraints can be included using the approach presented in [19]. This will be a subject of further research.

Summary
Following the MPC approach, a predictive closed-loop trajectory tracking algorithm has been presented. The trajectory tracking problem has been formulated as two conjugated root finding problems. In each iteration of the MPC scheme, the solution of the tracking problem is provided by the open-loop task priority Lifted Newton method. The performance of the closed-loop algorithm has been tested with computer simulations. The computer simulations have shown that the presented algorithm is able to track desired trajectory very efficiently even in the presence of a relatively large state feedback noise. This is manifested by a very good algorithm's convergence, stability and robustness. The tracking quality and stability of the presented predictive algorithm can be improved in the very similar way as with a traditional MPC, either by extending the prediction horizon or by increasing the number of solver steps executed in each iteration. Due to the properties of the Lifted Newton algorithm, it is better to increase the dimension of the control parametrization M than number of intermediate states N, so the algorithm has more degrees of freedom whereby provides a better solution. Comparison of the ECSA with the NMPC algorithm shows that in close proximity to the desired trajectory both algorithms behaves similar, however the NMPC converges faster at the expense of larger initial control signals values, and greater sensitivity to the state disturbances. The ECSA algorithm seems to be more robust to feedback noise, and in the presence of control constraints may show equivalent convergence rate.
As a further research we plan to develop a formal proof of the closed-loop algorithm's stability and robustness, and implement the real-time iteration scheme. The presented algorithm is open to further improvements. A more general framework can be defined that involves multiple tasks with different priorities e.g. tracking different trajectories by subsystems, respecting the state and control bounds, and minimizing the control energy.