Je tiens à remercier Philippe, mon Directeur de Thèse, qui m'a guidé, surtout au cours des derniers mois, et a su me fournir de précieux commentaires lors de la rédaction de ce manuscrit malgré ma tendance à tout terminer à la dernière minute. Je remercie Olivier, mon co-superviseur ainsi que gourou du codage, pour son aide pendant ces trois années, tant sur le plan théorique que pratique. Un grand merci aussi à Guillaume, qui m'a initié au sujet du contrôle prédictif paramétré.

Je remercie les rapporteurs, qui ont accepté de relire mon manuscrit et m'ont envoyé des retours très précieux malgré les complications dues á la pandémie et au nouvel confinement, ainsi que les examinateurs, qui ont montré de l'intérêt pour mon travail.

Je remercie tous les membres de l'équipe ARMEN, qui ont tous joué un rôle important dans mon développement personnel et professionnel. In particular, I want to thank all PhD students from the team, with whom I have had countless experiences, from serious discussions related to our researches to friendly aperitifs at the bar. A special thank goes to my office desk mate, Zhongmou: his commitment to reminding me to drink water, day after day, prevented my death from dehydration.

I thank all my friends, met during this long experience in Nantes, who made my staying worth the effort. E tra questi, ringrazio in particolare i "kuki": Gino, Simo e Pupi.

Infine, il ringraziamento più grande va alla mia famiglia. Questi ultimi anni sono stati lunghi e difficili, in particolare gli ultimi mesi, e diverse volte mi sono chiesto che senso avesse continuare. Ma nonostante tutto, siete sempre rimasti al mio fianco, incoraggiandomi ad andare fino in fondo. Se sono arrivato fin qui, lo devo in gran parte a voi. 5 1. https://atlanstic2020.fr/ Chapter 1 presents an overview of classical visual servoing modeling and Image Based Visual Servoing (IBVS), followed by details on few more advanced schemes that try to enhance the quality of the sensor motions and to ensure satisfaction of generic constraints along the path. The following chapter introduces second-order models, discussing their relation with the dynamics of the robot and detailing how feedback controllers can be defined on top of them. An brief review of MPC is then presented in Chapter 3, with a focus on predictive strategies applied to visual servoing, and the original work that combines MPC and second-order models is then detailed. Chapter 4 provides the fundamental ideas of "control parameterization" for MPC applications. It also describes several simulations that were performed to validate the concept. Finally, the general conclusions are reported, as well as the perspective for future works and improvements.

INTRODUCTION

In order to manufacture an item, several operations are generally performed in succession, such as drilling, cutting, welding, assembly. Some of these tasks can be executed by automatic machines, possibly with several repetitions per second. However, such machines lack of flexibility, in the sense that they are usually designed to accomplish a very specific task in controlled conditions. As an example, components might need to be precisely positioned on a conveyor belt in order for a machine to properly perform its task. On the other hand, a human operator offers much more flexibility. A badly positioned component does not pose major issues for a human worker, who can, in addition, perform a variety of tasks according to the needs of the production line. Nonetheless, human operators are inadequate for the execution of tasks that require millimeter (or even micrometer) precision, and in addition they can experience health issues such as repetitive motion injuries due to execution of similar tasks over and over.

The use of robotic systems can improve the productivity since, like automatic machines, they can execute tasks with high repeatability and reliability. However, they can also be reprogrammed, and therefore, like human operators, can be assigned to different tasks depending on the production needs. Nonetheless, their productivity can still be limited. As mentioned before, one of the main advantages of having humans perform a given operation is their ability to adapt to mutable contexts, in which, as an example, objects might not be precisely positioned on a conveyor. To provide the same level of robustness and flexibility, robotized manufacturing systems must be equipped not only with proprioceptive sensors, such as motor encoders used in low-level joints control, but also with exteroceptive ones, allowing to gather information about both the objects to interact with and the surrounding environment. These additional sensors often correspond to vision systems composed of one or more cameras, allowing to gather information about the relative position between a target object and the actuated robot. Visual signals can then be used to evaluate control inputs that are robust with respect to several uncertainties, such as modeling and calibration errors, ultimately allowing an arm to precisely position itself with respect to the observed object.

Unfortunately, the motion that results from such control strategies is generally slow, due to a combination of factors. As an example, images are affected by motion blur, making object detection and tracking harder and less reliable at higher speeds. Furthermore, the models that describe the interaction between the sensing and sensed systems are often formulated at a kinematic level, considering at most velocity information. With faster motions, however, the accelerations and the effects due to the dynamics become nonnegligible and should be considered in the design of robot controllers by means of new, richer models. Even if these have not yet been used extensively in the research or industrial domain, few existing works suggest that they are indeed a fundamental ingredient required to attain high-speed performances. More specifically, the studies by Dahmouche [START_REF] Dahmouche | Dynamic visual servoing from sequential regions of interest acquisition[END_REF] and Ozgur [START_REF] Ozgur | High speed parallel kinematic manipulator state estimation from legs observation[END_REF] showed that including this "second-order" information to describe the evolution of the visual features allows to obtain a simultaneous estimation of the pose and velocity of the platform of parallel manipulators moving at high speed. Additional investigations in this field were conducted by Vandernotte [START_REF] Vandernotte | Dynamic sensor-based control[END_REF], who used acceleration models to establish a direct connection between vision and robot dynamics.

As for the problem of controlling the robot, almost all works that investigated the use of this new type of interaction focused on the use of feedback linearization techniques, usually in the form of a proportional and derivative control [START_REF] Dahmouche | Dynamic visual servoing from sequential regions of interest acquisition[END_REF][START_REF] Keshmiri | Augmented Imaged Based Visual Servoing controller for a 6 DOF manipulator using acceleration command[END_REF]. While this ensures desirable properties such as robustness with respect to calibration errors, it also presents some drawbacks. Just to provide some examples, motions can be longer than necessary, there are no guarantees of maintaining object visibility, joint limits could be violated. To deal with these issues, Model Predictive Control (MPC) can be employed [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF]. Not only it can take into account constraints imposed on the system, but it can also guide the robot through a path that is shorter, thus reducing the time to completion of a task. These concepts are just some of the topics investigated by the PROMPT (Performances de la Robotique Manufacturière en Perception de Tâche) project, of which this thesis is part. It is one of the projects funded by RFI Atlanstic 2020 1 , and its overall goal is to define a set of new methodologies which allow to enhance the execution speed of sensor-based servoing tasks. This can be achieved by considering multiple, parallel, improvements in different fields. First of all, more advanced perception algorithms can be used to fusion altogether data gathered by different sensors, both exteroceptive and proprioceptive ones. As an example, information relative to an observed object can be acquired via a perspective camera, while an Inertial Measurement Unit measures the velocity and acceleration of a moving sensor. Using filtering techniques, it is possible to better estimate the relative motion of the two entities. Moreover, by optimally placing the sensors within the environment and on the controlled system, it is possible to enhance the reliability of the estimation. Another mean to reduce the execution time of a task is the use of better models that provide a richer information about the evolution of the system with respect to its environment, in particular, allowing to properly express the dynamics relating them. Finally, advanced control laws should be considered that are capable of guiding the system from an initial to a desired configuration by following a better path and in a shorter amount of time.

The work presented in this manuscript focuses on these last topics. In particular, it studies the second-order interaction of eye-in-hand robotic configurations, potentially for any type of feature, and compares it to more classical strategies. Furthermore, it builds, on top of the acceleration models, advanced control schemes based on the MPC formulation. The goal is to use the acceleration information to define better predictors that can more adequately steer the sensor towards the desired configuration. The approach is validated both in simulation and with real experiments, and for different types of features. Finally, a last contribution aims at reducing the computational burden of the nonlinear optimization involved in predictive strategies, which in some cases might prevent real-time feasibility. It is based on the work by Alamir [START_REF] Alamir | Stabilization of nonlinear systems using receding-horizon control schemes: a parametrized approach for fast systems[END_REF] on input parameterizations, and can effectively reduce the time needed to evaluate control commands. A test involving a visual trajectory tracking task is proposed to underline the appeal of this approach.

Chapter 1

VISUAL SERVOING

Visual Servoing is a robust control technique that allows precise positioning of a sensor with respect to an observed object. It is robust to modeling errors and features a large convergence domain thanks to the use of sensor measurements directly in the control loop.

The positioning task is achieved by actively regulating a set of descriptive features, which are extracted from the visual information. The key idea is that when the value of these features is changed, the relative pose of the sensor with respect to the object has to change accordingly. More specifically, the positioning task can be achieved by regulating the features to what the sensor would measure when properly positioned in front of the object, as sketched in Figure 1.1.

Several servoing schemes can be found in the literature, each one characterized by different kind of features and different control laws. In particular, a distinction is commonly made between 2D features, that can be extracted directly from images, and 3D ones, which need to be reconstructed from sensor measurements [START_REF] Wilson | Relative end-effector control using cartesian position based visual servoing[END_REF][START_REF] Benoit Thuilot | Position based visual servoing: Keeping the object in the field of vision[END_REF]. Schemes that use the first set of features are commonly referred to as Image Based Visual Servoing (IBVS), while those based on 3D information are named Position Based Visual Servoing (PBVS). Hybrid approaches are also possible, in which both 2D and 3D features are used at the same time. Despite these differences, all schemes exploit the same formalism based on the interaction matrix, which provides a connection between the motion of a sensor and the resulting changes of the features. Using these models, a feedback control law can be established that steers the features to the desired configuration, and consequently moving the sensor in the proper location.

Simulations and experiments included in this thesis used only 2D features, and for this reason the second section of this chapter briefly illustrates IBVS control, focusing on the case in which normalized image points are used as features. More advanced approaches are then discussed, tackling the issues that come from the basic approach. These approaches attempt to solve some well-known issues arising from the use of normalized coordinates. The goal is to obtain better sensor motions in the operational space, which represents Moving the camera from one pose to the other is equivalent to moving the corners of the projected shape from one image to the other.

indirectly a way to shorten motions and thus speedup task execution.

Finally, some works that focused on servoing under constraints are presented. Typical constraints that are to be enforced are the visibility of the observed object and keeping the joints within their mechanical limits, which were considered in the original contribution presented in Chapter 3.

Modeling and control

The Interaction Matrix

In order to achieve the task of regulating the relative position between a sensor and an observed object, a set of descriptive features is firstly selected. Their values is gathered in a vector, denoted as s ∈ R ns , which can be extracted directly or by reconstruction from the measurements acquired by one or more sensors. The goal of the servoing task then becomes to regulate the value of the features to a desired one, denoted as s . For this purpose, it is first of all necessary to describe how the feature vector evolves as a function of the motion of the sensor and of the object. The most common approach in this sense is to use a kinematic relation that expresses the time derivative of the features, ṡ, in function of the relative sensor-object motion: ṡ = L s (s, z) v

(1.1)

The matrix L s (s, z) ∈ R ns×6 is known in the literature as interaction matrix, and is in general a function of the features themselves and of a set of n z parameters z. To shorten the notation in the sequel, the dependency on s and z will be implicitly assumed, and only L s will be written unless needed otherwise.

The vector v in (1.1) represents a kinematic screw, or twist, describing the instantaneous motion of the sensor frame F c relative to the object frame F o . In practice, it consists in the concatenation of both the linear and angular velocities (v and ω respectively) of the sensor with respect to the object, and expressed in the coordinate system of the sensor itself.

One interesting aspect of this formulation is that it unifies multiple setups at once, since in the kinematic relation defined above it is not specified which frame is the moving one. When the observed object is stationary and the features change due to the motion of the sensor, the setup is commonly named eye-in-hand configuration. In this case, the relative twist can be expressed as: (1.2) wherein v w c is the twist of the sensor with respect to a fixed "world" frame F w , while R w c ∈ SO(3) is the rotation matrix representing the orientation of F c with respect to F w . Conversely, a eye-to-hand configuration is obtained whenever a fixed sensor observes a moving object. In this case, denoting with t c o/c the translation vector from the origin of F c to that of F o and expressed in the sensor frame, the expression of the kinematic screw v writes as: It is also possible to consider hybrid cases with multiple sensors in different configurations [START_REF] Flandin | Eye-in-hand/eyeto-hand cooperation for visual servoing[END_REF][START_REF] Lippiello | Eye-in-hand/eyeto-hand multi-camera visual servoing[END_REF], or in which both the sensor and the object are moving [START_REF] Murao | Predictive Visual Feedback Control with Eye-in/to-Hand Configuration via Stabilizing Receding Horizon Approach[END_REF]. However, it usually the case that only one of the two motions is caused by the actuation of the robotic system, while the other is assumed to be a disturbance. In these cases, it is rather common to rewrite the interaction model as:

ṡ = L s v + ∂s ∂t (1.4)
wherein v corresponds to the relative kinematic screw that would be obtained solely due to the actuation. The second term takes into account the contribution due to the uncontrolled motion, and usually has to be estimated and compensated for [BC95; [START_REF] Ginhoux | Active filtering of physiological motion in robotized surgery using predictive control[END_REF][START_REF] Shahriari | Robotic visual servoing of moving targets[END_REF].

The interaction model (1.1) provides a link between the rate of change and the relative sensor motion in Cartesian space. However, it can also be rewritten in terms of the generalized coordinates of the controlled system. In particular, for a serial kinematic chain featuring n q joints, the time derivative of s can be expressed as a function of the joint velocities q ∈ R nq : ṡ = J s q (1.5)

J s is sometimes referred to as feature Jacobian [START_REF] Chaumette | Visual servo control. II. Advanced approaches[END_REF], and takes a different form depending on the type of servoing configuration:

J s = L s T c w J (eye-in-hand) (1.6a) J s = -L s V c o T c w J (eye-to-hand) (1.6b)
In both cases, J is the robot Jacobian providing the end-effector velocity expressed in the base frame of the robot [START_REF] Khalil | Modeling, identification and control of robots[END_REF], and is a function of the current joint coordinates q. Moreover, it is assumed that F w coincides with the base of the robot, while the endeffector frame F ee coincides with F c in the case of an eye-in-hand configuration, and with

F o for a eye-to-hand setup.

Control via Visual Feedback

To control the relative positioning of the sensor with respect to an object it suffices to regulate the values of the descriptive features to a given desired configuration s . This is commonly achieved using the task function formalism [START_REF] Samson | Application of the task-function approach to sensor-based control of robot manipulators[END_REF], in which the objective is to regulate the feature error e s . = ss (1.7) to zero. The most common approach in this sense is to attempt to enforce an exponential decay of the error by imposing the desired profile ės = -λe s , in which λ is a positive proportional gain. Considering (1.5), and assuming the joint velocity of the manipulator to be the control input of the system, the problem is thus equivalent to solve the linear system J s q = ṡ -λe s (1.8) in which ṡ provides a feed-forward derivative term, which can be assumed null if a fixed configuration has to be attained. The system admits a unique solution in q only if J s is squared and non-singular, which is not necessarily always true, and thus only an approximate solution will be possible in general. The system is thus solved in the least squares sense, obtaining the following proportional control law: q = J + s ( ṡ -λe s ) (1.9) where J + s represents an estimation of the pseudo-inverse of the feature Jacobian. This control law can be proven locally stable in a neighborhood of s [START_REF] Fran | Visual servo control. I. Basic approaches[END_REF] given that a sufficiently good approximation of J s is used. There are different factors that can influence the stability of such control law, such as geometric calibration errors, and uncertainty in the parameters z. Nonetheless, the advantage of visual servoing is that it is rather robust to such modeling uncertainties, and a coarse estimation of the parameters is often sufficient to ensure local stability.

Image-Based Visual Servoing

IBVS schemes focus on the case of features that can be extracted directly from image measurements, as opposed to PBVS [WHB96; Thu+02] and hybrid approaches [START_REF] Malis | 2-1/2-D visual servoing[END_REF][START_REF] Malis | Theoretical improvements in the stability analysis of a new class of model-free visual servoing methods[END_REF] in which at least some of the features need to be reconstructed via pose estimation algorithms [START_REF] Daniel | Model-based object pose in 25 lines of code[END_REF][START_REF] Comport | Real-time markerless tracking for augmented reality: The virtual visual servoing framework[END_REF]. One of the advantages of regulating quantities that come directly from the vision sensor is thus that little if no knowledge at all of the observed object is required.

Arguably the most common servoing scheme is based on the use of normalized points as features, even though other geometric primitives can be used such as lines or ellipses [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF][START_REF] Chaumette | Classification and realization of the different vision-based tasks[END_REF]. Given a set of 3D points, each with coordinates X i , Y i and Z i expressed in the sensor frame F c , their normalized coordinates can be used as features, i.e.,

s =            x 1 y 1 x 2 y 2 • • •            =            X 1 /Z 1 Y 1 /Z 1 X 2 /Z 2 Y 2 /Z 2 • • •           
(1.10)

The extraction and tracking of these coordinates from an image can be achieved using several well-established techniques, such as those based on SIFT (Scale Invariant Feature Transform) [START_REF] David | Object recognition from local scale-invariant features[END_REF] or SURF (Speeded Up Robust Features) [START_REF] Bay | SURF: Speeded up robust features[END_REF] descriptors, and a conversion from pixel to normalized coordinates can be obtained from the sensor projection model.

The interaction matrix of the i-th point can be obtained easily using the law of composition of velocities from classical mechanics, and takes the following well known form:

L s i =   -1 Z i 0 x i Z i x i y i -x 2 i -1 y i 0 -1 Z i y i Z i y 2 i + 1 -x i y i -x i  
(1.11) while the interaction matrix of the whole set of points is obtained by vertically stacking the individual matrices. The vector of parameters z consists in this case in the collection of the depths Z i of each point. These values need to be estimated [START_REF] De Luca | Online estimation of feature depth for image-based visual servoing schemes[END_REF][START_REF] Spica | A framework for active estimation: Application to structure from motion[END_REF] or approximated, as an example using the depths at the desired configuration, i.e., Z i = Z i . Finally, at least four points should be used [START_REF] Michel | Singularities in the determination of the situation of a robot effector from the perspective view of 3 points[END_REF] to avoid singularities in the interaction matrix.

Schemes based on image points as features are simple to implement, but can lead to unsatisfactory results due to the structure of the interaction matrix. First of all, since the total number of feature coordinates n s is usually at least 8, there exist configurations known as local minima [START_REF] Chaumette | Potential problems of stability and convergence in image-based and position-based visual servoing[END_REF], in which the proportional control law presented before leads to v = 0 despite having e s = 0. Furthermore, the components of the relative twist v are not decoupled. This fact is particularly important when the sensor has to perform a large rotation in order to reach the desired configuration. Due to the high coupling of the degrees of freedom, the motion of the sensor presents not only the desired rotation, but also an unwanted translation, usually in the form of a possibly large retraction from the observed object. This is obviously an issue when the sensor is attached on the endeffector of a manipulator, since the tip of the serial chain can only move within a limited workspace.

Features with higher decoupling

One way to solve the issues that appear when using image points is to consider alternative features which provide better decoupling. In particular, it would be desirable to find a set of features partitioned as:

  ṡ1 ṡ2   =   L 1 L 2   v (1.12)
whose interaction matrices have orthogonal rows, i.e., L 1 L T 2 = O. From this orthogonality condition, it would follow that the proportional control law would be written in the form

v = -λL + 1 e 1 v 1 -λL + 2 e 2 v 2
(1.13) in which v 1 ⊥ v 2 . In other words, each of the two features would govern a selected number of degrees of freedom, and the two components would not influence each other. This is the principle at the base of partitioned visual servoing, in which the goal is usually to isolate the motions related to the focal axis of the camera or to decouple translational and rotational motions. More commonly, only a partial partitioning is used, in which the interaction model writes as

  ṡa ṡb   =   L a L ab L b     v a v b   (1.14)
in which v a and v b represent a partition of the coordinates of v (not necessarily with the same ordering). As a consequence of this specific structure, the value of v a is dependent solely on that of e a , while that of v b is evaluated as

v b = -L + b (-λe b + L ab v a ) (1.15)
and thus depends not only on e b but also on e a , with the term L ab v a acting as a disturbance compensation. This approach is similar to that of 2.5D visual servoing [START_REF] Malis | 2-1/2-D visual servoing[END_REF], in which the rotational motion is decoupled from the translational one. However, partitioned IBVS schemes are based solely on image measurements and no 3D reconstruction is needed.

As an example, in [START_REF] Corke | A new partitioned approach to image-based visual servo control[END_REF] two features are introduced in addition to image point coordinates. The first one, being the orientation of a line joining two of the observed points, is strictly linked to the rotational motion around the optical axis. The second feature is related to the area of the polygon defined by the image points, which is invariant to translation and to rotation around the focal axis. Instead, it changes mainly with the distance of the sensor from the points, and is thus closely linked to the translation.

Another way to solve the issues that appear when using image points is to consider alternative features which provide different motions in the Cartesian space. As a first example, polar coordinates can be used [START_REF] Iwatsuki | A new formulation of visual servoing based on cylindrical coordinate system[END_REF], in which the interaction matrix of the i-th point is given by

L s i =   -cos θ i Z i -sin θ i Z i ρ i Z i (ρ 2 i + 1) sin θ i -(ρ 2 i + 1) cos θ i 0 sin θ i Z i ρ i -cos θ i Z i ρ i 0 cos θ i ρ i sin θ i ρ i -1   (1.16)
with ρ i = x 2 i + y 2 i and θ i = atan2 (y i , x i ). The advantage of this formulation is that when large rotations around the optical axis are to be performed, retraction issues are not present since the translation along the focal axis is now decoupled from the rotation normal to the plane (third and last columns of the interaction matrix).

Still with the objective of determining a set of features which guarantees good decoupling properties, planar image moments can also be considered [START_REF] Chaumette | Image Moments: A General and Useful Set of Features for Visual Servoing[END_REF][START_REF] Tahri | Point-based and region-based image moments for visual servoing of planar objects[END_REF]. Given an object whose projection on the image plane is described by the dense set O, its moments can be defined as:

m i/j = O x i y j dx dy
(1.17)

Similarly, for a finite set of N points, the discrete moments of order i + j are expressed as:

m i/j = N k=1
x i k y j k

(1.18)

The interaction matrix can be obtained after differentiation by considering the feature model of each point (x k , y k ). In addition, under the planar assumption it is possible to express the inverse depth as a function of the image coordinates as in 1 Z k = Ax k + By k + C, wherein A, B and C are the parameters that characterize the 3D plane to which the observed shape belongs, which can be estimated online [START_REF] Spica | Plane estimation by active vision from point features and image moments[END_REF] or otherwise approximated. On top of these features, more complex ones can be obtained, such as a normalized area of the shape or the main axis of orientation. Interestingly, it was shown that when the object plane is parallel to the projection plane of the sensor (A = B = 0) it is possible to build a feature set based on moments whose interaction matrix features good decoupling properties, in which each feature almost controls a separate degree of freedom and thus leading to satisfactory sensor motions in 3D.

Efficient Second Order Minimization

The retraction issues obtained when servoing from image points can be explained by the highly nonlinear map between the operational and sensor space. In particular, the proportional control law (1.9) will attempt to move each feature along a straight line from the initial to the target position. Despite this being the shortest path in the feature space, it can lead to relative motions of the sensor in SE(3) which do not necessarily correspond to the geodesic connecting the initial and goal poses. A nice explanation for this issue was provided by [START_REF] Malis | Improving vision-based control using efficient second-order minimization techniques[END_REF], which compares different control strategies to minimization algorithms. More specifically, the proportional control law based on the pseudo-inverse of the interaction matrix was compared to the Gauss-Newton minimization algorithm, which provides good performances in presence of mild nonlinearities or in proximity of the desired configuration.

Additionally, still based on the parallelism control/minimization, new schemes were proposed with the objective of reaching the desired configuration by following a shorter path. They consist in using, inside the proportional control law (1.9), one of the following:

J + s = Ĵs + + Ĵs + 2 (1.19a) J + s = Ĵs + Ĵs 2 + (1.19b)
in which Ĵs represents an estimation of the feature Jacobian evaluated from the current measured features, while Ĵs is calculated by replacing the desired features in the interaction matrix.

A similar philosophy led to the development of a Hessian-based control scheme [START_REF] Lapresté | A hessian approach to visual servoing[END_REF], while an extension of (1.19b) was proposed in [START_REF] Marey | Analysis of classical and new visual servoing control laws[END_REF]. Finally, in [TM08; TM10] some limitations of the approaches proposed in [START_REF] Malis | Improving vision-based control using efficient second-order minimization techniques[END_REF] were discussed, in particular focusing on the case of large translations and rotations. In particular, it was shown that the control laws have to be adjusted by integrating information related to the displacement to be performed by the sensor from the current to the desired location.

Dealing with Constraints

Weighted control

One first way that can be used to deal with constraints is to consider them as additional tasks in the control law. As an example, if visibility of an image point is to be ensured, the task could consist in moving the feature towards the center of the image. More generically, the idea is to add a task in the form J c q = ėc in addition to the original servoing task (1.8). It must be noted that this additional task cannot be permanently active since it could otherwise interfere with the main one, e.g., by introducing local minima even when the constraint would be satisfied at the goal configuration. One solution is thus to dynamically activate or deactivate the task by means of a proper weighting matrix. In other words, the new control law would become:

q = J + a ėa =   J s W c J c   +   ṡ -λe s W c ėc   (1.20)
in which the pair J a , ėa represents the "augmented task" and W c is a diagonal weighting matrix with non-negative entries. In principle, when far away from the constraints it suffices to select W c = O, leading to the original control law (1.9) Increasing the weights in W c allows instead to activate the corresponding constraints.

In [START_REF] Remazeilles | A qualitative visual servoing to ensure the visibility constraint[END_REF] it was proposed to define the weights using the activation function:

w(e) =            0 e ≤ 0 1 2 1 + tanh 1 1-e/β -β e 0 < e < β 1 e ≥ β (1.21)
in which β is a scaling parameter that regulates how sharply constraints activate. Additionally, a set of weights were also added to the main task. These value were chosen complementary to those associated to constraints, in such a way that when a constraint is fully activated then the weight associated to the corresponding feature is reduced to zero.

A similar concept is used in [START_REF] Kermorgant | Dealing with constraints in sensor-based robot control[END_REF]. The weight of the main task is kept equal to 1, while the weights associated to the constraints are modulated according to a hyperbolic profile:

w(e) =      0 e ≤ e +
e-e + emax-e e + < e < e max (1.22) wherein e max is the maximum allowed value for the quantity e, while e + represents a "soft maximum" in which the constraint should activate. In addition, means to escape from local minima are provided, as well as strategies to smooth out the behavior of the controlled system when moving between multiple active constraints.

Finally, it must be noted that these approaches can introduce discontinuities in the control laws. In fact, the pseudo-inverse operation is not continuous under rank changes, and thus irregular control signals will be obtained if J a changes rank due to the (de)activation of one or more constraints. This can be a major problem in systems which are redundant with respect to the main task, in which the additional degrees of freedom are often used precisely to satisfy constraints such as joint limits avoidance. Nonetheless, the inversion operator proposed in [START_REF] Mansard | Continuity of varying-feature-set control laws[END_REF] can be used to solve (1.20) while ensuring continuity. Such operator behaves exactly like the pseudo-inverse when a constraint is either fully active or inactive. Furthermore, it can guarantee local asymptotic stability and the resulting controller can remove oscillation that arise when near to constraint boundaries due to repeated activation and deactivation of the weights.

Hierarchical control

If the system presents redundancy with respect to the target task, constraints can be enforced using the additional degrees of freedom. In fact, one can use the redundancy framework [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF] to execute a secondary task of lower priority while ensuring that the main task is executed as expected without any perturbation. In practice, an extension of (1.9) can be written as:

q = J + s ( ṡ -λe s ) + P Js q (1.23)
in which P Js = I -J + s J s is the projector into the kernel of J s , i.e., such that J s P Js = O, and q is an arbitrary vector. If the kernel of J s is not empty, then q can be chosen in order to enforce existing constraints without affecting the desired features dynamics.

Oe possible application of such hierarchical approach is joint limits avoidance [START_REF] Chaumette | A redundancy-based iterative approach for avoiding joint limits: Application to visual servoing[END_REF], using the so-called gradient projection method [START_REF] Samson | Robot control: the task function approach[END_REF]. In this context, a cost function V (q) is defined, which increases when approaching the constraint boundary and stationary while sufficiently far away from it. Denoting with q i each coordinate of the joint position vector, such a function can be written as:

V (q) = i α i w 2 i w i =            q - i -q i q i < q - i 0 q - i ≤ q i ≤ q + i q i -q + i q i > q + i (1.24)
with qi and q + i are soft activation limits for the i-th joint, while the coefficients α i can be modulated online by a proper algorithm. The gradient of the cost function, ∂V ∂q T , can then be used as an artificial repulsive potential to steer the joint configuration away from the limits by setting q = -∂V ∂q T .

The main issue with the gradient projection method is that it allows constraints to be included only as low-priority tasks, and thus there is no guarantee that they can always be satisfied during the control. To overcome this issue, the idea of using a hierarchy of tasks was expanded in [START_REF] Mansard | Task sequencing for high-level sensor-based control[END_REF]. The main task is decomposed in a multitude of sub-tasks, each one thus requiring a lower number of degrees of freedom, and constraints are still handled at the lowest priority using the gradient projection method. However, contrarily to existing approaches, two supervisory controllers are added as well, which can respectively remove or add tasks in the stack. The first one is required to check if, due to the current set of active tasks, the produced control law will lead to violation of any constraint. If this is the case, the auxiliary controller selects the active task which is responsible for the predicted constraint infringement and deactivates it. The second controller is in charge of determining when it is safe to re-activate a task that had been previously removed from the stack. Since each task is responsible for controlling only few degrees of freedom, the system will still tend to evolve towards the goal configuration thanks to the remaining active tasks. In addition, by proper modification of the control law (1.9) it is possible to make the computed command continuous even during insertion or removal of a task.

Even though [START_REF] Mansard | Task sequencing for high-level sensor-based control[END_REF] introduced substantial improvements in handling of constraints, they are still managed at the lowest priority level. It is thus essential to properly decompose the main task in smaller ones so as to be able to dynamically change the stack and allow constraints to be taken into account. The method proposed in [START_REF] Mansard | A unified approach to integrate unilateral constraints in the stack of tasks[END_REF] is based on the continuous inverse [START_REF] Mansard | Continuity of varying-feature-set control laws[END_REF] detailed before, extending its use to a stack of tasks. It allows to add unilateral constraints at arbitrary priority levels, and not necessarily at the lowest ones.

A different approach was instead considered in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], in which a new projection operator was proposed. Constraints are still handled using the gradient projection method, and are thus still located at a lower priority with respect to the main task. However, the main task is modified: rather than acting directly on the feature error e s , the control objective becomes to steer its norm e s to zero. This task requires a single degree of freedom, thus leaving much more flexibility to satisfy the secondary task, i.e., constraints satisfaction. The new norm nullification task is characterized by a new Jacobian J es whose pseudo-inverse takes the following expression:

J + es =
e s e T s J s J T s e s J T s e s (1.25)

It must be noted that this matrix approaches a singularity when J T s e s → 0, which hap-pens either when the main task is completed (e s = 0) or when the system reaches a local minimum for the servoing task, i.e., when e s ∈ ker (J + s ) = ker J T s . It is therefore proposed to use the new projection strategy in the beginning of the task (when the main task error is still significant), and to smoothly switch to the classical projection method once a neighborhood of the target configuration has been reached.

Quadratic Programming

The classical control law based on the pseudo-inverse of the interaction matrix can be viewed as an unconstrained, multi-objective, quadratic minimization. More specifically, it can be shown that (1.9) is equivalent to solving:

min q J s q -( ṡ -λe s ) 2 min q q 2 (1.26)
in which a b means that the minimization problem b is of secondary importance with respect to a. The controller thus evaluates the command that achieves at best the main task (1.8), also featuring the least magnitude if possible.

Starting from this parallelism, it would thus be possible to consider the control problem explicitly as a quadratic minimization. This opens to the possibility of inserting constraints in the minimization, as long as they can be described as a set of inequalities that are linear in the control input, and to obtain the commands using any Quadratic Programming (QP) algorithm. As an example, in [START_REF] Joven Agravante | Visual Servoing in an Optimization Framework for the Whole-Body Control of Humanoid Robots[END_REF] constraints of this type are introduced to ensure visibility and avoid occlusion on humanoid robots, while in [START_REF] Perez-Morales | Automatic Perpendicular and Diagonal Unparking Using a Multi-Sensor-Based Control Approach[END_REF] they are used to ensure collision avoidance for an autonomous vehicle performing a parking maneuver.

The illustrated concept allows to consider a single task subject to one set of constraints, which are all to be satisfied at the solution. However, it is possible to combine the concept of a stack of tasks and the quadratic minimization formulation to create a more complex control scheme. More specifically, the methodology proposed in [START_REF] Oussama Kanoun | Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task[END_REF] extends the notion of task to include inequalities relations. At each priority level, a new quadratic problem is formed, in which the objective is to satisfy a set of (in)equality tasks as much as possible, while not influencing the tasks coming from higher priority levels. Constraints can thus be incorporated in the control scheme with a given priority, an not necessarily at the last level. The method is further refined in [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], in which subsequent quadratic problems are formulated efficiently using a change of variable. The basic idea is that the projector P Js in (1.23) can be replaced with an orthogonal basis of the kernel of J s , denoted as Z Js , and such that J s Z Js . However, while the projector is a square matrix, the orthogonal basis has a smaller number of columns in comparison. The new hierarchical control law can be written using the orthogonal basis as:

q = J + s ( ṡ -λe s ) + Z Js y (1.27)
The difference with respect to (1.23) is that, while dim( q ) = dim( q), y is a lowerdimensional vector with only dim( q) -rank (J s ) components. Therefore, after a priority level is solved in the stack, the following problem can be parameterized in a lowerdimensional variable. Starting from this simple concept, the complete hierarchical approach is developed and it shown that it greatly improves the performances of the controller.

Conclusions

In this chapter, the fundamental notions of visual servo control have been introduced, with a focus on Image Based Visual Servoing. As discussed, servoing schemes that use image points as features are simple to implement, but can lead to few problems. In particular, when large displacements are involved the sensor motion can be longer than necessary, thus leading to larger time to convergence. One way to improve the performances can be found in the use of better features that lead to higher decoupling in the control degrees of freedom, or by using efficient laws which attempt to steer the sensor along geodesic paths.

Two issues are however still open. First of all, the control laws detailed so far work under the assumption that the evaluated joint velocity commands can be readily tracked by the low-level controllers of the manipulator. This hypothesis is reasonable when the robot moves at relatively low speed. However, in order to obtain good performances at high speed it might be necessary to employ high gains to achieve the required velocity tracking performances. This can be problematic, since higher gains will lead to noise amplification. This problem is thus briefly investigated in the next chapter.

A second issue is related to the exponential decrease in velocity imposed by feedback control laws. As the sensor approaches the desired configuration, the speed of the manipulator can decrease significantly. Faster convergence can be obtained using larger gains, but at the cost of having very large velocities and accelerations in the beginning of the task. One way to tackle this problem, as proposed in Chapter 3, can be found in the use of Model Predictive Control, in which the control inputs can be optimized in order to achieve high velocities for a longer period, but also with the ability of decelerating in a controlled manner once the servoing task is completed. Furthermore, the advantage of considering a predictive control strategy is that it allows to accomplish two further goals using a unified approach: first of all, not only the motions can be faster, but also shorter thanks to the ability of predictive controllers to foresee future states and thus optimize the sensor motions. Secondly, unilateral constraints can readily be included in the optimization process. It is thus possible to ensure not only a faster convergence, but also to guarantee feasibility of the motion.

Chapter 2

VISUAL SERVOING USING SECOND-ORDER MODELS

The control laws classically used in visual servoing are formulated at the velocity level. It is generally assumed that low-level controllers are able to properly track velocity references generated by the visual controllers, thus assuming that dynamic effects are negligible. This is a fair assumption when the task to be performed is executed at mediumlow speed. However, dynamic effects increase with the velocity [START_REF] Corke | Dynamic effects in visual closed-loop systems[END_REF] and therefore, as faster motions are involved, new and more accurate models have to be considered.

One way to enhance the performances of the servoing schemes is to establish a direct link between the evolution of the features in the image and the dynamic parameters of the controlled system. This naturally leads to the use of second-order models which express the acceleration of the features as a function of both the velocity and the acceleration of the sensor. The advantage is that no low-level velocity controllers are needed anymore, and the desired features evolution can be obtained by computing directly the required motor torques by taking into account the dynamic model of the manipulator. This chapter focuses precisely on this topic, in particular on the study of secondorder acceleration models. First of all, the next section includes several related works that explored the use of features acceleration and attempted to exploit it in the context of dynamic control. Afterwards, in Section 2.2 a procedure to derive features acceleration models is detailed, and it is shown how they can be used to directly link the feature space with the dynamics of manipulators. Section 2.2.3 proposes different dynamic controls. The first one does not need the use of acceleration models and relies instead only on velocity information. It will be used later for comparison with another type of controller that is instead based on the proposed second-order models. The comparison, performed in simulation, is presented in Section 2.3. In particular two variations of the velocitybased controller are tested against the control law based on second-order models, and the benefits of using this approach are discussed.

Related Works

The idea of using acceleration information in a sensor-based control scheme can be backtracked to almost the origins of visual servoing itself. Indeed, few early works can be found in the literature which propose controllers that are not based solely on the classical interaction model. The idea of computing commands using a PD controller based on second-order models of image features and incorporating it into a CTC was already mentioned in [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], even though it was then proposed to consider a kinematic control under the assumption of low accelerations and the use of high-gain joint velocity controllers.

Even when the dynamics of the manipulator is disregarded, integrating higher order information in kinematic control laws can still be beneficial, as illustrated in [START_REF] Martinet | Use of first derivative of geometric features in visual servoing[END_REF]. The study shows that using both the position and velocity of a point as feature can enhance the performances of the controller in presence of relatively large processing delays.

A series of works by Dahmouche et al. [Dah+09;[START_REF] Dahmouche | Efficient high-speed vision-based computed torque control of the orthoglide parallel robot[END_REF][START_REF] Dahmouche | Dynamic visual servoing from sequential regions of interest acquisition[END_REF] focuses on the use of spatio-temporal point features whose evolution depends not only on the current relative twist v but also on its derivative. More precisely, these works aim at performing simultaneous pose and velocity estimation of the end-effector of a parallel robot using a eye-to-hand setup. To decrease significantly the cost of image acquisition and processing, features are detected by analyzing specific ROIs (Region of Interests) in a sequential manner, and thus at different time instants. As a consequence, the value of each feature is dependent not only on the position of the corresponding point, but also on the time at which the ROI is captured and the object velocity. In particular, using a first order linearization, [START_REF] Dahmouche | 3D pose and velocity visual tracking based on sequential region of interest acquisition[END_REF] estimates the position of a measured feature as s i = p i + ∆t i L i v, in which p i is the point that is being measured and the second component represents the displacement of the point due to the time delay of the sequential acquisition. The time derivative of each feature thus depends both on the relative velocity and acceleration of the sensor with respect to the object. In [START_REF] Dahmouche | Efficient high-speed vision-based computed torque control of the orthoglide parallel robot[END_REF] a virtual visual servoing approach [START_REF] Marchand | Virtual visual servoing: A framework for real-time augmented reality[END_REF] is used to obtain an estimation of the pose and velocity of the end effector, under the assumption of piece-wise constant acceleration. This estimation is then used in a CTC scheme with the objective of tracking a given trajectory at high speed, with the experiments showing that the vision-based approach outperforms model-based schemes based only on proprioceptive information. Furthermore, in [START_REF] Dahmouche | Dynamic visual servoing from sequential regions of interest acquisition[END_REF] a further scheme was proposed consisting in a Image Based Visual Servoing (IBVS) scheme formulated directly at the dynamic level, and in [START_REF] Ozgur | High speed parallel kinematic manipulator state estimation from legs observation[END_REF] the same approach is extended to control a parallel mechanism from legs observations rather than using image points.

An alternative approach to directly express the dynamics of the features at the dynamic level can be found in [START_REF] Zhang | Visual servoing with dynamics: control of an unmanned blimp[END_REF]. By considering a bijective map between image features and robot state, a Lagrangian is written directly in terms of the feature vector s and its derivative ṡ. In particular, the center and radius of a circular shape are used to control the motion of a simplified miniaturized blimp.

Yet another approach is used in [START_REF] Thomas | Toward image based visual servoing for aerial grasping and perching[END_REF], which considers a simplified drone model moving in the sagittal plane. The position of the drone is controlled using two features projected on a virtual image plane parallel to the ground. The authors show that these features constitute a flat output for the system, allowing to evaluate the thrust and torque needed to pilot the drone directly from the feature error and its derivatives.

A second-order IBVS scheme using the joint acceleration as control input was proposed in [START_REF] Keshmiri | Augmented Imaged Based Visual Servoing controller for a 6 DOF manipulator using acceleration command[END_REF] for a non-redundant manipulator observing four image points, and was later extended to take into account the dynamic model of the robot in a CTC fashion [START_REF] Keshmiri | Augmented image-based visual servoing of a manipulator using acceleration command[END_REF]. Using the same formalism, [START_REF] Mohebbi | An acceleration command approach to robotic stereo image-based visual servoing[END_REF] proposes an acceleration control for a serial robot equipped with a stereo-vision system on its end-effector.

A hierarchical multi-objective control scheme was proposed in [START_REF] Vandernotte | Dynamic sensor-based control[END_REF], in which a 7dof manipulator performs a hybrid force-vision control. The tasks are formulated directly at the dynamic level and the priorities in the hierarchy are respected using a projection strategy similar to that of [START_REF] Mansard | Task sequencing for high-level sensor-based control[END_REF] and [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF].

Finally, whole-body control of a humanoid robot was proposed in [START_REF] Joven Agravante | Visual Servoing in an Optimization Framework for the Whole-Body Control of Humanoid Robots[END_REF], which has been already mentioned in the previous chapter. In this case, an acceleration control command is computed using QP to minimize three weighted objectives under multiple constraints.

Second-Order Visual Servoing & Dynamics

This section focuses on the theory behind modeling and feedback control via secondorder model. In particular, starting from the kinematic relation described by (1.1), it is shown in the next section how to obtain an expression that provides the second derivative of the features in function of the spatial acceleration of the sensor. It is then detailed how these relations can be linked with the dynamic model of a serial manipulator, and how the new models can be used for visual servo control.

Since the final objective is to control a redundant manipulator, for which the visual task can constrain at most 6 dof, it is shown how redundancy can (and should) be resolved in the new schemes.

Second-Order Visual Servoing

The class of models presented in the sequel considers the second-order time derivative of the features and how it depends on the acceleration of sensors, in addition to their velocity. In some related works the derivation of the second-order model was achieved by considering the geometric meaning of the considered features and by evaluating s from such geometric definition. As an example, let's consider a stationary 3D point p c observed from a moving camera, for which the interaction matrix writes as [START_REF] Fran | Visual servo control. I. Basic approaches[END_REF]:

L p c = -I [ p c ] × (2.1)
It is possible to express the second derivative of p c from the laws of classical mechanics, e.g., as done in [START_REF] Keshmiri | Augmented Imaged Based Visual Servoing controller for a 6 DOF manipulator using acceleration command[END_REF]. In particular, using the well-known rule of composition of accelerations in non-inertial frames [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF]:

p c = -a + p c × ω + 2ω × v + ( p c × ω) × ω (2.2)
in which a and ω are the linear and angular acceleration of the sensor expressed in the coordinate system of the sensor itself. Rearranging the terms, one can realize that p c can be expressed using the interaction matrix of the 3D point as

p c = -I [ p c ] ×   a ω  + 2ω × v + ( p c × ω) × ω = L p c a + h p c (2.3)
In the formula, a is the concatenation of the linear and angular sensor accelerations, i.e., a T = a T ωT , and the affine term h p c groups the quantities that depend solely on the kinematic screw v and on the feature itself. As it will be soon shown, this structure is common to other features as well.

A similar procedure could be followed to evaluate the second-order models of any feature, such as lines and planes [START_REF] Vandernotte | Manipulation référencée multi-capteurs d'objets manufacturés[END_REF]. However, the procedure can become rather long and error-prone. Instead, we propose in the following a general approach that we believe facilitates the derivation of these models, and in addition provides a specific structure for the affine term h. First of all, the interaction model (1.1) is directly differentiated:

s = L s d dt v + Ls v (2.4)
Assuming an eye-in-hand setup, the derivative of the kinematic screw writes as follows:

d dt v = a +   v × ω 0   (2.5)
The additional term is a consequence of the fact that the camera frame is not inertial with respect to the object. In order to proceed, it is now necessary to differentiate L s .

For this purpose, it is convenient to consider the interaction matrix as a stack of vectors

i ∈ R 6 (i = 1, • • • , n s ).
In particular, each of these vectors represents one the rows of L s :

L s =      T 1 • • • T ns      (2.6)
Given this structure of the interaction matrix, the required quantity Ls readily follows from the total differentiation rule:

Ls =      ∂ 1 ∂s ṡ + ∂ 1 ∂z ż T • • • ∂ ns ∂s ṡ + ∂ ns ∂z ż T     
(2.7)

The derivative ṡ can be obtained from the classical interaction model (1.1). In the following it is assumed for simplicity that an expression for ż is available, and that furthermore it takes the form ż = L z v. Under this assumption it is thus possible to write:

Ls =      ∂ 1 ∂s L s v + ∂ 1 ∂z L z v T • • • ∂ ns ∂s L s v + ∂ ns ∂z L z v T      =      v T H 1 • • • v T H ns     
(2.8) in which the matrices

H i . = ∂ i ∂s L s + ∂ i ∂z L z
T are a function of the features s and the parameters z only. Injecting this result and (2.5) into (2.4) leads to:

s = L s a + L s   v × ω 0   +      v T H 1 • • • v T H ns      v (2.9)
The second term in the right-hand side can be rearranged in a nicer form that matches the structure of the last term. By denoting with (v) i ∈ R 3 the vector containing the first three elements of i , the second term in the last equation can be written as:

L s   v × ω 0   =      T 1 (v × ω) • • • T ns (v × ω)      =      ω T ( 1 × v) • • • ω T ( ns × v)      =      v T L 1 v • • • v T L ns v     
(2.10) in which the circular-shift invariance of the mixed product has been used in the second passage, and the matrices L i ∈ R 6×6 in the last expression are defined as:

L i . =   O O (v) i × O   (2.11)
Finally, using (2.9), (2.10) and by defining

G i . = (H i + L i ) + (H i + L i ) T 2
(2.12) leads to the sought expression of second-order models:

s = L s a +      v T G 1 v • • • v T G ns v      = L s a + h s (2.13)
It must be noted that, as already mentioned, the second-order model of the 3D point developed in the beginning of this section takes a similar form, the only difference being the h p c vector which was not expressed as a set of quadratic forms in (2.3). Appendix A.1 includes the second-order model of 3D points rewritten according to (2.13), which, after some development, can be shown to be equivalent to (2.3). More in general, Appendix A includes the second-order models of few other features, such as normalized image points.

The main interest in the illustrated procedure is that, in order to get the second-order model of any feature, the only required work is the evaluation of ∂ i ∂s and ∂ i ∂z . In addition, the constructive procedure shows that the affine term h s consists in a series of quadratic forms, with G i ∈ R 6×6 depending on s and z. This can be useful, e.g., if the partial derivatives of s are to be computed.

To conclude this section, it is shown how a second-order model can be obtained from another set of features for which an acceleration model has already been obtained. Let However, directly using this expression does not provide explicitly the form of the Gmatrices. To obtain them it is necessary to perform few additional steps, leading to the final result:

G b,i = L T a ∂ 2 ψ i ∂s 2 a L a + j ∂ψ i ∂s a,j G a,j (2.17)
wherein G b,i is the i-th G-matrix of the feature set s b and G a,j follows the same notation. Finally, ψ i is the i-th entry of the vector-valued function ψ and s a,j is the j-th coordinate of the feature set s a .

Robot Kinematics and Dynamics in Sensor Space

As previously mentioned, the obtained second-order model (2.13) is valid under the assumption that the relative motion between the sensor and the object depends solely on the motion of the robot (eye-in-hand setup). In this case, the velocity and acceleration of the sensor, expressed in the base coordinate system, are given by:

v w c = J q (2.18a) a w c = J q + J q (2.18b)
wherein J and J are the robot Jacobian (as used in (1.6a)) and its derivative. By projecting (2.18a) and (2.18b) on the sensor frame and injecting the result into the second-order model, the feature acceleration can be obtained as a function of the joint velocities and accelerations:

s = J s q + L s T c w J q + h s = J s q + h q (2.19)
where J s is the feature Jacobian (1.6a) defined in the previous chapter.

A further step consists in expressing a direct link between the features and the dynamic model of the robot. First of all, it is recalled here that the Inverse Dynamic Model (IDM) of a robotic system composed of a set of rigid bodies writes as [START_REF] Khalil | Modeling, identification and control of robots[END_REF]: τ = A(q) q + C(q, q) q + g(q) + F v q + F s sign( q) + J T f e = A q + b + J T f e (2.20) wherein τ represents joint efforts while A is the generalized inertia matrix of the system, grouping inertial effects due to masses and inertiae of both rigid bodies and motors.

C q represents Coriolis and centripetal efforts, while g gravitational ones. F v and F s are diagonal matrices used to model friction, and f e represents an external wrench applied on the end-effector. For brevity, b groups Coriolis and centripetal forces, the efforts exerted on the system by gravity and the forces and moments acting on the actuators due to friction.

Assuming that A is invertible, which is always true for a serial kinematic chain, the joint accelerations are given by q = A -1 τb -J T f e . In conjunction with (2.19), it leads to:

J s A -1 τ = s -h q + J s A -1 b + J T f e (2.21)
which provides the sought relation between motor effort commands and features evolution.

Dynamics based Feedback Control

In this section, few controllers are discussed, which try to evaluate effort commands by considering the dynamics of the maneuvered manipulator with the objective of regulating the feature error e s . = ss to zero.

A first solution would be to disregard second-order models and to separate the control law in two distinct loops. An outer visual servoing loop evaluates a reference joint velocity using the proportional control law (1.9):

q = J + s ( ṡ -λe s ) (2.22)
The goal of the second control loop is then to track such velocity profile. An auxiliary acceleration pseudo-control can be designed for this purpose as:

w = qff + γ ( q -q) (2.23)
given a positive proportional gain γ. The feed-forward term qff is added in order to remove tracking delays, and should ideally correspond to the derivative of q . Since it cannot be obtained analytically, it has to be estimated numerically from (2.22). This auxiliary command can be injected in the IDM, thus giving the control:

τ = Âw + b (2.24)
in which  and b provide an estimation of A and b respectively, and f e was assumed to be null. The main drawback of this control scheme, as it will be shown later in this chapter, is that it requires the feed-forward to be effective. However, since the signal has to be estimated via numerical differentiation, it is a source of noise amplification.

An alternative approach that does not require the use of second order models consists in using the following control law, corresponding to a proportional and derivative feedback with gravity compensation [START_REF] Kelly | Stable visual servoing of camera-in-hand robotic systems[END_REF]:

τ = g -γ q -λJ T s e s (2.25)
Intuitively, this control law moves the end-effector of the robot under the action of a virtual force that equals -L T s e s . Moreover, asymptotic stability can be proved rigorously considering the Lyapunov function 1 2 qT A q + λ 2 e T s e s . However, the resulting sensor trajectories seem less satisfactory and harder to anticipate than those obtained with other methods.

Finally, control laws based on second-order models can be considered. The rationale behind them is similar to that of first-order proportional controllers, the goal being to enforce an exponential decrease of the error according to the evolution of the autonomous linear system ës + k d ės + k p e s = 0, which is asymptotically stable if positive gains are selected. Perfect linearization will not be achievable in general, but asymptotic stability can nonetheless be proved [START_REF] Keshmiri | Augmented image-based visual servoing of a manipulator using acceleration command[END_REF] when using the auxiliary acceleration control

q = J + s s -k d ės -k p e s -h q (2.26)
Note that in addition to an estimation of the pseudo-inverse of J s , the two quantities h q and ės are to be estimated as well. Similarly to the case of the first controller presented in this section, the acceleration command q can be used in conjunction with the IDM to evaluate a set of effort commands to be sent to the actuators.

Dealing with Redundancy

The case in which the robot has more degrees of freedom than those required to perform the servoing task is address separately here. First of all, it is important to recall that the controls returned by the feedback control laws (2.22) and (2.26) are nothing but particular solutions to the following minimization problems:

min q J s q -ṡ + λe s (2.27a) min q J s q -s + k d ės + k p e s + h q (2.27b)
In particular, as mentioned in the previous chapter, they correspond to the solutions that, in addition to be minimizers of the objectives, have the least norm possible. Assuming that no singularities are encountered, a visual task can constrain up to six degrees of freedom. If the manipulator features n q > 6 joints, it follows that the kernel of J s is not null, and thus infinite solutions can be found that are compatible with (2.27a) and (2.27b).

Assuming that no secondary task needs to be performed, the control law based on first-order models will move the sensor in the proper configuration and then stop the manipulator in the attained position. In fact, once the visual error has been nullified, the minimum norm solution solving (2.27a) is trivially q = 0.

On the contrary, when the control input is an acceleration, the minimum norm solution returned by (2.27b) does not necessarily correspond to a stable manipulator configuration. In fact, to a null acceleration -the minimum norm solution -corresponds a constant and possibly non-zero velocity. The reality is slightly more complicated due to the nonlinearities in the geometric and kinematic model of the manipulator, which will also cause the kernel of J s to change continuously. Nonetheless, without modifications of the proposed control law the joint velocity of the manipulator will not be null in general, and will instead continuously change in order to fall within ker (J s ).

In order to solve the issue at hand, one first solution is to attempt to evaluate the control not as a minimum norm acceleration, but rather by using directly (2.21) to compute the efforts from the desired task [START_REF] Vandernotte | Dynamic sensor-based control[END_REF]. One possible choice in this sense would be to compute the actuation command τ having the least norm. However, this strategy can again lead to large uncontrolled velocities, and it is generally preferable to use a weighted pseudo-inverse [DO18; Man12]:

τ = (J s ) + D s -k d ės -k p e s -h q + J s A -1 b (2.28) wherein (J s ) + D . = D (J s D) + , with D > 0.
If repetitive motions are to be performed, other solutions might be preferable, such as the method proposed in [START_REF] Pomares | Dynamic visual servoing with chaos control for redundant robots[END_REF] which causes the system to stabilize on a periodic orbit, thus eliminating the chaotic behavior that would result otherwise.

Finally, a last, simple, solution would be actively damp the velocity of the manipulator. This would ensure that, when reaching a fixed visual configuration, the joint velocity of the robot is decreased, eventually stopping the robot once the visual task has been completed. This active velocity minimization objective can be considered as a secondary task in the form q = -k v q (2.29) k v > 0 being a positive gain. Using the redundancy framework to ensure that this task does not perturb the visual one, the final acceleration control law becomes: q = q -ZZ T (k v q + q ) (2.30) in which q is the acceleration evaluated using (2.26). Z is an orthogonal basis of the kernel of J s , so that the orthogonal projector is given in the efficient form ZZ T [EMW14].

Figure 2.1 -A Kuka LWR4+ manipulator in a eye-in-hand configuration, observing a target with four dots. This same manipulator was simulated in Gazebo to perform the comparison between the different control schemes presented in this section.

Analogously to other schemes, efforts can finally be obtained by injecting into q into the IDM.

Comparison of Velocity and Acceleration-based controllers

In this section, a comparison between two control schemes based only on first-order information and one based on the presented second-order models is proposed. The comparison has been performed in a simulated environment, using Gazebo for physics simulation and ROS for the implementation of controllers and communication. ViSP [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] has been used for Visual Servoing related computations.

The test scenario involves a positioning task from four coplanar points, whose secondorder interaction models is reported in Appendix A.2, which are located at the vertices of a square. A perspective camera is mounted on the tip of a 7-dof Kuka LWR4+ robot, as shown in Figure 2.1. A perfectly calibrated sensor with no distortion is assumed for simplicity, and the normalized coordinates of the observed points are used as features. Uncertainties in the Geometric and Dynamic parameters of the robot are not considered in this analysis.

The control period of the simulation is set to 1 ms, and it is assumed that the visual feedback comes from a mixture of image processing and state estimation, and that the unknown parameters z can be estimated as well [START_REF] De Luca | Online estimation of feature depth for image-based visual servoing schemes[END_REF][START_REF] Spica | Plane estimation by active vision from point features and image moments[END_REF]. To take into account uncertainties due to these operations, normally distributed noise is added to both s and z. In addition, the presence of encoders on each motor is simulated by quantization of the readings to a given resolution. More details on noise are presented in the next section. Three control strategies have been analyzed. The first one, namely First Order (FO), corresponds to a velocity-based scheme as described in the beginning of Section 2.2.3. However, in the computation of the acceleration pseudo-control (2.23) the feed-forward signal qff is not considered. The second strategy takes this term into account and will be referred to as First Order with Feed-Forward (FOFF).

It must be noted that a problem with the first-order law (2.22) is that it presents a discontinuity in velocity at the very first iteration since the manipulator is initially at rest. In order to guarantee a "smooth start", the computation of q in (2.22) was slightly modified, according to the concepts presented in [START_REF] Mansard | Task sequencing for high-level sensor-based control[END_REF].

The last controller, denoted as Second Order (SO), evaluates joints accelerations using (2.26) and (2.30). Details about the implementation and tuning of the three controllers are discussed in Section 2.3.2.

Finally, results are provided for two cases: servoing to a fixed feature configuration s in Section 2.3.3 and tracking of a variable reference trajectory in Section 2.3.4.

Sensor Noise

Noise is introduced in the simulation to obtain a more realistic study-case. The first source of noise that is considered affects the current value of the features s. In a real setup this might come from, e.g., the image processing routine that extracts the points of interest, estimators that produce a continuous feedback when an image is not available or the sensor itself.

Feature noise is thus included by adding to each coordinate of s a random value, drawn from a truncated Gaussian distribution with zero mean. The truncation is performed symmetrically at ±3σ, σ being the standard deviation of the unbounded distribution. In the experiments, a value of σ = 2 • 10 -3 (corresponding to about 1 pixel) was used. A similar strategy has been used to simulate uncertainties in the unknown depths of the observed points, by adding to z a noise with σ = 3 • 10 -3 .

Regarding the measurement of joint positions and velocity, an assumption is made: only encoders are available to estimate the current configuration. These devices are simulated by discretizing the values of joints angles with a given resolution ∆q. In particular, ∆q = 10 µrad was used.

All noisy signals were then filtered using low-pass Butterworth 4 th -order filters and finally used in all the calculations performed by controllers.

Controllers Implementation and Parameters Tuning

First-Order (FO) Controller

As briefly mentioned above, the computation of the desired joint velocity (2.22) was modified so as to ensure smoother control signals in the beginning of task execution. In particular, in [START_REF] Mansard | Task sequencing for high-level sensor-based control[END_REF] it was shown that whenever a new task is activated, it is possible to achieve a smooth transition in the control input by adding a homogeneous term to the command. Applied to the present setup, the velocity reference q can be changed into q = J + s ṡ -λe s + e -µt e s (0)

(2.31)

where e s (0) corresponds to the features error at t = 0, and under the assumption of null desired features velocity at the beginning, i.e., ṡ (0) = 0. Due to this modification, the command at the very first iteration evaluates to zero, thus enforcing a smooth transition of the desired velocity of the manipulator.

The effect on the error dynamics is that it now behaves as an equivalent second-order system -if perfect linearization was possible -according to the autonomous equation ës + (µ + λ) ės + µλe s = 0

(2.32)

One interesting consequence of this choice is that it is possible to enforce the same desired behavior of the features evolution with both first and second-order controls. Indeed, in a perfect world not affected by noise, the two control strategies are perfectly equivalent in terms of error evolution.

After computing the desired velocity (2.31), the effort command is computed as τ = γA ( qq) + b.

Regarding gains tuning, it was decided to impose a critically damped behavior for the transient response, which corresponds to the fastest convergence without oscillations:

λ = µ = ω n (2.33)
The parameter ω n > 0 defines the roots of the characteristic polynomial of the secondorder system, and theoretically regulates the autonomous response of the error according to e s (t) = e -ωnt (1 + ω n t) e s (0). The selection of ω n is thus performed so that, after a time T , the error corresponds to a given fraction of its original value, i.e., e -ωnT (1 + ω n T ) = α, α ∈ (0, 1). The equation is solved numerically for α = 0.05 and T = 2 s, leading to the value ω n 2.37 s -1 .

Finally, the proportional velocity gain γ appearing in (2.23) has to be chosen. From a theoretical point of view, higher values will lead to better tracking of the desired velocity profile. However, this would also lead to increased control efforts, and therefore a trade-off has to be found. In the presented simulations, a value of γ = 8 s -1 was used, corresponding to a step response that can reach the 80 % of the target value in 0.2 s.

First-Order with Feed-Forward (FOFF)

The implementation of this controller is very similar to the one of FO. In particular, the smooth velocity transition and the same set of gains are used here as well. The feedforward term qff is computed as the numerical derivative of the reference command q . It is well-known that this process highly amplifies noise, possibly resulting in unfeasible, if not just undesirable, control signals. In order to lessen the impact of noise and thus obtain a better behavior, (2.31) is thus filtered prior to differentiation. This considerably reduces the shakiness of the controller. The final efforts sent to the motors are evaluated using (2.23) and the IDM.

Second-Order (SO) Controller

This controller exploits the acceleration in the feature space. As explained, it computes joints accelerations using (2.30) to complete the visual task while simultaneously damping the velocity of the manipulator, finally exploiting the IDM to compute the actuation signal.

It must be noted that the control law requires an estimation of ės . For this purpose, it was decided to use directly the first-order interaction model, i.e., ės = J s qṡ .

The control gains k p and k d are selected to obtain a critically damped evolution as in the FO and First Order with Feed-Forward (FOFF) cases, with k p = ω 2 n , k d = 2ω n , and ω n = 2.37 s -1 . The last parameter that need to be tuned is k v , i.e., the gain appearing in the secondary damping task (2.29). With the selected value of ω n , the servoing task is expected to be completed in about 2 s. Therefore, k v is tuned so that the convergence of velocity to zero is achieved after a slightly longer time. k v = 1 s -1 was used in practice, theoretically ensuring that any non-null velocity is reduced to about the 5 % of its initial value in 3 s.

Regulation to a Fixed Configuration

The first set of tests involves positioning the end-effector in front of a set of four points at a distance of 0.5 m, see Figure 2.2. As the desired value of the features does not change during the tests, in the controllers the quantities ṡ and s are set to zero. The initial relative transformation from the camera to the object presents a rotation of about 90°around the optical axis of the sensor, leading to the well-known retraction problem. Nonetheless, this affects all control laws and therefore no attempt to solve the problem was performed.

Results related to two cases are reported: an ideal test run in absence of noise (Figure 2.3) and a more realistic case with disturbances as previously discussed (Figure 2.4). When signals are not influenced by any perturbation, all control strategies show nice convergence behavior and smooth control inputs. Compared to FOFF and SO, FO presents a deviation in convergence from the ideal evolution, justified by the intermediate velocity regulation not being able to perfectly track the desired command q . FOFF and SO are instead able to better follow the desired behavior and the error evolves according to the expected second-order exponential decay. The difference in these two controllers can be seen only in the control input, yet not significantly different. When noise is considered, more evident differences appear. FO's convergence worsens, with a little overshoot near to the equilibrium configuration, whereas both FOFF and SO still feature a nice evolution of the error towards zero. In terms of control inputs, all strategies suffer from the added noise. In particular, the oscillations corresponding to FOFF are quite large, with peak-to-peak amplitude of about 30 N m even at equilibrium. Oscillations of FO and SO controllers are bounded instead to a reasonable amount, between 0.5 N m and 1 N m of amplitude in both cases. It is possible to enhance the convergence of FO by increasing the gain γ or selecting a faster behavior (higher ω n ), at the cost of higher oscillations in the commanded efforts. Similarly, to reduce the shakiness of the control signal in FOFF, the response has to be penalized with lower values of γ and/or ω n .

It is also worth mentioning that, when noises amplitudes are increased, the relative performances of these controllers are not significantly altered. FOFF and SO still give satisfactory results in terms of error convergence, and the oscillations in the control commands produced by FO and SO are smaller compared to those introduced by FOFF.

Tracking of a Visual Trajectory

In this last section, the case of varying features references is considered. For each normalized point s i = x i y i T , the motion was defined as a circular trajectory starting at the instant t 0 and ending after a time period T :

     x i (t) = x i (t 0 ) + R (cos (ρ(t -t 0 )) -1) y i (t) = y i (t 0 ) + R sin (ρ(t -t 0 )) (2.34)
In this definition, ρ(t) is a quintic polynomial whose value changes from 0 to 2π in T = 2 s, such that ρ(0) = ρ(T ) = 0 and ρ(0) = ρ(T ) = 0. The desired initial position of each point s i (t 0 ) is assumed to be the one attained after the regulation task described in the previous section. The amplitude R is set to 0.2, and thus the radius of the physical trajectory is expected to be of 0.1 m since the sensor should be at 0.5 m from the four points, as detailed before. In addition, it is assumed that t 0 0, so that the homogeneous term in (2.31) does not affect the control, since smooth transition are already enforced by the choice of the time-varying reference.

Results are depicted in Figure 2.5, which shows that FO is once again the worst controller in terms of tracking error. FOFF still presents high oscillations in the input commands during the motion, but with good tracking performances. Finally, SO gives the best results, with tracking errors of the same magnitude of FOFF and reduced effort oscillations.

Conclusions

This chapter investigated the use of second-order models in order to establish a direct link with the dynamics of a manipulator. The resulting control scheme is a Computed Torque Control based on a Proportional and Derivative acceleration pseudo-command. Its performances were compared against two controllers that used instead a two-stages control strategy composed of an outer servoing loop based only on velocity information. The inner loop attempts to track the produced joint velocity reference also by taking into account the dynamics of the robot. However, it was shown by means of simulations that performances are in this case less satisfactory. More specifically, it is necessary to either sacrifice tracking accuracy or to pilot the robot with very noisy control inputs. Instead, the use of second-order controllers provides the best results, allowing good tracking with more regular input efforts. These results were the subject of a conference article [START_REF] Fusco | A Comparison of Visual Servoing from Features Velocity and Acceleration Interaction Models[END_REF] that was accepted for publication and presented at IROS 2019.

The interest of this class of models is not only related to feedback linearizing control. Indeed, as it will be shown in the next chapter, they can be used to formulate future predictions for a set of observed features. The quality of these predictions is generally higher than that obtained considering only velocity information. This allows to create new control schemes based on the Model Predictive Control paradigm thanks to which the robot can move faster and with shorter sensor trajectories.

Chapter 3

VISUAL PREDICTIVE CONTROL

Even though feedback linearization techniques are frequently exploited in Visual Servo control, they present some limitations. One important issue is that they might lead to nonoptimal behaviors with respect to the motion of the sensor in the Cartesian space. In fact, these strategies rely on the pseudo-inversion of the interaction matrix (or some estimation of it), which, depending on the features configuration, might lead to sub-optimal motions for the sensor. This comes directly from the structure of the interaction matrix of the chosen feature set, and therefore the options allowing to improve the motion are rather limited.

Moreover, it is not always straightforward to include and satisfy general constraints that are imposed on the system. Practical examples of such constraints are the need to keep an observed object inside a camera's field of view, or ensuring that the joint angles of a manipulator always lie within their physical limits.

A last relevant aspect -especially in the context of fast systems -is the one of the time required to converge to a given configuration. When the system is controlled via feedback strategies, the error is usually steered to zero with exponential decay. The rate at which the configuration of the system approaches the desired target is mainly controlled via parameters such as the gain of a proportional controller. In principle, the higher the gain, the faster the response. However, physical limitations of the actuators impose to consider control limits. To deal with them, it would be possible to simply saturate the command on a per-coordinate policy, but this might have undesired consequences such as loss of stability. Another way would be to scale all control input coordinates by a common factor, which ensures the system to follow the same path (thus retaining stability) at lower speed. However, the time needed to converge to the desired configuration would increase.

One of the reasons why feedback linearization techniques are subject to these limitations is that they aim at reducing the mismatch between the current features set and their desired value by following a locally optimal path. To some extent, they behave like a gradient-descent minimization algorithm, looking for the control that locally reduces the error as quickly as possible. In this sense, one improvement could consist in considering how a control input affects future states reached by the system. This would allow to take better decisions in order to reach the desired state both in shorter time and with nicer motions, while simultaneously ensuring satisfaction of existing constraints. This naturally leads to the study of a powerful technique from the domain of optimal control, Model Predictive Control (MPC), which is the subject of study of this chapter, in particular with a focus on visual servoing applications. This chapter is divided mainly into three parts: it begins with a quick introduction to the fundamentals of MPC, in particular focusing on existing mathematical formulations and the strategies that can be employed to solve the presented problems. A short review of existing works that used predictive strategies for visual servo control follows. Finally, the original contributions of this work are illustrated, which appeared in a peer-reviewed publication [START_REF] Fusco | Integrating Features Acceleration in Visual Predictive Control[END_REF]. They constitute a point of junction between Visual Predictive Control (VPC) and the second-order models presented in the previous chapter. Without entering into details yet, the core idea behind the proposed methods is that second-order models can be used to design more accurate predictors which ultimately lead to better robot motions. This claim is supported both by simulations and real experiments. In addition, the generality of these results are confirmed by the use of different types of features in the experiments: both normalized image points and their representation in polar coordinates.

Model Predictive Control

Model Predictive Control is an optimal control technique that aims at determining the best input signal to be applied to a system by taking into account the future evolution of its state. A continuous-time system is supposed to be described by the generic state-space form:

ẋ = f (x, u) (3.1)
x ∈ R n and u ∈ R m being respectively the state vector and the control input of the system. A common formulation of the MPC problem is a finite-horizon, open-loop, optimization that takes into account the model (3.1) and a set of constraints. More precisely, denoting the current time instant as t 0 , the mathematical formulation of the control problem can be described as the following functional optimization:

min u(•) t 0 +Tp t 0 C (x(t), u(t), t) dt (3.2)
subject to the constraints:

x(t 0 ) = x 0 (3.3a) ẋ(t) = f (x(t), u(t)) t ∈ [t 0 , t 0 + T p ] (3.3b) x(t) ∈ X ⊆ R n t ∈ [t 0 , t 0 + T p ] (3.3c) u(t) ∈ U ⊆ R m t ∈ [t 0 , t 0 + T p ] (3.3d)
In this formulation, the constant T p > 0 is named prediction horizon, and it corresponds to the time interval up to which future states are evaluated and control inputs are to be designed. The function C -sometimes referred to as stage cost -models the desired behavior of the system. As an example, a simple, yet common in practice, choice is the use of a quadratic cost:

C (x, u, t) = (x -x (t)) T Q(t) (x -x (t)) + (u -u (t)) T R(t) (u -u (t)) (3.4)
in which x (t) and u (t) represent the desired state and input signals over time, while Q(t) and R(t) are positive (semi)definite weighting matrices.

Regarding the constraints, the first one is simply used to fix in the optimization the initial state of the system, i.e., x(t 0 ), while the second constraint reflects the fact that states should be predicted by means of the provided evolution model. Finally, (3.3c) and (3.3d) define the regions of admissible states and control values, and are often as simple as pairs of lower and upper bounds, e.g., x -≤ x(t) ≤ x + .

It is worth remarking that the problem is meant to be solved online at each control step. In the case of T p = +∞ and with perfect knowledge of the model, it would be theoretically possible to solve it just once and then to apply the computed control signal u(t) to the system in an open-loop fashion. This is however generally infeasible for different reasons.

First and foremost, perfect knowledge of the model is impossible, especially considering the existence of disturbances and identification uncertainties, and the control input will need to be adjusted while the system evolves. A further reason is that the time to solve the optimization problem generally depends on the chosen horizon, and in the case of infinite horizons it might be unsuited for real-time applications. As a consequence, the problem is repeatedly solved online and only the first portion of the obtained control signal is sent to the actuators.

Regarding the way to find solutions to the problem, different strategies can be considered [START_REF] Findeisen | An introduction to nonlinear model predictive control[END_REF][START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF]. Those belonging to the family of indirect methods try to solve the original problem "exactly", using, e.g., the calculus of variations, dynamic programming, or so that an optimal feedback function u = ϕ(x) is determined in closed-form. One famous example in this sense is the Linear Quadratic Regulator (LQR) [START_REF] Frank L Lewis | Optimal control[END_REF], which provides the optimal solution to the optimization problem in case the system is linear and not subject to state or control constraints. Such solution has the form of a linear state feedback, which can be obtained by solving a continuous-time Riccati differential equation -or an algebraic Riccati equation in case of an infinite-horizon problem. Note, however, that the problem is very challenging in the general nonlinear, constrained, case and usually too computationally expensive for an online implementation.

In order to achieve real-time feasibility, it is instead rather common to solve the problem using a different class of techniques, known as direct methods. In this context, the original continuous-time, constrained, functional optimization is approximated with a finite-dimensional minimization problem. First of all, the continuous-time dynamics (3.1) is approximated by a non-linear discrete-time system having the general form:

x k+1 = f d (x k , u k ) (3.5)
In this sense, Euler's forward integration is arguably the most commonly used technique, according to which x k+1 is approximated by a first-order Taylor expansion:

x k+1 = x k + ∆t ẋk = x k + ∆t f (x k , u k ) (3.6)
∆t being the time sampling step.

For the purposes of MPC, once the continuous-time dynamics has been approximated with a discrete-time system, the objective becomes to compute a finite number of control samples, rather than the signal u(t) over the whole prediction horizon. More pre-cisely, at the discrete time-step τ , a control sequence u τ , u τ +1 , • • • , u τ +np-1 is to be evaluated, which will steer the system from the current state x τ to the new samples

x τ +1 , x τ +2 , • • • , x τ +np .
In this context, the original prediction horizon T p is replaced by the positive integer n p , the two constants being related by the simple formula T p = ∆t n p . In order to shorten the notation in the following, τ will be omitted in the subscripts. It is implicitly assumed that discrete-time samples are relative to the "current" sample τ : as an example, u 3 will stand for u τ +3 .

The new MPC optimization problem can finally be stated. The objective (3.2) is replaced by the following:

min u , x np-1 k=0 C d (x k+1 , u k , k) (3.7)
in which the decision variables u and x correspond to the stacked sequences of control samples and predicted states, i.e.,

u =      u 0 • • • u np-1      x =      x 1 • • • x np      (3.8)
It must be noted that the prediction vector x does not contain the value x 0 , as it corresponds to the current state of the real system, measured or estimated, and thus cannot be freely changed during the optimization.

Like in the continuous-time MPC optimization problem, the objective is often in the form of a quadratic stage cost, the obvious differences being that the (continuous) time variable t is now replaced by k:

C d (x, u, k) = x -x k+1 T Q k+1 x -x k+1 + (u -u k ) T R k (u -u k ) (3.9)
Finally, the constraints (3.3) can be kept almost unchanged:

x k+1 = f d (x k , u k ) k = 0, • • • , n p -1 (3.10a) x k ∈ X k = 1, • • • , n p (3.10b) u k ∈ U k = 0, • • • , n p -1 (3.10c)
According to the above formulation, the problem has a total of (n + m) n p decision variables. Depending on the model to be controlled and on the desired prediction horizon, the necessary time to evaluate a solution might still be significant. Nonetheless, there exists a widely exploited strategy so as to reduce the computational burden associated with the optimization, consisting in limiting the free control samples to be in the number of n c < n p . The usual way of dealing with this reduction of parameters is generally as simple as reducing the sequence u to include only the first n c samples, while the remaining n p -n c inputs are kept constant and equal to u nc-1 (see Figure 3.1). For this reason, the constant n c is named control horizon in the literature. Depending on the way the problem is solved, the number of decision variables can be further reduced. In particular, the so-called single-shooting methods allow the decision vector to be limited to the control variables u. On the other hand, multiple-shooting algorithms do solve the problem as stated by (3.7) and (3.10), but are able to better exploit parallelization and can thus still provide competitive solutions with respect to single-shooting methods [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF]. In both cases, once the problem has been solved the first control sample returned by the optimization, u 0 , is used as input to actuate the real system for the current control cycle. As in the continuous-time case, the new state reached by the system is then measured (or estimated) and used to formulate the new optimization problem to be solved at the next control iteration.

Single-shooting methods

The core idea behind single-shooting strategies for the solution of an MPC optimization is that the whole prediction vector x can be removed from the decision variables thanks to the recursive structure of the discrete-time system (3.5). In fact, one can write:

x 1 = f d (x 0 , u 0 ) x 2 = f d (x 1 , u 1 ) = f d (f d (x 0 , u 0 ), u 1 ) x 3 = f d (x 2 , u 2 ) = f d (f d (f d (x 0 , u 0 ), u 1 ), u 2 ) • • • (3.11)
which shows that x is ultimately a function of x 0 and u only. Since the former is simply a constant supplied to the optimization problem, only the latter is still to be considered within the decision variables, effectively reducing the dimensionality of the problem. Note that an additional advantage of single-shooting methods is that the nonlinear constraint (3.10a) is enforced by construction, and is not to be checked during the optimization. In order to solve the optimization, various non-linear programming routines can be exploited, such as those detailed in Appendix B. Frequently, these algorithms will require the gradient of the objective and of the constraint functions to be supplied in order to guide the search for the optimal solution. Obviously, these might be approximated by finite differences methods, but this generally leads to lower performances. In single-shooting algorithms, the main challenge is the computation of the jacobian of the prediction vector x with respect to the control sequence u, since other quantities can be computed rather trivially from it. The first step for the computation of the jacobian ∂x ∂u is to focus on the partial derivatives of any predicted state with respect to the j -th control sample, u j . Starting from the prediction model (3.5) and employing the chain rule it is possible to write:

∂x k+1 ∂u j = ∂f d (x k , u k ) ∂x k ∂x k ∂u j + ∂f d (x k , u k ) ∂u k ∂u k ∂u j (3.12)
It should be noted that, for k, j < n c , ∂u k ∂u j = δ j,k I (δ j,k being Kronecker's Delta), which is a direct consequence of the fact that all elements in u are independent variables. Furthermore, since all control samples from n c and beyond are simply a copy of u nc-1 , it is also holds:

∂u k ∂u j = δ j,nc-1 I ∀k ≥ n c , ∀j < n c (3.13)
It is also important to notice that ∂x 0 ∂u j = O since the initial state of the system is a given constant for the optimization. Finally, by looking at the relations reported in (3.11) it is possible to notice that, more generally, ∂x k ∂u j = O if k ≥ j -which reflects the impossibility for a control input to affect a past state. By putting all these pieces together, the complete jacobian can be computed, and takes the lower block-triangular form shown in (3.14) (zeros have been omitted for easier reading). In the formula, sub-matrices F i,j u and F i,j

x correspond respectively to ∂f d ∂u and ∂f d ∂x , evaluated at the points (x i , u j ).

Multiple-shooting methods

As opposed to single-shooting strategies, multiple-shooting methods solve the optimization problem using both the control sequence and the prediction vector as decision variables. Figure 3.3 provides a graphical example of how the prediction of future samples is achieved during the optimization. First of all, one states is selected for each prediction step and, gathered altogether, these states correspond to a candidate prediction vector. In conjunction with a control sequence u, each state x k can be mapped into

f d (x k , u k ),
i.e., the following state that would be visited according to the model of the controlled system. Obviously, if both x and u were chosen arbitrarily, the prediction would be discontinuous, with the successor of x k not corresponding necessarily to x k+1 . In order to ensure continuity over the prediction horizon, constraint (3.10a) is therefore taken into account explicitly, usually in the form:

d k . = x k+1 -f d (x k , u k ) = 0 ∀k = 0, . . . , n p -1 (3.15)
Given that multiple-shooting methods require both a higher number of decision variables and a set of nonlinear equality constraints, it may seem that they necessarily perform worse than single-shooting solutions. However, it was shown that this is not the case and that multiple-shooting methods can offer even superior performances [START_REF] Diehl | Fast direct multiple shooting algorithms for optimal robot control[END_REF]. One of the reasons for this is that they allow for a higher degree of parallelization. As an example, given a decision vector, the entries of constraints (3.15) can be evaluated simultaneously for different values of k, since they do not depend on each other. A similar example is related to the computation of the jacobians of the objective and of the equality constraints. In the first case, the evaluation is trivial since the objective is now a quadratic form in the decision variables directly. In the second case, since prediction samples are now independent variables, the jacobians of the constraint vector d (being the stack of all individual equality constraints) take the very simple and sparse forms reported in (3.16). The spar-

∂x ∂u =                              F 0,0 u F 1,1 x F 1,1 u F 2,2 x F 2,2 x F 2,2 u F 3,3 x F 3,3 x F 3,3 x F 3,3 u . . . . . . . . . . . . . . . F nc-1,nc-1 x F nc-1,nc-1 x F nc-1,nc-1 x F nc-1,nc-1 x F nc-1,nc-1 x F nc-1,nc-1 u F nc,nc-1 x F nc,nc-1 x F nc,nc-1 x F nc,nc-1 x F nc,nc-1 x F nc,nc-1 x . . . . . . . . . . . . . . . . . .                              (3.14)
Figure 3.3 -Pictorial example of state prediction in multiple-shooting methods. Given a control sequence (not depicted here) and a set of initial samples (the points in magenta), future states (in red) are evaluated using the model function f d . The result is a "discontinuous signal", whose jumps, each of height

d k = x k+1 -f d (x k , u k )
, should be nullified by the optimization algorithm.

sity is particularly important not only for the parallelization opportunity, but also because it can be exploited in order to efficiently compute solutions to the optimization problem. The result is potentially an algorithm that can converge to a solution in less time than single-shooting methods and possibly returning more stable solutions [START_REF] Giftthaler | A Family of Iterative Gauss-Newton Shooting Methods for Nonlinear Optimal Control[END_REF].

Predictive Control in Visual Servoing

Visual Predictive Control consists in the application of MPC to Visual Servoing tasks. In this section, a short summary of existing VPC schemes taken from the literature is presented, starting from early works that formulated the optimization in terms of a desired

∂d ∂x =             I n -F 1,1 x I n . . . . . . -F n c -1,n c -1 x I n -F n c ,n c -1 x I n . . . . . .             ∂d ∂u =          -F 0,0 u . . . -F n c -1,n c -1 u -F n c ,n c -1 u . . .          (3.16)
sensor pose in the Cartesian space. Afterwards, works that used sensor measurements (image point coordinates) directly in the prediction model are illustrated. The section then concludes with mentions on VPC approaches that introduced new ideas such as the use of reference trajectories or the use of image moments as features.

Generalized Predictive Control

The application of Generalized Predictive Control (GPC) [START_REF] David W Clarke | Generalized predictive control-Part I. The basic algorithm[END_REF] to visual control proposed by Gangloff et al. [GDA98; GM00; GD03] is one of the first examples of Visual Predictive Control, in which a velocity-controlled robot is considered. In this approach, the objective is to control the value of a minimal parameterization of camera's pose with respect to an equilibrium configuration. This relative pose is denoted as p, and its components are given as the 3D position and the roll-pitch-yaw angles of the camera frame with respect to the desired configuration. Its time variation is related to the kinematic screw of the camera by the relation v = J p ṗ, where J p is defined as:

J p =   R c c Φ rpy   (3.17)
wherein R c c is the rotation of the sensor frame with respect to the nominal desired pose, and Φ rpy transforms the rate of change of Euler's angles into angular velocities.

Direct measurements of image points can then be taken into account. Assuming the coordinates of the measured points to be collected in the feature vector s, their variation is related to the pose vector by the interaction matrix and the relation above as:

ṡ = L s J p ṗ (3.18)
An assumption is now made to simplify the problem, that the current value of p is not considerably different from p = 0, i.e., the current sensor pose is close to the desired configuration. Since the value of J p tends to the identity matrix when p → 0, the authors propose therefore to approximate v with ṗ. After this simplification, a firstorder approximation defined around the desired equilibrium values p = 0 and s = s is introduced to obtain an initial estimation of p from the sensor measurements:

p -p L + s (s -s ) (3.19)
This estimation of p is used to initialize the state of the predictive model.

The next step in the modeling task is to consider the dynamics of the controlled manipulator. The robot is supposed to be controllable in velocity, but to take into account that a change in velocity is not immediate, an intermediate block is considered. Assuming that each joint velocity is regulated independently, the input-output relation between the reference command qc i and the actual velocity qi is described by a set of transfer functions F i (s). These are in turn collected in the diagonal transfer matrix F(s). The velocity in the joint space is then linked to the operational space via the analytic jacobian J, such that ṗ = J q. This is used to transform both actual and reference operational velocities, so that the transfer matrix from the command ṗc to the resulting rate of change ṗ is given by JF(s)J -1 . Few additional blocks are also considered: a Zero-Order Holder (ZOH) models the fact that a reference joint velocity command qc is applied for an entire control period, while an integrator is used to produce the signal p from its derivative ṗ. The overall sub-system obtained in this way is commonly referred to as Virtual Cartesian Motion Device (VCMD) since its final objective is to track a given operational velocity set-point. The model of the manipulator is completed by introducing two unit-delays, each one representing respectively the control computation and the image-processing step. The overall linearized model is depicted in Figure 3.4.

An equivalent discrete-time model of the VCMD is finally obtained using a stepinvariant transformation. Considering also the two unit delays, the overall discrete-time transfer matrix from the sampled input ṗc k to the predicted output pk is given by:

G(z) = z -2 1 -z -1 Z JF(s)J -1 s 2 (3.20)
where Z [•] corresponds to the transformation of a continuous-time transfer function to the discrete domain. At each control iteration, J is assumed to be constant, and, considering the diagonal structure of F(s), the overall function is therefore rewritten as:

G(z) = z -2 JH(z)J -1 = z -2 Jdiag a i (z -1 ) b i (z -1 ) J -1 (3.21)
with a i (z) and b i (z) representing a set of polynomials.

The model is then used for the prediction of n p samples, given n c control inputs. The objective is formulated using a quadratic stage cost and takes the form:

z -1 J -1 ZOH F(s) J 1 s z -1 ṗc p VCMD
np k=1 pk -p k 2 + w nc-1 k=0 ṗc k -ṗc k-1 2 (3.22)
with w > 0 being a weighting factor. The objective thus aims at minimizing the predicted pose error as well as reducing variations in the control law.

Although the proposed method does not deal with constraints such as visibility or retraction, it is still interesting as it takes into account the effect of low-level controllers and of the joint dynamics. The validity of the prediction model is only local, as the transfer matrix F(s) should be evaluated around a given configuration in order to get consistent approximations of the joint dynamics, and has therefore to be identified for multiple local configurations.

Features in the optimization objective

Even though the simple linear structure of the previous approach makes the problem easy and efficiently solvable, more complex approaches based on nonlinear predictive controls should be considered for enhanced performances. A work that explored this direction was [START_REF] Sauvee | Image Based Visual Servoing through Nonlinear Model Predictive Control[END_REF], in which the nonlinear dynamic model of a manipulator with 6 degrees of freedom was considered to control the pose of its end-effector, assuming a eye-to-hand configuration and four image points as visual features.

In the design of the prediction model, the VCMD is not used. Instead, a low-level proportional regulator is considered directly at the joint level, which produces an auxiliary acceleration signal that is then transformed into motor efforts by considering the IDM of the manipulator. The joint position and velocity of the robot are updated directly from the acceleration signal, assuming perfect identification of the dynamic parameters.

An interesting improvement proposed in this work is also the direct use of the visual features to define the objective. To take into account the evolution of image points in the image, two assumptions are made:

1. The 3D properties of the object are well-known, i.e., the position P o i of each i-th point in the object frame is available; 2. The transformation T w c from the camera frame to the robot base is available. From these assumptions, the 3D position of each feature point is built at each prediction step from the geometric model of the robot. Image coordinates are then computed from the classical pinhole camera projection. The overall prediction model, from the joint velocity command to the predicted states, is reported in Figure 3.5.

k v 1 s 1 s DGM Sensor Projection qc + q q - Low-level controller q T w o P o i T w c s b (q, q) A(q) q + + τ IDM IDM
Figure 3.5 -Open-loop prediction model considered in [START_REF] Sauvee | Image Based Visual Servoing through Nonlinear Model Predictive Control[END_REF]. The input is a joint velocity command which is internally regulated via a low-level proportional controller.

Assuming the dynamic parameters to be perfectly identified, the current velocity and position are obtained simply by integrating the acceleration, and a prediction of the effort is obtained using the IDM. The geometric model of the manipulator in conjunction with the camera perspective projection allows to obtain image features coordinates.

The nonlinear MPC is then built on top of the presented model. In order to accomplish the visual task, a quadratic stage cost is considered directly on the feature error. In addition, minimization of the control inputs is taken into account as well. Constraints are also introduced in order to ensure features visibility and torque limits, as well as to keep joints within their bounds. This control strategy was compared in simulation against a PI controller, using a sampling period of 5 ms. The control and prediction horizons were set to 1 and 10 respectively, and the optimization was solved using a Sequential Quadratic Programming (SQP) algorithm (refer to Appendix B.3 for additional details). Simulations showed the effectiveness of the proposed approach, in particular concerning constraint handling. The robustness of the controller was also analyzed against relatively large identification errors, both in dynamic parameters and camera calibration. Nonetheless, the computation time was about 400 ms in average, unacceptable for a real-time implementation.

The work by Allibert et al. [START_REF] Allibert | Visual predictive control for manipulators with catadioptric camera[END_REF] considered a similar setup. The studied system is composed by a Scara-like robot equipped with an omni-directional visual sensor which is mounted on the end-effector. The dynamics of the robot is not considered in this case, and the joint state is simply obtained from the control input, i.e., a joint velocity command, using Euler's forward integration:

q k+1 = q k + ∆t qk (3.23)
The measurement of features is instead modeled by the following equation:

  u v   =   α u 0 u 0 0 α v v 0        βX βY 1      β = ξ + m Z + ξ √ X 2 + Y 2 + Z 2 (3.24)
with X, Y and Z being the 3D coordinates of an observed point and the other constants representing the intrinsic parameters of the sensor. As in [START_REF] Sauvee | Image Based Visual Servoing through Nonlinear Model Predictive Control[END_REF], the position of the image points are evaluated using the geometric model of the arm and knowledge of the object structure and location.

This work also proposed to use the so-called Internal Model Control Structure to adjust the desired value of the image features. When predicting the future state of observed points, the approximations introduced in the model will induce an error in the new value. Formally, at the current iteration τ the value of the feature vector s(τ ) and the one predicted by the model, s m (τ ), will differ by a given quantity ε(τ ):

ε(τ ) = s(τ ) -s m (τ ) (3.25)
If we assume that this error is constant for the given model, then the task of tracking the desired sensor configuration s with the real output can be assumed to be equivalent as tracking the modified reference s m with s m . Therefore, it is suggested to use, as desired state in the objective, the values:

s m = s -ε(τ ) (3.26)
The simulations provided in this work clearly show the advantage of using nonlinear predictive controllers to regulate the sensor output while taking into account constraints.

In particular, when considering large displacements classical feedback control might cause the features to leave the field of view, leading to failure. MPC strategies can instead optimize the control input in a way that ensures visibility.

Models based on the interaction matrix

All prediction models presented so far require full knowledge of the observed object, since the calculation of visual features is based on this information and on the use of the geometric model of the controlled robot. However, to update the state of image features one could directly consider the differential relation between their derivatives and the sensor twist by using the interaction matrix. This idea seems to appear for the first time in two different works [LB09; AC09].

In the former, the update model considers the robot as a VCMD whose continuous-time transfer matrix corresponds to a proportional regulator G(s) = diag kv s+kv -analogous to the low level controller of Figure 3.5, but directly applied to sensor's velocity. After transforming it in the discrete domain, the update equation for the feature vector is given as:

s k+1 = s k + ∆t L s G(z -1 )v c k (3.27) in which v c
k corresponds to the desired velocity of the camera at step k, which is assumed to be the control input of the overall system. The interaction matrix is computed at each iteration, using the current feature prediction s k . To deal with depth changes, the authors propose to use a scheme that gives the depth as a function of the desired point coordinates and the current image projection [START_REF] Lazar | Modeling of visual servo open-loop for robot manipulators[END_REF].

After validating the proposed predictor, simulations were presented. They confirm as well that MPC can outperform feedback strategies by finding better motions for the system. In particular, a small pure rotation about the optical axis of the camera is considered. Compared to a PI control, which tries to move the features along straight lines towards their desired location, the MPC is able instead to optimize the motion so that a simpler circular one is performed.

The approach presented in [START_REF] Allibert | What can prediction bring to image-based visual servoing?[END_REF] is similar to that of [START_REF] Lazar | Visual servoing of robot manipulators using model-based predictive control[END_REF], in the sense that an update scheme analogous to (3.27) is exploited. The main differences in this case are that the VCMD is not considered and that the depth at the desired configuration is used to approximate the interaction matrix. Extensive simulations are included to analyze different test-cases, all considering a unitary control horizon.

A first set of simulations focuses on pure rotations around the optical axis of the camera, with different parameters tuning. First of all, a unitary prediction horizon and a 90°rotation are considered. In this case, the path followed by the features is close to the one given by classical control strategies. However, the decay of the feature error is not exponential, due to the fact that some camera velocities will be kept saturated to achieve lower values of the objective. This result is rather interesting in the context of this thesis, as it shows that the convergence rate of the servoing task can be improved even with a unitary prediction horizon. The value of n p is successively increased to either 10 or 20, with a stage cost matrix Q k = I. Thanks to the possibility of foreseeing future samples, the features follow a circular path in the image space, with less retraction in the z-direction. Finally, an increasing weighting policy is tested in the objective, by setting

Q k = 2 k I.
The 2D behavior of the features is similar to the previous case, but in addition the 3D motion of the sensor appears nicer, with even less retraction than in the previous case. This is justified by the fact that the optimization has to favor the minimization of the error in future samples, thus discouraging "greedy behaviors" and leading to motions that, despite being sub-optimal in the short-term, are globally more satisfactory.

A second set of simulations considered larger displacements, requiring both rotational and translational motions of the sensor. Similarly to the case of a pure rotation, it is shown that the use of a larger prediction horizon and of an increasing weighting policy can lead to superior performances, with smooth motions and better decoupling in the sensor's degrees of freedom. The same scenario was also simulated in presence of calibration errors and image processing disturbances to show the robustness of the proposed scheme.

The authors later extended the approach by considering new models in [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF], classified in two categories: Local Models (LM) and Global Models (GM). First of all, the previously proposed predictor, based on L| z=z , was renamed LM p , and a new model was introduced by assuming that depths are estimated at each iteration and used in place of z . This second local approach has been named LM c .

Global methods assume instead to know the 3D coordinates (X i , Y i , Z i ) of the observed points in the camera frame. In this case, point locations are assumed to evolve according to:

     X i Y i Z i      k+1 = R (v k )      X i Y i Z i      k + t (v k ) (3.28)
where R ∈ SO(3) and t ∈ R 3 define a homogeneous transformation matrix. This matrix corresponds to the local change of pose of the sensor between the steps k and k + 1 due to the action of the control input v k . From the 3D information, the output is obtained by normalizing the coordinates of the points:

s i k =   u i v i   k =   X i /Z i Y i /Z i   k (3.29)
Similar to the case of Local Models, two schemes were considered depending on the availability of points depth at the current control iteration. GM p assumes that the 3D state is initialized from the current image measurements s and the depth at equilibrium

Z i :      X i Y i Z i      0 =      x i Z i y i Z i Z i      τ (3.30)
whereas GM c substitutes Z i with the current depth estimation Ẑi τ . Simulation results reported in [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF] show the benefits of predictive controllers, especially when the depth is updated during the estimation. A remarkable observation included in the analysis concerns the so-called Optimal IBVS strategy. This scheme corresponds to a VPC with unitary control and prediction step. As already reported before, this choice leads to a behavior that is generally similar to what a feedback control would do, but with enhanced convergence rate. Nonetheless, a major advantage consists in the ability of Optimal IBVS to deal with constraints such as visibility. As shown in Figure 3.6(a), the controller is able to find a solution that accomplishes the visual task without letting features leave the field of view. There is however a drawback: since the prediction horizon is limited to a single step in the future, the control law is not smooth, with a sort of "bang-bang" action (see Figure 3.6(b)).

The problem with Optimal IBVS is easily solved by extending the prediction horizon, giving smoother and more effective results. In addition, the use of variable depth in the prediction process allows to explicitly enforce better 3D behavior of camera's mo- tion. Considering the case of large rotational displacements, previously existing VPCs were exploiting the simple idea that when a short control horizon is selected, the optimal input sequence generally corresponds to an almost pure rotation with reduced translation components, at least in comparison with classical approaches. However, once depth is considered the retraction problem can be handled explicitly by imposing workspace bounds. As an example Figure 3.6(d) includes results from the study in which both 2D and 3D constraints are included. By constraining the vertical workspace of the sensor as

Z c ∈ [-0.6 m, -0.5 m],
retraction is directly prevented.

Visual Predictive Control with reference trajectory

As stated in the beginning of this section, the stage cost for the optimization does not need to consider a fixed setpoint. Instead, the predicted future states x k might be required to track a given reference trajectory x k (cfr. (3.9)). If a way to define such a (possibly feasible) trajectory is available, this can be advantageous in order to help the system evolve in a controlled way.

An example in this regard can be found in the work from Lazar et al. [START_REF] Lazar | Predictive control architecture for visual servoing of robot manipulators[END_REF] which adds to the VPC presented in [START_REF] Lazar | Visual servoing of robot manipulators using model-based predictive control[END_REF] a trajectory generator based on homographies as described in [START_REF] Allotta | 3D Motion planning for imagebased visual servoing tasks[END_REF]. It is well known that when a set of coplanar (and non-collinear) points is observed from two different perspectives a and b, their coordinates obey the homography relation [START_REF] Hartley | Multiple view geometry in computer vision[END_REF]:

P b = H P a = R b a + t b a n T a d a P a (3.31)
where R b a and t b a represent the homogeneous transformation from a to b, while the couple n a , d a defines the plane equation in a, i.e., such that n T a P a = d a . By taking into account camera intrinsic parameters by means of a matrix K, the relation between points given in image coordinates becomes:

  s b 1   = αKHK -1   s a 1   = G   s a 1   (3.32)
with α a scaling parameter (theoretically equal to the depth ratio of the points in the two perspectives) and G named collineation matrix. By considering the feature set at the current iteration and at equilibrium, a link is established by (3.32) involving the current pose of the camera with respect to its desired one. In order to produce a suitable reference trajectory in the image space, it is possible to consider a continuous sequence of homogeneous transformations in the form:

H (ρ) = R (ρ) + t (ρ) n T i d i (3.33)
with d i and n i defining the equation of the observed plane at the initial view, and ρ a monotonic function of time ranging from 0 to 1. If we impose R(0) = I, t(0) = 0, R(1) = R i and t(1) = t i , the resulting collineation matrix is such that G(ρ)s i will change from the current values of s to s .

In order to obtain a smooth motion from the current time τ and the prediction horizon, the interpolation variable ρ can be chosen, e.g., as a quintic polynomial in the normalized time t-τ Tp , with initial conditions ρ(τ ) = 0, ρ(τ + T p ) = 1 and ρ(0) = ρ(τ + T p ) = ρ(0) = ρ(τ + T p ) = 0. An alternative design of the interpolation variable might impose a nonzero initial condition on ρ [START_REF] Copot | Predictive control of nonlinear visual servoing systems using image moments[END_REF]. This results in reference trajectories that approach the equilibrium configuration in shorter time -obviously, the initial condition should be selected not to cause overshooting.

Regarding the interpolation of R and t, three cases can be considered:

1. Pure translation: the rotation matrix R is kept as the identity, and linear interpolation is performed as t(ρ) = ρ t i .

2. Pure rotation: the translation is kept constant to zero, while spherical linear interpolation (SLERP) can be used for the rotation matrix (either in terms of Rodrigues' formula or using quaternions).

3. Rotation and translation: the desired orientation is still obtained via SLERP, while the translational path is computed using a helicoidal shape. The linear translation axis of the helix is chosen aligned to the rotation vector θu corresponding to R i .

The overall VPC strategy is then almost identical to previously presented approaches, with s being replaced by the generated time-dependent reference samples s k .

As a concluding remark on the reported procedure, it must be noted that in practice it is possible to recover the relative translation t i only up to a scaling factor. Therefore, in the proposed approach the translational motion should actually be replaced by a scaled version. Nonetheless, this does not alter the validity of (3.32), in the sense that the obtained feature projection will be identical no matter the value of the unknown scale factor.

Visual Predictive Control with image moments

Image moments have been considered as well for the design of Visual Predictive Controllers [CBL11; CLB12]. As already discussed, (planar) image moments can be evaluated from a set of image points using either (1.17) or (1.18) depending on whether continuous or discrete moments are considered, with the related interaction matrices given by (A.38). Following the same notation adopted in related works, the transformation giving image moments from a set of points will be denoted by the symbol M.

Two main approaches can be considered in order to establish a prediction model [START_REF] Copot | Predictive control of nonlinear visual servoing systems using image moments[END_REF]. In the first case, a set of point features s is considered and their future value is estimated using the interaction matrix as in [START_REF] Lazar | Visual servoing of robot manipulators using model-based predictive control[END_REF] (see (3.27)). In this matrix, however, the inverse depth is replaced by the corresponding value computed from the plane parameters A, B and C, i.e., for the i-th image point 1

Z i = Ax i + By i + C.
This strategy gives a sequence of predicted states s k which can then be transformed into corresponding image moments m, i.e., m k = M (s k ).

The second strategy operates directly on moments sets. Starting from the current value of point features, s 0 , corresponding moments m 0 are obtained using M. For future predictions, the interaction matrix of image moments is assumed constant over the prediction horizon, and is used to obtain predictions for the image moments as:

m k+1 = m k + ∆t L m v k (3.34)
In practice, the update model considers L m to be the half-sum of the interaction matrix computed at the current iteration and at equilibrium, i.e.,

L m = 1 2 (L m (m 0 ) + L m (m )) (3.35)
Two VPC schemes were designed from the given prediction models. The objective can be designed to either reach a fixed set-point as in the classical setup or to track a reference trajectory using the generator illustrated in the previous section. In this case, to define the target motion in terms of image moments, it is possible to exploit the homographybased approach to firstly define a reference trajectory for the image points and then to transform them into reference moments via M. Regarding the weighting matrices used in the stage cost, it is proposed to use Q k = e k I when a fixed set-point is considered as target, thus increasing the weight to achieve a faster response towards the goal. Conversely, when considering a trajectory over the prediction horizon, Q k = e -k I is used, in order to enforce better tracking of the time-varying reference.

Proposed Contributions to Visual Predictive Control

In previous sections, an introduction to Model Predictive Control has been presented, with a focus on Visual Servoing applications. The reason to consider such kind of control strategy for the purposes of this thesis is that, as discussed, when the initial configuration is characterized by large displacements with respect to the desired one, the optimization generally leads to shorter and more effective motions of the controlled system. In addition, the feature error usually decreases with a faster response than the one given by the exponential decay even when the current configuration approaches the target one.

There are different directions that can be considered to extend existing approaches, one of them consisting in considering new prediction models. In particular, the use of second-order approximations of the discrete-time feature prediction seemed a viable new method to be investigated. Another factor to be taken into account is related to redundancy. While the outlined studies focused on manipulators featuring 6 degrees of freedom, our experimental platform is instead redundant with respect to the visual task. As such, a proper modified objective should be considered to properly exploit the additional degree of freedom. Finally, it must be noted that the computational cost of the nonlinear optimization can be relatively high. Given the objective of designing a fast control scheme, some attention should be dedicated to an efficient implementation allowing to obtain control inputs in real-time with sufficiently fast motions.

Predictors Based on Second-Order Models

The first proposed contribution to VPC in this thesis consists in taking into account higher-order models in the predictive update equation. In fact, by taking into account also the acceleration of the visual features, the resulting prediction should be more accurate and therefore lead to better control sequences.

Similarly to what was proposed in [LB09; AC09], we propose to use a local model in which the feature vector of the system is updated only using local information on the features derivatives. However, contrarily to existing models, we propose to exploit not only the interaction matrix of the feature set, but also second-order information in the update equations. More precisely, it is proposed to predict the value of the feature vector at the next iteration using the following model, corresponding to a second-order Taylor expansion of the features evolution:

s k+1 = s k + ∆t L s v k + 1 2 ∆t 2 (L s a k + h s ) (3.36)
In the formula, both L s and h s are evaluated using the values contained in s k and z k ,

i.e., the interaction models are updated at each prediction step.

Regarding the estimation of the features parameters z k along the prediction horizon, there are mainly two possibilities. The first one is to keep their value constant, either to the equilibrium value z or to the last available estimation before starting the optimization. Another strategy would be to predict their value as well, which we propose to do in a similar fashion to (3.36):

z k+1 = z k + ∆t L z v k + 1 2 ∆t 2 (L z a k + h z ) (3.37)
In principle, the two prediction equations presented above require the spatial velocity v to be a state of the model as well, and the spatial acceleration a to be the control input. One could design a control scheme in this way and, at each iteration, recover the joint commands to be sent to the manipulator by means of its geometric jacobian. However, directly taking into account the evolution of the joint state of the robot during the predictive optimization allows to consider a broader set of constraints, such as joint limits. For this reason, we assume that the system is controlled via joint acceleration commands, i.e., u = q. In this case, the full state of the manipulator can be selected as:

x =         q q s z         (3.38)
that is, by considering the joint positions/velocities and the observed features (with the related parameters). Considering a piece-wise constant input, the joint state of the manipulator can be updated exactly from:

  q k+1 qk+1   =   q k + ∆t qk + 1 2 ∆t 2 qk qk + ∆t qk   (3.39)
whereas the second-order approximations (3.36) and (3.37) are used to evaluate the features and the related parameters. Vectors v k and a k are evaluated in function of joints positions, velocities and accelerations via the Forward Kinematic Model of the arm. Note that keeping track of the time derivatives of s and z inside the state is not necessary, since they can be obtained at each prediction step directly from the interaction matrix and v k .

As it will be shown later, the new predictor proposed above has been implemented and tested both in simulation and with the real robot, and, with the objective of evaluating its performances against classical approaches, a predictor based solely on first-order information was also considered. Its state consists in the joint coordinates q of the manipulator, altogether with the features s and the visual parameters z. The control input is assumed to be the joint velocity of the manipulator, i.e., u k = qk , and the prediction model thus writes as:

     q k+1 s k+1 z k+1      =      q k s k z k      + ∆t      I J s J z      u k (3.40)
wherein J s is the feature Jacobian of s, as defined in (1.6a). Similarly, J z corresponds to

L z T c w J.
In order to distinguish between this prediction model and the one that integrates also features acceleration information, we will refer to the former as M 1 and to the latter as M 2 .

One disadvantage related to M 2 compared to M 1 is its larger state vector, which leads to a higher computational burden in the optimization. To overcome this issue, a new model is thus proposed, lying between classical approaches based solely on velocity information and the acceleration-controlled model presented above. Its objective is to keep the state vector of the same size as in M 1 , while integrating higher-order information similarly to M 2 . In particular, the new model assumes the control input u to be the velocity of the manipulator like in classical approaches. However, it also exploits the fact that the second term in (2.13) depends only on the current features/parameters and the velocity of the sensor. As it is done in classical local models, we assume that the twist of the camera is piece-wise constant, and at the time-sample k it is thus possible to evaluate v k as T c w Ju k . Altogether with s k and z k , this allows to approximate features acceleration as sk h s (the acceleration of the sensor is therefore not taken into account). Using a similar procedure to evaluate zk , this provides the following update scheme:

     q k+1 s k+1 z k+1      =      q k + ∆t qk s k + ∆t L s v k + 1 2 ∆t 2 h s z k + ∆t L z v k + 1 2 ∆t 2 h z      (3.41)
Since this model is a mix between those based on first and second order models, we like to refer to it as "hybrid" predictor, M H .

To illustrate why the proposed models can lead to better results, Figure 3.7 shows a simple example of predictions made using the three schemes detailed above. This case considers a free-flying camera that observes four image points while retracting and rotating about its optical axis. The velocity and acceleration of the sensor, which are both continuous and non-constant, are sampled with a period of 50 ms, and fed to each model to perform predictions for s and z in open-loop. M 1 slightly drifts from the actual state during time, whereas M 2 provides the most reliable predictions. Finally, the hybrid predictor M H , despite being less accurate than M 2 due to the truncated approximation of s, provides a rather good prediction of the real evolution. Figure 3.7 -On the left, the predictions of the evolution of a normalized point's coordinates. The solid black line is the ground-truth, while dashed lines are obtained using different models fed with the applied control inputs sampled at ∆t = 0.05 s. On the right, the prediction error of each model with respect to the ground-truth.

Visual Predictive Control Scheme

The proposed overall VPC scheme solves the optimal control problem using a singleshooting direct method, as presented in Section 3.1 and 3.1.1 in particular, with some adaptations specific to our studied case.

First of all, the main objective of the controller is to steer the features from the initial configuration to a fixed value s . There is instead no need to ensure the convergence of neither q nor z to any specific value. For this reason, the matrices Q k+1 in the stage cost (3.9) are selected as diagonal, with the elements corresponding to the joint configuration and to the features parameters equal to zero.

Regarding the weight associated to the features, the diagonal weighting matrix Q (s) k+1 = α k I is adopted, α being a tunable scalar parameter, similarly to what was done in some of the related works presented in previous sections. More specifically, it appears that decreasing weighting policies are beneficial when used altogether with an online trajectory generator [CBL11; BCL14], while it was shown that increasing weights can bear to higher decoupling in the camera motion, generally shorter paths and faster convergence [AC09; ACC10]. As the scenarios presented in the following sections fall into this second category, α > 1 was chosen.

Given the redundancy of the controlled manipulator, additional objectives can be considered in the optimization. In particular, a contribution should be added to minimize the velocity of the manipulator, since the visual task constrains only six degrees of freedom and is thus not sufficient to fully stabilize the joint position. By minimizing the velocity of the manipulator, once the robot has completed the visual task the optimal solution becomes to stop all motors, thus solving the problem. To do that, it is possible to use the constant diagonal matrix Q ( q) = βI, β > 0 controlling the relative importance with respect to the visual task. It must be noted that the objective is to minimize the joint velocity independently from the model used for predictions. Since M 1 and M H both assume a velocity control, the matrices of the quadratic stage cost write in these cases as:

Q k+1 =      O α k I O      R k = βI (3.42)
On the other hand, when dealing with M 2 the joint velocity is part of the state vector.

As such, the weighting matrix is formally to be inserted as a part of Q k+1 , i.e.,

Q k+1 =         O βI α k I O         (3.43)
with the desired state q k simply being a zero-vector. Note that in this case, a term that tries to minimize the input is not considered, i.e., R k = O. Indeed, minimizing the acceleration would lead to a conservative velocity behavior, resulting in potential instability or longer time to convergence.

In principle, one could try to exploit the redundancy in other ways than to damp the joint velocity of the controlled system. As an example, three other criteria have been taken into consideration: the minimization of the joint efforts, of the gravitational potential and of the kinetic energy. Despite these options being attractive in theory, it turned out that they are not necessarily effective in practice. In fact, the first criterion encourages the system to move according to its free dynamic evolution, which is almost always in contrast with the Servoing task and does not guarantee the stabilization of the joint angles to a given value. Similar issues are encountered when attempting to minimize the gravitational efforts, since the optimal solution would "push" the manipulator downward, which is generally in contrast with the visual task. The optimization thus ends up in a local minimum that constitutes a compromise between the two components, without however satisfying neither of them completely. Finally, the minimization of the kinetic energy can be viewed as a special form of the quadratic stage cost, in which the weighting matrix Q ( q) corresponds to the generalized inertia matrix A(q k ). In this sense, the criterion is compatible with the formulation presented above, as it simply provides a different weighting of the velocity components. However, it was found that in practice it does not bear major differences with respect to the simpler, computationally lighter, constant weighting matrix.

Two kinds of state constraints are considered during the optimization, in addition to control input bounds. The first one is introduced to keep the joint configuration of the robot within the allowed limits, i.e., q min ≤ q k ≤ q max , ∀k = 1, • • • , n p . The second set of state constraints is introduced instead to prevent the object to leave the field of view of the camera. In particular, when servoing from image points this constraint simply writes as s min ≤ s k ≤ s max , with s containing the x and y coordinates of the observed points.

When polar coordinates are used, the constraints above are expressed for each point i in terms of the features ρ i and θ i as:

x min ≤ ρ i,k cos θ i,k ≤ x max y min ≤ ρ i,k sin θ i,k ≤ y max (3.44)

Implementation Details

The optimization problem of the VPC scheme detailed in the previous section was solved using a single-shooting direct method. More in details, the problem was formulated as in Section 3.1.1, exploiting also the Internal Model Control Structure already adopted in [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF] in order to reduce the effect of modeling uncertainties.

It must be noted that, in order to achieve relatively high performances, the optimization needs to be solved rather quickly. In fact, assuming a desired control frequency of 50 Hz, the maximum allowed optimization time amounts to 20 ms. To solve the optimization it was thus decided to use Sequential Quadratic Programming, which is a fast local optimization strategy. The algorithm is well suited to the problem at hand for different reasons, the main one being related to the iterative nature of the VPC scheme. In fact, if on one hand a new optimization has to be solved at each control iteration, on the other it is also reasonable to expect that successive problem definitions -and consequently their solutions -will not differ too drastically, given the relatively high control frequency. This allows to re-use the solution to one problem instance as the initial guess for the following iteration, and to perform a local refinement to evaluate the new optimum. SQP algorithms can provide quadratic convergence to a local optimum, while simultaneously enforcing equality and inequality constraints, and they are thus an ideal candidate for VPC optimizations.

Another interesting aspect of using SQP in a single-shooting method is that, during a single optimization process, the objective is monotonically reduced during successive iterations. The consequence is that, even if the optimization is halted prematurely for some external reason, the algorithm can return a control sequence that cannot be worse than the initial guess, and hopefully is "near enough" to the optimum. In addition, the returned solution is consistent with the prediction model, since constraint (3.10a) is implicitly enforced in single-shooting methods. This aspect is important, since, as it will be detailed later in this chapter, under some circumstances the VPC optimization would have required more than 20 ms to complete. Since this is incompatible with a real-time implementation on the real hardware, the optimization needs to be stopped even if the optimum has not been reached. As it will be shown, this is not necessarily a hard limit and the proposed schemes can still run in practice.

The only thing that is still required for the scheme to be effective, is a good initialization of the control inputs. Here, it is proposed to let a further optimization algorithm evaluate this initial guess. The computation is performed as an offline pre-optimization, which is allowed to consume a large maximum time, while the system is held static in its initial state.

To conclude this section, few details are now presented regarding the software implementation of the outlined control scheme, which ultimately resulted in a C++ library. To solve the constrained nonlinear optimization problems, the open-source library NLopt [Joh] was chosen. Among the large list of available algorithms, SLSQP [Kra88; Kra94] and ISRES [START_REF] Philip | Search biases in constrained evolutionary optimization[END_REF] were employed. In particular, the former is a SQP implementation that was used mainly for the local optimization during each VPC iteration. It was also used to obtain a candidate guess in the pre-optimization phase, starting from a null control sequence. The second algorithm, which is instead a global optimization routine, was also exploited in the pre-optimization step to obtain a further initial guess of the control inputs. In practice, the sequences returned by the two strategies were compared at the end of the offline initialization and the best one was kept and used as seed for the VPC iterative scheme. SLSQP requires gradients, both for the objective and the constraints, to be supplied in order to build and solve the optimization. These can be evaluated on top of (3.14) since a single shooting method is used. Given the possibly large dimensionality of the problem, the evaluation of the different functions and their gradients can become computationally expensive. Therefore, a set of libraries was selected in order to speed up as much as possible the process. In particular, the Eigen library [GJO10] was used for the matrix arithmetic, allowing to exploit processor parallelization and resulting in overall faster computations. Pinocchio [CM18; Car+19] was instead exploited to represent the manipulator and to compute its geometric and kinematic models. Finally, the code generation utilities provided by CppADCodeGen [START_REF] Rui | CppADCodeGen: C++ Algorithmic Differentiation with Source Code Generation[END_REF] were used to compile the prediction models to further reduce the execution times.

Simulations

To investigate the performances of the proposed models, several simulations were conducted. The main goal was to gather preliminary data related to the behavior of the VPC schemes resulting from the choices detailed in the previous sections, and also to study the effect of the different parameters that appear in the problem formulation. This was also convenient in order to obtain a rough initial estimation of the values that could then be used in real experiments.

All simulations were run in a simple environment that updates the state of the robot by integrating velocity or acceleration commands, therefore not taking into account the dynamic model of the manipulator. Four image points were used as visual features, corresponding to the vertices of a squared shape of side 0.2 m. In all tests, the arm was required to reposition the camera in front of the squared object at a distance of 0.5 m, starting from different poses depending on the simulation. The object was kept static for the entire duration of each test, and thus the feature vector changed only due to the motion of the manipulator. To simulate the visual feedback gathered by a camera, the coordinates of the four points were evaluated using the geometric model of the Kuka arm, since all transformations were perfectly known in simulation. The evaluated features were finally sent at a rate of 50 Hz, to match the acquisition frequency used in real experiments, and the control period ∆t c was thus set to 20 ms. As discussed in the previous section, the optimization might require more time than the control period to be solved to the desired precision. To achieve real-time feasibility a time limit of 0.9 • ∆t c = 18 ms was imposed in NLopt as termination condition in addition to other stopping criteria.

Regarding the remaining parameters of the VPC scheme, the prediction horizon was set to n p = 10. This implies that the control horizon n c has to be chosen between 1 and 10, with smaller values corresponding to faster optimizations, but with less flexibility. In the next sections, simulations are reported for the two cases n c = 1 and n c = 10.

The sampling time ∆t used in the three predictors could be chosen equal to the control period ∆t c . However, it would lead to a prediction vector spanning a rather short time period, i.e., T p = n p • ∆t = 200 ms. Larger values would lead to better performances in theory, but it would require either to increment the prediction horizon or to use a larger value for ∆t. While the first option implies an increase in the computational load, the second one does not change the complexity of the optimization. However, it leads to larger modeling errors due to a less accurate discrete-time approximation of the controlled system. Nonetheless, it was shown by experimental comparison that this is not a critical issue for the proposed predictors, which are more accurate thanks to the inclusion of acceleration information. To support this claim, the included simulations consider different values for the prediction sampling time. In particular, both ∆t = ∆t c and ∆t = 5∆t c = 0.1 s have been used.

Finally, the coefficients α and β appearing in the objective were tuned by trial and error. In particular, the process was split in two different steps, the first one consisting in fixing α to 1 while tuning β. Its value was chosen to be small compared to the one of α in order to allow motions to be rather fast in the beginning, thus achieving quicker convergence to the desired feature configuration. It was noticed that depending on the kind of control input, the value needed to be adjusted differently: β = 10 -3 was used for velocity-controlled models (M 1 and M H ). Regarding M 2 , a smaller factor proved to work better in general, and thus β was set to 10 -4 in that case. The second step in the tuning procedure consisted in finding a suitable value for α. As discussed in Section 3.3.2, it was decided to use a value greater than 1 in order to disfavor greedy motions that would move the features straight towards the goal configuration. It was noticed that this is a suitable choice especially when the camera has to perform large rotations, like in most of the reported simulations. The parameter was tuned by considering the weight associated to the last prediction sample, α . = α np = α 10 . By considering few different values for it, we could coarsely tune α. Our final choice was α = 2.5, leading to α 1.1.

First set of simulation results

This first set of simulations was used to form a base case for later comparison. The prediction sampling time was set equal to the control period, i.e., ∆t = ∆t c , and a unit control horizon was used. This corresponds to a reduced dimension of the decision vector in the optimization, and consequently solutions can be evaluated in a relatively short amount of time.

To test and compare the performances of the different predictors, three scenarios were considered, each characterized by a different initial pose of the sensor. In the first two, the camera was located in the correct position with respect to the object, but rotated around its optical axis of either 90°or 180°. The last scenario also required the sensor to perform a half-turn around the optical axis, as well as a small translation in all directions, for a total displacement of 7 cm. Figure 3.8 shows the results obtained in the first scenario. In the case of classical feedback laws, the paths followed by image point features in the image plane would be almost straight lines, resulting in sensor retraction. Instead, the optimal motion would be a pure rotation about camera's optical axis, and as shown in Figures 3.8(a) and 3.8(g), the predictive laws based on M 1 and M H are almost able to perform it. Distortions are likely introduced by the fact that the minimization of the velocity is done in the joint space, and therefore the camera velocities are not perfectly decoupled. Nonetheless, their performances are sufficiently satisfactory, especially in terms of convergence, which is achieved in about 0.5 s.

On the other hand, with the current settings the controller based on M 2 provides unsatisfactory results. In particular, despite presenting a very smooth motion in the beginning, the sensor retracts quite significantly. Moreover, when reaching a neighborhood of the desired configuration the system shows an oscillatory behavior. The oscillations are damped along the simulation, and are eventually suppressed after a significant amount of time -more than the 3 s shown in Figure 3.8. This behavior can be explained by the fact that, when n c < n p , the last evaluated input is applied again for n p -n c times. Since this model assumes an acceleration command, a constant input over a relatively large time interval leads to a growth in velocity and steers the system away from the desired features configuration. In addition, the prediction time interval is not long enough to anticipate and compensate for this unwanted velocity growth. It would be possible to reduce the oscillations by increasing the weight of the velocity objective coefficient, but the time required to complete the task would also grow significantly.

Before advancing, it is important to remark that the evolution of the two velocitycontrolled VPCs is almost indistinguishable. This is justified by the fact that the only difference in the two predictors is given by the terms 1 2 ∆t 2 h s and 1 2 ∆t 2 h z introduced in (3.41): given that the sampling period ∆t is rather small, these terms are not significant enough to make a difference in the control scheme. This holds for the other two scenarios presented in this section. However, as it will be shown later, with a larger value of ∆t differences can finally be appreciated.

It is now possible to consider the second scenario, involving a π rad rotation about the optical axis, whose results are depicted in Figure 3.9. As anticipated in the remark, M 1 and M H perform similarly, and, notably, worse than in the previous test. The reason is likely to be found in the prediction period T p not being long enough to properly optimize the motion. Furthermore, the acceleration-controlled VPC is now unstable, with persistent oscillations, for the same reasons detailed before. Again, one way to reduce the oscillations could consist in increasing the weight associated to the velocity minimization task, but the total time needed to complete the task in this case would be significantly larger than that needed by M 1 and M H .

To conclude this first set of simulations, Figure 3.10 shows the results obtained in the last scenario. Performances are not dissimilar to the previous case, with velocity-controlled models converging to the desired configuration with a sub-optimal motion and M 2 still featuring large, uncontrolled, oscillations.

Second set of simulation results

Results reported so far seem to suggest that there is little interest in using the models proposed earlier, since M H performs very similarly to M 1 and M 2 seems to be impractical when medium-large motions have to be performed. However, the results reported in this section will show that this is not always true, and that the performances of the proposed models can fortunately be increased.

One issue that appears clearly in all approaches is that, having a high control frequency, the prediction horizon is not long enough to foresee what the optimal motion should be in the case of large displacements. In the presented setup, the prediction horizon allows in fact to predict over a time interval of just 0.2 s. It is argued here that this is one of the key factors limiting the performances of the models in the previous simulations. As already mentioned, one possible way to improve performances would therefore be to increase n p . However, this would also imply a larger computational cost. One alternative is instead to use a larger time discretization step ∆t in the prediction models, as this allows to increase the time horizon without having to change n p , thus keeping the same complexity for the optimization. This second option has its own different drawback: it introduces larger modeling errors, since the predictors are obtained from a truncated expansion of the real features evolution. Nonetheless, it is claimed here that models including the acceleration information are less affected by this issue, since they are intrinsically more accurate. In conclusion, in the attempt to obtain better performances the tests shown in this section all used a larger sampling time, ∆t = 5∆t c = 100 ms. Note that the increased sampling time in the prediction does not mean that the control frequency is lowered as well. On the contrary, the optimization is still repeated every ∆t c = 20 ms. Considering this fact, a slight change is also introduced in the definition of the seed fed to the optimization algorithm at the next iteration. As remarked in Section 3.3.3, the SQP algorithm used for the optimization successively refines an initial guess supplied by the user, and providing a good starting point is important in order to achieve faster performances. Thus, to help the optimization start from a better point, we propose a very simple heuristic in which the controls u (init) k used for initialization of the next step are evaluated from the last available solution samples u k by interpolation:

u (init) k =      u k + ∆tc ∆t (u k+1 -u k ) (k < n c -1) u k (k = n c -1) (3.45)
The same tests presented in the previous paragraph were then repeated with the increased time-step. Results from the first scenario, reported in Figure 3.11, show that good motions are still found by both M 1 and M H , apparently even a little smoother than before. Interestingly, the motion obtained in the space using M 2 is much shorter than before, and the evolution in the image space is also closer to that of the other two predictors. However, oscillations near to the target configuration are still present.

It must be noted that the convergence time is higher in comparison with the previous simulations. A possible explanation for this behavior is the simultaneous use of a longer sampling time ∆t and of a unitary control horizon. To explain why this might be the cause for an overall slower evolution, imagine the system to be like a car that has to drive at constant velocity v for a fixed amount of time T p . If the goal is to reach a location at distance d, then the "optimal" velocity at which the car should proceed in order to be at the target after the time T p would trivially be v = d/T p . Even if this does not exactly describe what happens in the simulations with the manipulator, it provides an close enough example: if T p is increased -which is exactly what has been done in this section, by increasing ∆t -we can expect velocities to be lower, justifying the increment in time to complete the task.

The results from the other two scenarios are particularly interesting. Specifically, when the camera has to perform only the 180°rotation (see Figure 3.12) two facts should be noted. First of all, the performances obtained with M 2 are similar to those of the classical predictor. Secondly, M H now performed a much shorter trajectory compared to the one obtained previously and shown in Figure 3.9. Unfortunately, like in the first scenario, the execution of the task is slower compared to the timing performances obtained in the previous section.

The third scenario offers similar results. As depicted in Figure 3.13, M 2 almost provides a nice motion, was not for the familiar oscillations that appear as soon as the camera has reached a neighborhood of the desired configuration. Moreover, M H ranks first also in this scenario, with the shortest motion among the three predictors.

Third set of simulation results

It has just been shown that using a larger discretization step allows to obtain better results with the proposed predictors, as they can take into account states that are farther in the future and consequently optimize their motion. A side-effect was however found, ultimately resulting in poorer timing performances. As discussed, this is likely due to the fact that, as a single control sample has to be re-used over the whole prediction, it has to ensure that no overshoots appear.

A better trade-off could be found by increasing n c . The optimization algorithm would have in this way the ability to exploit larger inputs at least in the beginning of the prediction horizon, slowing down the robot with the last control samples once the sensor has already approached the desired configuration.

Another reason for considering the use of larger control horizons is related to M 2 . Up to now, no stable behavior could be fully ensured using this model, ending up almost always with oscillations. As already suggested, this might be due to the fact that a single acceleration signal prevents the system to stably reach the desired image configuration and stop the robot at the same time. Not forcing the optimizer to re-use the same acceleration sample along the whole prediction horizon would hopefully allow to find better motions, in which the system initially gains velocity and then decelerates as needed.

For the reasons above, a further set of simulations was run, featuring a larger control horizon. In particular, n c = n p = 10 was used, providing the highest number of degrees of freedom to the optimization. This came at the price of a heavier computational load, causing the optimizations in each simulation to halt due to the time-limit criterion. Nonetheless, this was not a major limitation, as discussed later.

Figure 3.14 depicts the results obtained with the increased control horizon in the first scenario. The superiority of the proposed predictors is here arguably undeniable. First of all, the features are moved by M 1 along the sides of an almost rectangular shape, which somehow reminds of the trajectories obtained with proportional controllers, also resulting in retraction. On the other hand, both M 2 and M H lead to better motions: the motion of the sensor is much closer to a pure rotation, with smooth movements and nearly no retraction. Remarkably, M 2 is almost no longer affected by oscillations, with just a slight overshoot in the features error (see Figure 3.14(e)), yet far from what it used to be in the previous tests. Finally, the timing performances are also satisfactory, suggesting the correctness of the explanation for the longer execution times provided in the previous section. Similar results are found when the sensor has to perform a doubled rotation, as shown in Figure 3.15, as well as in presence of translation, cfr. Figure 3.16. All VPC schemes are able to quickly complete the servoing task in both scenarios. However, M 1 appears to be the worst strategy, in the sense that the observed image features present discontinuities and the resulting 3D behavior is the one with the largest retraction. The proposed predictors lead instead to smooth trajectories with a more decoupled sensor motion in Cartesian space.

Discussion

Before presenting the experimental results obtained with the real robot in the next section, few remarks are included to clarify the influence of some factors that were not discussed in the previous sections.

Let us start with a note on the time required to solve the optimizations. Table 3.1 reports the average computation times needed by each optimization strategy depending on the test set and the scenario. When considering a simple optimization with just a single control sample (test sets 1 and 2) the optimization can be completed much earlier than the maximum allowed time. However, when the control horizon is increased all strategies do not actually converge to the minimum within the allotted time. Instead, they simply return the best control sequence found so far. One obvious question is whether the performances of M 1 could be improved to match those of the other models simply by allowing longer computations, and in such case, how much time would be needed. The last set of simulations was therefore repeated by changing the time limit, while keeping the control frequency unchanged (this investigation was thus possible only in simulation). It turns out that the performances of the three predictors are not significantly enhanced even if the controller is allowed to take as much as 200 ms to solve the minimization problem. Conversely, the time limit was also reduced to better understand when the premature stopping can induce instability in the control. In this sense, the performances were not heavily affected until the limit was reduced to less than 13 ms.

Another aspect worth discussing is the sensitivity of the three VPCs to changes in the objective coefficients. Since these values are tuned by trial and error, it is interesting to better understand how precisely (or coarsely) they need to be estimated. In this regard, it was noted that the parameter β does not need to be finely tuned and that it is instead sufficient to find a suitable order of magnitude. The situation is different when tuning the value of α. Since it is the base of an exponential term, small changes can lead to completely different values of the individual state weights in the objective. This is why, while discussing the tuning selected for the presented simulations, it was proposed to select a value for α np rather than a direct selection of α. Indeed, it was found that the predictive schemes are much less sensitive to changes in the value of α np , which also provides a more intuitive way of tuning the feature weights.

Finally, in all simulations presented so far, no rotations around the X and Y axes of the sensor were necessary, and the translational motion required in the last scenario was rather small. The choice of focusing on rotational motions was justified by the ease of interpretation of the results. However, in order to show that the performances obtained with the proposed predictors are consistent with other types of scenarios, we conclude this section by including a further test. In this case, the motion to be performed is more generic. A longer translation is required (20 cm in total), as well as a generic rotation of 15°, 30°and 165°around the X, Y and Z axes respectively. The motions are depicted in Figure 3.17, and were obtained with the exact same parameters used in the third set of simulations presented in Section 3.3.4. The proposed predictors provide smoother motions, less retraction and take slightly less to complete the servoing task, confirming the results found so far.

Experiments

This section details the experimental results obtained while comparing the different models. Similarly to what was done in simulation, the task consists in positioning the sensor in front of a planar object with four black circles, detected using the ViSP library [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. In addition, in order to support the effectiveness and generality of the proposed approaches, the controllers were tested with different features: standard image point coordinates were firstly considered, as in the simulations included above. In a second moment, also the polar coordinates of the centers of the four circles printed on the object were used (their second-order models can be found in Appendix A.3). In both cases, our predictors outperformed classical local models, confirming the validity of our work.

Let us mention for sake of completeness that since our hardware does not yet allow to directly control the robot in acceleration, a simple workaround was exploited. In order to test M 2 , we numerically integrated the acceleration commands produced by the VPC and sent the resulting signal to the low-level velocity controller running on our robot.

Reported experiments all used the same settings as in the last simulations set, i.e., the prediction and control horizon were both set to 10 samples, and the prediction sampling Figure 3.17 -Last simulation test, which uses ∆t = 5∆t c and n c = 10. In addition to a 20 cm translation, the sensor needs to perform rotations of 15°, 30°and 165°around the X, Y and Z axes. The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right). time ∆t was set to 0.1 s. The weights in the objective were the only parameters that needed to be adjusted, as it will be discussed in each experiment.

Servoing from Image Points

The tests reported in the following were performed by placing the planar object in front of the robot and rotating it multiple times about its normal axis, of an angle of almost 90°. Since the configuration s was still kept to a constant value, the robot is ideally supposed to follow the motion of the object.

As anticipated, experiments were run with almost the same settings used in the last sets of simulations. In particular, in this this test only the value of β needed to be modified. It was in fact noticed that a larger value better suits real experiments in which noise is also present. Increasing the value of this parameter mainly reduces the overall speed of the system, but also smooths the resulting trajectories. In practice, the parameter was modified from the original 10 -3 to 10 -2 for the two velocity-controlled schemes, M 1 and M H . In the case of M 2 , it was necessary to increase it slightly more in order to reduce oscillations and prevent the velocity to grow excessively. The final choice was β = 5 • 10 -3 , which, although not sufficient to completely remove all oscillations, was found to be a good trade-off in order to maintain acceptable time to convergence. An example of features trajectories obtained with the three predictors is visible in Figure 3.181 . Note that the points in the image can change not only due to the motion of the robotic arm, but also due to the aforementioned rotation of the object itself. While analyzing the recorded image streams produced by the camera, it was noticed that controllers necessitate a short amount of time before reacting to the object motion. To help interpreting the results, the paths crossed by the features in these phases are colored in blue, and correspond to nearly perfect circles. Features motions induced by the action of the VPCs are instead colored in green.

As the results show, the features behavior is quite different depending on the involved predictor. M 1 is not able to perform a nice rotation, with the features going either too near or too far from the ideal circumference. This also results in an undesirable motion in Cartesian space. On the contrary, the trajectories obtained using M 2 are much closer to a pure rotation about the optical center of the camera, and the motion in 3D is satisfactory as well. Finally, M H gives intermediate results, with features paths that, despite not being as nice as in the case of the acceleration-controlled predictor, are not deviating too much from a pure rotation. Furthermore, the motion of the sensor features almost no retraction.

The evolution of the feature error during time for the first rotation of the object is also reported in Figure 3.19. It can be seen that the predictor based solely on the interaction matrix is outperformed by M H , taking almost half a second longer to reach the same error magnitude. It should also be noted that M 2 , besides being the slowest controller, presents a rather large overshoot, which is hard to observe from Figure 3.18(b) alone. Such oscillatory behavior, as mentioned in the beginning of this section, could be reduced by increasing β, but at the cost of further sacrificing time performances. We believe that one explanation for these oscillations is to be found in the use of the numerical integrator to obtain the velocity command from the acceleration. As neither this block nor the low-level velocity controller are taken into account in the prediction, model uncertainties become larger and predictions are thus less reliable, finally leading to lower performances in practice.

Servoing from Polar Coordinates

The goal of this last set of experiments is to check if the performances of the proposed predictors remain consistent also when different features are selected. In particular, the polar coordinates of the point centers, whose second-order models are reported in Appendix A.3, are used in the sequel. In these experiments almost all parameters were kept unchanged from the previous section, with only a small modification in the objective being necessary. In fact, when considering polar coordinates as features, the angles e θ i (expressed in radians) tend to vary more than the radii ρ i for the same displacement. For this reason, their weight in the objective was reduced, so that angular and radial errors bore more homogeneous contributions to the objective. Specifically, the matrix Q (s) k+1 was updated from being simply α k I to:

Q (s) k+1 = α k diag(1, 1/5, 1, 1/5, 1, 1/5, 1, 1/5) (3.46)
It is well-known that, in classical servoing via feedback linearization, polar coordinates perform well when the object is subject to a pure rotation around the optical axis of the camera. Hence, the experiment of this section focus on the complementary case, in which the object performs a relatively large translational motion. This is known to lead to less satisfactory trajectories for the system due to the non-linearities in the interaction matrix.

The results obtained in this experiment, visible in Figures 3.20 and 3.21, are coherent with previous findings. In terms of points trajectories, M 1 is the predictor that features the worst results, while motions obtained with M 2 and M H are more regular and closer to straight lines. The motion in Cartesian space is also better when using the proposed predictors, with the controller inducing firstly a rotational motion that points the the sensor in the direction of the target, and subsequently a translational motion that completes the positioning task. In terms of features errors (cfr. Figure 3.21) it is worth noticing that all models present a less smooth evolution compared to the case of servoing from image coordinates. A possible explanation for this fact can be found in the optimization being harder to accomplish due to the rather high non-linearities corresponding to polar coordinates. Nonetheless, the benefits coming from considering the acceleration in the predictors are still evident, with M 2 and M H being characterized by lower maximal errors with respect to M 1 .

Conclusions

This chapter, and more specifically the last section, proposed two new models for Visual Predictive Control that lead to better robot motions thanks to the inclusion of features acceleration in the feature space. To support this claim, the performances of these models have been compared against another local predictor based solely on the first-order interaction matrix. By means of simulations, it was shown that convergence can be achieved in a short time while avoiding undesirable behaviors even in presence of large displacements, which is in line with the objectives of this thesis.

Moreover, the results were validated thanks to real experiments with an industrial redundant robot, not only confirming the benefits of integrating acceleration information into the predictors, but also demonstrating the practical feasibility of the control schemes. In addition, by considering different features to describe the observed object, it was practically demonstrated that the approach is general and that its effectiveness is not bound to a specific parameterization of the observed object. The work was proposed to the scientific community in the form of an article which was peer-reviewed and accepted for publication [START_REF] Fusco | Integrating Features Acceleration in Visual Predictive Control[END_REF]. The article was also accepted for presentation in IROS 2020.

There is still room for future improvements in different forms. As an example, it would be interesting to test the proposed approach using image moments as features, a case that is more complex due to different reasons. First of all, the number of intermediate calculations required to compute the second-order models is rather large, cfr. Appendix A.5, since each image moment depends on a large parameter vector. A second reason is to be found in the structure of the interaction matrix: it depends on higher-order moments, theoretically leading to a parameter vector z of infinite dimension.

In addition, as mentioned in the last section, the controllers are not able to react immediately to object motions. This is most likely because such event is not foreseeable by the VPC schemes, as it is not modeled in the predictors. In order to achieve high speed performances in dynamic environments, it is believed that a key improvement would be to actively estimate the kinematics of the observed object [START_REF] Ait-Aider | Simultaneous object pose and velocity computation using a single view from a rolling shutter camera[END_REF][START_REF] Dahmouche | High-speed pose and velocity measurement from vision[END_REF], and to take it into account explicitly inside the prediction models.

A further development might consider time-varying features references, rather than fixed configurations as it was done in simulations and experiments. This will likely require to adapt the formulation of the optimization problem, since the objective function currently contains a contribution that tries to minimize the joint velocity of the manipulator, which is in contrast with a task that requires the manipulator to continuously move. Remember that the second term is needed due to the redundancy, and it would not be sufficient to simply remove it. A possible solution would be to formulate the problem as a hierarchical, multi-objective, optimization in which the visual objective has the highest priority.

Chapter 4

PARAMETERIZED MODEL PREDICTIVE CONTROL

The previous chapter focused on the use of Model Predictive Control in Visual Servoing, since it provides means to complete the relative positioning task faster, while also ensuring the satisfaction of constraints. As explained, the optimization required by predictive schemes can be computationally demanding, and as a result the complexity of the problem should be kept as small as possible so as to obtain solutions quickly enough to keep up with the target control frequency of the robot. However, the optimization should be able to consider a long enough time interval for the prediction process. In this sense, the predictors proposed in the previous chapter allowed to use a larger sampling time in the prediction models, allowing to extend T p without necessarily increase the complexity of the optimization in terms of decision variables.

Nonetheless, in order to further improve the results obtained in the previous chapter, it could be necessary to extend the prediction horizon even more, or to slightly reduce the time sampling. As previously discussed, the optimization currently needs to be halted before the real minimum is attained, thus allowing to reach only a neighborhood of the actual optimum. As the control and prediction horizons are gradually increased, the optimization requires even more time to find solutions, meaning that, given the same amount of optimization time, the approximated minima will lie farther away from the real ones, ultimately leading in instabilities. Increasing only the prediction horizon clearly is a first way to limit a growth in complexity, but there are limitations as well. Indeed, as shown in the simulations of Section 3.3.4, when n c is small compared to n p the performances can deteriorate.

It seems that a stale has been reached: once the prediction horizon has been increased, we cannot increase n c since it would prevent real-time feasibility, but at the same time the limited control horizon leads to sub-optimal performances. The stale is due to the fact that, according to the MPC formulation presented so far, incrementing the control horizon directly implies a growth in the number of decision variables used in the optimization. However, what if a new, different, decision vector of equal dimension was used? Depending on what new variables are selected, it could be theoretically possible to change the result of the optimization -hopefully improving it -without significantly alter the computational load. This is precisely the subject of the present chapter, in which the concept of control parameterizations is studied. Rather than using the values of the first n c input samples as optimization variables, the control sequence is synthesized from a set of parameters. Some of these can be considered as design constants and are selected in advance, while others are to be tuned online by the optimization algorithm. Since this concept is rather new in the robotics field, this chapter will focus on presenting the topic and to test its performances on simple, academic examples. In particular, the next section introduces the general concept of control parameterization and then focuses on some examples of linear parameterizations. Section 4.2 addresses the case of MPC with linear parameterizations for the control of linear systems, in order to show the benefits of this new approach in a simpler setup. Finally, the more challenging case of parameterized nonlinear MPC is presented in Section 4.3, which includes simulation results obtained with both simple nonlinear model -an inverted crane-pendulum -but also with a free-flying camera performing a visual tracking task.

Control Parameterizations

The idea behind Parameterized Model Predictive Control [Ala06; Ala13] is to use, as decision variables for the problem, not the control inputs directly, but rather a smaller set of parameters which can define the entire control sequence. In other words, control samples are obtained as functions of the set of parameters p ∈ R nπ , i.e.,

u k = π k (p) π k : R nπ → R m (4.1)
The goal is thus to find a suitable parameterization that allows the controls to vary along the whole prediction horizon without having to increase the complexity of the problem in terms of number of free variables.

One sub-family of parameterizations that can be considered is the one of linear parameterizations, according to which control input samples are expressed as a linear combination of the parameters p:

u k = Π k p (4.2)
for a given set of matrices Π k ∈ R m×nπ . An equivalent relation can be written to map the parameter vector directly into the whole control sequence, simply by stacking the individual combination matrices Π k :

u = Π p Π . =      Π 0 • • • Π np-1      (4.3)
It is interesting to note that the classical way of selecting the input samples in Model Predictive Control can be viewed as an example of linear parameterization. In particular, with a control and prediction horizons of n c and n p respectively, the combination matrix takes the following block form:

Π =                I . . . I I . . . I                (4.4) n c n p -n c
wherein each block is of size m-by-m. In practice, the entries of p correspond to the free input samples, with the last m values being repeated as many times as needed to complete the control sequence. One issue with this "simple parameterization"1 is that almost all degrees of freedom are located at the beginning of the control sequence. The consequence is that, if the prediction horizon is extended, the last control sample will be used for a significant amount of time. This is not desirable, especially when considering unstable systems. In this sense, an example is given by the first two sets of simulations presented in the previous chapter: when considering M 2 , a single acceleration sample was not sufficient to stabilize the system.

Parameterizations can thus be used to avoid the aforementioned issue, in particular by distributing the degrees of freedom along the prediction horizon. In particular, in the following sections three types of linear parameterizations are introduced, which can be viewed as successive generalizations of the classical approach.

Zero-Order-Holder

This first parameterization is the most similar to the simple one. However, rather than keeping all free control samples at the beginning of the sequence, the degrees of freedom are distributed along the prediction horizon. Since the concept is better illustrated with an example, Figure 4.1 depicts a possible control sequence produced by this parameterization. 0 1 2 3 4 5 6 7 8 9 10 k free held Here, free samples are located at k = 0, 2, 5, 8, thus leading to a parameter vector of dimension n π = 4.

In practice, some "free indices" are selected within the available samples, with the value of the corresponding control inputs being added to the set of parameters p. The remaining, non-free, control values are evaluated simply by "holding" the same value of their nearest preceding free sample. For this reason, the parameterization is named in the sequel Zero-Order-Holder (ZOH).

The combination matrix Π writes in this case in a block-diagonal form that is similar to the following:

Π =                     I I I I I . . . I I                     (4.5)
in which each sub-block consists in the vertical concatenation of as many identity matrices as the times the corresponding parameters are held to produce control samples.

Linear Interpolation

One drawback of the ZOH parameterization presented just now is that, by construction, it alternates between sudden changes of the control input and flat regions in which the value remains constant. It could be instead interesting to gradually change the values from one free sample to another. This would still allow to distribute the degrees of freedom along the control sequence, but also to obtain a smoother signal. One simple, intuitive, way to achieve this goal is to generate intermediate values by linear interpolation from a free sample to the following one. Once again, this new parameterization can be written as a linear combination with a block matrix that is similar to the following:

Π =                        I 1 2 I 1 2 I I 2 3 I 1 3 I 1 3 I 2 3 I I . . . I I                        (4.6)
The effect of this matrix is better visualized in Figure 4.2, which shows that the control sequence is obtained by firstly selecting the free samples (in red) and then by performing interpolation in between to obtain the missing intermediate samples -with the exception of the trailing input values, which are eventually taken as a repetition of the last free sample.

0 1 2 3 4 5 6 7 8 9 10 k free held Here, free samples are located at k = 0, 2, 5, 8, thus leading to a parameter vector of dimension n π = 4.

This parameterization, whose name will be shortened to LERP, has an additional benefit with respect to the simple and ZOH ones. Consider adding constraints on the control inputs in the following form:

-∆u lim ≤ u k -u k-1 ≤ ∆u lim (4.7)
which simply sets a limit on how much successive input samples can differ in a control sequence. Given a certain value of u -1 , i.e., the last control sample used to actuate the system, the constraint reduces the range of achievable control inputs, since free values are no more independent from each other. Given that the simple and ZOH parameterizations allw control changes only in a limited number of locations, the range of achievable inputs is thus reduced substantially, as shown in Figure 4.3. Conversely, the LERP parameterization allows to reach a broader range of inputs, since the control sequence is allowed to change along the whole prediction horizon.

0 1 2 3 4 5 6 7 8 9 10 k lerp simple zoh

Basis Functions

In this section, rather than a single, specific, parameterization, a whole family of parameterizations is introduced, which relies on the concept of function spaces and function bases. It is well known that certain classes of functions can be generated by the sum of some (possibly infinitely many) functions. As an example, polynomials can be summed together to generate any analytical function according to its Taylor expansion. Similarly, sinusoidal signals with harmonic frequencies can be combined to form any periodic function as stated by Fourier's series expansion.

Taking inspiration from this idea, it is possible to consider parameterizations that combine a certain number of functions to generate at once the whole control sequence, which can be viewed as an extension of the proposal by Alamir to parameterize the control sequence with a decaying exponential profile [START_REF] Alamir | A pragmatic story of model predictive control: self-contained algorithms and case-studies[END_REF]. In practice, it is possible to select a certain number n b of basis functions B i : R → R (i = 1, • • • , n b ) and, considering for simplicity the case of a scalar-valued control input, combine them together to produce a continuous-time signal:

u(t) = n b i=1 h i B i (t) (4.8)
wherein the scalars h i represent combination coefficients. Intuitively, the use of a finite number of functions will generally limit the shapes that can be produced as output. However, it turns out that in practice even a reduced number of functions is sufficient to define a sufficiently rich variety of signals that can be effectively used in predictive control schemes.

h 1 h n b + • • • • • • • • • B 1 (t) B n b (t) u k = h i B i (k∆t)
Starting from (4.8), in order to produce the corresponding discrete-time control sequence it suffices to sample the generated function with proper time spacing ∆t:

u k = n b i=1 h i B i (k∆t) (4.9)
Note that by adjusting the coefficients h i , it is possible to change the shape of the resulting signal, which naturally leads to the idea of using h 1 , • • • , h n b as parameters. In other words, rather than acting on individual samples as all previous parameterizations did, the parameterization now acts on the components (the basis function) that constitute the control signal. In addition, similarly to the case of the LERP parameterization, control sequences obtained with these parameterizations tend to change more smoothly from sample to sample if the basis functions are smooth themselves.

Interestingly, if the shape of each basis function B i is fixed in advance, then the parameterization is linear in the coefficients h i . In fact, the control sequence vector can be written as:

u =      u 0 . . . u np-1      =         B 1 (0) • • • B n b (0) B 1 (∆t) • • • B n b (∆t) . . . . . . . . . B 1 ((n p -1)∆t) • • • B n b ((n p -1)∆t)              h 1 . . . h n b      (4.10)
showing that this indeed constitutes a linear parameterization, with the free parameters p corresponding to the collection of all coefficients h i . Finally, even if only the case of scalar-valued controls was discussed here, the extension to a multidimensional input is trivial.

Coming now to the problem of choosing a proper set of basis functions, several options can be considered. One possibility would be to spread along the prediction horizon Gaussians in the form

B i (t) = e -(t-t i ) 2 /σ 2 i (4.11)
or any other kind of radial function. These are often used to interpolate unknown functions from a given set of points, and are known for providing quite good approximations for a large number of cases. In other words, when combined altogether they can produce a large number of shapes, which is exactly what is required for the proposed parameterization. Another example could be that of sinusoidal functions which, according to Fourier's expansion, can be viewed as the building blocks of any function within a given time window. A third option considered in the following is the one of exponentially damped Laguerre polynomials [START_REF] Muehlebach | Parametrized infinite-horizon model predictive control for linear time-invariant systems with input and state constraints[END_REF], which take the following form:

B i (t) = e -εt i j=0 i j (-2εt) j j! (4.12)
Before concluding this section, let us remark that all "internal" parameters appearing in the selected basis functions must be set prior to the optimization for the resulting parameterizations to be linear. As an example, the scaling factor ε in (4.12) or the variances and centers t i and σ i in (4.11) need to be fixed in advance using either some knowledge on the problem or by trial and error. This is perhaps the biggest drawback of these parameterizations, as they introduce an additional user-based tuning which might affect the final performances. One possibility to remove this additional step would consist in considering these values not as constants to be set by the user, but within the set of parameters that the optimization algorithm can use. This would lead to loss of linearity and likely higher computation times, but almost surely enhancing the performances since the tuning of the internal basis parameters becomes automatic and not delegated to the user.

Linearly Parameterized Linear Model Predictive

Control

In this section, the simpler case of MPC for linear systems with linear parameterizations is considered, which is still of high interest for robotics applications despite its simplicity. In fact, even when dealing with generic systems, a simple tangent linearization can be exploited to obtain an approximate linear systems, if the nonlinearities are mild. Furthermore, some nonlinear systems, such as differentially flat ones, can be transformed in equivalent linear models and a MPC scheme can be designed to control the new system without having to deal with the nonlinear dynamics in the optimization problem [ACT07; De +16]. As it will be shown, the specific structure of linear models allows to formulate the optimization problem as a linearly constrained least squares minimization problem, which can be solved using any quadratic programming approach.

Given that both formulating and solving this class of problems does not pose major challenges, linearly parameterized linear MPC schemes are perfect candidates to benchmark the proposed parameterizations and gather insightful preliminary results. With this objective in mind, the following sections thus focus firstly on stating the general optimization problem and secondly on analyzing the performances with and without parameterizations in the case of two specific linear systems: a triple integrator and an under-damped oscillator.

Problem Formulation

We consider in the following the class of discrete-time linear-affine systems, written in state-space form as:

x k+1 = Ax k + Bu k + w (4.13)
where A and B are matrices of appropriate dimension and w ∈ R n . These quantities are assumed to be invariant with respect to time, but a generalization of the following results is trivial.

The goal is to show that the optimization problem that arises from a MPC scheme applied to this class of systems is a quadratic program. For this purpose, the first step is to show that the prediction vector x can be expressed concisely as a linear combination of the initial state x 0 and of the inputs. The first terms of the prediction sequence write as:

x 1 = Ax 0 + Bu 0 + w = Ax 0 +w +Bu 0 (4.14a)
x 2 = Ax 0 + Bu 1 + w =A 2 x 0 +(A + I)w +ABu 0 +Bu 1 (4.14b)

x 3 = Ax 2 + Bu 2 + w =A 3 x 0 +(A 2 + A + I)w+A 2 Bu 0 +ABu 1 +Bu 2 (4.14c)
from which a pattern clearly emerges, with the general formula being:

x k+1 = A k+1 x 0 + k i=0 A i w + k i=0 A k-i Bu i (4.15)
which precisely means that predicted samples are a linear combination of the initial state x 0 and of the control inputs (plus a constant term that depends on the affine term w). Moreover, the whole prediction vector can be written in the following linear-affine form:

x = η + Bu (4.16)
wherein η and B are defined as

η . =         A A 2 A 3 • • •         x 0 +         I A + I A 2 + A + I • • •         w (4.17a) B . =         B AB B A 2 B AB B . . . . . . . . . . . .         (4.17b)
We can finally introduce a linear input parameterization in the form of (4.3), leading to:

x = η + BΠp (4.18)
The objective is defined by the quadratic cost (3.9), while linear constraints in the control inputs and predicted states are introduced in the form:

C u u ≤ d u (4.19a) C x x ≤ d x (4.19b)
which, among others, allow to set limits both on the magnitude and on the variation of the controls and states. After some trivial calculations, the overall problem can be finally stated as:

min p 1 2 p T Hp + f T p (4.20a)
subject to:

C p p ≤ d p (4.20b)
wherein the different matrices and vectors are defined as follows:

D x . = diag(Q k ) (4.21a) D u . = diag(R k ) (4.21b) H . = 2Π T BT D x B + D u Π (4.21c) f . = 2Π T BT D x (η -x ) -D u u (4.21d) C p . =   C u C x B  Π (4.21e) d p . =   d u d x -C x η   (4.21f)
The problem thus corresponds to a linearly constrained quadratic minimization, as stated in the beginning of this section, which can be solved using, e.g., the active set method described in Appendix B.2. Note that if the constraints are feasible, the problem is convex and always admits a solution, since H is at least positive semidefinite.

Control of a Triple Integrator

In this section, a first example of linearly parameterized MPC is presented, in which the controlled system is a simple triple integrator. The continuous-time dynamics is discretized assuming piecewise constant inputs due to a zero-order-holder device with sampling period ∆t = 0.1 s. The model in state-space representation is given by the following:

x k+1 =      1 0.1 0.005 0 1 0.1 0 0 1      x k +      0.000167 0.005 0.1      u k (4.22)
The objective of the control scheme is to track a variable reference using the first coordinate of the state vector, or, with a little abuse of terminology, "position". This reference is initially constant and equal to 0 for the first 2.5 s of the simulation. Afterwards, it is changed according to a sinusoidal profile with unitary amplitude and 1.25 s -1 pulsation ( 0.2 Hz frequency). Symmetric bounds on the scalar input are set so that it lies in the interval [-5, 5], and in addition constraints in the form of (4.7) are added, with maximum variation set to ∆u lim = 1.

Finally, the prediction horizon is set to n p = 20, allowing to predict over a period of time of T p = 2 s, and four types of parameterization are considered: the simple one from classical MPC theory, Zero-Order-Holder, Linear Interpolation and one based on the damped Laguerre polynomials (4.12) with ε = 0.75. These parameterizations are compared in the sequel using a different number of parameters. More precisely, three sets of simulations are proposed, with n π equal to 6, 3 and 2 respectively.

First test: 6 parameters

In this first test, the decision vector in the optimization has size n π = 6. Since the control input is a scalar signal, this means that the control horizon n c is set to the same value for the simple parameterization, while ZOH and LERP parameterizations feature 6 free-samples each. In particular, these samples were set to k f ree = 0, 1, 3, 5, 10, 15 for the holder parameterization and to k f ree = 0, 2, 5, 10, 15, 19 when using LERP. Finally, a total of n b = 6 Laguerre's functions have been used.

A simulation was performed for each parameterization, recording the position of the system as well as the tracking error, which are shown in Figures 4.5(a) and 4.5(b) respec-tively. In addition, the control signal used to command the system and its variation are depicted with their respective bounds in Figures 4.5(c) and 4.5(d).

Note that, in addition to the data associated to the four parameterizations detailed above, Figure 4.5 includes the results obtained with a classical scheme characterized by n π = n c = n p = 20. Since it features the highest number of degrees of freedom, it is named "full" parameterization for brevity. It provides the theoretically best case for the MPC scheme, and is thus meant to serve as a reference to better interpret simulations results. and Laguerre (magenta) parameterizations, all having n π = 6 parameters. An additional case is reported for comparison, consisting in a full parameterization with n π = n p .

It is interesting to notice that parameterizations positively influence the evolution of the system. Focusing on the tracking error depicted in Figure 4.5(b) in the different cases, the three proposed parameterizations exhibit, after the initial transient phase, a lower error compared to the classical case. Furthermore, an interesting result is related to the control inputs. When the reference changes from a constant value to a sinusoidal profile, all optimized controls tend to vary significantly, becoming smoother only after this transient phase. Nonetheless, the classical strategy features few spots in which the control inputs experience rather large and abrupt changes even after the transient. The ZOH parameterization suffers from a similar problem, but with more modest oscillations in the control signal. Finally, LERP and Laguerre parameterizations nicely adapt to the output and produce smooth and regular control inputs, apparently reaching steady-behavior even before the full case.

Second test: 3 parameters

The simulations proposed above were all repeated, reducing the degrees of freedom to just 3. The free samples were changed to k f ree = 0, 2, 5 and k f ree = 0, 4, 8 for the ZOH and LERP parameterizations respectively, while the coefficient ε was left unchanged for the Laguerre parameterization.

Having halved the total number of parameters with respect to the previous simulations, clearer differences appear (see Figure 4.6). In particular, the classical approach provides the least satisfactory performances, with peak errors that are almost twice as large as those obtained with a holder parameterization. LERP and Laguerre parameterizations perform rather nicely, with reduced errors and smoother control inputs, even though differences with respect to the full parameterization are more evident.

Third test: 2 parameters

This third and last test considers as few as 2 degrees of freedom for the optimization, with the free-samples for the holder and LERP parameterizations set to k f ree = 0, 2 and k f ree = 0, 10.

The results are reported in Figure 4.7, in the same format used for previous simulations. The holder parameterization presents large tracking errors in the first half of the test, while in the end of the simulation it stabilizes closer to the reference. Other parameterizations required less time to settle to a steady behavior, with the best performances being those of LERP and Laguerre parameterizations. and Laguerre (magenta) parameterizations, all having n π = 2 parameters. An additional case is reported for comparison, consisting in a full parameterization with n π = n p .

Final Comparison

To conclude the analysis of the parameterizations used to control a triple integrator, a synthesis of the data collected from several simulations is now presented.

First of all, it must be noted that, for a given number of parameters, there is no reason to expect significant differences when solving the optimization problem (4.20) with different parameterizations, given that the matrices should be of equal size. However, this is just a heuristic estimation, and the times might fluctuate depending on which constraints are activated and deactivated to reach the optimum, as well as the current CPU load. For this reason, tests similar to those included in the previous sections were run multiple times to gather statistical information related to the time required to complete the optimization. The resulting data is included in Figure 4.8, which shows, as solid lines, the median optimization time of each parameterization in function of the number of parameters n π . Note that the times are scaled relative to the median time obtained when using the full parameterization. In addition, semi-transparent regions are added to provide the maximum and minimum times for each case. The presented data shows that the optimization times obtained with each approach can indeed fluctuate, but within a relatively small range. The LERP parameterization seems to be the best performing one, while the Laguerre and simple parameterizations are the worst, the former when 5 or less parameters are used and the latter when n π ≥ 6. It is believed that one reason for the initial gap between the performances of the Laguerre parameterization with respect to the other strategies is to be found in the number of constraints. In fact, while control limits can be applied simply at the free samples to ensure that other values will remain within the given bounds, this is not necessarily the case when using the Basis function approach. This means that in this case the number of control constraints is proportional to n p rather than n π as it is with simple, ZOH and LERP parameterizations.

The three sets of simulations proposed before suggest that the LERP and Laguerre parameterizations lead to better results in terms of tracking performances, but it is hard to quantify how much the performances depend on the number of parameters. For this reason, average errors were calculated for different parameterizations with an increasing dimension of the optimization variables. The resulting curves were then normalized with respect to the average error obtained with the full parameterization, and are reported in Figure 4.9. It must be noted that the solution to the optimization problem is deterministic, and thus no statistical analysis was performed in this case.

The reported results support the claim that parameterizations bear improvements to MPC. In particular, LERP and Laguerre parameterizations provide the best performances, with an average error that is comparable to the one of the full parameterization already with as few as 5 parameters. The Zero-Order-Holder parameterization does not provide results that are as nice as those of the other two approaches, but nonetheless outperforms the simple parameterization almost everywhere.

Control of a Mass-Spring-Damper

A linear system that often appears in robotics applications is the mass-spring-damper model, which can be described by the following ordinary differential equation:

mẍ = -2mξω n ẋ -mω 2 n x + u (4.23)
wherein m, ω n and ξ are respectively the mass, the natural frequency and the damping coefficient of the system. The variable x represents the position of the mass, and the input u corresponds to a force applied to the body.

This section briefly analyzes the use of parameterized MPC to control such kind of system, with the objective of showing that the good performances obtained using the parameterized approach were not limited to the case of the triple integrator, but are indeed a characteristic of the proposed method. To simplify the test, the mass is selected as m = 1 kg, while the natural frequency is chosen as ω n = 5 s -1 . The damping coefficient is selected as ξ = 0.3, thus leading to an under-damped system. The model is converted in state-space representation and discretized, assuming a Zero-Order-Holder device for the input, via matrix exponentiation. The frequency is chosen as 100 Hz, while the sampling time used for predictions is set to ∆t = 0.05 s. The prediction horizon is set to n p = 30, thus allowing to foresee within a time interval of T p = 1.5 s. Control inputs are bounded in the interval [-30 N, 30 N], with a maximum variation ∆u lim = 1 N.

The considered parameterizations are again ZOH, LERP and Laguerre ones, in addition to the classical MPC parameterization. The objective is to track a variable reference position, which corresponds to a "trapezoidal wave". It starts at level 0 m, and shortly after rises at a rate of 1 m s -1 until it has reached the value of 1 m. After 5 s, it starts decreasing, attaining the value -1 m in 2 s, and the process is repeated periodically.

First test: 6 parameters

In this first simulation, the optimization has to find the optimal control sequence using a total of 6 parameters. The tuning of the parameterizations for this test was almost the same as the one used in the first test shown with the triple integrator. The main difference is that the Laguerre parameterization here uses one damped polynomial less than before. Nonetheless, it features an additional static offset whose height is to be chosen by the optimizer, thus still using a total of 6 basis functions. Results from the simulation are reported in Figure 4.10, which shows the position of the mass along time and the tracking error obtained with the different parameterizations. The simple and ZOH parameterization do not perform as desired, moving the mass too early with respect to the desired reference. This fact can be explained by the tight control variation limits, which, as briefly anticipated earlier in this chapter, can greatly reduce the range of achievable control values, especially when using simple and ZOH parameterizations. To support this claim, the same simulation has been run also with increased variation limits (∆u lim = 5 N). As visible in Figure 4.11, when these constraints are relaxed, all parameterizations provide better results in terms of convergence. In particular, the issue previously encountered with the simple and ZOH parameterizations is no longer present, and the mass is no longer moved before it is necessary. However, the relative per-formances are more or less unaffected, with the LERP and Laguerre parameterizations that once again provide an evolution that is quite close to that of the full parameterization.

Second test: 3 parameters

A final simulation is now presented to conclude the tests on linear systems. In here, the variation limit of the control input is restored to its original value, ∆u lim = 1 N. The parameters are reduced to n π = 3, which is worth remarking is just a tenth of the samples used for the prediction. Figure 4.12 shows the results obtained with the discussed setup. The LERP parameterization shows issues similar to those of simple and ZOH ones, forcing the mass to move in advance and thus resulting in peak errors that are twice as large as those obtained with the full parameterization. On the other hand, the parameterization based on basis functions seems almost unaffected by the decrease in the parameters number, with tracking errors that closely resemble those obtained with the full parameterization.

Final Comparison

The last two sets of simulations offered a glimpse at the potential benefits coming from the use of parameterizations that allow to cover broader sets of control samples along the prediction horizon, especially when the distance between subsequent values is .12 -Control of a mass-spring-damper system using a simple (red), ZOH (green), LERP (blue) and Laguerre (magenta) parameterizations with n π = 3 parameters, as well as a full parameterization (yellow) with n π = n p = 30.

tightly constrained. In addition, so as to provide a wider analysis of the performances, Figure 4.13 includes for each parameterization the average error (normalized by the one obtained with the full parameterization) over the number of parameters used in the test.

Even at a first glance result are clear, with the Laguerre parameterization raking first and followed by the LERP one. Moreover, the gap between these two strategies and the classical one is much larger than in the case of the triple integrator. This is encouraging, especially considering that the number of parameters used here is rather limited, confirming the interest in further investigating the topic of Parameterized Model Predictive Control.

Another fundamental remark is related to the ZOH parameterization being always outperformed by the simple one. A possible cause for this could simply be that the holder parameterization might not be well suited for this case of scenario, which involves a long prediction horizon and small variation limits. This argument is supported by the evidence that, when constraints are loosen, this parameterization can perform much better (recall Figure 4.11). Moreover, this is the parameterization whose range of achievable inputs is the smallest, as it had been depicted in Figure 4.3. With these two facts in mind, the conclusion might be as stated: the holder parameterization is simply worse than the simple one in this scenario. However, there is also another factor that plays an important role and that has not been discussed yet: the choice of the "characteristic parameters" of the Provided values are relative to the average error obtained when using the full parameterization. parameterization, i.e., the free samples. As mentioned before, these values were chosen by trial and error. This means that only a subset of the total possibilities has been analyzed, and that a "human factor" was necessary in the process to heuristically choose which sets of indices to consider for the trials. One better approach would be to design a systematic tuning protocol, which nonetheless presents some critical aspects. The major issue is that the set of possible unique choices of the free samples can be incredibly large. In fact, once the prediction horizon and the dimension of the parameter vector have been fixed, the total number of unique free samples selections amounts to 2 np-1 nπ-1 -1. As an example, in the first test presented in this section, where n p = 30 and n π = 6 were used, a total of 118754 combinations was possible. With such a large set of potential candidates, it is only natural to rule out brute force methods that explore all the possibilities and choose the best one. It would be possible to design some sort of automatic procedure that, with the help of some heuristic criteria, could reduce the set of candidates to an acceptable amount. However, it does not seem worth the trouble, especially considering that, at least up to now, the LERP and Laguerre parameterizations can lead to better results without the need of complicated tuning procedures.

Nonlinear Model Predictive Control with Parameterizations

As shown in the previous section, the introduction of parameterizations positively affects the results obtained with MPC schemes designed for linear systems. As a natural extension of previous results, this section considers the broader class of nonlinear dynamical systems. In particular, the following section briefly details the optimization problem resulting from the parameterized formulation, while Section 4.3.2 focuses on simulations that were performed on a simple nonlinear system. Finally, Section 4.3.3 applies the parameterization to visual servoing, considering a free-flying camera that has to perform a given trajectory in front of a simple target.

Problem Formulation

The nonlinear problems arising from the use of parameterizations in predictive control are not so different from those that were formulated in the beginning of Chapter 3 when discussing direct methods.

A first possible formulation is the equivalent of single shooting methods when dealing with parameterizations. In this case, given a particular value for the parameter vector p, a first control sample can be evaluated using (4.1), which in turn is used to evaluate the first predicted state. At the next step, (4.1) is used again to obtain a second control input, and a new prediction is formulated from its value and that of the first predicted state sample. The procedure is repeated for the whole prediction vector, and is conceptually equivalent to the one that had been outlined in (3.11). Assuming the quadratic stage cost (3.9), the objective function and the constraints can be evaluated on top of the parameterization, and thus the optimization problem can be transcribed as:

min p k C(x k+1 (x 0 , p), u k (p), k) (4.24a)
subject to:

x k (x 0 , p) ∈ X ∀k = 1, • • • , n p (4.24b) u k (p) ∈ U ∀k = 0, • • • , n p -1 (4.24c)
wherein the explicit dependencies on p and x 0 have been used to remind that u and x are fully defined by the parameters and the initial state of the system.

A second possible problem formulation consists in extending multiple-shooting algorithms to the case of parameterizations. As outlined in Section 3.1.2, the core concept is to "promote" dependent variables to the role of decision variables, adding adequate equality constraints to ensure that they are coherently linked to the original optimization variables according to their previous definition. In practice, the whole optimization problem could be written as:

min p,x,u k C(x k+1 , u k , k) (4.25a)
subject to:

x k ∈ X ∀k = 1, • • • , n p (4.25b) u k ∈ U ∀k = 0, • • • , n p -1 (4.25c) x k+1 -f d (x k , u k ) = 0 ∀k = 0, • • • , n p -1 (4.25d) u k -π k (p) = 0 ∀k = 0, • • • , n p -1 (4.25e)
Further options could be considered, e.g., by adding only either x or u to the decision vector (and consequently removing (4.25d) or (4.25e) as needed). In addition, inspired by what was done in [START_REF] Giftthaler | A Family of Iterative Gauss-Newton Shooting Methods for Nonlinear Optimal Control[END_REF], hybrid multiple shooting variants of problem (4.25) could be considered in which only a subset of the control and state samples are added to the decision variables.

The simulations presented in the next section all exploit the first formulation, and are solved using a custom implementation of the Gauss-Newton minimization algorithm (cfr. Appendix B.4), which can be viewed as a special case of Sequential Quadratic Programming. Only linear parameterizations have been used up to now, subject to control bounds and control variation limits. As a consequence, all constraints are linear, which simplifies the problem at hand, ensuring that the returned parameter values are always within the valid domain.

Implementations based on the multiple-shooting formulation have yet to be tested, mainly since the optimization algorithm needs to be created ad-hoc in order to properly exploit the sparsity of the problem and to enhance parallelization.

Control of an Inverted Crane-Pendulum

We consider here an example in which an inverted crane is to be controlled. The system, represented in Figure 4.14, consists in a moving support of mass M which can slide along a rail under the action of a horizontal force u. A pendulum, which is able to freely rotate thanks to a frictionless joint, is attached to the base. The rod of the pendulum has length and is considered to be massless, while the terminal weight has given mass m and inertia I. The system is underactuated, as the rotation of the pendulum is not directly controlled by any actuation system. The state of the system consists in the horizontal position of the base, denoted as x, and the angle of rotation of the pendulum, ϕ. The derivatives of these two variables are also part of the state vector, while the horizontal force u applied at the base corresponds to the unique input of the system.

To obtain the equations of motions, the Lagrangian mechanics is used. In particular, the kinetic energies of the moving support and of the pendulum write as:

E 1 = 1 2 M ẋ2 (4.26a) E 2 = 1 2   m   ẋ + φ cos ϕ φ sin ϕ   2 + I φ2    = 1 2 m ẋ2 + (m 2 + I) φ2 + 2m ẋ φ cos ϕ
(4.26b) Regarding the potential energy, since the moving support moves horizontally the only contribution that needs to be considered is the one of the disc, and corresponds to:

U 2 = -mg cos ϕ (4.27)
The Lagrangian L of the system is defined as the difference between the total kinetic and potential energy, which, for the crane-pendulum, results in:

L = E 1 + E 2 -U 2 = 1 2 (M + m) mt ẋ2 + 1 2 m 2 + I I 0 φ2 + m ( ẋ φ + g) cos ϕ (4.28)
The following step consists in the evaluation of the derivatives of L with respect to the generalized coordinates . = x ϕ T :

∂L ∂ =   0 -s ϕ m ( φ ẋ + g)   T (4.29a) ∂L ∂ ˙ =   φc ϕ m + ẋm t I 0 φ + ẋc ϕ m   T (4.29b) ∂ ∂t ∂L ∂ ˙ =   φc ϕ m + ẍm t -φ2 s ϕ m I 0 φ + ẍc ϕ m -φ ẋs ϕ m   T (4.29c)
wherein s ϕ and c ϕ have been used to shorten the notation, and correspond respectively to the sine and cosine of the angle ϕ. Knowing these derivatives, the inverse dynamics writes as:

  u 0   = ∂ ∂t ∂L ∂ ˙ T - ∂L ∂ T =   m t c ϕ m c ϕ m I 0     ẍ φ  +   -φ2 s ϕ m gs ϕ m   (4.30)
By inversion of said model, the Direct Dynamics Equations of the system are finally found:

ẍ = I 0 ( φ2 s ϕ m + u) + gm 2 2 s ϕ c ϕ I 0 m t -c 2 ϕ m 2 2 (4.31a) φ = - m (c ϕ ( φ2 s ϕ m + u) + gm t s ϕ ) I 0 m t -c 2 ϕ m 2 2 (4.31b)
Despite this being a rather simple model with reduced dimensionality and a single control input, the equations of motion present several highly nonlinear terms which justify the use of this model as a first benchmark for nonlinear optimal control. The crane was simulated in Gazebo via the Bullet physics engine, using the geometric and physical properties reported in Table 4.1. To obtain more realistic results, the simulation was updated at a much higher frequency (10 kHz) than the one used for the control (50 Hz), holding the same input for the entire duration of a control period. A total of n p = 20 samples was used for predictions, with sampling time of 50 ms. Control inputs were limited to a maximum of 0.1 N in absolute value, with a maximal allowed variation of 0.02 N between consecutive control samples. The objective of the controller in each simulation is to move the base of the crane, switching between the two positions x -= 0 m and x + = 0.3 m. While doing so, the pendulum should remain vertical and pointing upward, i.e., ϕ = π rad. Since the goal is to stabilize the system at fixed configurations, the desired linear and angular velocities ẋ and φ are both set to zero. To mitigate the relative importance of each task, in the objective the weights of the different state variables were set to 200, 10, 7 and 2 respectively for the base position, the pendulum orientation, the linear velocity of the base and the rotation rate of the pendulum.

In addition to the full parameterization used as reference for comparisons, the usual four parameterizations have been considered:

1. The simple parameterization from classical MPC theory.

2. ZOH parameterizations with free samples located at various spots along the prediction horizon. The goal is to have enough samples to steer both in the short and long term.

3. LERP parameterizations with free samples located similarly to ZOH.

4. Laguerre parameterization with ε = 1.5.

Similarly to the simulations performed with linear systems, the parameterizations have been compared using different values of n π . In particular, Figure 4.15 reports the results from three specific cases, in which the number of parameters is respectively 3, 6 and 8. A first necessary observation is related to the test presented in Figures 4.15(a) and 4.15(b), in which n π = 3. In this test, the simple parameterization is the only one that is not able to properly stabilize the crane, with the base position deviating more and more from the desired reference. It must also be remarked that the holder parameterization, despite being able to at least bound the oscillations in the tracking error, provides results which are far from satisfactory, with peak errors that reach the 0.4 m. On the other hand, the Laguerre parameterization and especially the LERP one perform really well. Indeed, the former features only small overshooting when a desired position is reached, while the latter presents a behavior that is nearly indistinguishable from that of the full parameterization.

Increasing the number of parameters allows the simple parameterization to find stabilizing controls, but, other than that, the results confirm what was found in the first simulation. More precisely, in both remaining simulations, the ZOH parameterization slightly outperforms the simple one, even though the maximum overshooting is still too large, i.e., around 11 cm and 3.5 cm when using n π = 6 and n π = 8 respectively. On the other hand, the LERP and Laguerre parameterizations perform as well as the full parameterization in both remaining simulations.

To conclude the present analysis, an overall comparison of the average position error as function of the number of parameters is provided in Figure 4.16. The overall performances reflect what shown in the selected simulations, and provide similar conclusions to those found when dealing with linear systems. The holder parameterization can provide some benefits, but they are often marginal and less consistent. On the other hand, the advantages in using a LERP or a Laguerre parameterization are clear, with performances that are comparable to those of the full parameterization, but using as few as 3 or 4 parameters, i.e., 80 % less decisions variables than those used in a full parameterization.

Control of a Free-Flying Camera

In this last section, the use of parameterizations in a VPC scheme is investigated. In particular, a set of simulations was run considering a free-flying camera with 6 dof. The control input is assumed to be the sensor twist projected in its own frame, i.e., v. The considered prediction model is a local model that updates both the features s and the parameters z using the interaction matrix evaluated at each iteration. The model is thus simpler than those considered in the previous chapter, since the manipulator and its kinematics are not taken into account.

The observed object is once again a simple target with four black dots printed on it, and the normalized coordinates of the four centers are used as features. As done in all previous servoing simulations and experiments, the object is assumed to be motionless.

In each simulation, the camera is initially positioned in front of the target at a distance of 2.5 m, with the task of performing a circular trajectory in front of the observed object (the diameter of this circular motion being of 1 m). In terms of predicted visual features, the objective is to ensure that the coordinates s i of the i-th feature point follow the desired profile along the prediction horizon:

s i,k = s (0) i + R   cos (t + k∆t) sin (t + k∆t)   (4.32)
wherein s (0) i are the coordinates of the i-th point when the camera is located in front of the object, at the beginning of the simulation. The parameter R is proportional to the radius of the camera trajectory (R = 0.2 was used in the simulations), while t corresponds to the time at which the current MPC problem has to be defined and solved. Finally, ∆t is the sampling time used in the predictors. The desired features thus change continuously during the simulation, as opposed to the cases considered in the previous chapter, in which the values of s i,k were kept constant. As explained before, a generic visual task constrains 6 dof, allowing in this case to fully determine the control input (the 6-dimensional sensor twist) without the need of a secondary task, as it was done when controlling the 7 dof LWR4+ manipulator.

The simulation was run with a control frequency of 50 Hz to simulate typical camera acquisition frequencies. Since the task to be performed corresponds to the tracking of a time-varying trajectory, it is important to use a rather precise sampling step for the prediction. In particular, ∆t = ∆t c = 20 ms was used in all simulations discussed in the following. If the tracking is performed properly, the prediction horizon needs not to be extremely large. However, it would be desirable to still have a sufficiently long prediction vector in case any perturbation leads to an increase in the feature error. This can happen, e.g., if the observed object performs some kind of motion, since this is not modeled in the predictor. It was decided to consider a prediction time of T p = 1.5 s, thus leading to a prediction horizon of n p = 75 samples.

All parameterizations described so far were tested in simulation, with an increasing number of decision variables. In particular, the simple parameterization was considered with a control horizon n c ranging from 2 to 10, and thus the number of parameters n π varies from 12 to 60 (since a single control sample has dimension m = 6). Similarly, ZOH and LERP parameterizations were considered, each featuring a number of free samples ranging from 2 to 10, with the free samples being uniformly spread along the prediction horizon. Finally, Laguerre parameterizations were also used, each featuring a static offset (like in the case of the mass-spring-damper of Section 4.2.3) and as many basis functions as needed to have the same amount of parameters as other control parameterizations. The value ε = 0.5 was used to tune the damped Laguerre polynomials.

The optimization problems were formulated using the single-shooting formulation (4.24) and solved using an iterative Gauss-Newton method. Constraints on the control inputs were added to limit the linear and angular velocities to 0.5 m s -1 and 1 s -1 respectively. In addition, the maximum variations were set to 0.017 m s -1 and 0.033 s -1 , approximately corresponding to acceleration limits of 0.83 m s -2 and 1.67 s -2 .

A synthetic representation of the results is represented in Figure 4.17, which provides the features error norm along time for different parameterization types and number of parameters. In particular, each type of parameterization is plotted with a specific hue component, with darker colors representing the use of fewer parameters. For ease of comparison, the results obtained using the maximum number of optimization variables (n π = m • n p = 6 • 75 = 450) are included as well. As visible, all parameterization feature two distinct peaks in the features error vector. The first one is obtained at the very beginning of the simulation, and is simply due to the features being initially located at the centers of the desired circular trajectories. In addition, a second peak appears after 4 s of simulation. This is caused by the object, which performs a rotational motion of 70°between t = 4 s and t = 5 s. The center of rotation corresponds to the origin of the object itself and is performed around the unit axis 1/6, 1/6, 2/3 at constant velocity. As mentioned before, this motion is not modeled in the prediction and thus it cannot be anticipated. As a result, it leads to an increased tracking error.

Two observations can be made from the results depicted in Figure 4.17. First of all, the ZOH parameterization does not lead to satisfactory performances when a small amount of parameters is used. The tracking error after transient becomes smaller than that of the simple parameterization only if at least 8 free samples are used. As already discussed, given the extremely high number of possible choices of the free samples, the existence of a better selection cannot be easily ruled out. However, this once again shows one (and perhaps the main) weakness of this parameterization, which seems to be effective only after careful selection of the free samples locations.

The second observation is that very few parameters are needed to obtain good performances with LERP and Laguerre parameterizations. In particular, using just 3 free samples in a LERP parameterization leads to tracking errors that are almost equivalent to those obtained with the full parameterization, which uses instead 75 samples. A slightly worse, but definitely very close, behavior is obtained with Laguerre parameterizations. This can be seen also in Figure 4.18, which compares the features trajectories obtained with the different parameterizations when 3 parameters per input coordinate are used. In particular, the motions obtained using the full 4.18(a), LERP 4.18(d) and Laguerre 4.18(e) are really close in the image. Moreover, it must be noted that the rate at which peak errors are reduced is relatively high. Indeed, it generally takes around 0.5 s to almost nullify the tracking error when LERP and Laguerre parameterizations are used, given that a sufficient amount of degrees of freedom are used (again, this roughly corresponds to a minimum of 18 or 24 total decision variables).

Finally, from a practical point of view it must be noted that even with the reduced number of n π = 18 parameters used to obtain the results of Figure 4.18 the optimization time required to evaluate the control at each iteration is about 300 ms, which is evidently too large for a real-time implementation. Nonetheless, it corresponds to just slightly more than the 5 % of the time required when using the full parameterization, and thus it provides a very significant relative improvement. Furthermore, the results were obtained with a simple implementation written entirely in Python, and it seems reasonable that real-time execution could be achieved with some additional code optimization and by translating the source to a compiled language.

Conclusions

This chapter focused on the concept of control input parameterizations, which allows to synthesize whole command sequences from a low-dimensional set of parameters. After detailing some possible kinds of parameterizations, their performances were tested in simulation, both in the case of linear and nonlinear systems, including a simple example of visual predictive control. The discussed results show that better performances than the classical formulation can be obtained even when using a small amount of parameters with respect to the number of prediction samples. This topic has not yet been considered extensively in robotics yet, but it is believed that it deserves to be studied more in details, with few open questions that need to be answered. First of all, the use of parameterizations for more complex nonlinear systems should be considered in order to asses their performances in practical scenarios. Examples might include underactuated systems or serial manipulators performing a visual servoing task.

Another direction of investigation should focus on different formulations of the nonlinear optimization problem. In fact, as discussed before, only single-shooting methods have been investigated so far, while it is worth to extend the analysis to multiple-shooting approaches as well.

Moreover, the tuning problem should be addressed as well. All parameterizations contain a certain number of "design constants" that are to be set before the optimization. This task is currently performed by the user, which has to resort to his/her experience, knowledge of the system and some intuition as well. Instead, it would be desirable to have some tuning criteria or even some automatic procedures that can reduce the amount of human intervention. In this sense, a first attempt could be to include the values that define a parameterization within the decision variables of the optimization. An example could be to consider nonlinear Laguerre parameterizations in which the scaling factor ε is also optimized by the nonlinear programming routine.

Last but definitely not least, it would be of great interest to mix the concepts illustrated here with those presented in Chapter 3, assessing the performances of parameterized VPC schemes based on second-order feature models.

CONCLUSIONS

This thesis dealt with the objective of increasing the productivity of manipulators performing visual servoing tasks. In particular, different ways to allow faster execution of the relative positioning of a sensor with respect to an observed object were investigated. To address the problem, several strategies were considered, either involving the use of new models to describe the interaction between the controlled robot and its environment, or the employment of advanced control schemes that try to optimize the motion of the robot. The contributions were validated via simulations and experiments, using mainly a Kuka LWR4+ as target platform. It was shown that the proposed methods are well suited for the control of such system, allowing to reach medium speeds and relatively short execution times.

Contributions

More than few existing works suggest that a key to unlock high-speed performances is the use of higher-order models that allow to establish a link between the features and the acceleration and actuation efforts of the controlled system. Following this suggestion, this thesis firstly investigated second-order models for visual servoing. In particular, a simple procedure was proposed that allows to obtain these models for a generic eye-in-hand system, detailed in the beginning of Chapter 2. It allowed to obtain second-order models for various sets of features, cfr. Appendix A, including image points, polar coordinates and planar image moments.

The use of second order models allows to create new classes of control schemes which use the acceleration -either of the sensor or of the manipulator depending on the formulation -as input. In these controllers, the state is not only given by the features, but also by their velocities, which can in turn be estimated using existing first-order models. One of the main interests in obtaining an acceleration control signal directly from the features observations is that it can be used in conjunction with the dynamic model of the controlled manipulator to establish a Computed Torque Control directly in the feature space. As it was shown by means of simulations in [START_REF] Fusco | A Comparison of Visual Servoing from Features Velocity and Acceleration Interaction Models[END_REF], such controller is less af-fected by sensor noise and allows to obtain satisfactory performances in terms of features convergence.

One limitation of the feedback controllers proposed in Chapter 2 is that, similarly to classical proportional velocity control laws, they constrain the features evolution in a very specific way. In particular, controllers attempt to steer the features along straight hyper-lines connecting the current configuration to the desired one, which can result in unsatisfactory motions for the sensor in SE(3). In addition, the rate at which this motion is performed is dictated by an exponential decay. As a consequence, if the control gains are increased to reduce execution times, very high accelerations and velocities might be produced for a short period of time, while near completion the sensor will tend to move rather slowly.

To overcome these issues, predictive control strategies were examined. The principal advantage over feedback controllers is the ability of MPC schemes to explore a set of solutions and select the control inputs which optimally steers the system towards the desired configuration. By properly shaping the cost functional of the optimization it is possible to favor shorter paths and faster motions, both contributing in a reduction of the execution time for a task. Moreover, as the control computation is the result of a nonlinear optimization, arbitrary constraints can be added to the problem definition, thus ensuring physical feasibility of the returned solution.

The main drawback of MPC is the computational burden that it introduces: rather heavy nonlinearly constrained nonlinear minimization problems in several variables must be solved in few milliseconds in order to allow feasible real-time implementations. To make the problems manageable in this sense, two different strategies have been considered that improve state of the art VPC techniques.

The first attempt was inspired by works on second-order models, which underlined that features and sensor accelerations become non-negligible in more dynamic contexts. It was decided to consider predictors that exploit also the second derivative of the features to predict their future values. These models allow to use coarser time samplings without sacrificing reliability of the predictions. As a consequence, less samples need to be considered so as to cover a target period of time. The complexity of each optimization problem can be reduced, and the problem can be solved within the allotted time with sufficient precision. This is supported via several simulations and experiments, as presented in Chapter 1 and [START_REF] Fusco | Integrating Features Acceleration in Visual Predictive Control[END_REF]. In particular, the results show that models that incorporate acceleration information are associated with generally better and shorter motions, and can outperform predictive strategies based on velocity models. Moreover, the use of different features sets in real experiments suggest that this result is due to the proposed methodology and not specific to a single type of descriptive features.

A second way to deal with the complexity of predictive strategies was analyzed as well, based on the ideas introduced in [START_REF] Alamir | Stabilization of nonlinear systems using receding-horizon control schemes: a parametrized approach for fast systems[END_REF]. As opposed to classical predictive strategies, which try to reduce the dimensionality of the optimization by using as decision variables the first samples of the control sequence, this theory proposes to introduce a parameterization for the control inputs so that the "degrees of freedom" are better distributed along the prediction horizon. Given that the concept was rarely applied to robotics and very few related works are available, the majority of Chapter 4 has been dedicated to a thorough analysis of these parameterizations on simple systems. In particular, they were used in simulation to control two common linear systems. The results were discussed, underlining the advantages coming from the use of two parameterizations in particular. The method was then applied for the control of a simple nonlinear system -an inverted crane-pendulum -and the results confirmed what was previously obtained when considering linear systems. The parameterized approach was finally exploited to perform a simulated visual servoing task. In particular, a free-flying perspective camera was controlled in velocity using different types of control parameterizations, subject to actuation constraints. A relevant result was obtained in this case: using just a 25 th of the maximum parameters, it is possible to obtain very satisfactory tracking performances, comparable to those obtained when all control samples are individually optimized. This allowed to obtain reduced errors and fast responses in a short time (compared to that needed by the full parameterization). The presented results have not been published yet, but a journal article is currently in preparation in order to share these findings with the scientific community and valorize the thesis work.

Perspectives

This thesis contributed to the enhancement of visual servoing performances by following three different directions. Even if the outcomes of these investigations were positive and allowed to control sensor displacements at higher speed, there are still several aspects that need deeper exploration.

First of all, it must be noted that the models and controls presented in Chapter 2 depend on the derivatives of the feature vector. In the present study, it was simply as-sumed that such derivative could be estimated by multiplication of the feature Jacobian by the joint velocity, i.e., ṡ = J s q. The assumption was not unreasonable in the presented simulations since perfect knowledge of the geometric model of the manipulator was assumed, and the noise in the sensor measurements was relatively small. However, a second possibility could be considered, consisting in using an estimation scheme to evaluate ṡ from fusion of several measurements. As an example, an Inertial Measurement Unit (IMU) could be used to obtain an estimation of the end-effector velocity, and an optical flow strategy could provide a direct estimation of ṡ from image measurements. Fusing all these signals altogether could provide a better overall estimation of the feature velocity.

As explained in Chapter 2, simulations were run using a very high sampling time, assuming nonetheless that a rather reliable estimation of both the features and parameters would have been available. Therefore, in order to validate the presented simulations also on a real platform, a good estimation of the feature vector itself would be essential. To obtain it, different solutions could be considered. A first possibility would be to use a virtual visual servoing strategy similar to that presented in [START_REF] Dahmouche | Dynamic visual servoing from sequential regions of interest acquisition[END_REF], or to use a solution based on an extended Kalman filter which exploits both the first and second-order features interaction models to perform simultaneous estimation of the features and of their velocity.

Simulations and experiments presented in this manuscript did not consider high-speed systems, such as the parallel robots used in [START_REF] Dahmouche | Dynamic visual servoing from sequential regions of interest acquisition[END_REF][START_REF] Ozgur | High speed parallel kinematic manipulator state estimation from legs observation[END_REF]. Extending the results obtained in this thesis to such class of manipulators would be of high interest, since their mechanical design allows to reach very high speeds and accelerations. Of course, they constitute a higher challenge with respect to the control of serial manipulators such as the LWR4+ Kuka arm that was used in most of the simulations and experiments presented in this thesis. As an example, the visual servoing controller based on the Inverse Dynamic Model presented in this thesis did not take into account some behaviors that typically appears in highly dynamic contexts such as flexibility in the joints and links. It would be interesting to assess whether the performances obtained with the serial manipulators hold also with higher dynamic excitation, or if more accurate models are needed. Furthermore, if motions become significantly faster, the quality of acquired images might deteriorate. As an example, motion blur and rolling shutter might rend the visual feedback highly unreliable. A potential solution to this problem could consist in the use of event-based and hybrid cameras such as the Dynamic and Active-pixel Vision Sensor (DAVIS) [START_REF] Brandli | A 240 × 180 130 dB 3 µs latency global shutter spatiotemporal vision sensor[END_REF][START_REF] Tedaldi | Feature detection and tracking with the dynamic and active-pixel vision sensor (DAVIS)[END_REF].

Several improvements to predictive strategies are also possible. First of all, up to now only simple geometric primitives have been used as features, and the object used for simulations and experiments was simplified to facilitate features extraction. Even though image points can be used to represent many real-life objects, to deal with complex shapes it might be better to consider image moments as descriptors. To extend the domain of applicability of the presented predictive strategies, a relevant improvement would thus be to develop new predictors for this class of features. This poses some challenges for few reasons. A first, simple, one is the difficulty in obtaining second order models for these descriptors. To support this claim, the reader is referred to Appendix A.5, which contains the second order model of a generic moment feature m i/j , which is composed of several terms. Fortunately, despite the large amount of factors which make the modeling task tedious and error-prone, the algorithmic procedure detailed in Chapter 2 allows to evaluate the necessary quantities with the help of symbolic tools. Nonetheless, a second issue complicates the definition of predictors according to the formalism proposed in Chapter 3: each moment m i/j depends on moments of higher order such as m i+1/j . As a consequence, to predict m i/j one has to evaluate m i+1/j as well, which in turns requires to predict m i+2/j . This chain of dependencies on higher-order moments is infinite, and thus the predictor needs to be defined differently. One possibility would be, e.g., to use a constant value for all moments of a specific order, or to use a constant interaction matrix along the prediction horizon.

Another interesting development would consist in the application of parameterized MPC to systems with unstable behaviors, such as an aerial vehicle performing a servoing task. Being an underactuated and inherently unstable system, this case is similar to that of the inverted pendulum of Section 4.3.2. In addition, such systems often have a limited computational power. This makes them a perfect target for the methods presented in the last chapter, whose aim is precisely to reduce the time required to evaluate control inputs.

Finally, several other technical improvements could be attempted. For instance, the use of nonlinear parameterizations should be considered, as well as trying to solve the optimization problem using a multiple-shooting technique. Moreover, a mix between the parameterized approach and the new predictors presented in Chapter 3 should be considered, with the objective of further improving the performances during the execution of servoing tasks.

L s =      -1 0 0 0 -Z Y 0 -1 0 Z 0 -X 0 0 -1 -Y X 0      (A.2) G X =               0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 1 0 0 0 0 0 Y 2 Z 2 0 0 1 Y 2 -X 0 0 -1 0 Z 2 0 -X               (A.3) G Y =               0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 -1 -Y X 2 0 0 0 0 X 2 0 Z 2 1 0 0 0 Z 2 -Y               (A.4) G Z =               0 0 0 0 -1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 -Z 0 X 2 -1 0 0 0 -Z Y 2 0 0 0 X 2 Y 2 0               (A.5)

A.2 Image Points

In the case of normalized image points, the interaction matrix is function not only of the features coordinates x and y, but also of the 3D depth of the observed point. Therefore, in order to obtain the required second-order model, the interaction matrix of the depth is to be considered. It simply consists in the last row of the interaction matrix of a 3D point, in which the Cartesian coordinates X and Y are replaced by xZ and yZ. In this way, also L z depends only on s and z, and the matrices G x and G y can finally be obtained. The expression of G Z is also reported, since it is required by the predictors presented in Section 3.3.

s T = x y (A.6) z = Z (A.7) L s =   -1 Z 0 x Z xy -x 2 -1 y 0 -1 Z y Z y 2 + 1 -xy -x   (A.8) L z = 0 0 -1 -Zy Zx 0 (A.9) G x =                0 0 -1 Z 2 -y Z 2x Z 0 0 0 0 -x Z 0 -1 Z -1 Z 2 0 2x Z 2 2xy Z -2x 2 Z y Z -y Z -x Z 2xy Z x (2y 2 + 1) - y(4x 2 +1) 2 -x 2 2 + y 2 + 1 2 2x Z 0 -2x 2 Z - y(4x 2 +1) 2 2x (x 2 + 1) -3xy 2 0 -1 Z y Z -x 2 2 + y 2 + 1 2 -3xy 2 -x                (A.10) G y =                0 0 0 0 y Z 1 Z 0 0 -1 Z 2 -2y Z x Z 0 0 -1 Z 2 2y Z 2 2y 2 Z -2xy Z -x Z 0 -2y Z 2y 2 Z 2y (y 2 + 1) - x(4y 2 +1) 2 -3xy 2 y Z x Z -2xy Z - x(4y 2 +1) 2 y (2x 2 + 1) x 2 -y 2 2 + 1 2 1 Z 0 -x Z -3xy 2 x 2 -y 2 2 + 1 2 -y                (A.11) G Z =               0 0 0 0 -1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 -Z 0 Zx 2 -1 0 0 0 -Z Zy 2 0 0 0 Zx 2 Zy 2 0               (A.12)

A.3 Polar Coordinates

This case is similar to the previous one, in which normalized points were used, and the parameter vector still consists in the 3D depth of the observed point. Polar coordinates and image points are linked by the well-known relation

x = ρ cos θ (A.13) y = ρ sin θ (A.14)
which can be used to obtain an expression for L z and G Z that depends only on ρ, θ and Z.

s T = ρ θ (A.15) z = Z (A.16) L s =   -cos (θ) Z -sin (θ) Z ρ Z (ρ 2 + 1) sin (θ) (-ρ 2 -1) cos (θ) 0 sin (θ) Zρ -cos (θ) Zρ 0 cos (θ) ρ sin (θ) ρ -1   (A.17) L z = 0 0 -1 -Zρ sin (θ) Zρ cos (θ) 0 (A.18) G ρ =                sin 2 (θ) Z 2 ρ -sin (2θ) 2Z 2 ρ -cos (θ) Z 2 -sin (2θ) 2Z 2 ρ cos 2 (θ) Z 2 ρ -sin (θ) Z 2 -cos (θ) Z 2 -sin (θ) Z 2 2ρ Z 2 - (ρ 2 -1) sin (2θ) 2Zρ ρ 2 cos 2 (θ)-2ρ 2 -cos 2 (θ) Zρ 2ρ 2 sin (θ) Z -ρ 2 sin 2 (θ)+2ρ 2 +sin 2 (θ) Zρ (ρ 2 -1) sin (2θ) 2Zρ -2ρ 2 cos (θ) Z 0 0 0 - (ρ 2 -1) sin (2θ) 2Zρ -ρ 2 sin 2 (θ)+2ρ 2 +sin 2 (θ) Zρ 0 ρ 2 cos 2 (θ)-2ρ 2 -cos 2 (θ) Zρ (ρ 2 -1) sin (2θ) 2Zρ 0 2ρ 2 sin (θ) Z -2ρ 2 cos (θ) Z 0 (ρ 2 +1)(2ρ 2 sin 2 (θ)+cos 2 (θ)) ρ - (2ρ 4 +ρ 2 -1) sin (2θ) 2ρ - (ρ 2 +1) cos (θ) 2 - (2ρ 4 +ρ 2 -1) sin (2θ) 2ρ (ρ 2 +1)(2ρ 2 cos 2 (θ)+sin 2 (θ)) ρ - (ρ 2 +1) sin (θ) 2 - (ρ 2 +1) cos (θ) 2 - (ρ 2 +1) sin (θ) 2 0                 (A.19) G θ =                sin (2θ) Z 2 ρ 2 -cos (2θ) Z 2 ρ 2 0 cos (2θ) Zρ 2 sin (2θ) Zρ 2 0 -cos (2θ) Z 2 ρ 2 -sin (2θ) Z 2 ρ 2 0 sin (2θ) Zρ 2 -cos (2θ) Zρ 2 0 0 0 0 -cos (θ) Zρ -sin (θ) Zρ 0 cos (2θ) Zρ 2 sin (2θ) Zρ 2 -cos (θ) Zρ - (ρ 2 +2) sin (2θ) 2ρ 2 (ρ 2 +2) cos (2θ) 2ρ 2 sin (θ) 2ρ sin (2θ) Zρ 2 -cos (2θ) Zρ 2 -sin (θ) Zρ (ρ 2 +2) cos (2θ) 2ρ 2 (ρ 2 +2) sin (2θ) 2ρ 2 -cos (θ) 2ρ 0 0 0 sin (θ) 2ρ -cos (θ) 2ρ 0                (A.20) G Z =               0 0 0 0 -1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 -Z 0 Zρ cos (θ) 2 -1 0 0 0 -Z Zρ sin (θ) 2 0 0 0 Zρ cos (θ) 2 Zρ sin (θ) 2 0               (A.21)

A.4 Planes

3D Planes can be represented in Cartesian Space by the equation

n x X + n y Y + n z Z = d n 2 x + n 2 y + n 2 z = 1 (A.22)
wherein (X, Y, Z) is any point belonging to the plane, d > 0 represents the distance of the plane from the origin of the sensor frame and n T = n x n y n z is the normal of the plane. There are no parameters in this case, and the feature vector consists in the normal and the value of d.

s T = n x n y n z d (A.23) L s =   0 [n] × -n T 0   =         0 0 0 0 -n z n y 0 0 0 n z 0 -n x 0 0 0 -n y n x 0 -n x -n y -n z 0 0 0         (A.24) G nx =               0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ny 2 nz 2 0 0 0 ny 2 -n x 0 0 0 0 nz 2 0 -n x               (A.25) G ny =               0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -n y nx 2 0 0 0 0 nx 2 0 nz 2 0 0 0 0 nz 2 -n y               (A.26) G nz =               0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -n z 0 nx 2 0 0 0 0 -n z ny 2 0 0 0 nx 2 ny 2 0               (A.27) G d = O (A.28)
An alternative, minimal, representation of a 3D plane can exploited, relating the coordinates of normalized points as:

Ax + By + C = 1/Z (A.29)
It is linked to the previous definition of planes by

A = n x d B = n y d C = n z d (A.30)
The interest in considering this representation, and in obtaining the corresponding secondorder interaction models, is justified by the fact that these values appear as well in the interaction matrices of planar image moments.

s T = A B C (A.31) L s = ss T -[s] × =      A 2 AB AC 0 -C B AB B 2 BC C 0 -A AC BC C 2 -B A 0      (A.32) G A =               2A 3 2A 2 B 2A 2 C 0 -AC AB 2A 2 B 2AB 2 2ABC 0 -BC B 2 2A 2 C 2ABC 2AC 2 0 -C 2 BC 0 0 0 0 B 2 C 2 -AC -BC -C 2 B 2 -A 0 AB B 2 BC C 2 0 -A               (A.33) G B =               2A 2 B 2AB 2 2ABC AC 0 -A 2 2AB 2 2B 3 2B 2 C BC 0 -AB 2ABC 2B 2 C 2BC 2 C 2 0 -AC AC BC C 2 -B A 2 0 0 0 0 A 2 0 C 2 -A 2 -AB -AC 0 C 2 -B               (A.34) G C =               2A 2 C 2ABC 2AC 2 -AB A 2 0 2ABC 2B 2 C 2BC 2 -B 2 AB 0 2AC 2 2BC 2 2C 3 -BC AC 0 -AB -B 2 -BC -C 0 A 2 A 2 AB AC 0 -C B 2 0 0 0 A 2 B 2 0              
(A.35)

A.5 Image Moments

We conclude this section by reporting the matrices of the second-order model of planar image moments. Even though they were not used in the work presented in this thesis, they have been included here for reference since extensions of the predictors considered in [START_REF] Fusco | Integrating Features Acceleration in Visual Predictive Control[END_REF] could be based on these models. For the planar moment m i/j , the parameter vector is significantly larger than those found for other features, and includes other image moments. This leads to second-order models containing several terms. In particular, all entries of G m i/j can be expressed as a linear combination of image moments whose order varies from i+j -2 to i+j +2. Therefore, for each entry of (A.39), the linear combination coefficients are listed, and the overall expression is then given. Note that only the upper triangular portion of (A.39) has been reported, since the remaining terms follow from the symmetry of G m i/j . In addition, following the notation adopted in [START_REF] Tahri | Point-based and region-based image moments for visual servoing of planar objects[END_REF], the parameter δ is used, whose value equals 1 or 0 respectively when dense or discrete moments are used. s = m i/j (A.36) z T = m i/j-1 m i/j+1 m i-1/j m i+1/j m i-1/j+1 m i+1/j-1 A B C (A.37)

L T s =              
-Aδm i/j -i Am i/j + Bm i-1/j+1 + Cm i-1/j -Bδm i/j -j Am i+1/j-1 + Bm i/j + Cm i/j-1 -Cδm i/j + (3δ + i + j) Am i+1/j + Bm i/j+1 + Cm i/j jm i/j-1 + m i/j+1 (3δ + i + j) -im i-1/j + m i+1/j (-3δ -i -j) im i-1/j+1 -jm i+1/j-1

              (A.38) G m i/j =               G vx,
G m i/j
wy,wy = i (3δ + i + j -1) + (i + 1) (3δ + i + j) (A.58b)

G m i-2/j
wy,wy = i (i -1) (A.58c)

G m i+2/j
wy,wy = (3δ + i + j) (3δ + i + j + 1) (A.58d)

is not expected to change:

h(x + δx ) h(x ) + δx T ∇h(x ) (B.6)
Since x is a feasible point, it follows that h(x + δx) h(x ) = 0, and thus the new point x + δx should be feasible as well. However, using a similar argument, the value of the objective is expected to change of a quantity δx T ∇f (x ), which cannot be null since ∇f ∦ ∇h. This shows that there is always the possibility to locally change the value of f , and in particular to decrease it, which is equivalent to say that x cannot be a minimum. On the contrary, if ∇f has no component in the tangent space of the constraints as in Figure B.1(b), then δx T ∇f (x ) = 0 and thus x is a candidate optimum point. Note that one can also interpret the condition in terms of forces acting on a point mass. In practice -∇f acts as a force pulling the point towards unconstrained minima, while -λ∇h as the reaction force that is necessary to keep the mass "attached" to the constraint curve. In this sense, a minimum corresponds to a stable static equilibrium point for the mass.

B.2 Active Set Quadratic Programming

A quadratic minimization problem is a special case of nonlinear optimization in which the objective function is linear-quadratic in the decision vector. In addition, unless otherwise specified, it is almost always assumed that constraints are linear. According to this definition, the goal of Quadratic Programming is to find the solution to the following special form of (B. wherein the squared matrix Q is assumed to be symmetric and positive semidefinite.

One strategy that can be employed to solve this special class of problems is the one known as Active Set Quadratic Programming. It is an iterative algorithm which works by taking steps within D while activating or deactivating inequality constraints as needed and, as it will be shown in the following, it is closely related to the KTT conditions illustrated before. In each iteration, given a valid point x i-1 , the procedure mainly consists in the following steps:

1. Determine a step direction z i .

2. Evaluate a step length α i .

3. Perform the step: x i = x i-1 + α i z i . 4. Activate or deactivate constraints as needed.

B.2.1 Solution Algorithm

Determine a step direction z i .

In order to find the step direction z i , a change of variable is performed in the original quadratic problem, introducing the variable z as in x = x i-1 + z. After substitution into (B.7), the new (quadratic) sub-problem is obtained:

min z 1 2 z T Qz -s T i z (B.8a)      Q C T A T C A           z µ λ      = ≤ =      s i e i 0     
(B.9) wherein trivial zeros have been removed for readability. As stated in the previous section, inequality constraints can be divided into inactive ones (in which the inequality is strict) and actives (which locally act as equality constraints). One can thus partition, at the current point x i-1 , the inequality constraints as:

C a,i z = e a,i C na,i z < e na,i (B.10)

Note that also the associated Lagrange multipliers are to be partitioned in a similar way. In addition, since the constraints are all smooth, the partition will be constant at least in a neighborhood of z = 0. Using this fact, and considering condition (B.4d) (complementary slackness), it directly follows that µ a,i = 0, ultimately leading to a simplification of (B.9) to:

     Q C T a,i A T C a,i A           z µ a λ      =      s e a,i 0      (B.11)
Solving this linear system of equations leads to the optimal solution of the sub-problem, with the primal optimum z being the desired step direction z i .

Determine a step length α i

The solution z i found at the previous step is valid only if the partition of active and passive inequality constraints does not change. As mentioned above, it is true at least in a neighborhood of z = 0, but the question is whether it holds also at z i . Finding an answer to this question is equivalent to determine if there exist a value α ∈ [0, 1] such that the set of active constraints changes at z = αz i .

To solve the current problem, each of the currently inactive constraints is written as c T j z ≤ e j . While "moving" from the origin to z i , the current configuration z could either approach the boundary of an inactive constraint or not. The second case corresponds to situations in which c T j z i does not move towards the value e j , which can happen only if c T j z i ≤ 0. In fact, by definition, e j . = d j -c T j x i . Moreover, by assumption x i is a feasible point such that c T j x i ≤ d j , which implies 0 ≤ d j -c T j x i , i.e., e j ≥ 0. Since in this step we are considering z = αz i , with positive α, then c T j z i ≤ 0 implies αc T j z i ≤ 0 ≤ e j . One case has yet to be checked, i.e., when c T j z i > 0. Recall that the problem at hand is to determine if at some point z = αz i there are any constraints that activate. By definition, for each j such that c T j z i > 0 we must ensure αc T j z i ≤ e j , which corresponds to α ≤ e j c T j z i . To satisfy this system of inequalities, it suffices to take the smallest of the right hand sides. In addition, since α ∈ [0, 1], the desired value is finally: Note however that there exist a "degenerate case", so as to speak, in which z i = 0. In this case, looking for a step size using the outlined procedure is pointless, and α i = 1 can directly be selected. In practice, any value in the interval [0, 1] could be chosen. However, using 1 allows the following step to be performed "safely" without having to treat this case separately.

α i =

Activating or deactivating constraints

The new iterate x i can now be formed as x i = x i-1 + α i z i . By construction, it is a valid point and is such that f (x i ) ≤ f (x i-1 ). Before continuing with the new iteration, it is necessary to check the current active set of constraints.

If α i < 1, then the current iterate x i has reached the boundary of one of the inequality constraints. In other words, it is has now become active, and it is necessary to include it in the active set.

x 0 (a) The dark regions represent the invalid domain according to two linear constraints. The minimum value of the objective is found at the red point, which violates the constraints. The optimization starts at the yellow point shown in (a), and moves within the valid domain until the constrained optimum is reached in (f). Along the process, some constraints need to be activated (they are highlighted in red) and deactivated.

x 0 + z 1 x 1 = x 0 + α 1 z 1 α 1 < 1 (b) α 2 < 1 x 2 = x 1 + α 2 z 2 x 1 + z 2 (c)

Solving the quadratic sub-problem

While detailing the overall active set algorithm, it was simply stated that at each iteration the step z i defined by the quadratic sub-problem is given by the solution of the following linear system:

     Q C T a,i A T C a,i A           z µ a λ      =      s e a,i 0      (B.18)
without providing hints on how to solve it. In practice, there are two main possible solutions, the first one being to perform a Cholesky decomposition with pivoting (also known as LDLT decomposition) on the overall matrix, and to solve the problem using that factorization.

An alternative approach is similar to the one detailed in the previous section, and consists in firstly finding a particular solution z eq to the sub-system and then to perform the change of variable defined by: z = z eq + Z eq y s.t. This new parameterization can then be injected in the original sub-problem and a solution can be obtained in the lower-dimensional vector y.

B.3 Sequential Quadratic Programming

As already mentioned in Chapter 3, Sequential Quadratic Programming is an optimization algorithm that can be used to solve generic non-linearly constrained nonlinear minimization problems. The principle of this method is to locally approximate the problem at hand with a quadratic minimization, which can be solved using any QP algorithm. used to assign a score to new points. An example of such cost is given by [START_REF] Han | A globally convergent method for nonlinear programming[END_REF]: m(x) = f (x) + j r j max(g j (x), 0) + j s j |h j (x)| (B.23) wherein g j and h j are the j-th inequality and equality constraint. The values r j and s j are instead weights that penalize constraints infringement. They can be simply assigned the value of the j-th dual variable, i.e., r j = µ i and s i = λ i . Alternatively, [START_REF] Powell | A fast algorithm for nonlinearly constrained optimization calculations[END_REF] proposes to use weights that depend also on their past values r past The step size is finally obtained by a line search in the following form:

α i = arg min α∈[0,1] m(x i-1 + αz i ) (B.25)
and the new iterate x i is set to x i-1 + α i z i .

The procedure is repeated until one or more termination criteria are met, common choices in this sense being to set a time limit or a maximum number of iterations. In addition, it is possible to determine if a constrained local minimum has been attained by checking the norm of the update step α i z i or by checking the KKT conditions.

One last important remark is related to handling of linear constraints. In the formulation of (B.22) it was assumed that g and h were generic, possibly but not necessarily nonlinear, functions. However, it is more efficient to make an explicit distinction between linear and nonlinear constraints. In fact, no linearization is needed when formulating the quadratic sub-problem (B.22). Furthermore, and possibly more importantly, they do not contribute to the value of the merit function. In fact, assuming that x i-1 is in the valid domain defined by linear (in)equalities, it follows that the point x i-1 + z i will also be valid since the QP subroutine always enforces the validity of subsequent iterates. Finally, since domains defined by linear equalities and inequalities are convex, it follows that any point in the form x i-1 + αz i is necessarily valid for α ∈ [0, 1], and therefore they do not participate to (B.23). can be evaluated: Abstract: This thesis deals with increasing the productivity in manufacturing robots, when performing sensor-based tasks. Such tasks may be coming from the target not being absolutely positioned.

∂ ∂x φ 2 = 2φ
Visual servoing control schemes are well known for their robustness and precision, but generally require long execution times due to different factors. Control laws are generally formulated only at a kinematic level and characterized by exponentially decreasing velocities. Moreover, the nonlinear map from the operational space to the sensor space can lead to sub-optimal and longer paths.

To increase control performances and re-duce the time required to complete a task, this thesis investigates the use of second-order interaction models. Their use in dynamic feedback control laws is investigated and compared to classical controllers. They are then employed in Model Predictive Control (MPC) schemes, allowing to obtain higher velocities and better sensor trajectories. However, a drawback of MPC techniques is their computational load. In order to obtain even better results, a new type of predictive control is thus investigated, leading to a reduced number of variables involved in MPC optimization problems thanks to the use of a parameterization of the control input sequences.

  (a) An object observed from different poses. (b) Object projection in the two views.

Figure 1

 1 Figure 1.1 -A perspective camera observing a rectangular object from different views.Moving the camera from one pose to the other is equivalent to moving the corners of the projected shape from one image to the other.

  same meaning as before, and v w o represents the linear and angular velocity of the object in the fixed frame F w .

  Figure 1.2 -Examples of (a) eye-in-hand and (b) eye-to-hand [CSC16] configurations. On the left, the relevant frames are named as proposed in the text, while on the right the naming used in the original paper has been kept. In practice, F w corresponds to {t}, F c to {c} and F o to {h}.

Figure 2

 2 Figure 2.2 -Initial (left) and desired (right) configuration of the observed object. The points used for the task are the centers of the four circles.

Figure 2 . 3 -

 23 Figure 2.3 -Comparison of the visual error (left) and control input (right) using the different control strategies, in absence of noise. FOFF and SO are almost indistinguishable in terms of error evolution.

Figure 2 . 4 -

 24 Figure 2.4 -Comparison of visual error (left) and control inputs (right) when noise is injected in simulations. FOFF and SO are still almost indistinguishable in terms of error evolution.

Figure 2

 2 Figure 2.5 -Comparison of the visual error (left) and control input (right) during the execution of a trajectory. Time axes are relative to t 0 .

Figure 3 . 1 -

 31 Figure 3.1 -Example of control sequence with n c = 4 and n p = 10. The last n p -n c parameters are nothing but a repetition of u nc-1 .

Figure 3 . 2 -

 32 Figure 3.2 -Pictorial example of state prediction in single-shooting methods. Given a control sequence u 0 , • • • , u nc-1 and an initial sample x 0 , future samples are evaluated in succession using the model function f d . The result is a "continuous signal", whose samples constitute the prediction vector x.

Figure 3

 3 Figure 3.4 -Input-output linearized model of the robot and of the visual system [GD03]. The central block is the Virtual Cartesian Motion Device.

  (a) Features evolution using Otimal IBVS. (b) Control inputs resulting from Optimal IBVS. (c) Features evolution using GM c with n p = 10 and n c = 1. (d) Distance of the camera from the object in two VPC simulations.

Figure 3

 3 Figure 3.6 -Results from Visual Servoing simulations using either Optimal IBVS or GM c VPC [ACC10]. Visibility constraints are enforced during the motion using both strategies, which is generally not the case in classical feedback controllers. However, when using Optimal IBVS, input control signals are highly discontinuous (b), resulting in an irregular features evolution (a). On the other hand, with a larger prediction horizon (n p = 10) the resulting motion is much smoother (c). Finally, in (d), a comparison of the distance of the camera from the object along the vertical axis with and without workspace bounds to explicitly limit retraction.

Figure 3

 3 Figure 3.8 -First simulation set (∆t = ∆t c , n c = 1), first scenario (90°rotation around the optical axis). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.9 -First simulation set (∆t = ∆t c , n c = 1), second scenario (180°rotation around the optical axis). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.10 -First simulation set (∆t = ∆t c , n c = 1), third scenario (180°around the optical axis and short translation). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.11 -Second simulation set (∆t = 5∆t c , n c = 1), first scenario (90°rotation around the optical axis). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.12 -Second simulation set (∆t = 5∆t c , n c = 1), second scenario (180°rotation around the optical axis). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.13 -Second simulation set (∆t = 5∆t c , n c = 1), third scenario (180°around the optical axis and short translation). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.14 -Third simulation set (∆t = 5∆t c , n c = 10), first scenario (90°rotation around the optical axis). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.15 -Third simulation set (∆t = 5∆t c , n c = 10), second scenario (180°rotation around the optical axis). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.16 -Third simulation set (∆t = 5∆t c , n c = 10), third scenario (180°around the optical axis and short translation). The rows correspond to M 1 (top), M 2 (middle), M H (bottom), while the columns to the features trajectories in the image (left), the feature error (center) and a 3D view of the manipulator (right).

Figure 3

 3 Figure 3.18 -Features trajectory in the image using respectively M 1 , M 2 and M H . Blue segments correspond to features displacements induced by the object rotation, while green ones are due to the control moving the camera.

Figure 3 .

 3 Figure 3.19 -Evolution of the features error norm, while servoing from four image point coordinates. The plotted data correspond to the signals obtained during the first rotation of the object.

  Figure 3.20 -Trajectory of the point centers in the image using respectively M 1 , M 2 and M H when servoing from polar coordinates. Blue segments correspond to displacements induced by the object translation, while green ones are due to the control moving the camera.

Figure 3

 3 Figure 3.21 -Evolution of the feature error norms, while servoing from polar coordinates. In (a), the error associated to ρ, while in (b) the one associated to θ.

Figure 4 . 1 -

 41 Figure 4.1 -Example of sequence that could be produced by a ZOH parameterization.Here, free samples are located at k = 0, 2, 5, 8, thus leading to a parameter vector of dimension n π = 4.

Figure 4 . 2 -

 42 Figure 4.2 -Example of sequence that could be produced by a LERP parameterization.Here, free samples are located at k = 0, 2, 5, 8, thus leading to a parameter vector of dimension n π = 4.

Figure 4 . 3 -

 43 Figure 4.3 -Sets of achievable controls using the simple (red), ZOH (green) and LERP (blue) parameterizations, under the variations constraints (4.7).

Figure 4 . 4 -

 44 Figure 4.4 -Pictorial example showing how a control sequence is produced from a set of basis functions with given combination coefficients.

Figure 4 . 5 -

 45 Figure4.5 -Control of a triple integrator using a simple (red), ZOH (green), LERP (blue) and Laguerre (magenta) parameterizations, all having n π = 6 parameters. An additional case is reported for comparison, consisting in a full parameterization with n π = n p .

Figure 4 . 6 -

 46 Figure4.6 -Control of a triple integrator using a simple (red), ZOH (green), LERP (blue) and Laguerre (magenta) parameterizations, all having n π = 3 parameters. An additional case is reported for comparison, consisting in a full parameterization with n π = n p .

Figure 4 . 7 -

 47 Figure4.7 -Control of a triple integrator using a simple (red), ZOH (green), LERP (blue) and Laguerre (magenta) parameterizations, all having n π = 2 parameters. An additional case is reported for comparison, consisting in a full parameterization with n π = n p .

Figure 4

 4 Figure 4.8 -Optimization times obtained when controlling a triple integrator using different parameterizations, as function of the number of parameters n π . Solid lines represent median times, while semi-transparent regions are used to show maximum and minimum times. Provided values are relative to the median time required for the optimization using the full parameterization.

Figure 4 . 9 -

 49 Figure 4.9 -Average tracking errors obtained when controlling a triple integrator using different parameterizations, as function of the number of parameters n π . Provided values are relative to the average error obtained when using the full parameterization.

Figure 4 .

 4 Figure4.10 -Control of a mass-spring-damper system using a simple (red), ZOH (green), LERP (blue) and Laguerre (magenta) parameterizations with n π = 6 parameters, as well as a full parameterization (yellow) with n π = n p = 30.

Figure 4 .

 4 Figure 4.11 -Control of a mass-spring-damper using n π = 6 parameters. The control variation limit has been increased to ∆u lim = 5 N.

Figure 4

 4 Figure4.12 -Control of a mass-spring-damper system using a simple (red), ZOH (green), LERP (blue) and Laguerre (magenta) parameterizations with n π = 3 parameters, as well as a full parameterization (yellow) with n π = n p = 30.

Figure 4

 4 Figure4.13 -Average tracking errors obtained when controlling a mass-spring-damper system using different parameterizations, as function of the number of parameters n π . Provided values are relative to the average error obtained when using the full parameterization.

Figure 4

 4 Figure 4.14 -Sketch of the crane-pendulum system.

  Figure 4.15 -Simulation results from the crane-pendulum control problem. Each row corresponds to a different simulation set, with increasing number of parameters. For each set, the position of the base and the corresponding tracking error are shown respectively on the left and right graphs.

Figure 4

 4 Figure 4.16 -Comparison of four parameterizations while controlling a crane-pendulum system. The four signals represent the average position tracking error of each parameterization as function of the number of parameters n π , normalized with respect to the full parameterization.

Figure 4 .

 4 Figure4.17 -Comparison of tracking error norms for different parameterizations in the visual servoing simulations. The value of n π was m•j = 6j, with j ranging from 2 (n π = 12) to 10 (n π = 60). In the graph, darker colors correspond to smaller numbers of parameters.

Figure 4 .

 4 Figure 4.18 -View of the target as observed from the free-flying camera, with the desired (red) and actual (green) trajectories of the dots. The first four images correspond to parameterizations using 3 parameters for each control coordinate, for a total of n π = 18 parameters. The last image corresponds to a full parameterization with n π = 450 parameters.

===

  A 2 j (j -1) (A.46g)G vy,vz = G m i/j vy,vz m i/j + G m i/j-1 vy,vz m i/j-1 + G m i/j+1 vy,vz m i/j+1 + G m i+1/j vy,vz m i+1/j + G m i+1/j-1 vy,vz m i+1/j-1 + G m i+2/j-1 vy,vz m i+2/j-1 (A.47a) G m i/j vy,vz = -BC 2δ 2 + δi + 6δj + δ + 2ij + 2j 2 (A.47b) G m i/j-1 vy,vz = -C 2 j (2δ + i + j) (A.47c) G m i/j+1 vy,vz = -B 2 (δ + j) (3δ + i + j) (A.47d) G m i+1/j vy,vz = -AB (δ + 2j) (3δ + i + j) (A.47e) G m i+1/j-1 vy,vz = -ACj (5δ + 2i + 2j) (A.47f) G m i+2/j-1 vy,vz = -A 2 j (3δ + i + j) (A.47g) G vy,wx = G m i/j vy,wx m i/j + G m i/j-1 vy,wx m i/j-1 + G m i/j+1 vy,wx m i/j+1 + G m i+1/j vy,wx m i+1/j + G m i/j-2 vy,wx m i/j-2 + G m i+1/j-2 vy,wx m i+1/j-2 (A.48a) G m i/j vy,wx = -C 3δj + 3δ + ij + i + j 2 + j (A.48b) G m i/j-1 vy,wx = Bj (-δ -j + 1) (A.48c) G m i/j+1vy,wx = -B (δ + j + 1) (3δ+ i + j) (A.48d) G m i+1/j vy,wx = -A (j + 1) (3δ + i + j) (A.48e) G m i/j-2 vy,wx = Cj (1 -j) (A.48f) G m i+1/j-2 vy,wx = Aj (1 -j) (A.48g) G vy,wy = G m i/j-1 vy,wy m i/j-1 + G m i-1/j vy,wy m i-1/j + G m i+1/jvy,wy m i+1/j + G m i+1/j-1 vy,wym i+1/j-1 + G m i-1/j-1 vy,wy m i-1/j-1 + G m i+2/j-1 vy,wy m i+2/j-1 (A.49a) G m i/j-1 vy,wy = Aij (A.49b) G m i-1/j vy,wy = Bi (δ + j) (A.49c) G m i+1/j vy,wy = B (δ + j) (3δ + i + j) (A.49d) G m i+1/j-1 vy,wy = Cj (3δ + i + j) Aj (3δ + i + j) (A.49g) G vy,wz = G m i/j vy,wz m i/j + G m i-1/j vy,wz m i-1/j + G m i-1/j+1 vy,wz m i-1/j+1 + G m i+1/j-1 vy,wz m i+1/j-1 + G m i+1/j-2 vy,wz m i+1/j-2 + G m i+2/j-2 vy,wz m i+2/j-2 (A.50a) G m i/j vy,wz = -Ai (j Aj (j -1) (A.50g) G vz,vz = G m i/j vz,vz m i/j + G m i/j+1

Figure

  Figure B.2 -Different phases during the solution of a constrained quadratic optimization.The dark regions represent the invalid domain according to two linear constraints. The minimum value of the objective is found at the red point, which violates the constraints. The optimization starts at the yellow point shown in (a), and moves within the valid domain until the constrained optimum is reached in (f). Along the process, some constraints need to be activated (they are highlighted in red) and deactivated.

  subject to:CZ A y ≤ d -Cx eq (B.17b)

j and s past j :

 j r j = max µ j , µ j + r past j 2 s j = max |λ j | , |λ j | + s past j 2 (B.24)

Table 3

 3

	.1 -Computation time (wall clock) for the different VPC strategies.
	Test Set	Scenario	Model	Average time
			M 1	2.06 ms
		1	M 2	6.30 ms
			M H	3.54 ms
	1	2	M 1 M 2	1.43 ms 3.72 ms
			M H	3.38 ms
			M 1	1.80 ms
		3	M 2	3.79 ms
			M H	2.83 ms
			M 1	1.20 ms
		1	M 2	4.20 ms
			M H	3.09 ms
	2	2	M 1 M 2	1.18 ms 4.30 ms
			M H	3.01 ms
			M 1	1.32 ms
		3	M 2	3.53 ms
			M H	2.69 ms
			M 1	18 ms
		1	M 2	18 ms
			M H	18 ms
	3	2	M 1 M 2	18 ms 18 ms
			M H	18 ms
			M 1	18 ms
		3	M 2	18 ms
			M H	18 ms

Table 4 .

 4 1 -Constants used in Gazebo to simulate the crane-pendulum.

	M	m	I	m t	I 0
	0.25 kg 0.1178 kg 0.001 25 kg m 2 0.3 m 0.3678 kg 0.011 852 kg m 2

  vx G vx,vy G vx,vz G vx,wx G vx,wy G vx,wz G vy,vy G vy,vz G vy,wx G vy,wy G vy,wz G vz,vz G vz,wx G vz,wy G vz,wz G wx,wx G wx,wy G wx,wz G wy,wy G wy,wz G wz,wz

			G m i/j vx,vx = A 2 -δ -i + (δ + i) 2 G m i/j vx,wz = Bj (i + 1)	(A.40b) (A.45b)
	G m i-1/j vx,vx = 2ACi (δ + i -1) G m i-1/j+1 vx,vx = 2ABi (δ + i -1) G m i-2/j vx,vx = C 2 i (i -1) vx,wx m i/j+1 + G vx,wx m i/j-1 + G G vx,wx = G m i/j-1 m i/j+1 G m i/j-1 vx,wz = Cj (i + 1) m i-1/j vx,wx m i-1/j + G m i-1/j+1 vx,wx m i-1/j+1 + G m i-1/j-1 vx,wx m i-1/j-1 + G G m i-1/j+1 vx,wz = Ai (-δ -i + 1) m i-1/j+2 vx,wx G m i+1/j-1 vx,wz = Aj (δ + i + 1)	(A.40c) (A.40d) (A.40e) (A.45c) (A.45d) m i-1/j+2 (A.43a) (A.45e)
			G m i-2/j+1 vx,vx G m i-2/j+1 = 2BCi (i -1) vx,wz = Ci (1 -i)	(A.40f) (A.45f)
			G m i-2/j+2 vx,vx m i/j-1 G vx,wx = -Aj (δ + i) = B 2 i (i -1) G m i-2/j+2 vx,wz = Bi (1 -i)	(A.40g) (A.43b) (A.45g)
		G m i/j+1 vx,wx = -A (δ + i) (3δ + i + j)	(A.43c)
			G m i-1/j vx,wx = -Bij	(A.43d)
	G vx,vy = G G vy,vy = G	m i-1/j vx,vy m i-1/j m i-1/j+1 + G m i+1/j-1 vx,vy m i+1/j-1 + G m i-1/j-1 vx,vy m i-1/j+1 m i-1/j+1 + G vx,vy G vx,wx = -Ci (3δ + i + j) G m i-1/j-1 vx,wx = -Cij m i+1/j-1 vy,vy m i+1/j-1 G m i-1/j+2 vx,wx = -Bi (3δ + i + j) + G m i/j-2 vy,vy m i/j-2 + G m i+1/j-2 vy,vy m i+1/j-2 + G m i+2/j-2 vy,vy	m i-1/j-1 (A.41a) (A.43e) (A.43f) (A.43g) m i+2/j-2 (A.46a)
			G m i/j vx,vy = AB δ 2 + δi + δj -δ + 2ij G m i/j	(A.41b)
			G m i/j-1 vx,vy	= ACj (δ + 2i)	(A.41c)
			G m i-1/j vx,vy	= BCi (δ + 2j)	(A.41d)
	G m i-1/j+1 vx,vy vx,wy m i/j+1 + G vx,wy m i/j + G G vx,wy = G m i/j m i/j+1 m i-1/j vx,wy m i-1/j = B 2 i (δ + j)	(A.41e)
		G m i+1/j-1 vx,vy vx,wy m i+1/j + G + G m i+1/j m i-2/j = A 2 j (δ + i) vx,wy m i-2/j + G m i-2/j+1 vx,wy	(A.41f) m i-2/j+1 (A.44a)
					G m i-1/j-1 vx,vy	= C 2 ij	(A.41g)
		G m i/j vx,wy = C 3δi + 3δ + i 2 + ij + i + j	(A.44b)
		G m i/j+1 vx,wy = B (i + 1) (3δ + i + j)	(A.44c)
	G vx,vz = G	             m i-1/j+2 m i-1/j+1 + G vx,wy = Ai (δ + i -1) m i-1/j G m i-1/j vx,vz m i-1/j m i+1/j + G vx,vz m i+1/j + G m i-1/j+1 vx,vz vx,vz G m i+1/j vx,wy = A (δ + i + 1) (3δ + i + j) G m i-2/j vx,wy = Ci (i -1) G m i/j vx,vz = -AC 2δ 2 + 6δi + δj + δ + 2i 2 + 2ij G m i-2/j+1 vx,wy = Bi (i -1)	(A.44d) (A.44e) m i-1/j+2 (A.42a) (A.44f) (A.39) (A.42b) (A.44g)
			G m i/j+1 vx,vz	= -AB (δ + 2i) (3δ + i + j)		(A.42c)
			G m i-1/j vx,vz	= -C 2 i (2δ + i + j)	(A.42d)
			G m i+1/j vx,vz	= -A 2 (δ + i) (3δ + i + j)	(A.42e)
	G vx,vx = G m i/j vx,vx m i/j + G m i-1/j vx,vx m i-1/j + G m i-1/j+1 vx,vx G m i-1/j+1 vx,vz = -BCi (5δ + 2i + 2j) m i-1/j+1 G vx,wz = G m i/j vx,wz m i/j + G m i/j-1 vx,wz m i/j-1 + G m i-1/j+1 vx,wz m i-1/j+1	(A.42f)
		+ G + G m i+1/j-1 vx,wz	m i-2/j+1 vx,vx m i-1/j+2 G vx,vz = -B 2 i (3δ + i + j) m i-2/j+1 + G m i-2/j+2 vx,vx m i+1/j-1 + G m i-2/j+1 vx,wz m i-2/j+1 + G m i-2/j+2 vx,wz	m i-2/j+2 (A.40a) (A.42g) m i-2/j+2 (A.45a)

m i-2/j vx,vx m i-2/j + G m i/j vx,vy m i/j + G m i/j-1 vx,vy m i/j-1 + G m i/j vx,vz m i/j + G m i/j+1 vx,vz m i/j+1 + G m i/j vy,vy m i/j + G m i/j-1 vy,vy m i/j-1 + G vy,vy = B 2 -δ -j + (δ + j) 2 (A.46b) G m i/j-1 vy,vy

  j represents the j-th component of φ.Comparing (B.22a) and (B.28a) reveals that this Gauss-Newton method corresponds to a SQP one in which the hessian is approximated by discarding the second summation appearing in (B.29b). Note also that the approximated hessian is by definition at least positive semidefinite, and thus it can be used in alternative to the approximation method proposed in[START_REF] Powell | A fast algorithm for nonlinearly constrained optimization calculations[END_REF]. Asservissement Visuel Dynamique pour Bras Manipulateurs Rapides Mot clés : Asservissement Visuel, Commande par Modèle Prédictif, Manipulateurs Redondants Résumé : Cette thèse traite de l'augmentation de la productivité des robots manufacturiers, lors de l'exécution de tâches référencées capteurs. De telles tâches peuvent provenir de la cible non positionnée de manière absolue ou d'un environnement mal connu.Les commandes par asservissement visuel sont bien connues pour leur robustesse et leur précision, mais nécessitent généralement de longs temps d'exécution en raison de différents facteurs. La commande est généralement formulée uniquement à un niveau cinématique et caractérisée par des vitesses décroissantes de façon exponentielle. De plus, l'application non linéaire de l'espace opérationnel à l'espace des capteurs peut conduire à des chemins sous-optimaux et plus longs.Pour augmenter les performances de com-mande et réduire le temps nécessaire à la réalisation d'une tâche, cette thèse étudie l'utilisation de modèles d'interaction de second ordre. Leur utilisation dans une commande au niveau dynamique est étudiée et comparée aux approches classiques. Ils sont ensuite utilisés dans des schémas de commande par modèle prédictif, permettant d'obtenir des vitesses plus élevées tout en générant de meilleures trajectoires. Cependant, un inconvénient des techniques prédictives est leur charge de calcul. Afin d'obtenir de pallier ce défaut, un nouveau type de commande prédictive est étudié. Il conduit à une réduction du nombre de variables impliquées dans les problèmes d'optimisation grâce à l'utilisation d'un paramétrage des séquences d'entrée.

		T ∂φ ∂x					(B.29a)
	∂ 2 ∂x 2 φ 2 = 2	∂φ ∂x	T ∂φ ∂x	+ 2	j	φ j	∂ 2 φ j ∂x 2	(B.29b)
	wherein φ

Titre : Title: Dynamic Visual Servoing for Fast Robotics Arms Keywords: Visual Servoing, Model Predictive Control, Redundant Manipulators

A video is also available online at https://youtu.be/xJhzVkGdXPw

The name simple will be used throughout the chapter to label the classical approach, since it is arguably the simplest conceivable parameterization.

The formula comes from the fact that the first free sample is always located at k = 0, and thus one has to choose the other n π -1 samples from a total of n p -1 remaining possibilities. In addition, one choice is removed, in which all samples are consecutive, since it would correspond to the simple parameterization.

The previous explanation can be extended to inequality constraints, allowing to understand the remaining KKT conditions. First of all, note that if a point x does not lie on the boundary defined by an inequality constraint, then there exists a whole neighborhood of x which satisfies the constraint. In other words, at least locally, the constraint is said to be inactive, and can thus be ignored. Using the analogy of gradients acting as forces on a mass, the force produced by an inequality constraint behaves as a ground reaction, the "ground" being the locus where the inequality turns into an equality. While the mass is "in the air", the ground cannot exert any reaction, and thus the associated Lagrange multiplier must be zero. On the contrary, when the mass reaches contact with the ground, the force activates, explaining the last of the KKT conditions. In addition, sticking to the analogy of ground reaction forces, an active inequality constraint can exert a force only in the direction opposite to the ground. As an example, in FigureB.1(c), the constraint consists in keeping x over the depicted curve. At the represented point, the "objective force" -∇f pulls the mass down, and thus the constraint can react with a force in the direction -∇g, whose magnitude is regulated by the positive scalar µ. On the contrary, in FigureB.1(d) the constraint requires the point to stay below the curve. To keep the point exactly on the curve, the constraint force would require to pull the mass upward, which requires µ < 0. However, this is not possible: the ground reaction can only push the mass, which leads to (B.4c).

Appendix A

SECOND ORDER MODELS OF VISUAL FEATURES

In the following some second-order interaction models are reported. In particular, each section focuses on a specific feature set, detailing what are the features s and the parameters z (if any), the corresponding interaction matrices L s and L z , and finally the G-matrices that define the h s and h z vectors appearing in the second-order models.

A.1 3D Points

In this case, the 3D coordinates of a point are used as features, and no parameters are present. The three G-matrices can easily be obtained by following the procedure detailed in Chapter 2.

vz,wy = Ai (-3δ -i -j + 1) (A.53b)

vz,wy = Ci (-2δ -i -j + 1) (A.53c)

vz,wy = -C (3δ + i + j) (2δ + i + j + 1) (A.53d)

wx,wx m i/j+2 (A.55a)

wx,wx = (3δ + i + j) (3δ + i + j + 1) (A.55d)

wy,wz

wy,wz

wy,wz = -

OPTIMIZATION ALGORITHMS FOR MODEL PREDICTIVE CONTROL

This Appendix details the main algorithms that were used to solve the optimization problems arising in MPC schemes. In particular, three algorithms are presented: a Quadratic Programming method based on active set, a SQP for the minimization of generic nonlinear problems and a Gauss-Newton algorithm for the solution of constrained nonlinear least squares minimization.

B.1 Standard Problem & KKT Conditions

For the remainder of this Appendix, minimization problems are assumed to be written in the following standard form:

subject to:

wherein x ∈ R n is the decision vector, while f , g and h are twice-differentiable functions that model the objective function and the inequality/equality constraints.

So as to determine the optima of the minimization, a fundamental quantity that is defined for the problem is the Lagrangian function:

where µ and λ are known as "Lagrange multipliers" or "dual variables" associated to the problem (B.1). This function is fundamental for the solution of constrained minimization, since it can be used to determine a lower bound on the optima of the original problem. In particular, denoting with D the set of points in R n that satisfy the constraints (B.1b) and (B.1c), the following holds:

Furthermore, the Lagrangian can be used to define four necessary conditions that a triplet

x , µ , λ must satisfy to be qualified as an optimum of (B.1). These conditions are known as Karush-Kuhn-Tucker (KKT) conditions, and write as:

The first condition can be written synthetically as ∂L ∂x x ,µ ,λ = 0, and has a geometric interpretation that will be explained in the following, while the second condition trivially states that if an optimum x exists, it must belong to D, i.e., the set of feasible points for the problem. The meaning of the third and fourth conditions will also be explained below.

To understand the first condition, let's focus for a moment on the case of problems featuring equality constraints only. Condition (B.4a) states that, for x to be an optimum, the gradient of the objective function must be expressible as a linear combination of the gradients of the constraints, i.e., ∇f ∈ span (∇h) (B.5)

Or, equivalently, ∇f must not have any component that lies in the tangent space of the constraints. An intuitive explanation is provided in Figure B.1(a): since the gradient of the objective has a component that is tangent to the constraint, the depicted point cannot be a constrained optimum. In fact, one could imagine to perform a small motion δx in a direction that is tangent to the constraint curve. In this case, the value of the constraint The KKT condition of stationarity and primal feasibility for this problem can be written altogether as:

If instead α i = 1, then the "full step" did not cause any constraint activation. In principle this could mean that the optimum has been found. However, there is still one condition to check: according to the dual feasibility (B.4c), all Lagrange multipliers associated to inequality constraints must be non-negative at the constrained optimum. Therefore, if any entry of µ a is negative, the associated constraint must be removed and a new iteration has to be performed. Otherwise, if µ a ≥ 0, the algorithm can terminate, returning x = x i .

Note that, from a practical point of view, the active set must be modified only at this step. This is necessary because, when defining the partition (B.10), one must not include constraints that have just deactivated. Based only on their value, these constraints would evaluate as 0 and erroneously selected as part of the active set.

B.2.2 Practical Notes Initialization

The detailed strategy based on the active set iterates by stepping within the valid domain D. While this is enforced by construction for the values x i with i > 0, it is crucial to find a valid point x 0 to start the algorithm. One way to do it would consist in solving a simple linear optimization problem similar to the following:

If instead the user was able to provide an initial guess x u , it could be desirable to initialize the algorithm using the following procedure:

which can be useful especially if Q is only positive semidefinite and thus admits multiple solutions. In fact, it initializes x 0 so that it is as close as possible to the user-supplied value x u , and hopefully returns a final solution which is in turn closer to it.

Finally, if similar quadratic problems have to be solved in succession, a last option is available, consisting in using the last active set from a previous solution to define the seed iterate x 0 . In practice, by denoting as C a,w and d a,w the matrix and vector corresponding to the last active constraints, one possibility is to use

as starting point. Of course, the value should be tested to confirm that it does in fact correspond to a feasible point according the remaining constraints, and, if this is not the case, one of the solutions above can be used instead.

Finally, this warm start can be used not only to evaluate an initial solution x 0 , but also to initialize the active set. This can greatly reduce the computation times, e.g., when QP is used for the solution of linear MPC problems -in which most of the constraints are likely to remain active between successive iterations -or when the active set algorithm is used for the solution of a series of sub-problems, which is true in the case of SQP and Gauss-Newton methods presented later.

Efficient handling of equality constraints

Since the equality constraints (B.7c) of the original problem do not change across subsequent iterations, it is possible to perform a simple substitution that reduces the dimension of the sub-problems to be solved at each iteration. In particular, the decision vector can be parameterized as:

wherein x eq is any vector that solves the equality constraints, i.e., Ax eq = b, while Z A is a basis of the kernel of A and y is a new vector of free variables. With this parameterization, problem (B.7c) can be formulated in the lower-dimensional variable y as:

The obtained solution allows to update the current guess for the minimum point of the problem, and the whole procedure is sequentially repeated until termination criteria are met.

In particular, at each iteration i the value x i-1 is assumed as an approximate solution of the problem, and, similarly to what was done in the iterations of the active set QP algorithm, the goal is to find a step direction z i and a step length α i to produce a new iterate in the form

First of all, the objective function f is approximated in a neighborhood of x i-1 via a second-order Taylor expansion as:

wherein all derivatives are evaluated at x i-1 . In addition, with some abuse of notation, ∂ 2 f ∂x 2 has been used to compactly represent the hessian of f , whose correct expression should be written as ∂ ∂x ∂f ∂x T .

On top of the quadratic approximation of the objective function, a minimization problem in the step variable z can be formulated. Constraints are approximated as well, using a linear-affine first order expansion centered at x i-1 . The i-th sub-problem thus writes as:

The problem is clearly quadratic in z, and, assuming the hessian to be at least positive semidefinite, can be solved by standard QP algorithms to produce the step z i . This fact is crucial, and might be false in general. For this reason, an approximation of the hessian is often used [Pow78; Kra88], which is ensured to always be positive definite and also depends only on first-order derivatives of the objective.

The second phase in the SQP algorithm consists in determining the step length α i ∈ [0, 1] to be used to update the current iterate. In practice, a cost (or merit) function is

B.4 Iterative Gauss-Newton Method

This method can be used to solve nonlinear optimization problems in the following special form: wherein φ is a generic vector-valued function. The method can be viewed as a special case of the SQP algorithm, as it will be shown below, and works by formulating the quadratic sub-problem using a slightly different methodology. In practice, a first-order approximation of φ around x i-1 is performed and substituted in the objective: To see that it indeed corresponds to a SQP method, the derivatives of the objective