Simultaneous Calibration of Odometry and Head-Eye Parameters for Mobile Robots with a Pan-Tilt Camera

Abstract

In the field of robot navigation, the odometric parameters, such as wheel radii and wheelbase length, and the relative pose of the optical sensing camera with respect to the robot are very important criteria for accurate operation. Hence, these parameters are necessary to be estimated for more precise operation. However, the odometric and head-eye parameters are typically estimated separately, which is an inconvenience and requires longer calibration time. Even though several researchers have proposed simultaneous calibration methods that obtain both odometric and head-eye parameters simultaneously to reduce the calibration time, they are only applicable to a mobile robot with a fixed camera mounted, not for mobile robots equipped with a pan-tilt motorized camera systems, which is a very common configuration and widely used for wide view. Previous approaches could not provide the z-axis translation parameter between head-eye coordinate systems on mobile robots equipped with a pan-tilt camera. In this paper, we present a full simultaneous mobile robot calibration of head–eye and odometric parameters, which is appropriate for a mobile robot equipped with a camera mounted on the pan-tilt motorized device. After a set of visual features obtained from a chessboard or natural scene and the odometry measurements are synchronized and received, both odometric and head-eye parameters are iteratively adjusted until convergence prior to using a nonlinear optimization method for more accuracy.

1. Introduction

Robot navigation is one of the key challenges facing the field of mobile robotics. This is because mobile robots are required to drive themselves through a given environment using the information gathered from their sensors. These sensors include proprioceptive sensors such as motor-speed sensors, wheel-load sensors, joint-angle sensors, battery-voltage sensors and the inertial measurement unit (IMU), which produces a type of data called odometric data. Additionally, these robots are fitted with exteroceptive sensors such as image-feature sensors, distance sensors, light-intensity sensors, sound-amplitude sensors, and global positioning system (GPS) sensors. However, robot localization is typically inaccurate due to the uncertainty associated with measurement errors during robot configuration. Although the robot configuration data, such as wheel radii and wheelbase length, can be obtained simply from the robot specifications or by manual measurement, the actual parameters can be dissimilar in practice. This is due to systematic errors such as manufacturing errors, assembly errors, tire pressure variations, and load variations that reduce the precision associated with the movement of the mobile robot. It is therefore necessary to estimate these odometric parameters to improve the robot’s operational precision.

Antonelli’s calibration method [1] uses the least-squares technique to observe a linear mapping between the unknowns and the measurements. It aims to identify a 4-parameter model, while the modified version [2] estimates the physical odometric parameters and yields a 3-parameter model without any requirement for a predefined path. Some researchers, such as [3,4], have reduced the cumulative error in odometry by considering the coupled effect of errors in diameter, wheelbase errors, and scaling errors using a popular odometry calibration method for wheeled mobile robots, developed at and named the University of Michigan Benchmark test (UMBmark) [5]. Some trials required a static wireless sensor network and GPS devices to be equipped on mobile robots to correct for the odometry errors [6,7]. However, this approach is disadvantageous in terms of cost-effectiveness.

Nowadays, mobile robots equipped with cameras which provide single color images, stereo images, or depth images are widely used in the fields of robot navigation, reconstruction, and mapping. Using this information, the robot can perform more precise and varied tasks. However, the relationship between the camera and robot in terms of 3D position and orientation is also important for accurate operation. Shiu and Ahmad [8] introduced a solution for rigid transformation between the sensor and the robot, calculated in the form AX = XB. Hand-eye calibration, as proposed by Tsai and Lenz [9], is a similar, albeit more efficient solution which does not depend on the number of images. In mobile robots, the 3D position and orientation of the camera relative to the robot’s base is considered instead. Kim et al. [10] found that the head-eye transformation between the robot’s coordinate system and the camera’s coordinate system can be estimated simply and accurately by using the minimum variance technique, which is resistant to noisy environments.

In practical applications, the odometric parameters, such as wheel radii and wheelbase length, and the head-eye parameters are typically estimated separately, which requires a longer calibration time and increased inconvenience due to redundancy in these methods. To avoid the disadvantages, Antonelli et al. [11] have proposed a simultaneous calibration method that performs both odometry and the head-eye calibrations simultaneously. Since the method required only the synchronized measurement of odometric data and visual features, it was successful in terms of reducing the calibration time and improving efficiency in the mobile robot calibration. However, their approach is only applicable to the mobile robot on which a fixed camera is mounted, while recent mobile robots are equipped with pan-tilt motorized camera systems for wide view. This caused incomplete estimation of the head-eye parameters, that is, the method could not provide the z-axis translation parameter. Shusheng Bi et al. [12] presented an improved version of [11] in terms of accuracy, but still did not overcome the problem. Hengbo Tang et al. [13] solved the problem by taking the advantage of the planar constraints of the landmarks. Despite their accurate estimation of the head-eye parameters and the odometric parameters, there is a clear limitation in the sense that a very constrained environment is needed and several recognizable landmarks must be premeasured and fixed.

In this paper, we present a full mobile robot calibration of head-eye and odometric parameters, building on [14]. The full six parameters (rotation and translation) in the head-eye calibration and the three parameters (wheel radii and wheelbase length) in the odometry calibration are simultaneously estimated. The mobile robot equipped with a mono or stereo camera moves while the camera mounted on the pan-tilt motorized device is capturing chessboards or natural scenes. After simply planned robot movements, the full mobile robot calibration algorithm is performed using both odometry measurements and visual features such as chessboard’s corners or natural feature points from a stereo camera, which are obtained by Speeded-Up Robust Feature (SURF) [15]. The head-eye and odometric parameters are iteratively adjusted to obtain the values, which are searched as a good starting point close to the ground truth please confirm intended meaning is retained. and then finally fine-tuned with the direct search-based optimization as Powell’s method [16]. The remainder of this paper is organized as follows: Section 2 describes our mobile robot configuration and the relationship between each joint. Section 3 proposes an iterative-based calibration method for mobile robot having a camera mounted on a motorized neck. Section 4 presents our experimental results, and the conclusion is summarized in Section 5.

2. Mobile Robot Configuration

2.1. Robot Coordinate System

The mobile robot configuration in Figure 1a can be understood in terms of an overview of the coordinate system of a mobile robot with a pan-tilt camera, as depicted in Figure 1b. It consists of the vehicle coordinate (robot’s base), $O_{Vehicle}$; the neck coordinate includes a pan-tilt joint, ($O_{Neck}$), with a camera mounted on the top as camera coordinate, $O_{Cam}$. The relation between the vehicle system $O_{Vehicle}$ and $O_{Neck}$ is estimated using Denavit–Hartenberg parameters (DH parameters) depending on the mobile robot configuration. The rotation from the neck to camera and the rotations between robot’s bases are estimated in Section 3.2. The remain parameters are calculated in Section 3.4.

2.2. Robot Wheel Parameters

In the field of robot navigation, one of the important parameters for mobile robot calibration is the wheel parameters, which are the radii of the left and right robot wheels and the baseline (the axle length between left and right wheel), as shown in Figure 2. The robot kinematics can be expressed as

$$\begin{array}{cc}\hfill \dot{x}& =\upsilon cos\left(\theta \right),\hfill \\ \hfill \dot{y}& =\upsilon sin\left(\theta \right),\hfill \\ \hfill \dot{\theta }& =\omega ,\hfill \end{array}$$

(1)

where $\upsilon$, $\omega$, and $\theta$ are the velocity, angular velocity, and the orientation of the mobile robot, respectively, as depicted in Figure 2. These parameters can be obtained by using the following equation:

$$\begin{array}{cc}\hfill \upsilon & =\frac{r_R}{2}{\omega }_R+\frac{r_L}{2}{\omega }_L,\hfill \\ \hfill \omega & ={\alpha }_R{\omega }_R+{\alpha }_L{\omega }_L,\hfill \end{array}$$

(2)

where ${\alpha }_R=\frac{r_R}{b}$ and ${\alpha }_L=-\frac{r_L}{b}$. The wheel parameters as $r_R$, $r_L$, and b are right and left wheel radii and the length of the baseline, respectively. ${\omega }_R$ and ${\omega }_L$ are angular velocities, which are calculated using the encoder on the right and left wheels. The ratio between the radii of wheels and the length of wheelbase is represented in terms of intermediate parameters, (${\alpha }_R,{\alpha }_L$), which are used in the calibration process instead of the real wheel parameters.

The rotational angle of the mobile robot from frame i to frame j, by $t_i$ = 0 and $t_j$ = t, can be obtained through the integral of Equation (2) with respect to time as follows:

$$\begin{array}{cc}\hfill \theta \left(t\right)& ={\alpha }_R{\int }_0^t{\omega }_R\left(\tau \right)\mathrm{d}\tau +{\alpha }_L{\int }_0^t{\omega }_L\left(\tau \right)\mathrm{d}\tau ,\hfill \\ & ={\alpha }_R{\varphi }_R\left(t\right)+{\alpha }_L{\varphi }_L\left(t\right),\hfill \end{array}$$

(3)

where ${\varphi }_R\left(t\right),{\varphi }_L\left(t\right)$ are the encoder positions of the right and left wheels, respectively.

3. Simultaneous Calibration for Mobile Robot with Pan-Tilt Camera

In this section, we describe the proposed calibration method separately in six parts. The closed-loop transformation of the camera and the robot base to the robot neck between any frames i and j, where i = 1, …, N − 1 and j = i + 1 are concisely depicted in Section 3.1. Since a set of captured images, $I_{n=1}^{n=N}$, and the calibration data set (such as rotating angles of wheels (${\varphi }_{R_i},{\varphi }_{L_i}$), and transformation from robot base coordinate to robot’s neck coordinate, $T_{N_i}^{V_i}$) are obtained once as the calibration input data. Section 3.2, Section 3.3, and Section 3.4 explain how to use these data to obtain the head-eye rotation, intermediate wheel parameters, and head-eye translation, including the actual wheel parameters, respectively. These processes are estimated iteratively until the value of all parameters converge, as described in Section 3.5. Finally, Section 3.6 describes the non-linear optimization method that increases the accuracy of the calibration results.

3.1. Closed-Loop Transformations

Let us now consider the homogeneous transformation of different vehicle poses from frame i to j, as shown in Figure 3. The abbreviations $O_C,O_N,O_V$ are the camera, neck, and vehicle coordinate systems, respectively. The closed-loop diagram can be represented by Equation (4).

$$T_{N_i}^{V_i}{T}_{C_i}^{N_i}{T}_{C_J}^{C_i}=T_{V_j}^{V_i}{T}_{N_j}^{V_j}{T}_{C_j}^{N_j},$$

(4)

where $T_N^V$ is the physical relationship between $O_{Neck}$ and $O_{Vehicle}$. The head-eye homogeneous transformation is $T_C^N$. When the camera is directed at the same target or feature, camera motion is represented as $T_{C_j}^{C_i}$. The robot’s motion between frame i and j is presented to $T_{V_j}^{V_i}$. The homogeneous transformation of Equation (4) can be decomposed into rotational and translational terms as follows:

$$R_{N_i}^{V_i}{R}_{C_i}^{N_i}{R}_{C_j}^{C_i}=R_{V_j}^{V_i}{R}_{N_j}^{V_j}{R}_{C_j}^{N_j},$$

(5)

$$R_{N_i}^{V_i}{R}_{C_i}^{N_i}{t}_{C_j}^{C_i}+R_{N_i}^{V_i}{t}_{C_i}^{N_i}+t_{N_i}^{V_i}=R_{V_j}^{V_i}{R}_{N_j}^{V_j}{t}_{C_j}^{N_j}+R_{V_j}^{V_i}{t}_{N_j}^{V_j}+t_{V_j}^{V_i},$$

(6)

where R is the $3\times 3$ rotation matrix and t is $3\times 1$ translation vector. Equation (5) is used to obtain the head-eye rotation parameters, $R_C^N$, which consist of 3 degrees of freedom (DOF), $({\gamma }_x,{\gamma }_y,{\gamma }_z$). Equation (6) refers to the translation of the system, which is used to estimate the head-eye translation ($t_{C,x}^N,t_{C,y}^N,t_{C,z}^N$) and the actual size of the wheel parameters ($r_R,r_L,b$). These parameters are calculated in Section 3.4.

3.2. Head-Eye Rotation Estimation

The six parameters in head-eye calibration consisting of three for rotation and another three for the translation, which are necessary and required to be obtained before an operation. In this section, three parameters of the rotation between the robot’s neck and the camera are calculated precisely. A set of visual measurements and robot movement data were obtained by moving the robot and capturing images synchronously and continuously. The rotation between the camera and the robot’s base was estimated accurately, as by Antonelli et al. [11]. They obtained the rotation parameters between the robot’s base and camera using Equivalent angle-axis representation. However, their approach achieved because their mobile robot had a camera equipped on a fixed neck that did not change the relation between the robot’s base and camera. If the relation between the base and the camera was changed during the calibration data collection, the Equivalent angle-axis method could not be used to solve the problem. During collecting the input calibration data of our mobile robot, both the robot and its neck move that means the rotation between the base and the camera are also changed.

In fact, whenever the robot’s neck moves around the pan-tilt axis, the coordinate of the camera mounted on that neck is also moved significantly. Therefore, the relationship between the camera and the robot’s neck is static, which means the subscript i of $R_{N_i}^{C_i}$ can be omitted as $R_N^C$ = $R_{N_1}^{C_1}$ = $R_{N_2}^{C_2}$ = … = $R_{N_N}^{C_N}$, where N is the total number of input images. Moreover, the mobile robot rotation is performed only on a planar. In other words, the mobile robot rotates around z-axis only [11]. Hence, $R_{V_j}^{V_i}$, in Equation (5) can be replaced with $R_z\left(\theta \right)$. Assuming the rotation matrices $R_{C_j}^{C_i}$, $R_{V_i}^{N_i}$, and $R_{V_j}^{V_i}$ are known, which are thoroughly described in Section 4, an Equation (5) can be represented in form of AX = XB, and the rotation $R_N^C$ can be obtained likewise [8,9] as follows:

$$\begin{array}{cc}\hfill R_{C_j}^{C_i}{R}_N^C& =R_N^C{R}_{V_i}^{N_i}{R}_z\left(\theta \right)R_{N_j}^{V_j},\hfill \end{array}$$

(7)

where the matrix X is the estimated head-eye rotation, $R_N^C$. The camera rotations, $R_{C_j}^{C_i}$, are represented to matrix A. The remaining variables of the right side $R_{V_i}^{N_i}{R}_z\left(\theta \right)R_{N_j}^{V_j}$ are demonstrated to matrix B, which i = 1, …, $N-1$ and j = 2, …, N.

3.3. Intermediate Wheel Parameters Estimation

In this section, the linear relationships of intermediate wheel parameters, (${\alpha }_R,{\alpha }_L$), and rotational angle of the robot movement from the previous to current positions prior to capture any image i, ${\theta }_j$, which is obtained with Equation (3) using the period time of the robot movement between the previous and current positions, which is $\theta$(1) = 0 as no loss of generality. The change in the rotational angle of the robot’s base from frames i to j, on a planar $\theta \left(t_j\right)$ is also re-estimated. According to the robot’s movement on a plane, the change in rotational angle about the z-axis of the robot’s base coordinates, which is assumed to be perpendicular to the floor, between a pair of consecutive frames is $R_z\left(\theta \right)$. In practical applications, the z-axis at the robot’s base coordinates from one consecutive frame to another, ($V_i$ and $V_j$), may not be parallel because of an error in the estimated $R_C^N$ and the floor plane of any pair of positions are not parallel. Therefore, the rotational angle about the z-axis at the robot’s base coordinates between frames i and j can be calculated using the Euler angle (ZYX) as follows:

$$\begin{array}{cc}\hfill {\theta }_j& =\theta \left(t_j\right)=Atan2(r_{21},r_{11}),R_{V_j}^{V_i}=R_{N_i}^{V_i}{R}_C^N{R}_{C_j}^{C_i}{R}_N^C{R}_{V_j}^{N_j},\hfill \end{array}$$

(8)

where $r_{21}$ and $r_{11}$ are generic elements of $R_{V_j}^{V_i}$. If we consider for N images, the representation of Equation (3) can be used to obtain the parameters, (${\alpha }_R$, ${\alpha }_L$), similar to [11], as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\theta }_1\\ ⋮\\ {\theta }_N\end{array}\right]=\left[\begin{array}{c}{\varphi }_{{\theta }_1}\\ ⋮\\ {\varphi }_{{\theta }_N}\end{array}\right]\left[\begin{array}{c}{\alpha }_R\\ {\alpha }_L\end{array}\right]={\overline{\varphi }}_{\theta }\left[\begin{array}{c}{\alpha }_R\\ {\alpha }_L\end{array}\right],\end{array}$$

(9)

where ${\varphi }_{{\theta }_j}=\left[\begin{array}{cc}{\varphi }_R\left(t_j\right)& {\varphi }_L\left(t_j\right)\end{array}\right]$, are obtained from the rotational angles of both wheels from position i to j, (i = 1, …, N − 1 and j = 2, …, N). ${\overline{\varphi }}_{\theta }$ is a matrix with $N\times 2$ dimensions. The intermediate wheel parameters, (${\alpha }_R$, ${\alpha }_L$), can be calculated using the linear least squares method as follows

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\alpha }_R\\ {\alpha }_L\end{array}\right]={\left({\overline{\varphi }}_{\theta }^T{\overline{\varphi }}_{\theta }\right)}^{-1}{\overline{\varphi }}_{\theta }^T·\left[\begin{array}{c}{\theta }_1\\ ⋮\\ {\theta }_N\end{array}\right]\end{array}$$

(10)

3.4. Head-Eye Translation and Wheel Parameters Estimation

The head-eye rotation and intermediate parameters have already been obtained in Section 3.2 and Section 3.3. The remaining parameters that are estimated in this section are $t_C^N$, the head-eye translation vector, and the actual wheel parameters $r_R,r_L,b$. The translational and the rotational components of the robot’s base coordinates can be described using the mobile robot kinematic equations as

$$\begin{array}{cc}\hfill x_j& =x_i+\tau {\upsilon }_{i}cos\left({\theta }_i+\tau {\omega }_i/2\right),\hfill \\ \hfill y_j& =y_i+\tau {\upsilon }_{i}sin\left({\theta }_i+\tau {\omega }_i/2\right),\hfill \\ \hfill {\theta }_j& ={\theta }_i+\tau {\omega }_i,\hfill \end{array}$$

(11)

where i and j denote the previous and current frames, respectively, and $\tau$ is the period of time between frames. Substituting the intermediate wheel parameters and Equation (2) into Equation (11) yields

$$\begin{array}{cc}\hfill x_j& =x_i+\tau \left(-\frac{{\alpha }_R}{2{\alpha }_L}{r}_L{\omega }_{R,i}+\frac{r_L}{2}{\omega }_{L,i}\right)cos\left({\theta }_i+\tau {\omega }_i/2\right),\hfill \\ \hfill y_j& =y_i+\tau \left(-\frac{{\alpha }_R}{2{\alpha }_L}{r}_L{\omega }_{R,i}+\frac{r_L}{2}{\omega }_{L,i}\right)sin\left({\theta }_i+\tau {\omega }_i/2\right).\hfill \end{array}$$

(12)

In fact, $t_{V_j}^{V_i}$ is the relative translation in x and y directions, which can be rewritten as

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\lambda }_1\\ {\lambda }_2\end{array}\right]& =\frac{\tau }{2}\left[\begin{array}{c}(-\frac{{\alpha }_R}{{\alpha }_L}{\omega }_{R,i}+{\omega }_{L,i})cos\left({\theta }_i+\tau {\omega }_i/2\right)\\ (-\frac{{\alpha }_R}{{\alpha }_L}{\omega }_{R,i}+{\omega }_{L,i})sin\left({\theta }_i+\tau {\omega }_i/2\right)\end{array}\right]r_L,\hfill \\ \hfill t_{V_j}^{V_i}& ={\left[\begin{array}{c}{\lambda }_1{\lambda }_{2}0\end{array}\right]}^T·r_L,\hfill \end{array}$$

(13)

From Equation (13), the translation between two robot base positions can be substituted into Equation (6) representing the relationship between the coordinates as follows:

$$\begin{array}{c}\hfill (R_{N_i}^{V_i}-R_z\left(\theta \right)R_{N_j}^{V_j})t_C^N-{\left[\begin{array}{c}{\lambda }_1{\lambda }_{2}0\end{array}\right]}^T·r_L=-R_{N_i}^{V_i}{R}_C^N{t}_{C_j}^{C_i}-t_{N_i}^{V_i}+R_z\left(\theta \right)t_{N_j}^{V_j},\end{array}$$

(14)

where $R_{N_i}^{V_i}{R}_C^N$ and ($R_{N_i}^{V_i}-R_z\left(\theta \right)R_{N_j}^{V_j}$) are referred to the matrices A and B, respectively. Thus, Equation (14) can be simplified as

$$\begin{array}{c}\hfill \mathrm{B}{t}_C^N-\left[\begin{array}{c}{\lambda }_1\\ {\lambda }_2\\ 0\end{array}\right]r_L=-\mathrm{A}{t}_{C_j}^{C_i}-t_{N_i}^{V_i}+R_z\left(\theta \right)t_{N_j}^{V_j},\end{array}$$

(15)

$$\begin{array}{c}\hfill \left[\begin{array}{ccc}{\mathrm{B}}_{11}& {\mathrm{B}}_{12}& {\mathrm{B}}_{13}\\ {\mathrm{B}}_{12}& {\mathrm{B}}_{22}& {\mathrm{B}}_{23}\\ {\mathrm{B}}_{31}& {\mathrm{B}}_{32}& {\mathrm{B}}_{33}\end{array}\right]\left[\begin{array}{c}{t}_{C,x}^N\\ t_{C,y}^N\\ t_{C,z}^N\end{array}\right]-\left[\begin{array}{c}{\lambda }_1\\ {\lambda }_2\\ 0\end{array}\right]r_L=-\left[\begin{array}{ccc}{\mathrm{A}}_{11}& {\mathrm{A}}_{12}& {\mathrm{A}}_{13}\\ {\mathrm{A}}_{12}& {\mathrm{A}}_{22}& {\mathrm{A}}_{23}\\ {\mathrm{A}}_{31}& {\mathrm{A}}_{32}& {\mathrm{A}}_{33}\end{array}\right]\left[\begin{array}{c}{t}_{C_{j,x}}^{C_i}\\ t_{C_{j,y}}^{C_i}\\ t_{C_{j,z}}^{C_i}\end{array}\right]-\left[\begin{array}{c}{t}_{N,x}^V\\ t_{N,y}^V\\ t_{N,z}^V\end{array}\right]+\left[\begin{array}{ccc}C\theta & -S\theta & 0\\ S\theta & C\theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{t}_{N,x}^V\\ t_{N,y}^V\\ t_{N,z}^V\end{array}\right]\end{array}$$

(16)

from Equation (16), the third component of the vector ${\left[\begin{array}{c}{\lambda }_1{\lambda }_{2}0\end{array}\right]}^T$ is zero, which also makes the third row zero. It can be derived as follows:

$$\begin{array}{c}\left[\begin{array}{cccc}{\mathrm{B}}_{11}& {\mathrm{B}}_{12}& {\mathrm{B}}_{13}& -{\lambda }_1\\ {\mathrm{B}}_{21}& {\mathrm{B}}_{22}& {\mathrm{B}}_{23}& -{\lambda }_2\end{array}\right]\left[\begin{array}{c}{t}_{C,x}^N\\ t_{C,y}^N\\ t_{C,z}^N\\ r_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{A}}_{11}{t}_{Cj,x}^{C_i}+{\mathrm{A}}_{12}{t}_{Cj,y}^{C_i}+{\mathrm{A}}_{13}{t}_{Cj,z}^{C_i}+t_{N,x}^V-C{\theta t}_{N,x}^V+S\theta t_{N,y}^V\\ {\mathrm{A}}_{21}{t}_{Cj,x}^{C_i}+{\mathrm{A}}_{22}{t}_{Cj,y}^{C_i}+{\mathrm{A}}_{23}{t}_{Cj,z}^{C_i}+t_{N,x}^V-S\theta t_{N,x}^V-C\theta t_{N,y}^V\end{array}\right].\hfill \end{array}$$

(17)

Consider Equation (17) over all frames. The final equation can be expressed as

$$\begin{array}{c}\hfill \left[\begin{array}{cccc}{\mathrm{B}}_{11}& {\mathrm{B}}_{12}& {\mathrm{B}}_{13}& -{\lambda }_1\\ {\mathrm{B}}_{21}& {\mathrm{B}}_{22}& {\mathrm{B}}_{23}& -{\lambda }_2\\ ⋮& ⋮& ⋮& ⋮\\ {\mathrm{B}}_{2N1}& {\mathrm{B}}_{2N2}& {\mathrm{B}}_{2N3}& -{\lambda }_{2N}\end{array}\right]\left[\begin{array}{c}{t}_{C,x}^N\\ t_{C,y}^N\\ t_{C,z}^N\\ r_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{A}}_{11}{t}_{Cj,x}^{C_i}+{\mathrm{A}}_{12}{t}_{Cj,y}^{C_i}+{\mathrm{A}}_{13}{t}_{Cj,z}^{C_i}+t_{N,x}^V-C{\theta t}_{N,x}^V+S\theta t_{N,y}^V\\ {\mathrm{A}}_{21}{t}_{Cj,x}^{C_i}+{\mathrm{A}}_{22}{t}_{Cj,y}^{C_i}+{\mathrm{A}}_{23}{t}_{Cj,z}^{C_i}+t_{N,x}^V-S\theta t_{N,x}^V-C\theta t_{N,y}^V\\ ⋮\\ {\mathrm{A}}_{2N1}{t}_{Cj,x}^{C_i}+{\mathrm{A}}_{2N2}{t}_{Cj,y}^{C_i}+{\mathrm{A}}_{2N3}{t}_{Cj,z}^{C_i}+t_{N,x}^V-S\theta t_{N,x}^V-C\theta t_{N,y}^V\end{array}\right],\end{array}$$

(18)

where the left matrix of the left term has $2N\times 4$ dimensions; and the right term is a vector with $2N$ dimensions. N is a total number of frames, $N=1,$ …$,2N$. From Equation (18), the head-eye translation and the actual radii of the left wheel, $(t_{C,x}^N,t_{C,y}^N,t_{C,z}^N,r_L)$, are estimated using a linear least-squares method. The remaining parameters $r_R$ and b can be obtained with Equation (2). The wheel parameters, ($r_L$, $r_R$, b), which are obtained in this section, will be used to re-estimate the rotation between the robot’s head and neck, as previously described.

3.5. Linearly Iterative Estimation

Even though the previous approach [11] could estimate the odometric and head-eye parameters simultaneously, their method could not provide the completed six parameters. The translation of z-axis between head-eye coordinates, $t_{C,z}^N$ was not obtained by their method. The proposed method presents a fully mobile robot calibration of odometric and head-eye parameters, building on [14]. The rotation and translation in head-eye calibration including $t_{C,z}^N$ and the odometric parameters are simultaneously estimated precisely. Supposing that the values of all parameters are not obtained correctly before optimization, it leads all parameters to convergence with incorrect values or divergence. Therefore, this section explains our contribution that we apply iteration-based estimation to initially guess the values of all parameters correctly that leads all parameters to convergence with the correct values rapidly. The processes of Section 3.2, Section 3.3 and Section 3.4 are used to compute repeatedly until all parameters are converged. The head-eye parameters, $R_N^C$, are estimated from Section 3.2 prior to being used to calculate the intermediate wheel parameters, $({\alpha }_R,{\alpha }_L)$, of both wheels in Section 3.3. After that, they are used to compute the remaining parameters, as described in Section 3.4. These results are also used again to calculate the head-eye parameters following Section 3.2, as shown in Algorithm 1, which shows steps 3 to 5 compute repeatedly until convergence.

Algorithm 1 Full algorithm simultaneous calibration for mobile robot with pan-tilt camera.

Input:${\left\{T_{N_i}^{V_i}\right\}}_{i=1}^{i=N},{\{{\varphi }_{R_i},{\varphi }_{L_i}\}}_{i=1}^{i=N},I_{i=1}^{i=N}$

Output:$R_C^N,t_C^N,r_L,r_R,b$

for$i\in 1,$…$,N-1$do

Step 1: Obtain $T_{C_j}^{C_i}$ between each frame using chessboard’s corners or natural features

end for

Step 2: Initial $r_L,r_R,b$ with manual measurements and obtain $\theta$ with Equation (3)

while Convergence do

Step 3: Compute $R_C^N$ with Equation (7)

Step 4: Compute ${\alpha }_R,{\alpha }_L$ with Equations (8)–(10)

Step 5: Compute $t_C^N,r_L,r_R,b$ with Equation (18)

end while

Step 6: Refine $R_C^N,t_C^N,r_L,r_R,b$ with Equation (19)

3.6. Non-Linear Optimization

Even though all parameters that are estimated in the previous section can lead to a good initial estimation, they are probably not the correct and accurate values. Therefore, a method of minimizing a function as Powell’s method [16] is applied to fine-tune all parameters as closely as the ground truth. The variables consisting of the Euler angles of the head-eye rotation, $R_C^N$, (${\gamma }_x,{\gamma }_y,{\gamma }_z$), head-eye translation, ($t_{C,x}^N,t_{C,y}^N,t_{C,z}^N$), and wheel parameters, ($r_L,r_R$, b) are refined using the following equation:

$$\begin{array}{cc}\hfill {\mathsf{\Theta }}^{★}& ={argmin}_{R_C^N,t_C^N,r_L,r_R,b}C\left(\mathsf{\Theta }\right),\hfill \\ \hfill C\left(\mathsf{\Theta }\right)& =\sum _{i=1}^{N-1}\sum _{k=1}^K{\left[Q_{i,k}-Q_{i,k}^{\prime }\right]}^2,\hfill \\ \hfill Q_{i,k}^{\prime }& =T_N^C{T}_{V_i}^{N_i}{T}_{V_j}^{V_i}{T}_{N_j}^{V_j}{T}_C^N{\left[X_j{Y}_j{Z}_{j}1\right]}^T,j=i+1,\hfill \end{array}$$

(19)

where $Q_{i,k}^{\prime }$ is the predicted 3D features, which are used to transform any point k from frame j, $j=i+1$, to frame i. The 3D feature at frame i, represented with $Q_{i,k}$ and $T_C^N$, is constructed using head-eye rotational and translational parameters, (${\gamma }_x,{\gamma }_y,{\gamma }_z,t_{C,x}^N,t_{C,y}^N,t_{C,z}^N$). $T_N^V$ is calculated using DH parameters and pan-tilt data from the encoders; while $T_{V_j}^{V_i}$ is referred to $R_z\left(\theta \right)$, which is calculated using wheel encoder data and the estimated wheel parameters, $r_L,r_R,b$. N and K are the total number of images and features, respectively.

4. Experimental and Results

In our experiments, we used a mobile robot, which had two RGB cameras mounted on the pan-tilt motorized device, as shown in Figure 1. A chessboard and natural scenes were used as the calibration target for single camera and a pair of cameras (a stereo camera), respectively. The differential-drive mobile robot moved to any specified position and both the robot’s neck and pan-tilt axis also moved before capturing an image sequentially. The mobile robot moved and captured repeatedly to obtain a set of images, $I_{n=1}^{n=N}$. The data of the robot movements such as the rotational angles of both wheels (${\varphi }_{R_n}$, ${\varphi }_{L_n}$) and of any image n ( n = 1, …, N) were obtained by their angular velocity (${\omega }_{R_n},{\omega }_{L_n}$) and the movement period time, ${\tau }_i$, as shown in Equation (3). The angles of wheels at the starting position, n = 1, were determined with ${\varphi }_{R_1}$ = ${\varphi }_{L_1}$ = 0. The transformation, including rotation and translation between robot’s base and robot’s neck coordinate systems, $T_{N_{n=1,\dots ,N}}^{V_{n=1,\dots ,N}}$, were calculated with the rotational angles of the pan-tilt axis at the robot’s neck and Denavit–Hartenberg parameters (DH parameters).

In the case that used a chessboard as the calibration target, we captured a set of images using single camera with a resolution of $320\times 240$ pixels. The chessboard contained 10 × 7 black and white square grids (56 corner points), and the size of any grid was $5.4\times 5.4$ cm. We extracted the feature points of the chessboard’s corners manually using [17]. Since all feature points of the chessboard’s corners were obtained, the transformation $T_{C_i}^W$ between the camera at position i and the chessboard, which was determined to be the world coordinate, can be estimated with a plane-based transformation estimation [18]. Therefore, the transformation between any pair of camera positions i and j, $T_{C_j}^{C_i}$, was simply estimated with $T_{C_j}^{C_i}=T_W^{C_i}{T}_{C_j}^W$.

In the case of natural scenes, the natural features were observed from the real environment based on rectified images. The corresponding feature points of the stereo images were estimated with SURF [19]. The transformations between any pair of camera positions, $T_{C_j}^{C_i}$, were obtained by a closed-form solution of the least-squares problem of absolute orientation using orthonormal matrices [20]. The result of stereo matching is shown in Figure 4.

The transformations ($T_C^N$ and $T_C^V$), odometric parameters, and 3D back-projection error results of the proposed and Antonelli’s methods [11] were compared in

Table 1. Results of the proposed method and Antonelli’s method.

Coordinate	Unit	Parameter	Proposed Method	Antonelli’s Method
$T_C^N$	Deg.	${\gamma }_x$	$-45.5715$	-
		${\gamma }_y$	$-82.0160$	-
		${\gamma }_z$	45.4678	-
	mm.	$t_{C,x}^N$	31.5182	-
		$t_{C,y}^N$	$-75.1629$	-
		$t_{C,z}^N$	$-90.0127$	-
$T_C^V$	Deg.	${\gamma }_x$	78.6669	$-62.4582$
		${\gamma }_y$	$-84.3044$	$-87.9764$
		${\gamma }_z$	11.0493	153.6164
	mm.	$t_{C,x}^V$	321.9080	221.4345
		$t_{C,y}^V$	$-89.4305$	$-50.3979$
		$t_{C,z}^V$	969.8691	$unknown$
wheel	mm.	$r_L$	202.4040	225.3074
		$r_R$	200.6111	227.3912
		b	490.4046	513.1268
Error	mm.		4.4239	7.9798

. In the table of $T_C^N$, the head-eye rotation parameters (${\gamma }_x,{\gamma }_y,{\gamma }_z$) were obtained by the ZYX-Euler angle corresponding to the rotation matrix $R_C^N$. The comparison of the head-eye transformation result indicated that the rotation and translation calibrated with the proposed method showed completed parameters estimation, while Antonelli’s method did not obtain the transformation between the camera and the robot neck due to the fact that their mobile robot’s neck could not move.

Furthermore, we also compared the results of the transformation between the robot’s base and the camera, $T_C^V$, by calculation of the transformation at the starting position, $T_{C_1}^{V_1}$, which was obtained by $T_{N_1}^{V_1}$ and $T_C^N$. The transformation matrix, $T_{C_1}^{V_1}$, of the proposed method was similar to Antonelli’s except the translation, $t_{C,z}^V$, that Antonelli’s method could not provide due to the constraining of the origin of the vehicle reference frame on the inertial x-y plane. The error was a 3D back-projection error, which was calculated with the average of Euclidean distance from all 3D feature points between any image and the transformed 3D feature points of other images, which was shown in mm units. Figure 5 also presents the reprojection result between frames of our method.

Even though our method requires an iterative computation in Section 3.5, all parameters reach stability within just a few iterations, as shown in Figure 6. The calibration error after optimization using both chessboard and natural scenes with respect to the number of iterations is shown in Figure 7. However, the 3D back-projection error of the calibration using natural scenes also depends on accuracy of the stereo matching process. Although the back-projection error using natural features is significantly higher than using features from a chessboard, both cases required only a few optimized iterations before the error was steady, which demonstrated the back-projection error of 4.4239 mm, as represented in Table 1. The back-projection error before optimization (iteration = 0) and after optimization of both chessboard and natural scenes related to the number of images are represented in Figure 8. It shows the number of poses that affect to the calibration accuracy. However, the required number of input images using a chessboard as the calibration target is at least 30 input images, while using the natural features requires at least 35 input images for the steady results.

5. Conclusions

In this paper, we presented an approach for simultaneous calibration of head-eye and odometric parameters on the mobile robot equipped with a camera mounted on the motorized pan-tilt. Our proposed approach involves complete estimation of the wheel radii, wheelbase length, and the rotation and translation of the head-eye. Additionally, we obtain comprehensive results of the relative pose between the camera and the robot’s base, showing that our proposed method can compute the translation in z-axis while the previous studies could not. After the data from the visual features of either chessboard’s corners or natural scenes and odometry measurements were acquired, both head-eye and wheel parameters were simultaneously estimated by using iterative adjustment until all parameters converged—the experimental results showed a few iterations were necessary for the convergence. Furthermore, nonlinear optimization is used to minimize the cost function to more sufficiently and appropriate to perform on the mobile robot equipped with a pan-tilt camera precisely.