H Bildstein

A Durand-Petiteville

V Cadenat

Visual Predictive Control Strategy for Mobile Manipulators

This work aims at designing a visual predictive control (VPC) scheme for a mobile manipulator. It consists in combining image-based visual servoing with model predictive control to benefit from the advantages of both control structures. Two challenges are addressed in this paper: the choice of the visual features and the closed-loop stability. The first ones rely on image moments to improve the end effector positioning precision. The second one is tackled through a terminal constraint coupled with suitable input constraints to reduce the computational burden. Simulation results using ROS and Gazebo validate the proposed approach.

I. INTRODUCTION

In this paper it is proposed to design a Visual Predictive Control (VPC) strategy to control a mobile manipulator. VPC [START_REF] Allibert | Predictive control for constrained image-based visual servoing[END_REF] is the fusion between Image-Based Visual Servoing (IBVS) [START_REF] Chaumette | Visual servo control, part 1 : Basic approaches[END_REF] and Nonlinear Model Predictive Control (NMPC) [START_REF] Allgower | Nonlinear model predictive control: From theory to application[END_REF] [START_REF] Grüne | Nonlinear model predictive control[END_REF]. The obtained control scheme thus combines the advantages of IBVS, i.e., reactivity and absence of metric localization [START_REF] Chaumette | Potential problems of stability and convergence in image-based and position-based visual servoing[END_REF], with the ones of NMPC, i.e., ability to explicitly deal with constraints such as control inputs boundaries or camera field of view limits. Over the last decade, the interest for VPC-based controllers has grown and numerous schemes were designed to control different robotic systems: a camera mounted on a robotic arm [START_REF] Copot | Predictive control of nonlinear visual servoing systems using image moments[END_REF] [7] [START_REF] Mohamed | Sampling-based mpc for constrained vision based control[END_REF], a flying camera [START_REF] Heshmati-Alamdari | Robustness analysis of model predictive control for constrained image-based visual servoing[END_REF] [10], a mobile robot [START_REF] Pérez-Morales | Multisensor-based predictive control for autonomous parking[END_REF], an autonomous underwater vehicle [START_REF] Heshmati-Alamdari | A self-triggered position based visual servoing model predictive control scheme for underwater robotic vehicles[END_REF] or a tendon-driven continuum robot [START_REF] Norouzi-Ghazbi | Constrained visual predictive control of tendon-driven continuum robots[END_REF]. However, MPC strategies aiming at controlling a mobile manipulator are usually not defined in the image space. For example, in [START_REF] Pankert | Perceptive model predictive control for continuous mobile manipulation[END_REF] the cost function represents the end-effector pose error and is then minimized using a Sequential Linear Quadratic Model Predictive Control. Another approach consists in relying on the generalized coordinates of the system to express the cost function, such as in [START_REF] Giftthaler | Efficient kinematic planning for mobile manipulators with nonholonomic constraints using optimal control[END_REF], [START_REF] Avanzini | Constrained model predictive control for mobile robotic manipulators[END_REF] and [START_REF] Colombo | Parameterized model predictive control of a nonholonomic mobile manipulator: A terminal constraint-free approach[END_REF]. Cameras are sometimes used as the main sensor to control mobile manipulators but the task is not defined in the image space. For example, [START_REF] Martínez | Visual predictive control of robot manipulators using a 3d tof camera[END_REF] and [START_REF] Logothetis | A model predictive control approach for visionbased object grasping via mobile manipulator[END_REF] respectively use a time of flight camera and a RGB-D camera to retrieve the object pose, then both rely on a pose-based cost function to achieve the task. To our knowledge, the work presented in [START_REF] Gao | Hierarchical imagebased visual serving of underwater vehicle manipulator systems based on model predictive control and active disturbance rejection control[END_REF] is the only one to consider a VPC strategy to control a mobile manipulator. It relies on a hierarchical MPC using points as visual features to control an underwater manipulator vehicle, but it does not deal with issues related to stability. † H. Bildstein and V. Cadenat are with CNRS, LAAS, 7 avenue du colonel Roche, F-31400 Toulouse, France and Univ. de Toulouse, UPS, LAAS, F-31400, Toulouse, France When considering a VPC scheme to control a mobile manipulator, it is necessary to take into account two challenges. The first one is the choice of the visual features and their prediction model. Indeed, it is well known for classical IBVS, that the type of visual features has a significant impact on the trajectories in both the Cartesian and image spaces [START_REF] Chaumette | Visual servo control, part 1 : Basic approaches[END_REF]. For a VPC scheme, this choice might seem to be less important, as it is possible to influence the computed trajectories via the definition of the optimization problem. However, a mobile manipulator being redundant and the point visual features strongly dependent, it is challenging to efficiently weight the cost function to increase the accuracy of the end-effector pose. For this reason, it is proposed in this work to rely on image moments as the visual features to define the cost function. These visual features are independent and it is then straightforward to weight the cost function in order to increase the pose accuracy in a given direction. Second, autonomous robotic systems represent a challenge regarding the closed-loop stability. Indeed, they usually have to perform large displacements in environments possibly cluttered with a priori unknown obstacles 1 . From an optimal problem point of view, the presence of obstacles induces frequent updates of the constraints, leading to significant modification of the optimal trajectory. For this reason, it is then not possible to rely on dual mode methods and a terminal constraint has to be included to guarantee the closed-loop stability [START_REF] Allgower | Nonlinear model predictive control: From theory to application[END_REF]. Moreover, the prediction horizon has to be large enough to allow the predicted trajectory to reach goals, increasing the computational burden. Here, the constraints on the last velocities are relaxed as in [START_REF] Durand-Petiteville | Visual predictive control scheme for a mobile robot navigating in a cluttered environment[END_REF] to obtain a trajectory dealing with the terminal constraint and the computational burden.

In this work, it is proposed to control a mobile manipulator by relying on a VPC scheme (i) using image moments as visual features to increase the end-effector position accuracy, (ii) including a terminal constraint to guarantee the closedloop stability, (iii) relaxing some constraints on the velocities to reduce the computational burden, and (iv) including a constraint on the camera field of view to guarantee the success of the task. The rest of the paper is organized as follows. First, the different models are introduced. Second, the VPC strategy aiming at controlling a mobile manipulator is detailed. Next, results obtained by simulation are presented and discussed in order to highlight the relevance of the proposed approach. Finally, the future steps aiming at further developing the proposed approach are highlighted.

II. PRELIMINARIES

A. Robotic system description and modeling

In this paper, we aim at controlling a stereo camera embedded on a mobile manipulator with respect to a given landmark. The camera is an Intel Realsense D435 providing RGB-D data. The system is the TIAGo robot from PAL Robotics (see Figure 1a). It consists of a 7 degrees of freedom (DOF) arm embedded on a differential mobile base. The camera is mounted on the wrist of the arm, thus only 5 DOF of the arm are used to control it (n a = 5). To model the robotic system, the following frames are introduced: F 0 (O 0 , x 0 , y 0 , z 0 ) as the world frame, F b (0 b , x b , y b , z b ) as the mobile base frame and F c (O c , x c , y c , z c ) as the camera frame (see Figure 1b). The mobile base pose and its control vector are defined as:

χ b = X,Y, θ T , u b = v, ω T (1)
where X, Y and θ are respectively the base coordinates in F 0 and the angle between F b and F 0 . v and ω are the linear and rotational velocities along x b and around z b . The arm configuration and its control vector are expressed as:

χ a = q 1 , q 2 , q 3 , q 4 , q 5 T , u a = q1 , q2 , q3 , q4 , q5 T (2)
where q i is the i th joint angle and qi is the i th joint velocity. Thus, the mobile manipulator pose and its control vector are:

χ mm = χ T b , χ T a T , u mm = u T b , u T a T (3)

B. Visual features

In this work, the task is defined in the image space by a set of visual features S characterizing the considered landmark. Thus, the goal is to make vector S converge to its desired value S * . Classically -and in most VPC schemes -S is composed the coordinates (x i , y i ), expressed in the image space, of four landmark interest points. This leads to:

S ip = x 1 , y 1 , x 2 , y 2 , x 3 , y 3 , x 4 , y 4 T (4)
However, these features suffer from one major drawback: they are coupled and do not offer intuitive solutions to control the 6 DOF of the task [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]. It is thus difficult to deal with the x c and y c orientation errors using only interest points coordinates. Image moments can provide better information related to position and orientation errors. If O is the observed object and O p its projection in the image, image moments m i j and centered moments µ i j are defined by:

m i j = O p
x i y j dx dy

(5)

µ i j = O p (x -x g ) i (y -y g ) j dx dy (6)
where x g = m 10 /m 00 and y g = m 01 /m 00 . Combinations of such moments allows to build interesting sets of visual features. Here, 6 adequate visual features designed in [START_REF] Tahri | Determination of moment invariants and their application to visual servoing[END_REF] and [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF] have been used:

• The quantity a n = Z * a * a , which is closely related to the z-translation error, where the area a = m 00 , a * and Z * are respectively the desired area and depth.

• The normalized coordinates x n = a n x g and y n = a n y g of the center of gravity which are respectively closely related to the x-translation and y-translation errors.

• The orientation α = 1 2 arctan 2µ 11 µ 20 -µ 02 related to zorientation error. It corresponds to the orientation of the ellipse obtained with moments of order less than 3. • The features s x and s y , respectively related to the xorientation and y-orientation errors, and defined as:

s x = (c 2 c 3 + s 2 s 3 )/K s y = (c 2 s 3 -s 2 c 3 )/K (7)
where

c 1 = µ 20 -µ 02 , s 1 = 2µ 11 , c 2 = µ 03 -3µ 21 , s 2 = µ 30 -3µ 12 , c 3 = c 2 1 -s 2 1 , s 3 = 2s 1 c 1 , I 1 = c 2 1 + s 2 1 , I 3 = µ 20 + µ 02 , and K = I 1 I 3/2 3 /

√

a. The final visual features vector is then defined by:

S m = x n , y n , a n , s x , s y , α T (8)
Moments m i j and µ i j can be computed from the interest points of arbitrary polygons [START_REF] Steger | On the calculation of arbitrary moments of polygons[END_REF], which leads to S m = g(S ip ) where g is the corresponding mapping function.

III. VISUAL PREDICTIVE CONTROL

A. The VPC scheme

As mentioned before, VPC is the result of coupling NMPC with IBVS. It thus shares characteristics from these two particular control techniques. As NMPC, it is the solution of a constrained optimal problem. More precisely, it consists in finding an optimal control sequence U * (•) that minimizes a cost function J N p over a N p steps prediction horizon under a set of user-defined constraints C(U(•)). The obtained optimal control sequence is a N c -dimensional vector where N c is called control horizon. It means that the N th c first predictions of the N p long prediction horizon are computed using independent control inputs, while the remaining ones are all obtained using a unique control input equals to the N th c element of U(•). Similarly to IBVS, the cost function is defined in the image space. It is expressed as the sum of the quadratic error between the predicted visual features vector Ŝ and the desired ones S * over the horizon N p . The optimal problem is then defined as follows:

U * (•) = min U(•) J N p (S m (k),U(•)) (9)
with

J N p (S(k),U(•)) = k+N p ∑ p=k+1 Ŝm (p) -S * m T Q S Ŝm (p) -S * m (10) subject to Ŝip (k) = S ip (k) (11a) Ŝip (p + 1) = f ( Ŝip (p),U(p)) (11b) Ŝm (p) = g( Ŝip (p)) (11c) C(U * (•)) ≤ 0 (11d)
where

U * (•) = [u * mm (k), . . . , u * mm (k + N c -1)
] is the computed optimal control and k represents time t = kT s , T s being the control sampling period. f and C(U * (•)) respectively denote the prediction model and the inequality set of constraints (see next section). Q S is a diagonal matrix which allows to weight the error S -S * and thus to prioritize specific DOF against others. The efficient use of such a matrix has been made possible by using image moments instead of pointwise visual features as classically done in the VPC literature. Once the problem is solved, only u * mm (k) is applied to the robot and the process is repeated. The previous optimization results are used to warm-start the solver.

B. The prediction model

As shown in Equation 11, it is mandatory to design a model computing the predicted visual features Ŝm . As it is more straightforward to obtain a prediction model for pointwise visual features, it is proposed to first compute Ŝip (p + 1) = f ( Ŝip (p),U(p)), then to deduce Ŝm from the relation S m = g(S ip ). The mapping g being defined in II-B, we now present the 3 necessary steps to obtain f .

1) The image plane -camera frame relation: First, the metric coordinates in the image frame x i , y i , Z c , 1

T of a point of interest can be mapped to their corresponding pixel coordinates u, v, Z c , 1 T using the following relation:

    u v Z c 1     =     f x 0 0 u 0 0 f y 0 v 0 0 0 1 0 0 0 0 1         x i y i Z c 1     (12)
where f x and f y are the focal lengths, and (u 0 , v 0 ) the coordinates of the image center, both expressed in pixel 2 .

Next, the classical pinhole camera model is used to obtain the 2 In this work the image coordinates are measured on a plane obtained with an unity focal, i.e., when Z c = 1.

coordinates X c ,Y c , Z c , 1

T , expressed in the camera frame, from the ones in the image frame.

    x i y i Z c 1     =     1 Z c 0 0 0 0 1 Z c 0 0 0 0 1 0 0 0 0 1         X c Y c Z c 1     = i H c     X c Y c Z c 1     (13)
2) The mobile base frame -camera frame relation: The coordinates expressed in the camera frame, X c ,Y c , Z c , 1

T , are mapped to the ones expressed in the mobile base frame, X b ,Y b , Z b , 1 T , via the homogeneous transformation matrix b H c . This latter is obtained using the forward kinematics model and thus only depends on the arm configuration χ a .

X b ,Y b , Z b , 1 T = b H c (χ a ) X c ,Y c , Z c , 1 T (14)
3) The relation between two robot poses at different instants: To obtain such a relation, the kinematic models of a differential robot and of an arm are integrated over T s . During this interval, v and ω are constant. It leads to:

     X(k + 1) = X(k) + v(k) ω(k) sin(θ (k) + ω(k)T s ) -sin(θ (k)) Y (k + 1) = Y (k) -v(k) ω(k) cos(θ (k) + ω(k)T s ) -cos(θ (k)) θ (k + 1) = θ (k) + ω(k)T s (15) when ω = 0. For ω = 0, the solution is straightforward. Let us define ∆X = X(k + 1) -X(k), ∆Y = Y (k + 1) -Y (k)
and ∆θ = θ (k + 1)θ (k). Thus, the homogeneous matrix between two successive mobile frames F b (k) and F b (k + 1), denoted b k H b k+1 , is defined as follows:

b k H b k+1 =     cos(∆θ ) -sin(∆θ ) 0 ∆X sin(∆θ ) cos(∆θ ) 0 ∆Y 0 0 1 0 0 0 0 1     (16)
Finally, the prediction model f for point-wise visual features is given by:

Ŝip (k + 1) = i H c (k + 1) c H b (k + 1) b k+1 H b k b H c (k) c H i (k) Ŝip (k) (17)

C. The terminal constraint (TC)

In this work, the closed-loop stability is ensured by a zero terminal equality constraint [START_REF] Allgower | Nonlinear model predictive control: From theory to application[END_REF]. It imposes that the last predicted visual features vector is equal to the desired one. The respect of the TC guarantees the existence of a trajectory leading from the current state to the desired one, thus insuring the recursive feasibility.

Ŝmi (k + N p ) -S * m i = 0, ∀i ∈ 1, 6 (18)
As a strict equality constraint cannot be satisfied, the constraints are converted into inequalities by defining a small enough threshold δ tc .

Ŝmi

(k + N p ) -S * m i -δ tc ≤ 0, ∀i ∈ 1, 6 (19)

D. The velocity constraints

These constraints aim at taking into account the physical boundaries of the actuators. With the number of prediction steps, they define the maximal length of the predicted trajectory. However, in order to respect the TC, the predicted trajectory has to be long enough. In the mobile manipulation context, it might represent a challenge due to the distance the mobile base has to cover to reach the goal, while the number of prediction steps cannot be too large to minimize the computational burden. Thus, to insure the TC to be satisfied all the time while taking into account the physical boundaries of the actuators, it is proposed to constrain the velocities of the first part of the inputs according to the physical boundaries, and to relax the constraints for the second part of the inputs. This approach leads to the following set of constraints for the mobile base velocities:

u mm (i) -u u|t u l|t -u mm (i) ≤ 0, ∀i ∈ k, k + N c -N r u mm (i) -u u|r u l|r -u mm (i) ≤ 0, ∀i ∈ k + N c -N r , k + N c ( 20
)
where i ∈ k, k + N c , N r is the number of prediction steps with relaxed boundaries, u l|t and u u|t are respectively the lower and upper tight boundaries corresponding to the actuators limits, and u l|r and u u|r are respectively the lower and upper relaxed boundaries. N c N r , so that the command applied to the robot respects the actuators limits, while the TC can be ensured.

E. The visibility constraints

In the context of visual servoing, the target must remain always visible. The following constraint allows to guarantee that the visual cues do not leave the camera field of view.

S ip (i) -S u S l -S ip (i) ≤ 0, ∀i ∈ k + 1, k + N p (21)
where S l and S u are the lower and upper image boundaries.

F. The joint limits constraints

Finally, it is also necessary that the arm joints never exceed their lower and upper boundaries χ a l and χ a u , which leads to the following constraints:

χ a (i) -χ a u χ a l -χ a (i) ≤ 0, ∀i ∈ k + 1, k + N p (22)
IV. RESULTS

This section presents simulation results allowing to evaluate the proposed approach and to show its efficiency. To do so, the results are divided into two parts. In the first one, the VPC scheme is run in a 2D simulator in order to highlight the relevance of the image moments over the point-wise visual features. In the second one, the Gazebo software is used to simulate the considered framework in a more realistic context.

All algorithms are implemented using the C++ language and the optimization problem is solved with the SLSQP solver from the NLopt package [START_REF] Johnson | The nlopt nonlinear-optimization package[END_REF]. The matrix b H c is obtained with Pinocchio [START_REF] Carpentier | Pinocchio: fast forward and inverse dynamics for poly-articulated systems[END_REF], a rigid body dynamics library, which is using a URDF model of the robot. All tests are performed on an Intel Core i7-10850H and the VPC runs at a frequency of 5Hz. The solver timeout is set to 0.15s, N p is fixed to 10 steps with a sampling time3 T s = 0.4s. The target is a rectangle centered in (3, 0, 1.08625) and the initial mobile base pose is (0, 0, 0) in F 0 as shown on Figure 3a. The camera and the mobile base have to travel about 2m to reach the target. Finally, the bounds on the mobile base linear velocity is equal ±0.1 m/s, while the angular velocities ones are fixed to ±0.1 rad/s. The minimal and maximal joint limits are given by: χ au = [2.68, 1.02, 1.50, 2.29, 2.07] and χ al = [0.07, -1.50, -3.46, -0.32, -2.07].

A. Residual with visual feature vector S ip vs S m

This first set of simulations aims at showing the advantage of relying on moment visual features over point ones for a redundant systems such as a mobile manipulator. Indeed, as explained in [START_REF] Tahri | Determination of moment invariants and their application to visual servoing[END_REF], the use of point visual features makes challenging the regulation of the x c or y c orientation errors. To compare the two approaches, it is proposed to show the errors for DOF. To do so, we rely on a 6D cost scoring the difference between the resulting frame and the reference one. In this work, the score is expressed as:

R H = log 6 (H -1 H * ) ( 23
)
where H is the frame obtained with the VPC controller, H * the reference one, and log 6 is the function mapping the group SE(3) to its Lie algebra [START_REF] Sola | A micro lie theory for state estimation in robotics[END_REF]. Actually, R H is a 6D motion vector corresponding to the velocity that should be applied during 1s to move from the reference frame -H * -to the frame defined by H. For this first test, N c = N p = 10, N r = 0, Q S = I 6 and the TC is not included to obtain results allowing to compare the choice of the visual features. As it is shown in Figures 2a and2b, the VPC controller manages to drive the camera in order to make the visual features converge towards the desired ones (green crosses in the figure) for both configurations. However, the trajectory obtained in the image space is significantly shorter for the point-wise visual features than for the image moment ones. For this second case, the visibility constraint prevents the visual features from leaving the same field of view. Regarding the final pose accuracy, it can be seen in Figures 2g and2h that the use of image moments allows to obtain a significantly more accurate result. It is confirmed by the results presented in Figures 2c and2d, where the vector R H corresponding to S m is significantly smaller than the S ip 's one, showing the interest of using the moments. After convergence with S ip , R H ≤ 1e-1, while with S m , R H ≤ 1.5e-2. The benefit of choosing the adequate visual features vector is almost a ratio of 10 on the R H norm. In Figure 2c it can be seen that the difficulty is coming from ω x . Moreover, it can be seen that the evolution of image moments (Figure 2f) follows the same trend as R H (Figure 2d), while the one of the points (Figure 2e) is strongly different (Figure 2c). Thus, it is straightforward to tune the weighting matrix Q S to prioritize any direction when relying on image moments as visual features. In this section the proposed framework is tested with the Gazebo simulator. This latter allows to simulate robots in complex environments and to obtain more realistic scenarios, close to experimental conditions (see figure 3). The target is represented by aprilTags [START_REF] Wang | AprilTag 2: Efficient and robust fiducial detection[END_REF] which will be detected by a perception node. Moreover, the TC is included to deal with the closed-loop stability. Next, the prediction parameters are setup to N c = 9 and N r = 1 to obtain a sufficiently long predicted trajectory guaranteeing the problem feasibility. Finally, the weighting matrix is chosen as Q S = diag(1, 1, 1, 10, 10, 1) to prioritize ω x and ω y . The obtained results are presented in Figures 4 and5 In Figure 4a it can be seen that the task is correctly achieved. Indeed the VPC controller manages to drive the camera to make the visual features reach their desired values (the green crosses) corresponding the camera goal position (see Figure 3b). This is achieved by vanishing the error between the image moments and their desired values (see Figure 4b). In parallel to the positioning task, the controller has to deal with several constraints. For example, Figure 4c shows the error between the last predicted image moments and their desired values. An error close to zero means that the terminal constraint is respected. As it can be seen in the figure, the use of a relaxed input constraint allows to respect such a constraint despite the initial large distance between the current pose and the desired one. It can be noticed that the TC is initially and punctually not respected. The solver being setup with a timeout, the optimization process might stop and deliver a solution not dealing with the whole set of constraints. In Figure 4a a similar scenario can be observed for the field of view constraint. To deal with this issue, this constraint has been setup in a conservative way allowing the visual features to not leave the camera field of view. Finally, Figure 5 shows the velocities and joint angle evolution. For both, their values stay within the given boundaries despite the use of a relaxed constraint to guarantee the feasibility.

V. CONCLUSION

In this work, a VPC scheme has been designed to control a mobile manipulator. It relies on image moments to increase the end-effector position accuracy and ease the tuning of a weighting matrix to prioritise a given direction. It also ensures stability thanks to the introduction of a terminal constraint and to the definition of relaxed input constraints allowing to reduce the computational burden. The obtained results are promising, and highlight the interest and validity of the proposed approach. Several future extensions are considered: trajectory improvement (e.g., [START_REF] Durand-Petiteville | Visual predictive control scheme for a mobile robot navigating in a cluttered environment[END_REF]), additional constraints dealing with dynamical obstacles, dynamic weighting of Q S , and validation on an experimental testbed.

Fig. 1 :

 1 Fig. 1: The robotic system

Fig. 2 :

 2 Fig. 2: First test: Comparison of the visual servoing task execution with S ip on the left and S m on the right

Fig. 3 :Fig. 4 :

 34 Fig. 3: TIAGo robot and the landmark in Gazebo.

Fig. 5 :

 5 Fig. 5: Second test: Simulation results with Gazebo -part 2

In this preliminary work, the environment is obstacle free.

This sampling time is only used for prediction, the control loop is running at 5Hz.

A video of the resulting simulation can be found at https://youtu. be/jjhEfflvCNc.