Optimal Image-Based Guidance of Mobile Manipulators using Direct Visual Servoing

Abstract

This paper presents a direct image-based controller to perform the guidance of a mobile manipulator using image-based control. An eye-in-hand camera is employed to perform the guidance of a mobile differential platform with a seven degrees-of-freedom robot arm. The presented approach is based on an optimal control framework and it is employed to control mobile manipulators during the tracking of image trajectories taking into account robot dynamics. The direct approach allows us to take both the manipulator and base dynamics into account. The proposed image-based controllers consider the optimization of the motor signals sent to the mobile manipulator during the tracking of image trajectories by minimizing the control force and torque. As the results show, the proposed direct visual servoing system uses the eye-in-hand camera images for concurrently controlling both the base platform and robot arm. The use of the optimal framework allows us to derive different visual controllers with different dynamical behaviors during the tracking of image trajectories.

1. Introduction

This paper proposes a direct image-based visual servoing system for the guidance of a mobile manipulator. As described throughout the paper, the visual information is employed to guide both the robot manipulator and the base platform for the tracking of image trajectories. A common classification for visual servoing systems based on position and image is established [1]. Within the field of position-based visual servoing systems, it is worth mentioning some classic works [2] as well as some more recent ones such as the works of Huang et al. [3]. Besides these two works, it is worth mentioning other remarkable ones such as the works of Park et al. [4], which describes the use of vision for a robot’s positioning with regard to an object whose position was previously determined. Position-based systems are those where the reference of the control system is in a desired tridimensional position and orientation. In contrast to position-based approaches, image-based visual servoing systems determine the reference in the image space. This strategy is the one used in this paper. Although the literature regarding image-based visual servoing is quite extensive, works, such as the presented in [1,5], perform a review of the classical approaches and problems related to indirect image-based controllers. One of the elements which most affects the behavior of image-based visual servoing controllers is the interaction matrix, which is employed by the controller and depends on several parameters. The interaction matrix ${\mathit{J}}_s$ is employed by the image-based visual servoing controllers for relating velocities in a tridimensional point with velocities of a corresponding point in the image space. Deep learning approaches are currently in development for image-based control of robot manipulators (see e.g. [6,7]). When this kind of approaches has to be applied to a mobile manipulator, it is necessary to consider not only the manipulator but also the mobile platform, having a redundant system. When dealing with stability problems, it is worth mentioning those that stem from dealing with a redundant robot [8]. In this case, it is not only necessary to correctly perform the end effector guidance, but also to solve the redundancy by establishing the most appropriate joint configuration for each situation.

Another useful classification depending on the control strategy of visual servoing systems divides them in direct and indirect visual servoing. In indirect visual servoing, the control action is specified as velocities to be applied at the robot’s end effector and does not take robot dynamics into account. In this case, the internal robot controller is used and translates these velocities into torques and forces for the joints. When applying an indirect controller to a task only kinematic aspects of the robot are taken into account. However, in direct controllers, the control action is usually given as forces and torques applied directly to the manipulator joints. By using this approach, the internal feedback control loop for the servomotors is removed, and the visual information is used to directly generate the currents or torques to be applied to the motors of the robot’s every joint. Some direct visual control systems for redundant robots with chaos compensation have been developed to avoid robotic chaotic behaviors with new architectures to improve systems maintainability and traceability [9,10]. Within the field of direct image-based visual servoing systems it is worth mentioning works such as [11] that considers a FPGA (Field Programmable Gate Arrays) implementation, or [12] that extends the previous direct visual servoing for the optimal control of robot manipulators.

Within the field of indirect visual servoing, several approaches can be found for the guidance of mobile robots or mobile manipulators. In [13], a homography-based 2D approach for visual control of a mobile manipulator that does not need any measure of the 3D structure of the observed target has been proposed. A task-space sensing and control system designed to control the end effector motion of a mobile manipulator in the presence of dynamic and unknown base motion is proposed in [14]. In [15], an image-based approach with redundancy resolution is proposed for the guidance of a mobile manipulator. An indirect position-based visual control approach is described in [16] for the guidance of a mobile manipulator. In [17], a control scheme that uses image-based visual servoing was developed along with a functional mobile manipulator platform, where a hybrid camera configuration composed of monocular and stereo vision cameras was integrated into the system.

In contrast with previous approaches, the one proposed in this paper employs a direct visual servoing system for guiding mobile manipulators. In this case, the controller directly generates the torques, forces, and moments to be applied to the manipulator and base platform taking into account the robot dynamics. In the previous indirect approaches, the controller assumes that the guided device is a perfect positioning system, and therefore its dynamics is not considered. However, the use of a direct visual servo approach allows for the manipulator and base dynamics to be taken into account. The result is a faster and more accurate control that reacts more quickly to abrupt changes in the image trajectories. Additionally, an optimal control approach is used for the definition of the proposed controllers. This control approach considers the optimization of the motor signals sent to the mobile manipulator during the tracking of image trajectories. Using this optimal approach, different visual controllers with different dynamic properties can be derived.

The paper is divided into the following sections. First, Section 2 describes the robotic system and main coordinate frames. The system kinematics is described in Section 3 and the dynamics in Section 4. Section 5 describes the optimal controller and the visual servoing of the mobile manipulator. Finally, in Section 6, the main results obtained in the application of the proposed optimal controller are described.

2. System Architecture and Nomenclature

Figure 1 represents the main components of the mobile manipulator (TIAGo robot from PAL Robotics). The robot is composed of a mobile differential platform with a 7-DOF (degrees-of-freedom) arm, a RGB-D camera which is not employed in this paper, and an additional eye-in-hand camera. Frame ${\mathcal{F}}_p$ is attached to the mobile robot platform and ${\mathcal{F}}_m$ is the manipulator arm coordinate system attached to the base of the robot arm. The origin of ${\mathcal{F}}_m$ is translated with respect to the frame ${\mathcal{F}}_p$ a vector ($x_{pm},y_{pm},z_{pm}$). The world coordinate frame is called ${\mathcal{F}}_o$. The end effector frame ${\mathcal{F}}_e$ is attached to the manipulator end effector, and frame ${\mathcal{F}}_c$ is the eye-in-hand camera frame. In this paper we consider an eye-in-hand camera system where ${\mathcal{F}}_c={\mathcal{F}}_e$. The orientation of the mobile platform is considered as α, i.e., the angle between de X-axis of the frame ${\mathcal{F}}_p$ and the X-axis of the frame ${\mathcal{F}}_o$ and the origin of ${\mathcal{F}}_p$ is at the position ($x_{op},y_{op},z_{op}$) with respect to ${\mathcal{F}}_o$. From the manipulator kinematics we can obtain the position of ${\mathcal{F}}_e$ with respect to ${\mathcal{F}}_m$ as ($x_{me},y_{me},z_{me}$) and (${\varphi }_{xme},{\varphi }_{yme},{\varphi }_{zme}$), respectively.

Regarding the visual system, an eye-in-hand camera observes a set of four visual features located on the surface of the tracked object. As Figure 1 shows, a black pattern with four white points is located in the tracked object. Therefore, the computer vision extracts four visual feature points, $\mathit{s}={\left[{\mathit{f}}_{1,}{\mathit{f}}_{2,}{\mathit{f}}_{3,}{\mathit{f}}_{4,}\right]}^{\mathrm{T}}$ that correspond to the center of these white points. Figure 2 shows the different modules that compose the computer vision system. The first step preprocesses the image in order to obtain a single format of the image no matter what camera is employed (e.g., image conversion from RGB to gray scale, image resizing, noise filtering, etc.). In the second stage, the image is binarized through a thresholding process. The result is a binary image with all the essential data of the pattern. The third step is an erosion operation. This is a morphological operation on the image that removes the pixels located on the edge of any object in the image. This step is mainly used to reduce the noise of the image. The fourth phase is the most important stage of the vision module: blob detection. The purpose of this module is to detect the connected pixels which form an object in the image and then assign a label to them. Finally, the vision module has to provide the pixel coordinates of the center of gravity of the visual features. The fifth submodule (area and centroid computation) accomplishes this task. In [12], a detailed description of the computer vision components required to determine the set of visual features is presented.

3. System Kinematics

The main components and coordinate frames of the robotic system are presented in Section 2. This section describes the kinematic formulation required for the definition of the proposed controllers. ${\mathit{q}}_m^T\in {\Re }^{\mathrm{m}}$ represent the generalized joint coordinates of the manipulator (in our case, m = 7) and ${\mathit{q}}_p^T\in {\Re }^{\mathrm{np}}$ are the generalized coordinates of the platform. The generalized coordinates of both manipulator and platform can be represented as $\mathit{q}=\left[\begin{array}{cc}{\mathit{q}}_p^T& {\mathit{q}}_m^T\end{array}\right]$. In a general case, the computer vision system extracts k visual feature points from the observed object $\mathit{s}={\left[{\mathit{f}}_{1,}{\mathit{f}}_{2,}\dots ,{\mathit{f}}_k\right]}^{\mathrm{T}}\in {ℝ}^{{k}}$ (k = 4 in the experiments, see Figure 1). Therefore, the image-based direct visual controller must perform the guidance of the mobile manipulator to track the desired trajectory in the image space, s_d(t). The visual servoing control approach allows the control of the manipulator joints and base using k visual features, using an eye-in-hand configuration, where a camera is held by the end effector. Considering an image feature point, ${\mathit{f}}_{i,}$, the relationship between velocities in the camera image space, ${\dot{\mathit{f}}}_i$, and the velocity twist ${\dot{\mathit{r}}}_{\mathrm{c}}={\left[\begin{array}{cc}{\mathit{v}}_c& {\mathit{\omega }}_c\end{array}\right]}^T$ of the camera (expressed in its own frame), is given by:

$${\dot{\mathit{f}}}_i= {\mathit{J}}_{{\mathit{f}}_i}\left({\mathit{f}}_i,Z_i\right)\left[\begin{array}{c}{\mathit{v}}_c\\ {\mathit{\omega }}_c\end{array}\right]$$

(1)

where $Z$ is the depth of the image feature and the value of ${\mathit{J}}_{\mathit{f}i}$ can be found in [1]. From a kinematic point of view, the motor commands are considered as a set of platform command velocities, ${\mathit{u}}_p\in {ℝ}^{\mathrm{p}},$ and manipulator command velocities, ${\mathit{u}}_m\in {ℝ}^{\mathrm{m}}$, therefore

$$\mathit{u}= {\left[\begin{array}{cc}{\mathit{u}}_p^T& {\mathit{u}}_m^T\end{array}\right]}^T$$

(2)

In the above equation the manipulator motor commands are the joint velocities, i.e., ${\dot{\mathit{q}}}_m={\mathit{u}}_m$, and from the kinematic model of the mobile platform the following relation can be considered:

$${\dot{\mathit{q}}}_p=\mathit{\psi }\left({\mathit{q}}_p\right){\mathit{u}}_p$$

(3)

where, usually, ${\mathit{u}}_p={\left[\begin{array}{cc}v& \omega \end{array}\right]}^T$ is the platform linear and angular velocities and the function $\mathit{\psi }$ spans the admissible velocity space at each mobile platform configuration. In this paper, an image-based approach will be employed for the guidance of the mobile manipulator, therefore, an image Jacobian, ${\mathit{J}}_i$, must be defined which relates the differential mapping between the motor commands $\mathit{u}$ and the time derivative of the set of image features s:

$$\dot{\mathit{s}}={\mathit{J}}_i\mathit{u}$$

(4)

where ${\mathit{J}}_i$ will be defined throughout the paper as the product of the matrices ${\mathit{J}}_s$ and ${\mathit{J}}_c$, therefore, ${\mathit{J}}_i={\mathit{J}}_s{\mathit{J}}_c$. The interaction matrix ${\mathit{J}}_s\in {ℝ}^{2\mathrm{k}\times 6}$ for the set of image features points, s, is the stack of k matrices ${\mathit{J}}_{fi}$ for each feature:

$$\dot{\mathit{s}}={\mathit{J}}_s\left(\mathit{s},\mathit{Z}\right)\left[\begin{array}{c}{\mathit{v}}_c\\ {\mathit{\omega }}_c\end{array}\right]=\left[\begin{array}{c}{\mathit{J}}_{f1}\left({\mathit{f}}_1,Z_1\right)\\ ⋮\\ {\mathit{J}}_{fk}\left({\mathit{f}}_k,Z_k\right)\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_c\\ {\mathit{\omega }}_c\end{array}\right]$$

(5)

where $\mathit{Z}={\left[\begin{array}{ccc}{Z}_1& \dots & Z_k\end{array}\right]}^T$ $\in {ℝ}^{\mathrm{k}}$ is a depth vector for each image feature. The matrix ${\mathit{J}}_c$ relates the end effector velocity with respect to the camera coordinate frame ${\left[\begin{array}{cc}{\mathit{v}}_c& {\mathit{\omega }}_c\end{array}\right]}^T$ and the motor command $\mathit{u}$:

$$\left[\begin{array}{c}{\mathit{v}}_c\\ {\mathit{\omega }}_c\end{array}\right]={\mathit{J}}_c\left(\mathit{q}\right)\mathit{u}$$

(6)

As previously described, the mobile manipulator is composed of a differential drive platform and a 7-DOF robot arm with an eye-in-hand camera. In order to derive the value of the matrix ${\mathit{J}}_c$, first the position and orientation of the camera with respect to the world coordinate ${\mathcal{F}}_o$ is defined as

$$\mathit{r}={\left[\begin{array}{cc}{\mathit{t}}^T\left(\mathit{q}\right)& {\mathit{\varphi }}^T\left(\mathit{q}\right)\end{array}\right]}^T={\left[\begin{array}{cc}\begin{array}{ccc}{t}_x& t_y& t_z\end{array}& \begin{array}{ccc}{\varphi }_x& {\varphi }_y& {\varphi }_z\end{array}\end{array}\right]}^T$$

(7)

where ${\mathit{t}}^T\left(\mathit{q}\right)$ represents the position and ${\mathit{\varphi }}^T\left(\mathit{q}\right)$ is the minimal parametrization of the camera orientation by Euler angles. From the robot kinematics and the system architecture presented in Section 2, it is possible to obtain

$$t_x=\left(x_{m\mathit{e}}+x_{pm}\right)cos\alpha -\left(y_{m\mathit{e}}+y_{pm}\right)cos\alpha +x_{op}$$

(8)

$$t_y=\left(x_{m\mathit{e}}+x_{pm}\right)sin\alpha +\left(y_{m\mathit{e}}+y_{pm}\right)cos\alpha +y_{op}$$

(9)

$$t_z=z_{me}+z_{pm}$$

(10)

From the previous equations we can obtain the time derivatives of $\mathit{r}$, i.e., ${\left[\begin{array}{cc}{\mathit{v}}^T& {\mathit{\omega }}^T\end{array}\right]}^T$. First, the components of the translation velocity, ${\dot{\mathit{t}}}^T\left(\mathit{q}\right)$, can be defined as

$${\dot{t}}_x={\dot{x}}_{me}cos\alpha -\left(x_{me}+x_{pm}\right)sin\left(\alpha \right)\omega -{\dot{y}}_{me}sin\alpha -\left(y_{me}+y_{pm}\right)cos\left(\alpha \right)\omega +vcos\alpha$$

(11)

$${\dot{t}}_y={\dot{x}}_{me}sin\alpha +\left(x_{me}+x_{pm}\right)cos\left(\alpha \right)\omega +{\dot{y}}_{me}cos\alpha -\left(y_{me}+y_{pm}\right)sin\left(\alpha \right)\omega +vsin\alpha$$

(12)

$${\dot{t}}_z={\dot{z}}_{me}$$

(13)

Therefore, the motion of the manipulator end depends not only on the joint positions and velocities of the manipulator arm but also on the motion of the mobile platform and its orientation $\alpha$. Additionally, the components of the angular speed, ${\dot{\mathit{\varphi }}}^T\left(\mathit{q}\right)$ of the robot can be computed as [15]

$${\dot{\varphi }}_x={\dot{\varphi }}_{xme}cos\alpha -{\dot{\varphi }}_{yme}sin\alpha$$

(14)

$${\dot{\varphi }}_y={\dot{\varphi }}_{xme}sin\alpha -{\dot{\varphi }}_{yme}cos\alpha$$

(15)

$${\dot{\varphi }}_z={\dot{\varphi }}_{zme}+\omega$$

(16)

From Equations (11)–(16) the values of ${\left[\begin{array}{cc}{\mathit{v}}^T& {\mathit{\omega }}^T\end{array}\right]}^T={\left[\begin{array}{cc}\begin{array}{ccc}{\dot{t}}_x& {\dot{t}}_y& {\dot{t}}_z\end{array}& \begin{array}{ccc}\dot{{\varphi }_x}& {\dot{\varphi }}_y& {\dot{\varphi }}_z\end{array}\end{array}\right]}^T$ are obtained with respect to the motor commands. Using the rotation matrix ${\mathit{R}}_c$, which represents the orientation frame ${\mathcal{F}}_c$ with respect to ${\mathcal{F}}_o$, the values of ${\left[\begin{array}{cc}{\mathit{v}}^T& {\mathit{\omega }}^T\end{array}\right]}^T$ can be expressed with respect to the frame ${\mathcal{F}}_c$, ${\left[\begin{array}{cc}{\mathit{v}}_{\mathit{c}}^T& {\mathit{\omega }}_{\mathit{c}}^T\end{array}\right]}^T.$ Appendix A describes the computation of the numerical values of matrix ${\mathit{J}}_c$.

Another useful kinematic relationship for the definition of the proposed controllers is the image acceleration or second time derivative of s. This relationship is obtained by the definition of ${\mathit{J}}_i$ (Equation (4)) and differentiating w.r.t.

$$\ddot{\mathit{s}}={\mathit{J}}_i\dot{\mathit{u}}+{\dot{\mathit{J}}}_i\mathit{u}$$

(17)

4. System Dynamics

This section describes the main dynamic equations for the robotic system. The dynamic equations of motion of the mobile manipulator can be written as

$$\left[\begin{array}{c}{\mathit{F}}_b\\ \mathit{\tau }\end{array}\right]=\left[\begin{array}{cc}{\mathit{M}}_{bb}& {\mathit{M}}_{bm}\\ {\mathit{M}}_{bm}^{\mathrm{T}}& {\mathit{M}}_{mm}\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{u}}}_p\\ {\ddot{\mathit{q}}}_m\end{array}\right]+\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{c}}_m\end{array}\right]$$

(18)

where ${\ddot{\mathit{q}}}_m\in {\Re }^{\mathrm{m}}$ is the set of joint accelerations of the robot manipulator, ${\dot{\mathit{u}}}_p={\left[\begin{array}{cc}\dot{v}& \dot{\omega }\end{array}\right]}^T$ is the platform linear and angular accelerations, ${\mathit{M}}_{bb}\in {\Re }^{p\times p}$ is the inertia matrix of the platform, ${\mathit{M}}_{bm}\in {\Re }^{p\times m}$ is the coupled inertia matrix of the base and the manipulator, and ${\mathit{M}}_{mm}\in {\Re }^{m\times m}$ is the inertia matrix of the manipulator; ${\mathit{c}}_b\mathrm{and}{\mathit{c}}_m$ are a velocity/displacement-dependent, nonlinear terms for the base and manipulator, respectively, ${\mathit{F}}_b$ is the force and moment exerted on the base, and $\mathit{\tau }$ is the applied joint torque on the robot manipulator. From a dynamic point of view, the motion of the mobile manipulator is governed by the applied torques on the manipulator joints and by the force and moment exerted on the base. Hence, Equation (18) can be written in the following compact form.

$$\tilde{\mathit{\tau }}=\tilde{\mathit{M}}\dot{\mathit{u}}+\tilde{\mathit{C}}$$

(19)

where $\tilde{\mathit{\tau }}=\left[\begin{array}{c}{\mathit{F}}_{\mathit{b}}\\ \mathit{\tau }\end{array}\right]$, $\tilde{\mathit{M}}=\left[\begin{array}{cc}{\mathit{M}}_{bb}& {\mathit{M}}_{bm}\\ {\mathit{M}}_{bm}^{\mathrm{T}}& {\mathit{M}}_{mm}\end{array}\right]$, $\dot{\mathit{u}}=\left[\begin{array}{c}{\dot{\mathit{u}}}_p\\ {\ddot{\mathit{q}}}_m\end{array}\right]$, and $\tilde{\mathit{C}}=\left[\begin{array}{c}{\mathit{c}}_b\\ {\mathit{c}}_m\end{array}\right].$

5. Optimal Control of the Mobile Manipulator

5.1. Optimal Control Definition

In complex high dimensional systems, several tasks can be accomplished simultaneously taking advantage of the robot redundancy. This paper considers a system with $\mu$ constrains that represents the task for the mobile manipulator to be executed. These constraints can be represented as

$$\begin{array}{c}\mathit{A}\end{array}\left(t\right)\dot{\mathit{u}}=\mathit{b}\left(t\right)$$

(20)

where $\begin{array}{c}\mathit{A}\end{array}\left(t\right)\in {\Re }^{\mu \times (p+m)}$ and $\begin{array}{c}\mathit{b}\end{array}\left(t\right)\in {\Re }^{\mu \times 1}$. An advantage of this task formulation is that nonholonomic constraints can be treated in the same general way. In order to reduce the energy required for performing the visual servoing task, the proposed optimal controller is designed to minimize the control torque for the mobile manipulator, considering the following cost function.

$$\begin{array}{c}\mathit{\Omega }\left(t\right)\end{array}={\tilde{\mathit{\tau }}}^{\mathrm{T}}\mathbf{W}\left(t\right)\tilde{\mathit{\tau }}$$

(21)

where $\mathbf{W}\left(t\right)$ is a time-dependent weight matrix.

Let us assume that the robot model presented in the previous section and a constraint formulation are given. In this case, the optimal controller has to guarantee that the task is perfectly achieved, i.e., that $\begin{array}{c}\mathit{A}\end{array}\left(t\right)\dot{\mathit{u}}=\mathit{b}\left(t\right)$ holds at all times. Additionally, the minimization of the control command with respect to some given metric, i.e., $\begin{array}{c}\mathit{\Omega }\left(t\right)\end{array}={\tilde{\mathit{\tau }}}^{\mathrm{T}}\mathbf{W}\left(t\right)\tilde{\mathit{\tau }}$, is intended at each instant of time. The solution to this pointwise optimal control problem [18] can be derived from a generalization of Gauss’ principle, as originally suggested in [19]. It is also a generalization of the propositions in [20,21], where the following control action is obtained considering the general expression for the dynamic model expressed in Equation (19) (for the sake of clarity the time dependences are not indicated):

$$\widetilde{\mathit{\tau } }{=\mathbf{W}}^{-1/2}{\left({\mathit{A}\tilde{\mathit{M}}}^{-1}{\mathbf{W}}^{-1/2}\right)}^{+}·\left({\mathit{b}+\mathit{A}\tilde{\mathit{M}}}^{-1}\tilde{\mathit{C}}\right)$$

(22)

where the symbol + denotes the pseudo inverse of a general matrix. As it can be seen in Equation (22), the matrix W is an important factor in the control law that can be used to determine how the control efforts are distributed over the joints and the robot base.

5.2. Optimal Direct Visual Servoing

Once the optimal control framework is presented in Section 5.1, this section describes its application for the optimal control of the mobile platform. First, we define ${\ddot{\mathit{s}}}_r$ as the reference image accelerations that will be employed by the optimal controller. The value of these image accelerations can be obtained from Equation (17). This last expression can be expressed into the form of Equation (20) as

$${\mathit{J}}_i\dot{\mathit{u}}={\ddot{\mathit{s}}}_r-{\dot{\mathit{J}}}_i\mathit{u}$$

(23)

This way, the visual servoing task is defined by the following relationships.

$$\begin{gathered} {\mathit{A}=\mathit{J}}_i \\ \mathit{b}={\ddot{\mathit{s}}}_r-{\dot{\mathit{J}}}_i\mathit{u} \end{gathered}$$

(24)

Therefore, with the given definition of A and b, the optimal control will minimize the motor commands while performing the tracking in the image space. The final control law can be obtained replacing these variables into the function that minimizes the motor signals described by Equation (22):

$$\widetilde{\mathit{\tau } }{=\mathbf{W}}^{-1/2}{\left({\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}{\mathbf{W}}^{-1/2}\right)}^{+}·\left({\ddot{\mathit{s}}}_r-{\dot{\mathit{J}}}_i\mathit{u}{+\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}\tilde{\mathit{C}}\right)$$

(25)

As it can be seen, the visual controller represented by (25) implicitly depends on the weighting matrix W, and different values of this matrix can be used to coordinate the motion of the base and of the manipulator. The diagram of the considered control loop is depicted in Figure 3.

Considering $W={\tilde{\mathit{M}}}^{-2}$ and replacing this value in the control framework expressed in Equation (25), the result yields

$$\tilde{\mathit{\tau }}=\tilde{\mathit{M}}{\mathit{J}}_i^{+}·\left({\ddot{\mathit{s}}}_r-{\dot{\mathit{J}}}_i\mathit{u}{+\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}\tilde{\mathit{C}}\right)$$

(26)

This controller represents a direct visual servo control using inversion of the dynamic model. Additionally, new controllers can be obtained using different values of W. For example, an important value for W, due to its physical interpretation, is $W={\tilde{\mathit{M}}}^{-1}$, since it is consistent with the principle of d’Alembert:

$$\widetilde{\mathit{\tau } }{=\tilde{\mathit{M}}}^{1/2}{\left({\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}{\tilde{\mathit{M}}}^{1/2}\right)}^{+}·\left({\ddot{\mathit{s}}}_r-{\dot{\mathit{J}}}_i\mathit{u}{+\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}\tilde{\mathit{C}}\right)$$

(27)

Now, the definition of the reference control, ${\ddot{\mathit{s}}}_{\mathrm{r}}$, is described considering the eye-in-hand camera system that extracts a set of k image feature points. The task description as a constraint is given by the following equation in the image space.

$$\left({\ddot{\mathit{s}}}_d-\ddot{\mathit{s}}\right)+{\mathit{K}}_D\left({\dot{\mathit{s}}}_d-\dot{\mathit{s}}\right)+{\mathit{K}}_P\left({\mathit{s}}_d-\mathit{s}\right)=0$$

(28)

where ${\ddot{\mathit{s}}}_d$, ${\dot{\mathit{s}}}_d$, and ${\mathit{s}}_d$ are the desired image space accelerations, velocities, and positions, respectively. ${\mathit{K}}_P$ and ${\mathit{K}}_D$ are proportional and derivative gain matrices, respectively. This equation can be expressed in regards to image error in the following way.

$${\ddot{\mathit{s}}}_d{+\mathit{K}}_D{\dot{\mathit{e}}}_s{+\mathit{K}}_P{\mathit{e}}_s={\ddot{\mathit{s}}}_r$$

(29)

where ${\mathit{e}}_s$ and ${\dot{\mathit{e}}}_s$ are the image error and the time derivative of the image error, respectively. As stated, the variable ${\ddot{\mathit{s}}}_r$ denotes the reference image accelerations of the proposed image space based controller. Substituting this variable into the dynamic visual servo controller in Equation (25), the control law is set by the following relationship.

$$\tilde{\mathit{\tau }}{=\mathbf{W}}^{-1/2}{\left({\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}{\mathbf{W}}^{-1/2}\right)}^{+}·\left({\ddot{\mathit{s}}}_d{+\mathit{K}}_D{\dot{\mathit{e}}}_s{+\mathit{K}}_P{\mathit{e}}_s-{\dot{\mathit{J}}}_i\mathit{u}{+\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}\tilde{\mathit{C}}\right)$$

(30)

In order to demonstrate the asymptotic tracking of the control law, some operations must be done. First, the closed-loop behavior is computed from Equation (19) as

$$\tilde{\mathit{M}}\dot{\mathit{u}}+\tilde{\mathit{C}}{=\mathbf{W}}^{-1/2}{\left({\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}{\mathbf{W}}^{-1/2}\right)}^{+}·\left({\ddot{\mathit{s}}}_d{+\mathit{K}}_D{\dot{\mathit{e}}}_s{+\mathit{K}}_P{\mathit{e}}_s-{\dot{\mathit{J}}}_i\mathit{u}{+\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}\tilde{\mathit{C}}\right)$$

(31)

Equation (31) can be simplified by premultiplying its left and right sides by $\left({\mathit{J}}_i{\tilde{\mathit{M}}}^{-1}{\mathbf{W}}^{-1/2}\right){\mathbf{W}}^{1/2}$:

$${\mathit{J}}_i\dot{\mathit{u}}={\ddot{\mathit{s}}}_d{+\mathit{K}}_D{\dot{\mathit{e}}}_s{+\mathit{K}}_P{\mathit{e}}_s-{\dot{\mathit{J}}}_i\mathit{u}$$

(32)

Finally, using the relationship expressed in (23), it can be concluded that

$${\ddot{\mathit{e}}}_s=-{\mathit{K}}_D{\dot{\mathit{e}}}_s-{\mathit{K}}_P{\mathit{e}}_s$$

(33)

Therefore, when ${\mathit{J}}_i$ is full-rank, an asymptotic tracking is achieved by the visual servo controller expressed in Equation (30) for the tracking of an image trajectory.

6. Results

This section presents the system behavior during the tracking of different trajectories. The robot is guided by an eye-in-hand camera system. The parameters of a Gigabit Ethernet TM6740GEV camera is considered, which acquires 200 images every second with a resolution of 1280 × 1024 pixels. The eye-in-hand camera extracts four visual features from the four corners of the target.

Three different experiments are presented in this section. In these experiments the value of W = M⁻¹ is considered. Figure 4a,b represents the initial configuration employed in all of the experiments. Figure 4a represents the target and the mobile manipulator with the eye-in-hand camera system. The initial position of the visual features is represented in Figure 4b. The desired trajectory employed in the first experiment is a linear trajectory that represents a lateral translation of 200 px. in x and y directions in the image from the initial position of the visual features. As it can be seen in Figure 4d, the image-based controller correctly carries out the tracking and a straight line is obtained in the image space. Figure 4c represents the final pose of the mobile manipulator. The second experiment, represented in Figure 4e,f, evaluates the tracking when a displacement in depth must be performed. As it can be seen in Figure 4e, a displacement in depth is carried out and, once the experiment ends, the mobile manipulator is closer to the target. Figure 4f represents the obtained image trajectory. This trajectory corresponds to the desired trajectory specified as a linear increase in depth from the initial image configuration. Finally, the experiment represented in Figure 4g,h requires a change in orientation of the eye-in-hand camera with respect to the observed target. To do this, the desired image trajectories for the visual features are represented by a straight line between the initial and the final ones defined as ${\mathit{s}}^{*}={\left[{\mathit{f}}_{1d}^{}=\left(503, 584\right),{\mathit{f}}_{2d}^{}=\left(612, 600\right),{\mathit{f}}_{3d}^{}=\left(523, 472\right),{\mathit{f}}_{4d}^{}=\left(630, 477\right)\right]}^T$ px. As in the previous experiments, Figure 4g represents the final robot pose once the experiment ends and Figure 4h represents the image trajectory. As the experiments show, the proposed controller allows the concurrent guidance of both the manipulator and robotic platform during the tracking of the desired image trajectory.

Figure 5 represents the joint torques applied to the manipulator (m = 1…7) and force and moments applied to the robot platform during the first and second experiments. As it can be seen in Figure 5a (first experiment), the torques, force, and moment remain low, and a smooth behavior is obtained during the tracking. For the second experiment, Figure 5b represent the joint torques applied to the manipulator and force and moment applied to the base platform during the robot motion in depth. It is worth noting that the reduction of the distance between the two bodies (target and robot) is obtained by modifying both the base platform pose and the robot manipulator joint configuration. Finally, as in the previous experiments, Figure 6 represents the joint torques applied to the manipulator and force and moment applied to the base platform during the third tracking experiment. The desired relative orientation is achieved by controlling both the orientation of the base platform and the robot manipulator joint configuration.

One of the main advantages of the proposed controller is the possibility to define new direct image-based controllers by modifying the value of the matrix W of the optimal controller (see Equation (25)). A different controller with different dynamic behavior during the tracking of image trajectories can be obtained by modifying this matrix. In order to evaluate the proposed framework, in

Table 1. Mean image error in pixels (mm) during the tracking of the image trajectory.

$\mathbf{W}$	$\omega =0.5\mathrm{rad}/\mathrm{s}$	$\omega =1\mathrm{rad}/\mathrm{s}$	$\omega =2\mathrm{rad}/\mathrm{s}$
$W={\tilde{\mathit{M}}}^{-2}$	2.14 px (0.59 mm)	2.21 px (0.61 mm)	3.84 px (1.04 mm)
$W={\tilde{\mathit{M}}}^{-1}$	2.12 px (0.58 mm)	2.95 px (0.8 mm)	2.39 px (0.66 mm)
$W=\mathit{I}$	3.81 px (1.03 mm)	3.22 px (0.87 mm)	4.4 px (1.2 mm)
Previous	2.25 px (0.62 mm)	3.2 px 0.85 mm)	4.12 px (1.09 mm)

we represent the obtained image error during the tracking of a repetitive image trajectory defined by the following equation.

$${\mathit{s}}_d=\left[\begin{array}{c}{\mathit{f}}_{xd}\\ {\mathit{f}}_{yd}\end{array}\right]=\left[\begin{array}{c}320+166cos\left(\omega t+\pi /4\right)\\ 265+160sin\left(\omega t+\pi /4\right)\end{array}\right]$$

(34)

Table 1 represents the mean error in pixels and mm during the tracking of the image trajectory considering different tracking velocities. As it can be seen in this table, different tracking errors are obtained and the best behavior at high velocities is obtained considering $W={\tilde{\mathit{M}}}^{-1}$. Additionally, the performance of the controllers is compared with respect to previous controllers only based on inverse dynamics [22] (indicated as “previous” in Table 1).

7. Conclusions

This paper presents a direct image-based visual servoing system for the guidance of mobile manipulators. This approach is based on an optimal framework that considers the optimization of the motor signals sent to the mobile manipulator during the tracking of image trajectories. The stability of the controller has been proved analytically, and different dynamical behavior can be obtained by using different values of the matrix W.

As a result, an adaptive and flexible tool has been obtained, allowing for concurrent control of both the base platform and manipulator during the tracking of trajectories. With the given definition of A and b, the optimal control can minimize the motor commands while performing the tracking of trajectories in the image space. The viability of the controller has been tested in a variety of test case experiments, including approaches to an object, as well as different positioning tasks that require modifying the position and/or the orientation of the mobile manipulator. In all such cases, the experiments and trajectories have been accomplished.