Mauricio Garcia
email: mjgarciav@cimat.mx.

Olivier Stasse
email: ostasse@laas.fr.

Jean-Bernard Hayet
email: jbhayet@cimat.mx.

Claire Dune

Claudia Esteves
email: cesteves@cimat.mx.no

Jean-Paul Laumond

A C Cimat

Vision-guided motion primitives for humanoid reactive walking: decoupled vs. coupled approaches

This paper proposes a novel visual servoing approach to control the dynamic walk of a humanoid robot. Online visual information is given by an on-board camera. It is used to drive the robot towards a specific goal. Our work is built upon a recent reactive pattern generator that make use of Model Predictive Control (MPC) to modify footsteps, center of mass and center of pressure trajectories to track a reference velocity. The contribution of the paper is to formulate the MPC problem considering visual feedback. We compare our approach with a scheme decoupling visual servoing and walking gait generation. Such a decoupled scheme consists in first, computing a reference velocity from visual servoing; then, the reference velocity is the input of the pattern generator. Our MPC based approach allows to avoid a number of limitations that appears in decoupled methods. In particular visual constraints can be introduced directly inside the locomotion controller, while camera motions do not have to be accounted for separately. Both approaches are compared numerically and validated in simulation. Our MPC method shows a faster convergence.

I. INTRODUCTION

Humanoid robots are meant to function in unstructured and dynamic environments, where objects may move outside the robots control. Therefore, to complete specific tasks, robots have to be able to perceive and react to environment changes. Visual sensors endow robots with this capability, allowing them to build local representations of their surroundings and to adapt their behavior according to these representations. Most of the existing humanoid platforms are equipped with video cameras. They constitute rich (geometry, texture, color, etc.) sources of information at rather low costs both in terms of price and additions to the weight and size of the robot. Furthermore, cameras embedded in the platform avoid the need of outfitting the environment with external sensors, greatly increasing the autonomy of the robot. However, the image quality from video cameras embedded in humanoid robots is, in general, quite poor: blurring effects or vibrations due to the walk make interpretation of these images for visual tasks such as localization and tracking really challenging. This situation has prompted considerable effort in the computer vision comunity, with In this paper, we build upon recent advances in walking pattern generation (WPG) and visual servoing to construct a new monocular visual servoing scheme that is able to control the walk of a humanoid robotic platform towards an object of interest. An example setup can be seen in Fig. 1 where the robot has to walk towards the cupboard with the image of the cupboard as a goal reference. We use the information from the sensors as feedback in certain tasks, e.g. positioning. We find that this visual servoing control scheme is a very good candidate for closing the perceptiondecision-action loop, due in particular, to its robustness to model errors.

Moreover, as we show below, the use of a visual servoing control fits well in its traditional role as a "black box" of the WPG. The main feature of our approach is the direct use of visual errors within the WPG, i.e. a visual predictive control where visual servoing and WPG, the generation of footsteps and center of mass trajectories, are completely coupled. We compare our approach to a recently-proposed decoupled approach, where visual servoing takes its traditional role as a provider for reference velocity for the camera, whose output is then used as a reference velocity for the WPG. Our main claim is that, in contrast to other methods available in the literature, we do not need to model the particular motion of the camera induced by the robot walk, thereby gaining in robustness and flexibility. Also, our approach allows to fully take into account the robot constraints as well as visual constraints in a unified framework. Finally, our experimental results show that our approach achieves a better overall behavior of the whole control.

A. Visual servoing vs. (re-)planning

Two paradigms for vision-based navigation have been coexisting for a long time in mobile robotics. The first one tackles navigation more globally using a three-step sequence:

(1) vision-based mapping and localization; (2) trajectory planning; and (3) control. With this approach, a robot can navigate towards a visual goal if this three-step sequence can be cycled on-board of the robot at a high frequency. The second paradigm deals with the visual navigation problem in a more reactive manner, using mostly visual servoing techniques. Here, planning is generally avoided and the visual perception output feeds directly the control. We believe that these two paradigms are in fact, more complementary than opposite: whereas planning may require higher computational costs than visual servoing, it can solve more complex problems of global path finding. Visual servoing is fast by nature but in general it is confined to solving local tasks. Visual servoing is a useful approach for controlling precisely the robot position in certain tasks such as in human-robot interaction, e.g. to set the robot in front of a person, or to place the robot's end-effector in a given posture before starting a manipulation task. The advantage of visual servoing is that the positioning task is defined relatively to a specific target. Even more, the precise positioning may not be necessary. In any of the aforementioned applications, the higher-level navigation can be left to a planner, which would determine the sequence of landmarks, or human interactions, to reach consecutively. However, as many advantages the second paradigm provides to the visual navigation of humanoid robots, it also generates some problems, which have to do precisely with the fact that the output of the vision system, serves as the input to the control of the robot WPG. Some of these are that:

(1) the output of the vision system may introduce noise to the WPG of the robot and degrade its behavior; (2) as a control process driven by visual errors and designed for an exponential decay of these errors, the classical visual servoing approach also generates control laws exhibiting this exponential decay for the velocity controls. While the latter may be desirable for a wheeled robot, it may not be adequate to a humanoid dynamic walk. In this work we try to deal with both of these problems. Our claim on the first point is that the visual data can be easily filtered to avoid the propagation of perturbations to the walking controller. On the second point, even though we start by recalling a classic visual servoing scheme where the gain is not adapted, adaptive gains exist in the literature of this problem. Furthermore, in the approach we propose here, this problem is highly alleviated because the error terms are now mixed with other terms (e.g. regularizing the jerks) into the pattern generator. We will discuss more on this point in Section VI-D.

B. Contributions

Building upon the WPG initially proposed by Herdt et al [Herdt et al., 2010a], [Herdt et al., 2010b], we propose an integration of a visual servoing scheme within the WPG through linear MPC. We adapt the non-linear global model for visual servoing using MPC described by Allibert et al in [START_REF] Allibert | Visual servoing via nonlinear predictive control[END_REF], by means of a linearization of the projection function. Here, we use the position-based version of [START_REF] Allibert | Visual servoing via nonlinear predictive control[END_REF], that requires 3-D information of the object of interest, therefore, localization is needed. We claim that a simple, local linearization of the projection model predicts well the behavior of the system and that the performance of this linearization depends on the distance traveled in the time horizon, which depends itself on the velocity of the robot and the size of the time horizon. By coupling this linearization to a simplified dynamical model of the locomotion, we can introduce predictive visual control to the WPG in a straightforward way. To our knowledge of the existing literature, this is the first time that such a strategy is used right at the pattern generation level. We finally compare our approach both, qualitatively and quantitatively to the one proposed recently in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF].

C. Paper Overview

First, in Section II, some work related to our approach will be discussed. In Section III, we will recall the principles of the reactive WPG with automatic footstep placement, and in Section IV, we make a brief reminder about existing visual servoing schemes. Then, in Section V, we describe the approach proposed in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF], that uses the visual servoing output velocities as an input to the pattern generation. In Section VI, we present a new scheme that integrates visual servoing within the pattern generator. In Section VII, we present simulation results and comparisons between the decoupled and the coupled approaches.

First, a few guidelines about the notations used throughout this paper should be mentioned: Bold, capital letters are used to refer to matrices; normal, lower-case letters refer to scalars or vectors, whereas normal, capital letters are used for the stacks of quantities in a time window, which is typically used in MPC. As for the indices, we mainly use indices k or j to refer to time indices, whereas other letters are used as indices and exponents to indicate 3D frames in transformations or twist matrices. For example, c T m is a 3D rigid transform mapping points expressed in the "m" frame (the center of mass frame) to their coordinates in the "c" frame (the camera frame).

D. Notations

MPC Notations

k, j Time indices. q k Full robot state at time k. u k Control variable at time k. q k = f (q k-1 , u k-1 )
Dynamic model of the robot.

Q k

Quadratic term of the cost function considered in this paper.

II. STATE OF THE ART

Early works on humanoid locomotion have assumed that the robot path on the ground is completely defined before computing the actual joint control to realize it. This clearly puts limits on the capacities of reaction when changes occur in the environment. These works generally follow a perception-decision-action scheme, in the sense that a sensor first acquires data on the world and/or the robot state, then, suitable footsteps over a time horizon are decided, and finally the trajectories of the Center of Mass (CoM) and the Center of Pressure (CoP) are computed while respecting the stability constraints and avoiding collision with the environment. Finally, the control of the legs and other joints is computed by inverse kinematics. This perception-decision-action loop has proven to be fast enough to realize impressive demonstrations for stair-climbing and obstacle avoidance [START_REF] Lorch | Experiments in visionguided biped walking[END_REF], [START_REF] Chestnutt | Locomotion among dynamic obstacles for the honda asimo[END_REF], [START_REF] Michel | Gpu-accelerated real-time 3d tracking for humanoid locomotion and stair climbing[END_REF], [START_REF] Gutmann | 3d perception and environment map generation for humanoid robot navigation[END_REF]. Our focus in this paper will be set only on one of the sub-problems necessary to implement this approach: the generation of footsteps and trajectories of the CoM and CoP. We stress that we will not address here the whole body control.

In the generation of the footsteps and the trajectory of the CoM or CoP, for a long time most of the works did not consider their online modification from an initial plan. The work presented in [START_REF] Morisawa | Experimentation of humanoid walking allowing immediate modification of foot place based on analytical solution[END_REF] considered online adaptation after showing that the modification of the next landing position of the flying foot might impose a new CoP trajectory going out of the support polygon and jeopardize the equilibrium of the robot. To solve the problem, the stepping period may be modified to reduce this instability [START_REF] Morisawa | Experimentation of humanoid walking allowing immediate modification of foot place based on analytical solution[END_REF], at the cost of slowing down the robot. A recent method proposes to modify the footsteps according to a perturbation applied to the CoP [START_REF] Nishiwaki | Online walking control system for humanoids with short cycle pattern generation[END_REF].

Later on, very efficient and much more flexible control systems for humanoid robots walking generation have been proposed. They are dynamically stable and may be very reactive since footstep placement can be computed online [Herdt et al., 2010a]. Moreover, they differ from the aforementioned works by the fact that the CoP is authorized to move freely inside the support polygon, leaving much more flexibility to the motion. Most of these techniques such as in [Herdt et al., 2010a] are based on linear MPC. MPC previews the behavior of the system within a time window in the future by applying a given virtual sequence of controls. In most approaches, the controls are encoded as the supposedly constant third derivatives (jerks) of the CoM position during single time intervals. MPC allows to estimate the optimal control sequence at some horizon, even if, in the next iteration, one simply applies the first control of the computed optimal control sequence, and starts again in a similar way for the next one. At the end, MPC can be expressed as a Quadratic Program (QP), i.e., the minimization of quadratic errors subject to a set of linear constraints (equalities and inequalities). Handling explicitly the constraints in the QP is one of the main advantages of the MPC. Furthermore there are very efficient techniques proposed to solve such a QP. The most important point is that, in [Herdt et al., 2010a], the only required input, besides the characteristics of the robot, is a reference velocity for the CoM. Hence, the MPC can be seen as a "black box" taking as an input a reference velocity to follow, and generating the corresponding CoM trajectory.

For reactive positioning tasks, visual servoing techniques have proven to be useful [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF], [START_REF] Chaumette | Visual servo control, part ii: Advanced approaches[END_REF]. In the humanoids community, visual servoing has been successful for grasping tasks while standing [START_REF] Coelho | Developing haptic and visual perceptual categories for reaching and grasping with a humanoid robot[END_REF], [START_REF] Taylor | Flexible self-calibrated visual servoing for a humanoid robot[END_REF] or for walking humanoids [START_REF] Courty | Through-the-eyes control of a virtual humanoïd[END_REF], [START_REF] Mansard | Visually-guided grasping while walking on a humanoid robot[END_REF]. In [START_REF] Courty | Through-the-eyes control of a virtual humanoïd[END_REF], visual servoing has been used to control a humanoid avatar navigating along landmarks. The upper body is approximated by the kinematic chain that links an on-board camera to the CoM. The lower body is controlled by adding two translational degrees of freedom to the CoM. The translational velocity of the CoM is sent to a kinematic locomotion module which controls the legs motion. In [START_REF] Mansard | Visually-guided grasping while walking on a humanoid robot[END_REF], a whole body visual servoing scheme based on a hierarchical stack of task is introduced. However, the footsteps are defined beforehand. The leg motion is set to be the task of higher priority, and visual servoing in this context is projected in the null-space of the pre-defined walking path.

More recently, in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF], which is the method we will compare ours to, the reference velocity of the CoM is computed based on a visual servoing based controller, associated to the "black box" pattern generator of [Herdt et al., 2010a]. The reference velocity computed from the visual servoing is used by the MPC to adapt the footsteps and the CoM trajectories, while ensuring at the same time walking stability constraints at time intervals of constant length. The main disadvantage of this approach is that the visual servoing scheme and the pattern generator are completely decoupled. This means that the visual information is not directly feeding the pattern generator. Instead, it simply gives a "trend" velocity to follow, and no visual constraints can be introduced inside the MPC problem. Moreover, it needs to handle the sway motion by virtually removing the motion of the camera due to the sway, based on the control inputs and outputs, which we do not. Another downside of this method is that the reference velocity introduced in the pattern generator is only taken as a reference, i.e. it is not exactly tracked.

Here, we adopt a similar approach as the ones mentioned above, in the sense that the footsteps are changed dynamically as in [Herdt et al., 2010a]. However, the main difference with the aforementioned work is that we do not introduce explicitly the reference velocity computed elsewhere by the visual servoing. Instead, we let the pattern generator decide for it by itself, within the optimization scheme.

One of the main motivations in this work is to tighten the link between the WPG and the visual servoing control. Basically, we aim at integrating more directly the visual servoing errors within the MPC. Outside the humanoid robots literature, Allibert et al. have successfully applied visual servoing within a MPC scheme [START_REF] Allibert | Visual servoing via nonlinear predictive control[END_REF]. The main problem in doing so is that the dynamics of the camera and the involved perspective projection are nonlinear functions so that non-linear programming would be required to be combined with the aforementioned QP problem. Hence, a direct application of a MPC visual servoing scheme for the WPG would be time consuming and not adapted to an online walking pattern generator.

III. MPC-BASED WALKING PATTERN GENERATION

Most of the current walking motion generation schemes for humanoid robots are based on the model initially proposed in [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], which focuses on determining the trajectory of the CoM to generate balanced and stable motions. The underlying dynamic model operating for each direction is the one of a cart-table system, that corresponds well to the distribution of mass in the humanoid robot. We will not be exhaustive here in describing the whole dynamic system, but let us revise its main components.

A. Dynamical system

First, we suppose that the CoM has periodic piece-wise constant jerks (third derivatives) on each time interval of duration T . This is because the accelerations of the CoM have to be continuous enough to be done without damage to the robot actuators. Let us discretize the time into these intervals of length T , and let us use the index k to refer to one such interval. Then, by writing (1) that the position at k + 1 results from the position at k and the integration of the constant jerks, and (2) that (by definition of the CoP) the sum of moments at the CoP cancels, we can express the CoM dynamics on the x-axis as a linear system

xk+1 = Ax k + B ... x k ξ x k = Cx k , (1)
where

xk def = (x k , ẋk , ẍk )
⊤ stacks the x-position, x-velocity and x-acceleration of the robot CoM at time k into a 3 × 1 vector, and where the three matrices A, B, C are given by

A def =   1 T 1 2 T 2 0 1 T 0 0 1   , B def =   T 3 /6 T 2 /2 T   ,
and

C def = 1 0 cz g . In these equations, ξ x
k is the CoP x-position at time k and c z is the CoM height, which is considered as a constant during all the following. Similar expressions can be determined for the y and z components.

Then, starting from the time index k, we can apply recursively the dynamics of Eq. 1 N times, and express a full trajectory of the CoM in a horizon of N time intervals, in function of the initial position, the initial velocity and the initial acceleration (at k), stacked into the vector xk , and in terms of the sequence of jerks applied during these intervals. We recall that this sequence will be considered as our controls to drive the robot steps. Let us denote the sequence of the CoM x-positions starting from k and ending at k + N -1 as

X k def = (x k , x k+1 , ..., x k+N -1 ) ⊤ ,
and, similarly, the sequence of jerks applied from k as ...

Xk def = ( ... x k , ... x k+1 , ..., ... x k+N -1 ) ⊤ .
Then, by applying the dynamics N times from k (the current time index), we get another linear system of the form

X k+1 = P x xk + P u ... Xk, (2)
where P x and P u are respectively N × 3 and N × N matrices that are easily deduced from the previous dynamics equations. Similar expressions can be obtained for the vectors stacking the velocities ( Ẋk+1 ) and accelerations ( Ẍk+1 ) of the CoM during the considered horizon time window.

In the original approach [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], the positions of the center of pressure (CoP) had to be predefined, so that preliminary footstep planning was necessary. Later on, an interesting re-formulation was proposed to handle automatic footstep placement [Herdt et al., 2010a]. This approach is two-step: (1) it first determines the sequence of robot trunk orientations to be followed and (2) given the computed orientations, it determines the x, y trajectory of the CoM. Let us describe these two steps.

B. Determination of the x, y CoM trajectory

Given the CoM orientations, this step makes only use of a reference velocity ( Ẋref k+1 , Ẏ ref k+1 ), given as an input, in such a way that the determination of the optimal controls can be written as the following constrained optimization problem:

min U k α T 2 ... Xk 2 + α T 2 ... Y k 2 (3) + β T 2 Ẋk+1 -Ẋref k+1 2 + β T 2 Ẏk+1 -Ẏ ref k+1 2 + γ T 2 Z x k+1 -Z x ref k+1 2 + γ T 2 Z y k+1 -Z y ref k+1 2 where U k def = ... X ⊤ k , (X f k ) ⊤ , ... Y ⊤ k , (Y f k ) ⊤ ⊤
contains the variables to be optimized, and (α T , β T , γ T ) are weighting parameters. The sequence of CoP reference positions Z

Z x ref k+1 = V c k+1 x f k + V k+1 X f k
where x f k is the (known) x-position of the current support foot, X f k the sequence of (relative) steps to be done (i.e.

our variable to optimize), and V c k+1 , V k+1 two constant selection matrices.

The variable

Z x k+1 = [ξ x k+1 • • • ξ x k+N ]
contains the sequence of CoP positions along the time window. Again, they can be derived as a linear function of U k from the second equation of the system 1.

All reunited, the optimization problem of Eq. 3 can be finally written as a canonical Quadratic Program (QP)

min U k 1 2 U ⊤ k Q k U k + p ⊤ k U k .
Now, of course, this optimization problem comes with a set of constraints described hereafter.

C. Constraints on the CoP and on the foot placement

Because the robot feet can only push on the ground, the CoP has to lie inside the support polygon, i.e. the convex hull of the contact points between the feet and the ground [Wieber, 2002]. We assume that the foot on the ground has a known polygonal shape, so that the CoP constraint at time j can be expressed as a set of inequalities on the position of the CoP (ξ x j , ξ y j )

a x 1 θ f j a y 1 θ f j ξ x j -x f j ξ y j -y f j ≤ b 1 θ f j .
where the vector a x 1 (resp. a y 1 ) contains the x (resp. y) coordinates of the vectors normal to the feet edges and b 1 the positioning of these edges along this normal. These equations are linear with respect to the position of the foot (x f j , y f j ) on the ground, each of which is an element of the aforementioned Z

x ref k+1 and Z y ref k+1 for k + 1 ≤ j ≤ k + N .
However, it is nonlinear with respect to the foot orientation θ f j . This is the explanation of the two-phase approach: since the orientations are computed beforehand, then the inequalities do not depend on them and are all linear in the problem variables.

As for the next foot placement, several constraints arise from the joints limits, the prevention of self-collision, maximum leg lengths, etc. and most of them are highly non-linear in the variables of the problem. Here, we use the approach of [START_REF] Perrin | Approximation of feasibility tests for reactive walk on hrp-2[END_REF] to approximate these constraints by a series of linear constraints on the variables of interest, here the feet positions at consecutive time indexes,

a x 2 a y 2 x f j -x f j-1 y f j -y f j-1 ≤ b 2 .

D. Determination of the θ trajectory.

As explained above, the robot trunk orientation is determined beforehand to avoid non-linearities in Eq. III-C and keep the optimization problem as a QP. Several schemes have been proposed to determine these orientations. The one in [Herdt et al., 2010a] sets up another QP problem where the cost function includes the minimization of the difference between the feet orientation and the θ angle, and the tracking of a reference angular velocity. For more details, the reader is kindly invited to refer to [Herdt et al., 2010a].

We will propose a variant of this method in Section VI-B.

E. Sway motion

As for any pattern generator, the stepping motion induces a sway motion, which can be read as the difference existing between the reference velocity used in the Eq. 3 of the Model Predictive Control and the real velocity effectively attained from the first control, as illustrated in Fig. 2. This sway motion is necessary for a proper walk, but it obviously generates non-desired effects on the robot visual perception. As stated in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF], the result of this sway motion on the real camera velocity v c can be modeled as:

v c = v c + c V m v b (4)
where v c is the "ideal" camera velocity that would exist without sway, and v b is the part of the CoM velocity that is induced by the sway. The matrix c V m is a twist matrix relating the camera frame (index "c") to the CoM frame (index "m") through the transform c T m .

We will now describe two approaches for introducing visual control in the described pattern generation: the first one (in Section V) is a decoupled one, where the visual servoing is used to determine the reference velocity in Eq. 3; the second one (Section VI) modifies Eq. 3 to replace the reference velocity term by a new term directly related to the decrease of the visual errors.

IV. VISUAL SERVOING SCHEMES

Visual servoing aims at controlling the motion of a robot equipped with a camera, by minimizing the errors between observed features (denoted in the following as s) and their corresponding reference, desired features (denoted as s * ) [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF]. The nature of the features differentiates schemes of visual servoing: Image based visual servoing (IBVS) uses only image features, i.e. s may be a vector of image points coordinates; Position based visual servoing (PBVS) uses the 3-D pose(s) of object(s) of interest. In any of these schemes, one may use a velocity controller based on these errors [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF],

v c = -λ L e + e, (5)
where e = s -s * is the vector of errors, v c is the velocity of the camera and L + e is the Moore-Penrose pseudo-inverse of an approximation L e of the interaction matrix L e related to e. That is, the matrix relating the velocity of the features and the velocity of the camera.

In subsection IV-A, we briefly describe the visual tracking system that allows us to track the visual features and estimate the pose of the camera with respect to the features; then in subsection IV-A, we describe the classical approach for IBVS used in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF]. Finally, in IV-C, we recall how IBVS has been previously expressed as a Model Predictive Control scheme coined as Visual Predictive Control.

A. Robust Model Based Tracking

In the case of the humanoid robots, one difficulty is that when the robot walks, the stepping causes the head to shake and oscillate. At each foot step, the impact propagates to the camera. Moreover, the inherent sway motion of the CoM translates into camera motion. Both causes generate blur and shift in the image. Hence, the extraction of the visual features, by image processing, is not that easy, e.g. compared to the case of wheeled robots. This explains in part why 3D information is more robust to be used in that case than only 2D, i.e., PBVS instead of IBVS.

To face the image processing problem, we use the robust approach presented in [START_REF] Comport | Real-time markerless tracking for augmented reality: the virtual visual servoing framework[END_REF] for the tracking of geometrical shapes (lines, cylinders, ellipsoids, etc.). This algorithm estimates the 3D position of a known object frame (index "o") in the camera frame c T o , based on two steps: (1) tracking of the contour points along the tracked shape and (2) optimization for the pose estimation. Basically, starting from a predicted pose, the lines of the geometric object model are projected on the image and sampled regularly. Then, at each of these sample points, the maximum gradient magnitude point p i is searched along the normal to the projected line. These locally registered points are used in the second step to optimize the camera-object transform

c T o = arg min c To i C(d ⊥ (p i , l i ( c T o ))) (6)
where C is a robust function that allows to handle outliers in an M-Estimator way. The distance d ⊥ is the orthogonal distance from a point to a line. p i are the extracted points in the current image and l i are the projections of the model lines. As the points p i should belong to the lines l i in the image plane, c T o is searched as the one minimizing these point-line distances. Hence, to implement satisfactorily the visual processing part of our approach, we rely on the algorithm presented in [START_REF] Comport | Real-time markerless tracking for augmented reality: the virtual visual servoing framework[END_REF] which assumes that the target, i.e. the entire set of visual features, is a polygon, with edges prominent enough for the aforementioned tracking approach to be successful. We also suppose that the possibly rough initialization is done manually before the first iteration of this visual tracking.

B. Elements for a Classical Visual Servoing Approach

To use an Image-Based Visual Servoing (IBVS) setup for the humanoid robot, we first define the current and the desired features s and s * in terms of the (u, v) coordinates of visual features in the image.

With that design choice, the approximated interaction matrix L e associated to the error for just one feature in this system, i.e., regarding two lines in the visual features error vector e, can then be determined as [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF]

] Le = -1/z c 0 u/z c uv -(1 + u 2 ) v 0 -1/z c v/z c (1 + v 2 ) -uv -u , (7)
where z c is the z coordinate (depth) of the single visual feature. It can be estimated from the transform c T o , for example, which estimation is described above.

Then, the velocity control for the camera ensuring that e decreases exponentially is defined as in Eq. 5. This translates into a separate control for the translational and rotational velocities [START_REF] Chaumette | Visual servo control, part i: Basic approaches[END_REF].

From this objective velocity computed for the camera, we can then deduce the CoM reference speed v m by making use of the twist matrix m V c :

v m = -λ m V c L e + e. (8)
The only problem in this approach is the one mentioned just before: The handling of sway motion. We will see in Section V how this has been handled in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF].

C. Elements for an MPC-based Visual Servoing Approach

Now, in WPG (see Section III), we saw that MPC is used to estimate a sequence of optimal controls at some horizon, because of the step-based nature of walking. Hence, we may want to orient the optimization of the foot placement by taking into account the expected evolution of the visual servoing (VS) errors so that, instead of minimizing the VS errors at current time k, one would like to foresee its evolution at some horizon [k + 1, k + N ]. In [START_REF] Allibert | Visual servoing via nonlinear predictive control[END_REF], outside of the humanoid context, such a time horizonaware scheme has been proposed for Image-Based Visual Servoing, and we will show that it can be applied to our own scheme. The visual predictive control (VPC) has been introduced in general terms as:

min U k k+N j=k+1 [s d j -s m j ] ⊤ W j [s d j -s m j ], (9)
subject to s d j = s * j -ǫ j , (10) q j = f (q j-1 , u j-1 ), (11) s m j = h(q j ).

In Eq. 9, U k = u k:k+N -1 are the series of controls to be applied to the camera, j refers to time indices in the future, q j is the state. The feature vectors s * j , s d j and s m j are respectively the reference, desired and predicted positions of the visual features. The terms ǫ j are the errors s j -s m j between real and predicted feature positions.

Allibert et al. assume ǫ j constant over the prediction horizon, equal to ǫ k = s k -s m k , i. e. the error at the current time k, because by definition the s j are not known for j > k. Since our landmarks are static, s * j def = s * , and since the prediction errors are constant on the horizon window, s d j = s d k = s * -ǫ k constant in the prediction horizon. Eq. 11 is the dynamic model, that estimates the new state given the last state/control pair. In general, this function f is non-linear. We will see how to deal with this non-linearity.

Eq. 12 is also a non-linear function h that estimates the output of the model s m j , given the current state q j . In practice, this equation implements the pinhole camera projection model.

The matrix W j in Eq. 9 is a positive definite matrix used to weight errors in the prediction horizon. As suggested in [START_REF] Allibert | Visual servoing via nonlinear predictive control[END_REF], we consider equal weights for all features errors, W j = diag(w j ).

Here, s m j is the collection of all the predicted features at time j, that is, for M features s m j = (s m 1,j , s m 2,j , . . . , s m M,j ) ⊤ . Eq. 9 can be rewritten as

min U k M λ=0 [S d λ -S m λ,k ] ⊤ W[S d λ -S m λ,k ], (13)
where λ ∈ [1, M ] is the visual feature index, S m λ,k stacks the positions of feature λ in the horizon:

S m λ,k = s m λ,k+1 , s m λ,k+2 , . . . , s m λ,k+N ⊤ ,
S d λ stacks the corresponding desired positions, and, finally, W = diag(w k+1 , w k+2 , ..., w k+N ). This approach is completely different from the one seen just before in IV-B, and we will see in Section VI how to use it in a MPC scheme for WPG.

V. USING THE VISUAL SERVOING OUTPUT AS THE WPG REFERENCE VELOCITY

In this section, we recall how in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF], a classical visual servoing scheme such as the one described in IV-B is adapted to be used for a humanoid robot. The idea, basically, is to use the output velocity derived from the visual errors through Eq. 8 as a reference velocity for Eq. 3. However, as we have pointed it out, the humanoid motion has an intrinsic sway component, that makes the visual features have an evolution in the image that do not correspond to the desired trajectory.

As mentioned above, the features motion in the image can be decomposed into a component due to the sway motion, and a component due to the "average" sway-less motion of the robot,

ṡ = L e v c + L e c V m v b . (14)
Then, the idea developed in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF] is (1) to use a virtual camera that corresponds to the position of the camera if we suppose that there is no sway motion applying to the robot, (2) to control this virtual camera based on the regulation of visual errors, and (3) to use its controlled velocity (v c ) as a reference velocity in the reactive WPG.

Dune et al. have shown that the relationship between the observed features s(t) and the "sway-less" features s(t) can be written as

s(t) = s(t) + t 0 L e c V m v b dτ -E, (15)
where E def = s(0) -s(0). From this, the corrected visual error to be used for the computation of the reference velocity is deduced as

e(t) = s(t) -s * = e(t) ( t 0 L e c V m v b dτ -E). (16)
Under this model, the virtual error e(t) is regulated to zero, and the real error e(t) is oscillating around zero, with a period T . The shift E is re-estimated regularly over one period of time (e.g., the last period) by

E = 1 T t t-T t 0 L e c V m v b dτ dτ ′ ,
and finally, it is used in the control law for the CoM (e.g., the reference velocities Ẋref k+1 , Ẏ ref k+1 for Eq. 3)

v m = -λ c V m L e + (e -( t 0 L e c V m v b dτ -E)). (17)
Note that the discrete form of the involved integrals are used in practice. More details can be found in [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF]. Please note that, as in any classical visual servoing approach, to apply this control, we need the actual measured errors e, an estimate of the sway motion period noted T , which is deduced from the stepping period, and a point-wise estimate for the sway motion at the level of the CoM, i.e. v b .

VI. INTEGRATING THE VISUAL SERVOING TO THE WALKING MOTION GENERATOR

In this section, we describe our MPC-based approach for plugging the visual servoing error regulation within the dynamic walk control.

As already mentioned, the basic idea is to use the visual errors between the observed features and the desired ones inside the pattern generator, with an MPC approach. However, it should be clear that if we introduce directly Eq. 13 as a new term for Eq. 3, we will not have a QP formulation anymore, due to the non-linear constraints, namely Eqs. 11 and 12. We can avoid the first non-linearity (Eq. 11) by using the dynamic model in Eq. 2. In this case, there is no rotation, but we will see that we can introduce it in a separate optimization process without losing the QP formulation.

A. Linearization of the observation model

As already mentioned, Eq. 12 implements the classical pinhole camera model. Let p o l = (x o l , y o l , z o l ) ⊤ be the position of the l-th landmark in the object of interest reference frame (index "o"). At time j, one can compute the projection of this landmark onto the image plane by first transforming the landmark position to the camera frame with the homogeneous transform c T m m T o,j and then by applying the perspective projection to the coordinates in the camera frame (x c l ′ , y c l ′ , z c l ′ ) ⊤ . Assuming a camera with canonical parameters, this takes the form

u l,j v l,j = u(x c l,j , y c l,j , z c l,j ) v(x c l,j , y c l,j , z c l,j ) = x c l,j /z c l,j y c l,j /z c l,j . (18)
At this point, we recall for clarity that the exponent "c" stands for the camera frame, the index "l ′ " for the landmark id, and "j" for the time index. Also note that m T o,j is the transformation at time j from the object of interest frame (index "o") to the CoM frame (index "m") and c T m is the transformation from the CoM frame to the camera frame, which is considered as constant in our approach. Hence, there is no index j associated to it. Observe that m To,j = ( o Tm,j)

-1 = ( o Rm,j) -1 -( o Rm,j) -1 o tm,j 01×3 1
where o t m,j is the position of the CoM in the object of interest frame at time j, which depends directly in our control variables through Eq. 2. The rotation matrix o R m,j is the direction of the robot waist according to the object of interest frame at time j. In our current formulation, there is no free variable modifying this quantity, because it would make the problem non-linear. This argument is a similar one as in [Herdt et al., 2010a], mentioned in Section III, which justifies the use of separate optimization processes for translation and rotation. More details about this problem are given in Section VI-B. For the moment, consider o R m,j and its inverse as constants.

If we use directly the non-linear equation Eq. 18 in o t m,j , we will lose the QP formulation. We know that Eq. 18 is a projection

h : R 3 → R 2 , h(x, y, z) = u(x, y, z) v(x, y, z) = x/z y/z .
We also know that in the MPC formulation, the prediction is done over a finite horizon, corresponding to relatively small distances. Typically, a few dozen centimeters are considered. So it might be enough to use a first order approximation of h for small variations (dx, dy, dz) of the features position in the camera frame, so that we can maintain the QP form.

By using a Taylor series for u(x, y, z) around some linearization point (x 0 , y 0 , z 0 ) and substituting the derivatives,

u(x 0 + dx, y 0 + dy, z 0 + dz) ≈ x0 z0 + dx z0 -x0dz z 2 0 , v(x 0 + dx, y 0 + dy, z 0 + dz) ≈ y0 z0 + dy z0 -y0dz z 2 0 .
,

with dx = x -x 0 , dy = y -y 0 and dz = z -z 0 .
We propose to apply such a linearization of Eq. 18 for the whole horizon, around the first position (j = k) of the l-th landmark, i.e. at the linearization point (x c l,k , y c l,k , z c l,k ). This way, we can express the predicted position of the l-th landmark, at time j > k in the horizon, in a linear way:

u l,j v l,j = π 11 l,k x c l,j + π 13 l,k z c l,j + u l,k π 22 l,k y c l,j + π 23 l,k z c l,j + v l,k
, where u l,k = x c l,k /z c l,k and v l,k = y c l,k /z c l,k are the initial positions of the landmarks in the horizon and the coefficients π ij l,k are the elements of the following matrix

Π l,k = 1/z c l,k 0 -u l,k /z c l,k 0 1/z c l,k -v l,k /z c l,k
, which is the classical interaction matrix in Image-Based Visual Servoing. Finally, we can express the projection of the l-th landmark (constraint 12) as:

u l,j v l,j = Π l,k u l,k v l,k c T m m T o,j p o l 1 , (19)
so that we can now introduce the visual errors regulation in the pattern generator. By expanding the first row in Eq. 19 and by referring to the first row of matrix Π l,k as Π u l,k ,

u l,j = Π u l,k ( c R o p o l + c R o o t m,j + c t m ) + u l,k . ( 20
) Since c R o o t m,j = c R o (1)x j + c R o (2)y j + c R o (3)z j , where c R o (i) is the i-th column of c R o
1 and x j , y j , z j the position of the CoM in the object of interest frame at time j (see Section III), we can rewrite Eq. 20 in the simpler form

u l,j = a u l,k x j + b u l,k y j + c u l,k , ( 21
) with    a u l,k = Π u l,k c R o (1) b u l,k = Π u l,k c R o (2) c u l,k = Π u l,k ( c R o p o l + c t m + c R o (3)z j ) + u l,k .
Note that z j = c z is assumed constant here (see Section III). Similarly to the previous equation, we also get 1 To simplify the notations we dropped the time index j.

v l,j = a v l,k x j + b v l,k y j + c v l,k . (22)
By stacking the features u l,j and the CoM positions for the whole horizon, and by using Eq. 2, we get a vector S u,m l,k similar to the one introduced in Eq. 13. We recall that S u,m l,k = s u,m l,k+1 , s u,m l,k+2 , . . . , s u,m l,k+N ⊤ stacks the l-th landmark u-values over the time window. It can now be expressed as,

S u,m l,k = A u l,k X k+1 + B u l,k Y k+1 + C u l,k , with A u l,k = a u l,k I N ×N , B u l,k = b u l,k I N ×N , C u l,k = c u l,k
(1, 1, ..., 1) ⊤ . This corresponds to the sequence of predicted coordinates u of the l-th landmark in the horizon. The equivalent equations for the v coordinates of the same landmark are straightforward.

Every projected landmark provides two coordinates (u, v) and we treat each one as an individual feature. This means that our λ-th feature is the u (resp. v) image coordinate of the landmark l = ⌊λ/2⌋ for λ even (resp. odd). In other terms, for any time index j,

s m λ,j def = u ⌊ λ 2 ⌋,j for λ even s m λ,j def = v ⌊ λ
2 ⌋,j for λ odd. Generalizing to all features, we have:

S m λ,k = A λ,k X k+1 + B λ,k Y k+1 + C λ,k , (23)
with A λ,k = A u l,k for λ even and A λ,k = A v l,k for λ odd, where l = λ 2 . The same holds for B λ,k and C λ,k . Finally, we introduce visual servoing in the walking generation with the QP:

min U k α T 2 ... Xk 2 + α T 2 ... Y k 2 + β T 2 M λ=0 [S d λ -S m λ,k ] ⊤ W[S d λ -S m λ,k ] + γ T 2 Z x k+1 -Z x ref k+1 2 + γ T 2 Z y k+1 -Z y ref k+1 2 ,
with the set of weights (α T , β T , γ T ). We can rewrite it as a canonical QP:

min U k 1 2 U ⊤ k Q k U k + p ⊤ k U k with Q k = Q ′ k 0 0 Q ′ k + Qk , Q ′ k = αT I + γT P ⊤ zu Pzu -γT P ⊤ zu V k+1 -γT V ⊤ k+1 Pzu γT V ⊤ k+1 V k+1 , Qk =       βT λ P ⊤ pu A ⊤ λ,k WA λ,k Ppu 0 βT λ P ⊤ pu A ⊤ λ,k WB λ,k Ppu 0 0 0 0 0 βT λ P ⊤ pu B ⊤ λ,k WA λ,k Ppu 0 βT λ P ⊤ pu B ⊤ λ,k WB λ,k Ppu 0 0 0 0 0       and p k = p ′ k + pk , p ′ k =      γP ⊤ zu (Pzs xk -V c k+1 x f k ) -γV ⊤ k+1 (Pzs xk -V c k+1 x f k ) γP ⊤ zu (Pzs ŷk -V c k+1 y f k ) -γV ⊤ k+1 (Pzs ŷk -V c k+1 y f k )     
, where (x f k , y f k ) is the foot position at the beginning of the time window,

xk def = (x k , ẋk , ẍk ) ⊤ , ŷk def = (y k , ẏk , ÿk ) ⊤ (see
Section III), and finally

pk =       βT λ P ⊤ pu A ⊤ λ,k W[A λ,k Pps xk + B λ,k Pps ŷk + C λ,k -S d λ ] 0 βT λ P ⊤ pu B ⊤ λ,k W[A λ,k Pps xk + B λ,k Pps ŷk + C λ,k -S d λ ] 0       .
For details on matrices P ps , P zs , P pu , P zu , V k+1 and V c k+1 see [Herdt et al., 2010a].

To summarize, our approach, together with the elements described hereafter (control of the rotation angle in VI.C and handling of the visual constraints in VI.D), follows a Position Based scheme, as depicted in Fig. 3.

B. Control of the rotation angle

So far, we have proposed a scheme to control the trajectory of the center of mass in the xy plane. However, introducing the rotation angle in the minimization problem is not straightforward without losing linearity. Furthermore, the rotation angle plays a very important role here since sometimes most of the error between the desired s d and the predicted s m may be due to the angle between the robot and the features.

An extension of the original linear MPC scheme with automatic footstep placement that deals with a reference angular velocity has been proposed in [Herdt et al., 2010b], as mentioned in III-D. The approach is a two-fold optimization process that first estimates the optimal rotation angles and then introduces these values as known in the main QP (here, as the matrix o R m,j ). This scheme should not affect the stability of the walking since inertial effects are not taken into account.

The same methodology is used in this approach. Hence, in a first stage, we optimize the orientations in the MPC time window by

min ... Θk, ... Θ f k αR 2 ... Θk 2 + αR 2 ... Θ f k 2 (24) + βR 2 Θ k+1 -Θ 0 2 + γR 2 Θ f k+1 -Θ 0 2 ,
where, with the same notations as for ... Xk and ... Y k , ... Θk is the sequence of N jerk values to be applied, and Θ k+1 is the sequence of N predicted θ values, i.e. the orientations of the trunk in the horizon,

Θ k+1 def = (θ k+1 , ..., θ k+N ) ⊤ ,
and similarly for Θ f k+1 , the feet orientations. A reference orientation Θ 0 is defined once for all at the starting configuration as a target feet orientation. Several conventions exist but in this paper, the trunk orientation Θ k is trying to follow the flying foot orientation Θ f k . The flying foot is the only one which can move during the single support phase. Indeed obviously the support foot is fixed, and both feet are fixed during the double support phase. Finally, zero speed, and zero acceleration are required at the beginning and the end of the trajectories.

Then in a second stage, we introduce these computed angles as constant in the main QP (Eq. 3). This approach gives us the advantage of introducing constraints like the maximum rotation between both feet, between the feet and the trunk, and also a rotation limit to keep the visibility of the landmarks.

C. Visual constraints

The visual constraints can be introduced by using Eq. 21 and Eq. 22. Any linear constraint in the image plane (u, v), can be expressed as a linear constraint in the variables U k .

Furthermore, a convex polytope can be expressed under a linear form. It means that we can have time-and landmarkvarying constraints. Commonly, we want all landmarks to follow trajectories inside some convex polytope. Hence, the constraints become constant in time and for all landmarks. This can be written as:

A ′ A u l ′ ,k X k+1 + B u l ′ ,k Y k+1 + C u l ′ ,k A v l ′ ,k X k+1 + B v l ′ ,k Y k+1 + C v l ′ ,k ≤ b ′ (25)
and then,

A ′′ U k ≤ b ′′ ,
where the matrix A ′ and the vector b ′ are related to the image constraints. For example, bound constraints in the (u, v) coordinates like visibility constraints are easily expressed in terms of Eq. 25 and are introduced directly in the QP.

D. Qualitative comparison with the classical approach

A first advantage of plugging the visual errors term in the Pattern Generator MPC and of avoiding the decoupled approach is that, with a pure visual servoing approach, the expected behavior of the controls to be applied would correspond to an exponentially decreasing velocity. Indeed, in classical visual servoing, when the goal is close, errors tend to zero, and the velocity controls requested to the robot get smaller and smaller. This is not a problem, e.g. with robotic arms, since in that case we just send rotational velocities to the motors, and these velocities can be as small as requested without consequence on the safety of the robot. In humanoid robots, having very small reference velocities is much more a problem, since it would involve more steps. As stepping involves balance, and as every step could break it, we must avoid unnecessary motion and reach the goal as soon and efficiently as possible. Here, with our approach, because the visual errors term is only one term in the QP problem, this exponential decay is strongly attenuated by the regularizing effect of other terms such as the jerks. This will be illustrated clearly in the experimental results section.

Another advantage of our approach is that the constraint on velocities such as maximal velocities are naturally handled, as inequality constraints in a QP problem.

Finally, since the velocity reference we set as an input to the pattern generator is not truly performed, due to the physical constraints of the robot, we have to re-inject this difference in the next iteration. In the coupled approach, those problems are handled intrinsically within the MPC. The sway motion is naturally filtered since we minimize the errors within a full cycle (the horizon in the MPC). Finally, in the MPC-based coupled approach, we minimize errors as long as the stability criteria permits it, so we always request and apply feasible controls and the error is instantaneously taken into account.

VII. EXPERIMENTAL RESULTS

A. Simulation results on the MPC-based approach only

We first tested our own approach (Section VI) in a simulated environment and we comment these results hereafter. We assume that no noise or modeling errors have been introduced. For all the tests, the initial position is (0, 0) and the desired features are set in the position (2, 1). The parameters α T , β T and γ T used for the translational component control are by default respectively set to the values 0.001 , 0.001, and 1. The parameters α R , β R and γ R used for the rotational component control are by default respectively set to the values 0.1 , 1, and 1. These default values were determined manually.

First, we perform an experiment with a desired final position that does not imply rotation, so that the robot has just to control the x and y velocities through the corresponding jerks, which are the variables in the QP. With the default parameters values mentioned above, we obtain the walk depicted in Fig. 4. As it can be seen, the visual errors (evaluated at each of the quadrilateron corners) converge to zero. We can note an offset in the oscillatory velocity in x (in blue) and y (in red) due to the features errors. Now, in a second experiment, we evaluate the trajectory with rotation. We recall that the rotation velocity control is done in a separate process from the x and y velocities control. It works as follows: The rotation velocity controller sets the angular position while the main controller (QP) adapts the x, y velocities in terms of these computed angular positions the visual errors, the footsteps centering and the jerks minimization. The results of a first simulation involving rotation is shown in Fig. 5. One can observe that the dynamical balance is kept, and that most of the correction related to the rotation is done at the beginning of the trajectory. Also, in Fig. 6, we can see that it is robust to perturbations: The same trajectory is followed as in Fig. 5 but a strong perturbation in the CoM position has been introduced. It is simulating an external force applying to the CoM or a strong error in the position estimation, inducing peaks in the velocities. However, the perturbation is recovered quasi-instantly.

We have already explained how a local linearization is made to maintain the QP formulation. The performance of this linearization depends of the distance traveled inside the horizon, which depends on the velocity of the robot and the size of the horizon. In Fig. 7, we can see the linearized and real features trajectories for a given CoM trajectory. As expected, close to the beginning (the linearization point) the trajectories are quite similar, while the final positions differ more. This is an extreme situation, since usual metric displacements in the horizon are much smaller than this one. In any case, horizon displacements are bigger when the visual errors are bigger. i.e. the robot is far from the desired position, in which case the robot just needs a tendency. But when the errors are getting smaller, the robot needs more precision. In this case, the displacements in the horizon becomes smaller so that the difference between the real model and the linearized one becomes negligible.

Due to the walking nature, we have oscillations in the features trajectories. One of the main advantages of using MPC is that it naturally filters out these oscillations because we minize the errors in a full cycle. It is remarkable that, in comparison with the decoupled approach [START_REF] Dune | Cancelling the sway motion of dynamic walking in visual servoing[END_REF] we do not need to model explicitly the sway motion of the robot and the resulting motion of the visual features. The system could oscillate inside the horizon, and it does, but at the end, the optimal control is taken without oscillations (Fig. 8). In Fig. 8, we only show three features errors evolution, the u component of each left (black) and right (red) lower side corners, since the upper ones are, by symmetry, the same. And we complete with a single v component (blue) for all the corners, with the same symmetry argument. Fig. 6. The behavior of our system in the third simulation, with the same graphical conventions as in Fig. 5. The robot is following a trajectory similar to the one of Fig. 5. After a few footsteps, a strong perturbation is applied to the CoM, inducing a peak in the velocities graph (rigth, bottom). This perturbation is small in distance metric, so it is not visible in the features trajectories. The perturbation is quasi-instantly recovered.

B. Influence of the weighting parameters

Depending of the weights of the QP (α T , β T , γ T ), we obtain different trajectories corresponding to the different priorities conveyed by each choice of parameters. For example, refer to 9. It depicts a simulation with the same objective as in Fig. 4. The difference between these two simulations is that we increased the β T parameter, with the other parameters fixed. In Fig. 4, β T = 0.001, the default value, and in 9, β T = 0.005. The result is that the robot minimizes first the visual features errors, disregarding the jerks regularization term, which produces higher velocities and a globally less smooth trajectory.

Similarly, we illustrate the effect of parameter γ T on the obtained trajectories in Fig. 10. This parameter is the weight given to the CoP centering term, making the CoP as close as possible to the footstep center. It is critical in the sense that if it takes a too low value, then the optimization may take configurations where in single support phase, the CoP is very Fig. 8. Evolution of the errors for the u, v components of each feature, in the second simulation. In dashed line, we depict the instantaneous errors along time, and in solid line, we depict the errors estimated in the horizon (i.e. individual terms of Eq. 13, normalized by the size of the horizon). Observe that the sway motion of the robot induces oscillations of the u components in the instantaneous errors, and that these oscillations are not present in the errors estimated in the horizon window.

close to the boundary of its admissible space (the support footprint), and may not converge at all. Experimentally, we have seen that for values of γ T < 0.3, the convergence is compromised. In Fig. 10, we depict four experiments with the same goal as in Fig. 4, with increasing values of γ T . What can be observed along these figures is the evolution of the CoP trajectories (in pink): with larger values of γ T , the CoP trajectory tends to have shorter periods of time away from the center of the footprint. As a consequence, the steps are shorter, but the resulting trajectories are safer to be done. Fig. 9. The behavior of our system in a variant of the first simulation, with βT is 5 times higher, and with the same graphical conventions as in Fig. 4. The robot is following a quite distinct trajectory, with a backwards part close to the end. As the priority is to minimize features errors, disregarding the jerks, high velocities are taken.

C. Comparisons: coupled vs. decoupled approaches

We conducted a simple experiment to illustrate better the differences between the two approaches of Section V and Section VI, and to have quantitative comparisons between them. In this experiment, the robot has simply to go three meters forward. In Fig. 11, we depict the robot trajectories and footsteps (left column) and the features trajectories (right column) for each approach. The coupled one is depicted in the upper line, the decoupled one is depicted in the lower line. We can clearly appreciate that the coupled approach converges faster. Indeed, with the coupled approach, it took 15 steps, including double supports, to reach the goal. However, with the decoupled approach, after 30 steps, the goal has still not been reached and keeps converging slowly. Here, the convergence criterion is the norm of the errors between the current features positions and the desired ones. Moreover, one can observe that, close to the goal, the features positions follow a smoother trajectory in the case of the visual predictive control than with the decoupled approach, where as a result of the immediate stepping, oscillations are visible.

In Fig. 12 left, we present the profiles of velocities performed by the robot, in both cases. It is interesting to note that in the coupled approach the amplitude of the oscillations is smaller, which is desirable. Also, when the error gets small, the classical approach slows down to have a slow convergence rate. This is a normal feature of classical visual servoing, i.e. the error evolves with an exponential decay. In the decoupled approach, this behavior is clearly present. However, in the coupled approach combining the MPC WPG with visual predictive control, this is not true anymore, see Fig. 12 right, in particular, because we take into account future information, so we converge faster. In the errors evolution we use the same symmetry argument as in Fig. 8. We should note that the u components in the image plane are theoretically the oscillatory ones from the stepping motion. In Fig. 12, we can see that the v component converges faster in the coupled approach. With the u components, there remains a small residual of the oscillation in both the coupled and decoupled approaches, which explains that the u components converge slower in the coupled approach.

VIII. CONCLUSION

Since the original proposal for walking generation proposed in [START_REF] Kajita | Biped walking pattern generation by using preview control of zero-moment point[END_REF], most of the efforts in the literature have focused in dynamical balance and stability. In this paper, we have proposed a novel approach to close the robot navigation control loop within the visual servoing paradigm, by coupling tightly visual information to the Model Predictive Control formulation for walking pattern generation. This way, our online pattern generator integrates the regulation of the relative pose of 3D image features while simultaneous ensuring safety and stability for the robot, and enforcing useful visual servoing constraints, such as the visibility of the features, maximal velocities, etc. In order to keep the optimization formulation as a Quadratic Program, the perspective projection equations have been linearized around the features positions at the beginning of each pattern generator cycle. For the moment, our current approach uses 3-D information (Position-Based Visual Servoing) that strongly depends on the localization. As a future work, we wish to drop the need of 3-D information by predicting the image positions of the landmarks in terms on the velocity of the robot in the horizon (IBVS). . 11. Comparison between the coupled (upper part) and decoupled approaches (lower part). On the left side, we depict the robot trajectory with the footsteps (red and blue), and the CoM (black) and CoP (magenta) trajectories. On the right side, we depict the evolution of the instantaneous features positions during the experiment (from the red rectangle, at the beginning of the simulation, to the blue one, at the end).

Fig

Fig. 1 .

 1 Fig. 1. An example setup for our approach: the robot has to walk towards a desired position with regard to an object viewpoint. Frames o, c and m refer to the object, camera and CoM respectively. o Tc is the transformation to view the camera points in the object reference frame. (x f , y f ) is the current footprint position.

k

  p k Linear term of the cost function considered in this paper. U k Vector of control variables. Free variables of the MPC-based optimization problem solved in this paper. WPG Notations x k , ẋk , ẍk Respectively the CoM position, velocity and acceleration in the x-axis at time k. y k , ẏk , ÿk Respectively the CoM position, velocity and acceleration in the y-axis at time k. x f Position of the support foot at time k. θ f Orientation of the support foot. ξ x k Center-of-Pressure (CoP) along the x-axis at time k. xk Vector stacking the CoM position, velocity and acceleration along the x-axis. X k Sequence of CoM x-positions for a preview window of size N starting at k. ... Xk Sequence of CoM x-jerks for a preview window of size N starting at k. X f k Sequence of support foot x-positions for a preview window of size N starting at k. Z x k+1 Sequence of CoP x-positions for a preview window of size N starting at k. Z ref k+1 Sequence of reference CoM positions along the x-axis for a preview window of size N . α T , β T , γ T Weighting parameters for the translational component optimization. α R , β R , γ R Weighting parameters for the rotational component optimization. A, B, C Matrices relating respectively xk with xk+1 , ... x k with xk+1 , xk with ξ k k , when considering the Linear Inverted Pendulum Model. (a x 1 (θ f ), a y 1 (θ f )) T Vectors normal to the support foot edges. b 1 (θ f ) Position of the support foot edges along the normal defined by a x 1 (θ f ), a y 1 (θ f

  are the centers of the support polygons in single support at each iteration and they depend linearly on the variables X f k , Y f k , which are the sequence of positions of the next footsteps in the horizon. The linear relationship has the form

  Fig.2. The pattern generator ensures that the input velocity is tracked on average on the preview horizon. The output of the Model Predictive Control is the first control computed on the preview horizon. The difference between the reference velocity and the real velocity is mostly a sway motion due to the stepping.

Fig. 3 .

 3 Fig. 3. In this scheme, the pose of the object in the camera frame m To is sent to our control scheme. First, the orientation is found as described in section VI-B. Then, by the coordinates of the landmarks specified in the object reference frame (p o l ) the visual servo based WPG provides a CoP reference trajectory Z ref , a set of footprints (X f , Y f ) ref and a Center-Of-Mass reference trajectory (X, Y ) ref .The reference trajectories are then followed by a Generalized Inverse Kinematics scheme which computes at each time step a velocity q for the robot actuators. The vision part (gray box) is usually running at 30 to 60 Hz. The control part (blue boxes) is typically running at 200 Hz in the HRP-2 robot. The landmarks are constant (green box) and the control part using q of the HRP-2 robot is typically running at 200 Hz.

Fig. 4 .Fig. 5 .

 45 Fig.4. On the left, we show the trajectory of the robot in the x, y plane, driven by our MPC-based coupled approach. The initial and final double stance phases appear in green. The single stance support feet appear in red (resp. blue) for the right (resp. left) foot. In pink, we depicted the CoP trajectory, which can be observed to remain safely in the support polygon, and in black the CoM trajectory. On the right, top, we depict (in black) the trajectory of the features in the image, with the initial positions in red and the desired positions in blue, for the first simulation. Finally, the evolution of the velocities is shown in the bottom. It is interesting to note the offset of the oscillatory velocity in x (in blue) and y (in red) due to the features errors.

Fig. 7 .

 7 Fig. 7. In this figure, we show the trajectory of the visual features in one iteration of the QP, and compare the evolution of the features obtained by the linearization model (green lines) and features obtained by the exact non-linear model (red lines).

Fig. 10 .

 10 Fig. 10. Variants of the first simulation in Fig. 4, with different values of the γ T parameter, controlling the weight given to the CoP centering term, making the CoP as close as possible to the footstep center. From left to right, and top to bottom, γ T = 0.55, γ T = 0.75, γ T = 1, and γ T = 2.

Fig. 12 .

 12 Fig. 12. Left, comparison of velocity profiles in the x (blue) and y (red) axes, for the decoupled (dashed line) and coupled (solid line) approaches. Right, comparison of the features errors evolution.

  ).

	Visual Servoing Notations
	λ	Visual features indices.
		image plane.
	v c	Real camera velocity.
	v c	Ideal camera velocity.
	s λ,k s * k s d k s m k s m k = h(q k ) S m λ,k	Observation of feature λ at time k. Reference feature vector at time k. Desired feature vector at time k. Predicted feature vector at time k. h is the observation model. Collection of the predicted positions for feature λ in the horizon starting at time k.
	p o l	Position of the l ′ -th landmark in the object of interest reference frame (index o).
	e	Visual servoing task.
	Le	Interaction matrix of task e.
	Le	Approximation of the interaction matrix of
		task e.
	L+ e	Moore-Penrose pseudo-inverse of an ap-proximation Le.
	W j	Weighting matrix j in the horizon.
		Projection matrix.

l Landmarks indices (one landmark gives two visual features, its u and v coordinates). u(x, y, z)

Projection of point (x, y, z) on the x-axis image plane. v(x, y, z)

Projection of point (x, y, z) on the y-axis c Tm Transform matrix relating the camera frame (index "c") to the CoM frame (index "m"). c Vm Twist matrix related to c Tm.

ACKNOWLEDGMENTS

The first author is supported by the grant 263150 from the Mexican National Council of Science and Technology (CONACYT). For this work the second author was supported by a grant from the RBLINK Project, Contrat ANR-08-JCJC-0075-01, from the OSEO ROMEO Project, and from the FP7-ICT-2013-10/611909 KOROIBOT project. For this work the fourth author was supported by Grant-in-Aid from the Japanese Society for the Promotion of Science (JSPS) Fellows P-09721. The authors want to thank especially Pierre-Brice Wieber and Alexander Sherikov. Special thanks to E. Marchand and F. Chaumette for providing us with Lagadic model-tracker and for their precious advice and discussion.