Hybrid Collaborative Stereo Vision System for Mobile Robots Formation

Abstract

This paper presents the use of a hybrid collaborative stereo vision system (3D-distributed visual sensing using different kinds of vision cameras) for the autonomous navigation of a wheeled robot team. It is proposed a triangulation-based method for the 3D-posture computation of an unknown object by considering the collaborative hybrid stereo vision system, and this way to steer the robot team to a desired position relative to such object while maintaining a desired robot formation. Experimental results with real mobile robots are included to validate the proposed vision system.

1. Introduction

Artificial vision systems have been widely used as external sensors in mobile robotics applications due to the large amount of information that they can offer. For this reason, they have nowadays become the most used sensors in tasks such as surveillance, search, exploration, rescue, mapping, obstacle detection and they have been used for autonomous navigation (Carelli et al., 2006b; Couto et al., 2008; Koyasu et al., 2001; Koyasu et al., 2002; Nebot & Cervera, 2008; Okamoto & Grassi, 2002; Soria et al., 2007; Toibero et al., 2009; Vassallo et al., 2005). Moreover, the use of two or more cameras simultaneously gives the system the 3D perception, allowing it to successfully perform different tasks in completely unknown environments. Additional advantages could be obtained by including onmidirectional cameras into the system (Correa & Okamoto, 2005; Gluckman et al., 1998, Koyasu et al., 2001; Koyasu et al., 2002). These cameras allow increasing the horizontal visual field up to 360°, but loosing image resolution. Another choice is to combine omnidirectional cameras with perspective-transformation cameras constructing a new hybrid stereo vision system, which have the advantages of both above mentioned kind of vision cameras (Adorni et al., 2001; Sturm, 2002). It is a well known fact that many tasks could be performed more efficiently by considering two or more robots working cooperatively (De la Cruz & Carelli, 2008; Carelli et al., 2006a; Das et al., 2002; Fierro et al., 2002; Renaud et al., 2004; Roberti et al., 2007; Tanner & Kumar 2005; Toibero et al., 2008). A similar situation could be thought for the stereo vision system, where each camera is mounted on a different robot introducing a new collaboration degree between the robots in the team. Hence, the robots not only execute a cooperative task but also help to perform a collaborative environmental sensing which is necessary to carry out the task. This concept of vision sensors distribution among the different robots in the team not only reduces the computational effort by dividing image processing tasks, but also allows introducing a reconfigurable stereo vision system able to be adapted to the varying conditions imposed by the robot surroundings (Zhu et al., 2004; Cervera, 2005).

In this paper it is considered the use of a collaborative hybrid stereo vision system (3D distributed visual sensing using different types of vision cameras) for the autonomous navigation of a mobile robot team. It is proposed a triangulation-based method for the posture computation of an unknown object in the tridimensional space by using the hybrid collaborative stereo vision system, and the steering the robot team to a desired goal position relative to such object (Soria et al., 2007) while maintaining a desired robot formation by a centralized formation control algorithm (Roberti et al., 2007).

Some previous papers consider collaborative visual sensing in robotics. In (Hajjawi & Shirkhodaie, 2002) one robot pick up an object using the information obtained by the camera of another robot of the multi-robot system. In (Spletzer et al., 2001) the catadioptric vision system of each robot is used to obtain the relative position between the robots in a decentralized scheme. Closer to our work, (Zhu et al., 2004) and (Cervera, 2005) consider reconfigurable stereo vision systems. In the work of (Zhu et al., 2004) it is introduced a reconfigurable vision system composed by two onmidirectional cameras and it is proposed its use to perform a human being following task within a surveillance scope. In the work of (Cervera, 2005) the vision system is integrated exclusively by perspective transformation cameras and exposes its use in object-manipulation tasks by considering a mobile manipulator. Different from these papers, our work proposes the construction of a hybrid collaborative vision system and its use for the autonomous navigation of a robot team.

The remainder of this paper is organized as follows. Section 2 summarizes the different vision systems models employed along the paper and presents the proposed hybrid stereo vision system. Section 3 explains the collaborative sensing implementation. Sections 4 and 5 deal with the control strategies considered for the autonomous robot team navigation. Section 6 exposes the experimental results obtained and finally, Section 7 states conclusions and describe future related works.

2. Vision system models

A vision camera transforms a 3D space into a 2D projection on the image plane, where the vision sensor is located. This projection causes the lost of the depth perception, which means that each point on the image plane corresponds to a ray in the 3D space.

2.1. PerspectivePprojection Camera Model

Several projection models for the representation of the image formation process have been proposed. The most used is the perspective projection model or “pin-hole” model. In this model, a coordinate system 〈0_P, ^PX, ^PY, ^PZ〉 attached to the camera is defined in such a way that the X and Y axes define a base for the image plane and the Z axis is parallel to the optic axis. The origin of the framework 〈0_P, ^PX, ^PY, ^PZ〉 is located at the focus of the camera lens. From Fig.1.a, a fixed point P in the 3D space with coordinates P = [X_P Y_P Z_P]^T on the framework attached to the perspective camera will be projected on the image plane as a point with coordinates ^Pξ = [u_P v_P]^T given by,

$${}^{\mathrm{P}}\mathit{\xi }=\frac{f_{\mathrm{P}}}{Z_{\mathrm{P}}}\left[\begin{array}{c}{X}_{\mathrm{P}}\\ Y_{\mathrm{P}}\end{array}\right]$$

(1)

Fig. 1.

a) Perspective projection camera model; 1. b) Catadioptric vision system

where f_P is the focal length of the camera expressed in pixels.

2.2. Omnidirectional Vision System

Omnidirectional vision systems allow obtaining 360° field-of-view images. These images could be obtained by using a single camera rotating around its own Y-axis, by using multiple cameras, each one oriented in a different direction or by using a catadioptric system (Yagi, 1999). Central catadioptric cameras combine convex reflective surfaces and perspective cameras with the aim of attaining a 360° field-of-view with a single image. These vision systems are formed by mounting a camera in front of a convex mirror in such a way that the camera sees the image reflected by the mirror (Baker & Nayar, 1998), as Fig.1b shows. The catadioptric camera used in this work was built with a conventional perspective projection CCD colour camera and a hyperbolic mirror, by aligning the camera optic axis with the mirror axis, and making the optic centre of the camera coincident with the focus F' of the hyperbola (Svoboda et al. 1998).

In the framework 〈0_ᴏ, ^ᴏX, ^ᴏY, ^OZ〉 attached to the perspective camera of the catadioptric system, the equation that describes the hyperbolic mirror geometric is,

$$\frac{(z-e{)}^2}{a^2}-\frac{x^2}{b^2}-\frac{y^2}{b^2}=1;\text{with}e=\sqrt{a^2+b^2}$$

(2)

2.3. Hybrid stereo vision system

Both vision systems briefly described in the above sections could be combined with the aim of constructing a stereo vision system in order to obtain depth perception (Adorni et al., 2001). With this stereo vision system, it will be possible to get the 3D coordinates of an object without any previous knowledge about it. The structure of the proposed vision system is shown in Fig. 2.

Fig. 2.

Stereo vision system

In Fig. 2 the coordinates of the relevant points are,

$$\begin{array}{rl}& {}^{\mathrm{O}}\mathbf{F}=[002e{]}^T\\ & {}^{\mathrm{O}}\mathbf{P}=[X_{\mathrm{O}}{Y}_{\mathrm{O}}{Z}_{\mathrm{O}}{]}^T\\ & {}^{\mathrm{P}}\mathbf{P}=[X_{\mathrm{P}}{Y}_{\mathrm{P}}{Z}_{\mathrm{P}}{]}^T\\ & {}^{\mathrm{O}}\mathbf{P}=[X_m{Y}_m{Z}_m{]}^T\\ & {}^{\mathrm{O}}\mathit{\xi }=[u_{\mathrm{O}}{v}_{\mathrm{O}}{]}^T\\ & {}^{\mathrm{P}}\mathit{\xi }=[u_{\mathrm{P}}{u}_{\mathrm{P}}{]}^T\end{array}$$

(3)

where ^ᴏF is the focus of the hyperbolic mirror expressed in the omnidirectional vision system framework, ^ᴏP is the point of interest P expressed in the omnidirectional vision system framework, ^ᴏP_m is the point where the ray $\overline{\mathbf{P}\mathbf{F}}$ crosses the mirror expressed in the omnidirectional vision system framework, ^ᴏξ is the projection of ^ᴏP_m on the omnidirectional image plane, ^PP is the point of interest P expressed in the perspective projection camera framework, and ^Pξ is the projection of P on the perspective projection camera image plane.

The objective of this Section is to find the equation system that allows getting the 3D coordinates of the point of interest P in the framework attached to the perspective projection camera. Initially, it is necessary to express the point ^ᴏP_m as a function of the ^ᴏP-coordinates, finding the expression of the ray PF in the omnidirectional vision system framework,

$$\{\begin{array}{l}x=\frac{X_{\mathrm{O}}}{Z_{\mathrm{O}}-2e}z-\frac{X_{\mathrm{O}}2e}{Z_{\mathrm{O}}-2e}\\ y=\frac{Y_{\mathrm{O}}}{Z_{\mathrm{O}}-2e}z-\frac{Y_{\mathrm{O}}2e}{Z_{\mathrm{O}}-2e}\end{array}$$

(4)

and introducing (4) in (2), a second order equation in z, that represent the z-coordinates of the two points where the ray $\overline{\mathbf{P}\mathbf{F}}$ crosses the hyperbole, is obtained

$$\begin{array}{rl}\frac{(z-e{)}^2}{a^2}& \hspace{0.17em}-\frac{{(\frac{X_{\mathrm{O}}}{Z_{\mathrm{O}}-2e}z-\frac{X_{\mathrm{O}}2e}{Z_{\mathrm{O}}-2e})}^2}{b^2}\\ & \hspace{0.17em}-\frac{{(\frac{Y_{\mathrm{O}}}{Z_{\mathrm{O}}-2e}z-\frac{Y_{\mathrm{O}}2e}{Z_{\mathrm{O}}-2e})}^2}{b^2}=1\end{array}$$

(5)

Operating and reorganizing (5),

$$A{z}^2+Bz+C=0$$

(6)

With

$$\begin{array}{c}A=[\frac{1}{a^2}-\frac{X_{\mathrm{O}}^2+Y_{\mathrm{O}}^2}{b^2(Z_{\mathrm{O}}-2e{)}^2}];B=[\frac{2e}{a^2}-\frac{4e{X}_{\mathrm{O}}^2+Y_{\mathrm{O}}^2}{b^2(Z_{\mathrm{O}}-2e{)}^2}];\\ C=[\frac{e^2}{a^2}-\frac{4e^2(X_{\mathrm{O}}^2+Y_{\mathrm{O}}^2}{b^2(Z_{\mathrm{O}}-2e{)}^2}-1]\end{array}$$

Then, (6) can be solved by using

$$z_{1,2}=\mathrm{\Lambda }=\frac{-B\pm \sqrt{B^2-4AC}}{2A}$$

(7)

The value Λ will be the z-coordinate of the point ^ᴏP_m. By introducing Λ in (4), the 3D coordinates of the point ^ᴏP_m can be obtained as functions of ^ᴏP 3D coordinates,

$$\begin{array}{rl}& X_m=\frac{\mathrm{\Lambda }-2e}{Z_{\mathrm{O}}-2e}{X}_{\mathrm{O}}\\ & Y_m=\frac{\mathrm{\Lambda }-2e}{Z_{\mathrm{O}}-2e}{Y}_{\mathrm{O}}\\ & Z_m\mathrm{\Lambda }\end{array}$$

(8)

Now, the pin-hole model of the perspective camera in the omnidirectional system,

$${}^{\mathrm{O}}\mathit{\xi }=\frac{f_{\mathrm{O}}}{Z_{\mathrm{O}}}\left[\begin{array}{c}{X}_{\mathrm{O}}\\ Y_{\mathrm{O}}\end{array}\right]$$

(9)

can be used to get the projection of the point ^ᴏP_m in the omnidirectional image. Next, by introducing (8) in (9), the equations that relate the projection of the interest point P after its reflection on the mirror with its 3D coordinates in the framework attached to the omnidirectional vision system can be obtained,

$$\begin{array}{l}{u}_{\mathrm{O}}=\frac{(\mathrm{\Lambda }-2e)f_{\mathrm{O}}}{(Z_{\mathrm{O}-2e})\mathrm{\Lambda }}{X}_{\mathrm{O}}\\ v_{\mathrm{O}}=\frac{(\mathrm{\Lambda }-2e)f_{\mathrm{O}}}{(Z_{\mathrm{O}-2e})\mathrm{\Lambda }}{Y}_{\mathrm{O}}\end{array}$$

(10)

By taking into account that ^PP and ^OP are the same point of interest represented in two different coordinate systems, it is possible to relate them by

$${}^{\mathrm{P}}\mathrm{P}=\mathbf{R}{[}^{\mathrm{O}}\mathrm{P}-\mathbf{T}]$$

(11)

where R ∈ ℝ^3times3 and T ∈ ℝ^3times1 are the rotation matrix and the translation vector that represent the relative position between both frameworks. Equation (11) can be split in the following three equations,

$$\begin{array}{rl}& X_{\mathrm{P}}=r_{11}[X_{\mathrm{O}}-t_1]+r_{12}[Y_{\mathrm{O}}-t_2]+r_{13}[Z_{\mathrm{O}}-t_3]\\ & Y_{\mathrm{P}}=r_{21}[X_{\mathrm{O}}-t_1]+r_{22}[Y_{\mathrm{O}}-t_2]+r_{23}[Z_{\mathrm{O}}-t_3]\\ & Z_{\mathrm{P}}=r_{31}[X_{\mathrm{O}}-t_1]+r_{32}[Y_{\mathrm{O}}-t_2]+r_{33}[Z_{\mathrm{O}}-t_3]\end{array}$$

(12)

where r _ij and t _i are the elements of the matrix R and the vector T respectively.

Now, X_P, Y_P, X_O and Y_O in (12) can be replaced using (1) and (10), thus obtaining the following three-equation system of three variables (γ, Z_O, Z_P),

$$\begin{array}{rl}& \hspace{0.17em}[r_{11}{u}_{\mathrm{O}}+r_{12}{v}_{\mathrm{O}}]\gamma +r_{13}{Z}_{\mathrm{O}}-\frac{u_{\mathrm{P}}}{f_{\mathrm{P}}}{Z}_{\mathrm{P}}=r_{11}{t}_1+r_{12}{t}_2+r_{13}{t}_3\\ & \hspace{0.17em}[r_{21}{u}_{\mathrm{O}}+r_{22}{v}_{\mathrm{O}}]\gamma +r_{23}{Z}_{\mathrm{O}}-\frac{v_{\mathrm{P}}}{f_{\mathrm{P}}}{Z}_{\mathrm{P}}=r_{21}{t}_1+r_{22}{t}_2+r_{23}{t}_3\\ & \hspace{0.17em}[r_{31}{u}_{\mathrm{O}}+r_{32}{v}_{\mathrm{O}}]\gamma +r_{33}{Z}_{\mathrm{O}}-Z_{\mathrm{P}}=r_{31}{t}_1+r_{32}{t}_2+r_{33}{t}_3\end{array}$$

(13)

where $\gamma =\frac{(Z_{\mathrm{O}}-2e)\mathrm{\Lambda }}{(\mathrm{\Lambda }-2e)f_{\mathrm{O}}}$ .

The three-equation system (13) allows getting the z-coordinate of the point of interest P in both coordinate systems (Z_O, Z_P) and γ parameter. Then, by using (1) and (10), the complete 3D position of the point in both frameworks can be obtained. As it is usual in stereo vision systems, it is necessary to know the extrinsic parameters (R and T), the focal length of the perspective projection camera f_P, and the coordinate of the interest point on the image planes (^Oξ and ^Pξ), which are measured directly from the images using some image processing method. Although the feature extraction and the points' correspondence between images are very interesting problems, they are not addressed and it is considered that ^Oξ and ^Pξ are obtained by some image processing technique.

Remark

Note that (13) has been found considering hyperbolic shape mirror and its application is restricted to this kind of catadioptric vision systems. Nevertheless, it can be generalized for any mirror shape if it is considered the general projection model (Geyer & Daniilidis, 2000). In this case, (13) becomes,

$$\begin{array}{rl}\frac{l(r_{11}{u}_{\mathrm{O}}+r_{12}{v}_{\mathrm{O}})}{l+m}& \mathrm{\Psi }+[r_{13}-\frac{r_{11}{u}_{\mathrm{O}}+r_{12}{v}_{\mathrm{O}}}{l+m}]Z_{\mathrm{O}}\\ & -\frac{u_{\mathrm{P}}}{{\lambda }_{\mathrm{P}}}{Z}_{\mathrm{P}}=r_{11}{t}_1+r_{12}{t}_2+r_{13}{t}_3\\ \frac{l(r_{21}{u}_{\mathrm{O}}+r_{22}{v}_{\mathrm{O}})}{l+m}& \mathrm{\Psi }+[r_{23}-\frac{r_{21}{u}_{\mathrm{O}}+r_{22}{v}_{\mathrm{O}}}{l+m}]Z_{\mathrm{O}}\\ & -\frac{u_{\mathrm{P}}}{{\lambda }_{\mathrm{P}}}{Z}_{\mathrm{P}}=r_{21}{t}_1+r_{22}{t}_2+r_{23}{t}_3\\ \frac{l(r_{31}{u}_{\mathrm{O}}+r_{32}{v}_{\mathrm{O}})}{l+m}& \mathrm{\Psi }+[r_{33}-\frac{r_{31}{u}_{\mathrm{O}}+r_{32}{v}_{\mathrm{O}}}{l+m}]Z_{\mathrm{O}}\\ & -Z_{\mathrm{P}}=r_{31}{t}_1+r_{32}{t}_2+r_{33}{t}_3\end{array}$$

(14)

where l and m are parameters of the general projection model; $\mathrm{\Psi }=\sqrt{X_{\mathrm{O}}^2+X_{\mathrm{O}}^2+Z_{\mathrm{O}}^2},Z_{\mathrm{O}}$ and Z_P are the unknown variables of the three-equation system (14).

2.4. Static experiments for the hybrid stereo vision system

The proposed hybrid stereo vision system was first tested in static experiments under laboratory conditions. The omnidirectional vision system includes a perspective projection camera “Flea” manufactured by Point Grey (resolution: 1024times768 pixels) and a hyperbolic mirror as explained in previous section; and the conventional vision system is a perspective projection camera Sony EVI-D31 (resolution: 640times480 pixels; and f _p = 765 pixels). The relative position adopted between both individual vision systems is defined by,

$$\begin{array}{rl}\mathbf{R}=& \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \frac{\pi }{2}& \sin \frac{\pi }{2}\\ 0& -\sin \frac{\pi }{2}& \cos \frac{\pi }{2}\end{array}\right]\\ & T=\left[\begin{array}{c}90\\ -30\\ -2\end{array}\right]\end{array}$$

With the constructed stereo system, a set of images was acquired. For each pair of images, the interest object was located at different places. After determining the projection of the interest object in both image planes (^ᴏξ and ^Pξ), the proposed equation system (13) is solved in order to determine the 3D position of the object in the framework attached to the perspective projection camera. The results are shown in Figures 3, 4, 5, 6 and 7.

Fig. 3.

3D points reconstruction.

Fig. 4.

Error between the actual and measured positions.

Fig. 5.

Position error between the actual and measured positions of the object in x-coordinate.

Fig. 6.

Position error between the actual and measured positions of the object in y-coordinate.

Fig. 7.

Position error between the actual and measured positions of the object in z-coordinate.

Figure 3 shows the results in the reconstruction of the object position. The position error between the actual position and the position obtained by solving (13) can be seen in Figure 4. This error is calculated as the 3D Euclidean distance between both positions. Figures 5, 6 and 7 show the position error between the actual position of the object and the position obtained by solving (13) in x, y and z coordinates respectively. These Figures show that measurement errors are in most cases fewer than 3 centimiters.

3. Collaborative sensing

The hybrid stereo vision system proposed in the above Section is used as a collaborative sensor in a leader based multi-robot system. The leader robot is equipped with a catadioptric vision system and the follower robot has the perspective projection camera. Both the leader and the follower robot collaborate to obtain information of the environment with the aim of guiding the multi-robot system to a desired final position relative to an unknown object, defined by the distance ρ and the angles ψ and φ as can be seen in Fig.8. These ρ-distance and the two angles ψ and φ are obtained from the (not vertically aligned) unknown object corner positions P₁ and P₂ as,

$$\rho =\frac{1}{2}\sqrt{(x_{\mathrm{P}1}+x_{\mathrm{P}2}{)}^2+(y_{\mathrm{P}1}+y_{\mathrm{P}2}{)}^2}$$

(15)

$$\phi =\arctan \frac{x_{\mathrm{P}2}+x_{\mathrm{P}1}}{y_{\mathrm{P}2}+y_{\mathrm{P}1}}$$

(16)

$$\psi =\phi +\arctan \frac{y_{\mathrm{P}2}-y_{\mathrm{P}1}}{x_{\mathrm{P}2}-x_{\mathrm{P}1}}$$

(17)

Fig. 8.

Robot-object relative posture

where (x_P1; y_P1) and (x_P2; y_P2) are de coordinates of the points P₁ and P₂ obtained through the hybrid stereo vision system.

Therefore, the proposed control system is shown in Fig.9. It includes a formation controller to ensure that the follower robot reaches and keep the desired formation while following the leader; and the leader controller to make it capable to guide the robot team to the desired position relative to the unknown object. Both, the formation controller and the leader controller run in the leader robot's on-board computer. The catadioptric vision system of the leader robot is also used to obtain the follower robot's posture needed in the formation control algorithm.

Fig. 9.

Proposed control system

4. Formation control

The coordinated navigation for the robot team is achieved by considering the formation control proposed by the authors in (Roberti et al., 2007), which allows the robots to reach a specific formation, and to maintain it while the robots navigate in the workspace. This formation controller is based on a centralized leader-following technique, i.e., the leader robot navigates under its own control law sensing the followers' posture (relative to its own reference framework). With this information the leader computes the control actions to be send to each follower in order to successfully accomplish the navigation objective under formation. Follower robots are considered as unicycle-like robots navigating with linear velocity v and orientation α on the coordinate system 〈O, ^LX, ^LY〉 attached to the leader robot. By considering the robot as the punctual object C, the following equation set can describe this movement

$$\begin{array}{c}\dot{x}=v\cos \alpha +\mathrm{\Omega }d\text{sen}\theta \\ \dot{y}=v\text{sen}\alpha -\mathrm{\Omega }d\cos \theta -v^{\mathrm{\prime }}\\ \dot{\alpha }=\omega -\mathrm{\Omega }\end{array}$$

(18)

where v'y Ω are the leader robot linear and angular velocities (and hence, the velocities that rule its associated framework movement); ω is the follower robot angular velocity; d-distance and θ-angle define the follower robot position with respect to the leader robot according to Fig.10.

Fig. 10.

Robot kinematic model

In order to compute an error between the actual positions of each robot and its desired positions in the formation, let ${}^{\mathrm{L}}{\mathit{\zeta }}_i=[x_i{y}_i{]}^{\mathrm{T}}$ be the position vector of the i-th robot, and ${}^{\mathrm{L}}{\mathit{\zeta }}_{di}=[x_{di}{y}_{di}{]}^{\mathrm{T}}$ be the i-th desired position, with i = 1, 2, 3, … n, both vectors are defined on the framework (O, ^LX, ^LY) attached to the leader robot, as Fig.11 shows. For each case, the n position vectors can be arranged in the global vectors: ${}^{\mathrm{L}}\mathit{\zeta }={[{}^{\mathrm{L}}{\mathit{\zeta }}_1^{\mathrm{T}}{}^{\mathrm{L}}{\mathit{\zeta }}_2^{\mathrm{T}}\dots {}^{\mathrm{L}}{\mathit{\zeta }}_n^{\mathrm{T}}]}^{\mathrm{T}}$ and ${}^{\mathrm{L}}{\mathit{\zeta }}_d={[{}^{\mathrm{L}}{\mathit{\zeta }}_{d1}^{\mathrm{T}}{}^{\mathrm{L}}{\mathit{\zeta }}_{d2}^{\mathrm{T}}\dots {}^{\mathrm{L}}{\mathit{\zeta }}_{dn}^{\mathrm{T}}]}^{\mathrm{T}}$ . The difference between the actual and the desired robot's position is,

$${}^{\mathrm{L}}\tilde{\mathit{\zeta }}{=}^{\mathrm{L}}{\mathit{\zeta }}_d{-}^{\mathrm{L}}\mathit{\zeta }$$

(19)

Fig. 11.

Actual and desired positions

The formation error is defined as follows,

$$\begin{array}{c}\tilde{\mathbf{h}}={\mathbf{h}}_d-\mathbf{h}\\ \mathbf{h}=\mathbf{h}{(}^{\mathrm{L}}\mathit{\zeta });{\mathbf{h}}_d=\mathbf{h}{(}^{\mathrm{L}}{\mathit{\zeta }}_d)\end{array}$$

(20)

where h is the output variable, which captures information on the actual conditions of the robot team and h _d represents the desired output variable. The function $\mathbf{h}{(}^{\mathrm{L}}\mathit{\zeta })$ must be defined in such a way that it is continuous and differentiable, and the Jacobian matrix J that relates $\dot{\mathbf{h}}$ with ${}^{\mathrm{L}}\dot{\mathit{\zeta }}$ ,

$$\begin{array}{c}\dot{\mathbf{h}}=\mathbf{J}{(}^{\mathrm{L}}\mathit{\zeta }{)}^{\mathrm{L}}\dot{\mathit{\zeta }}\\ \mathbf{J}{(}^{\mathrm{L}}\mathit{\zeta })=\frac{\mathrm{\partial }\mathbf{h}{(}^{\mathrm{L}}\mathit{\zeta })}{{\mathrm{\partial }}^{\mathrm{L}}\mathit{\zeta }}\in {\mathbb{R}}^{2\mathrm{n}\times 2\mathrm{n}}\end{array}$$

(21)

has full rank. Vector ${}^{\mathrm{L}}\dot{\mathit{\zeta }}$ has two different components, that is ${}^{\mathrm{L}}\dot{\mathit{\zeta }}{=}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_S{-}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_l$ , where ${}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_S$ is the time variation of ^Lζ due to the velocities of the follower robots; and ${}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_l$ is the time variation of ^Lζ due to the velocities of the leader robot. Now, the first equation of (21) can be written as:

$$\dot{\mathrm{h}}=\mathrm{J}({\hspace{0.17em}}^{\mathrm{L}}\zeta )[{\hspace{0.17em}}^{\mathrm{L}}{\dot{\zeta }}_{\mathrm{s}}-{\hspace{0.17em}}^{\mathrm{L}}{\dot{\zeta }}_{\mathrm{l}}]$$

(22)

The control objective is to guarantee that the multi-robot system asymptotically reaches the desired formation defined by h _d . Formally, the designed control system must satisfy that

$$\underset{t\to \mathrm{\infty }}{lim}\tilde{\mathbf{h}}(t)=0$$

First, from (22) a vector of reference velocities for the robots is defined as,

$${}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_r={\mathbf{J}}^{-1}{(}^{\mathrm{L}}\mathit{\zeta })[{\dot{\mathbf{h}}}_d+{\mathbf{K}}_{\tilde{\mathbf{h}}}{\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})]{+}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_l$$

(23)

where ${\mathbf{K}}_{\tilde{\mathbf{h}}}$ is a diagonal and positive definite matrix, ${\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})$ is a continuous saturation function applied to the output error, such that ${\tilde{\mathbf{h}}}^{\mathrm{T}}{\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})>0$ for all $\tilde{\mathbf{h}}\ne \mathbf{0}$ ; for example ${\hspace{0.17em}}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_r$ can be selected as $\mathbf{t}\mathbf{a}\mathbf{n}\mathbf{h}(\tilde{\mathbf{h}})$ . Vector ${\hspace{0.17em}}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_r$ represents the velocities of the follower robots on the framework attached to the leader robot that allows them to reach the desired formation and keep it while following the leader.

Now, assuming perfect velocity tracking, that is ${\hspace{0.17em}}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_r{\equiv }^{\mathrm{L}}{\dot{\mathit{\zeta }}}_S$ , and introducing (23) into (22), the following closed loop equation can be obtained,

$$\dot{\stackrel{~}{\mathbf{h}}}+{\mathbf{K}}_{\tilde{\mathbf{h}}}{\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})=\mathbf{0}$$

(24)

By introducing the following Lyapunov candidate and its time derivative (Slotine & Li, 1991),

$$\begin{array}{c}V=\frac{1}{2}{\tilde{\mathbf{h}}}^{\mathrm{T}}\tilde{\mathbf{h}}\\ \dot{V}={\tilde{\mathbf{h}}}^{\mathrm{T}}\dot{\stackrel{~}{\mathbf{h}}}=-{\tilde{\mathbf{h}}}^{\mathrm{T}}{\mathbf{K}}_{\tilde{\mathbf{h}}}{\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})<\mathbf{0}\end{array}$$

(25)

it is clear that $\tilde{\mathbf{h}}(t)\to 0$ asymptotically. Of course, this condition is verified for the ideal case in which the robots follow exactly the reference velocity. This will not be so for a real controller, that will eventually reach the reference velocity asymptotically. The convergence of the formation error to zero under this real condition will be analyzed at the end of this section.

The control actions for the linear and angular velocities of each robot will be calculated to ensure the robots reach the velocity reference ${\hspace{0.17em}}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_r$ asymptotically. The proposed control law for heading control is,

$${\omega }_{ci}=k_{{\omega }_i}f({\tilde{\alpha }}_i)+{\dot{\alpha }}_{ri}+\mathrm{\Omega }$$

(26)

where ${\tilde{\alpha }}_i={\alpha }_{ri}-{\alpha }_i$ is the angular error between the i-th robot and the heading of its reference velocity; ${\dot{\alpha }}_{ri}$ is the time derivative of the reference velocity heading for the i-th robot; $f({\tilde{\alpha }}_i)$ is a continuous saturation function applied to the angular error, such that ${\tilde{\alpha }}_{i}f({\tilde{\alpha }}_i)$ for all ${\tilde{\alpha }}_i\ne 0$ ; and k_ωi is a positive constant. For instance, $f({\tilde{\alpha }}_i)$ can be selected as $f({\tilde{\alpha }}_i)=\tanh (k_{\alpha i}{\tilde{\alpha }}_i)$ , with k_αi > 0.

By equating (26) with the third equation of (18), the close loop equation becomes,

$$k_{\omega i}f({\tilde{\alpha }}_i)+{\dot{\stackrel{~}{\alpha }}}_i$$

(27)

By introducing the following Lyapunov candidate and its time derivative [13],

$$\begin{array}{c}V=\frac{1}{2}{\tilde{\alpha }}_i^2\\ \dot{V}={\tilde{\alpha }}_i{\dot{\stackrel{~}{\alpha }}}_i=-k_{\omega i}{\tilde{\alpha }}_{i}f({\tilde{\alpha }}_i)<0\end{array}$$

(28)

it can be concluded that ${\tilde{\alpha }}_i\to 0$ as t → ∞.

The proposed control law for the linear velocity is,

$$v_{ci}={|}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_{ri}|\cos {\tilde{\alpha }}_i$$

(29)

which obviously produces that $v_{ci}={|}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_{ri}|$ because it was proven that ${\tilde{\alpha }}_i\to 0$ . The factor $\cos {\tilde{\alpha }}_i$ has been incorporated to attenuate the velocity module correction when a large angular error exists.

In the above controller design it has been proven that ${}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_r{-}^{\mathrm{L}}{\dot{\mathit{\zeta }}}_S=\mathit{\eta }$ with $\mathit{\eta }(t)\to \mathbf{0}$ . Now, (24) can be written as,

$$\dot{\stackrel{~}{\mathbf{h}}}+{\mathbf{K}}_{\tilde{\mathbf{h}}}{\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})=\mathrm{J}\mathit{\eta }$$

(30)

Equation (30) considers a more realistic situation in which the follower robots reach asymptotically the velocity reference instead of assuming perfect velocity tracking, as made before. Considering the following Lyapunov candidate and its time derivative (Slotine & Li, 1991),

$$\begin{array}{c}V=\frac{1}{2}{\tilde{\mathbf{h}}}^{\mathrm{T}}\tilde{\mathbf{h}}\\ \dot{V}={\tilde{\mathbf{h}}}^{\mathrm{T}}\dot{\stackrel{~}{\mathbf{h}}}=-{\tilde{\mathbf{h}}}^{\mathrm{T}}{\mathbf{K}}_{\tilde{\mathbf{h}}}{\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}})+{\tilde{\mathbf{h}}}^{\mathrm{T}}\mathbf{J}\mathit{\eta }<0\end{array}$$

(31)

a sufficient condition for the second equation of (31) to be negative definite is,

$$\parallel \tilde{\mathbf{h}}\parallel >\frac{\parallel \tilde{\mathbf{h}}(0)\parallel \parallel \mathbf{J}\parallel \parallel \mathit{\eta }\parallel }{{\lambda }_{min}({\mathbf{K}}_{\tilde{\mathbf{h}}})\parallel {\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}}(0))\parallel }$$

(32)

where ${\lambda }_{min}({\mathbf{K}}_{\tilde{\mathbf{h}}})$ represents the minimum eigenvalue of $({\mathbf{K}}_{\tilde{\mathbf{h}}})$ ; and $\tilde{\mathbf{h}}(0)$ is the initial formation error. From (18), it can be concluded that the formation error is ultimately bounded as,

$$\parallel \tilde{\mathbf{h}}\parallel \le \frac{\parallel \tilde{\mathbf{h}}(0)\parallel \parallel \mathbf{J}\parallel \parallel \mathit{\eta }\parallel }{{\lambda }_{min}({\mathbf{K}}_{\tilde{\mathbf{h}}})\parallel {\mathbf{f}}_{\tilde{\mathbf{h}}}(\tilde{\mathbf{h}}(0))\parallel }$$

(33)

But, as β(t) → 0, it can be concluded that $\parallel \tilde{\mathbf{h}}\parallel \to 0$ as t → ∞, for any initial formation error $\tilde{\mathbf{h}}(0)$ .

5. Leader robot navigation control

The leader robot navigates accordingly to the control laws proposed by (Soria et al., 2007). This controller generates the linear and angular velocity commands (v' _C and Ω _C ) in order to set its position (and consequently, the whole team) in front of an unknown object. Such commands are obtained as functions of the robot-object relative posture, defined by the distance ρ and the angles ψ and φ, as explained in Section 3.

The control objective is to maintain the robot at certain fixed distance ρ _d behind the object with φ = φ _d considering only the robot-object information provided by the collaborative stereo vision system. This way, some characteristic problems due to odometry errors can be avoided. Nevertheless, the vision system must be fast and precise, guaranteeing the controller quality. Being $\tilde{\rho }={\rho }_d-\rho$ and $\tilde{\phi }={\phi }_d-\phi$ , then the control objective can be specified as

$$\tilde{\rho }(t)\to 0;\tilde{\phi }(t)\to 0\text{with}t\to \mathrm{\infty }$$

(34)

The robot-object relative position evolution will be given by their time derivatives. Where the distance error variation is given by the difference between the velocity projections of the robot (v') and the object (v_T) on the line $\overline{{}^{\mathrm{L}}{\mathbf{O}\mathbf{P}}_3}$ (Fig.8),

$$\dot{\stackrel{~}{\rho }}=v_T\cos \psi -v^{\mathrm{\prime }}\cos \phi$$

(35)

Analogously, the φ-angle variation has three terms: the leader robot angular velocity Ω and the rotational effect produced by the linear velocities of both: the robot and the objective. This could be written as,

$$\dot{\stackrel{~}{\phi }}=\mathrm{\Omega }+v_T\frac{\sin \psi }{\rho }+v^{\mathrm{\prime }}\frac{\sin \phi }{\rho }$$

(36)

Next, it is proposed the following controller which satisfies the control objective (34),

$$\begin{array}{rl}& v_C^{\mathrm{\prime }}=\frac{1}{\cos \phi }(v_T\cos \psi +f_{\rho }(\tilde{\rho }))\\ & {\mathrm{\Omega }}_C=-f_{\phi }(\tilde{\phi })-v_T\frac{\sin \psi }{\rho }-v^{\mathrm{\prime }}\frac{\sin \phi }{\rho }\end{array}$$

(37)

where $f_{\rho }(\tilde{\rho }),f_{\phi }(\tilde{\phi })$ are continuous saturation functions such that xf(x) > 0 for all x ≠ 0. In this work, $f_{\rho }(\tilde{\rho })=k_{\rho }\tanh ({\lambda }_{\rho }\tilde{\rho })$ and $f_{\phi }(\tilde{\phi })=k_{\phi }\tanh ({\lambda }_{\phi }\tilde{\phi })$ , being k_ρ, Λ_ρ, k_φ and Λ_φ, positive constants.

For more details about these control laws and their stability proof, refer to (Soria et al., 2007).

6. Experimental results

In order to validate the proposed method for reconstructing the 3D position of an unknown object, a collaborative sensing experiment using the hybrid stereo vision system was carried out. The experimental setup is a mobile robot team consisting of two mobile robots Pioneer (manufactured by Mobile Robots Inc.). The leader robot has the catadioptric vision system and the follower robot has the conventional perspective projection camera. Figure 12 shows the experimental setup. In the experiment, the team of robots must navigate autonomously maintaining a desired formation (ζ _d = [500 0]^T expressed in millimetres) until they reach a desired posture (ρ _d = 1000 mm and φ _d = 15°) relative to a static unknown object (v _T = 0).

Fig. 12.

Experimental setup

By using the hybrid collaborative stereo vision system, the team gets the posture of the unknown object relative to the leader robot, necessary in (37); and with the catadioptric vision system, the leader robot obtains the posture of the follower robot relative to its own framework, required in (23), (26) and (29). Additionally, the posture of the follower robot allows the leader to determine the matrix R and the vector T that define the relative posture between both vision systems, needed in (13). Figures 13 to 18 show the results of the experiment.

Fig. 13.

Robot-object position error

Fig. 14.

Robot-object angular error

Fig. 15.

Linear and angular commands for the leader robot

Fig. 16.

Formation error

Fig. 17.

Linear and angular commands for the follower robot

Fig. 18.

Trajectories described by the robots

Figure 13 shows the evolution of the distance error $\tilde{\rho }={\rho }_d-\rho$ ; whereas Fig. 14 shows the evolution error $\tilde{\phi }={\phi }_d-\phi$ ; and Fig. 15 shows the linear and angular control actions for the leader robot. On the other hand, Fig. 16 shows the formation control error while Fig. 17 illustrates the linear and angular commands for the follower robot. Finally, the trajectories described by the robots are shown in Fig. 18.

The results obtained show the good performance of the collaborative hybrid stereo vision system proposed for mobile robotics applications.

7. Conclusions

In this paper, it has been presented a collaborative hybrid stereo vision system, i.e., a stereo vision system composed by a perspective transformation camera and a catadioptric camera, each one mounted on different mobile robots. This way, both robots collaborate on the environment information extraction needed to satisfy the proposed control objectives. Also, the stereo vision system can be re-configured with the aim of obtaining the best quantity and the better environment information quality. Furthermore, experimental results that clearly show the good performance of this vision system when applied to the mobile robot navigation have been presented.

Future works on this subject, will address the implementation of collaborative vision systems with more than two cameras, the proposal for new algorithms to compute the best vision system configuration and consequently the desired position of each robot into the desired formation. Furthermore, the consideration of Scale Invariant Features Transform (SIFT) algorithms for the image features extraction and for the determination of correspondence between the points on different images would add robustness to the vision system.