A Switched Approach to Image-Based Stabilization for Nonholonomic Mobile Robots with Field-of-View Constraints

Abstract

This paper presents a switched visual servoing strategy for maneuvering the nonholonomic mobile robot to the desired configuration while keeping the tracked image points in the vision of the camera. Firstly, a pure backward motion and a pure rotational motion are applied to the mobile robot in succession. Thus, the principle point and the scaled focal length in x direction of the camera are identified through the visual feedback from a fixed onboard camera. Secondly, the identified parameters are used to build the system model in polar-coordinate representation. Then an adaptive non-smooth controller is designed to maneuver the mobile robot to the desired configuration under the nonholonomic constraint. And a switched strategy which consists of two image-based controllers is utilized for keeping the features in the field-of-view. Simulation results are presented to validate the effectiveness of the proposed approach.

1. Introduction

External visual sensors are more effective for the closed-loop control of nonholonomic mobile robots than the dead reckoning of internal sensors which can not achieve precise mobile robot poses because of slippage [1,2,3,4]. Visual servoing directly uses the visual information from the onboard camera to design the controller or to estimate the pose of the mobile robot. Existing works on visual servoing of wheeled mobile robots can be mainly formulated as tracking moving objects [5,6,7], tracking given path [8,9,10] and regulating toward a set-point [11,12,13]. Due to the peculiar nature of nonholonomic mobile robots, the set-point stabilization control of the mobile robot is more challenging than tracking problems. This paper deals with the stabilization problem of a nonholonomic mobile robot equipped with a fixed onboard pinhole camera that has a limited field-of-view.

According to Brockett’s necessary condition [14], mobile robots can not be stabilized via smooth time-invariant static feedback control which makes the existing manipulator visual servo controllers fail in mobile robots. Thus new nonlinear strategies have been developed, such as discontinuous controllers [15] and time-varying controllers [16]. Mariottini et al. [11] exploit the epipolar geometry to build the system model and zero the epipoles to align the robot with the goal in a straight line by using an input-output linearizing feedback control law. Then a proportional control law can decrease the translation error in the last stage. Fang et al. [12] utilize the 2D and 3D hybrid method to design a time-varying controller which introduces a sinusoidal function to rebuild the system state. Zhang et al. [13] also decompose the homography matrix to get the system states which are used in discontinuous change of coordinates to cope with the nonholonomic constraints of the mobile robot. Furthermore, a pan-tilt camera is used to deal with the vision constraint. All above control strategies do not need any priori 3D knowledge of the scene. However, the camera intrinsic parameters should be precisely calibrated in advance.

Consider the mobile robots with an uncalibrated camera, some robust visual servo strategies and self-calibration methods have been reported in [17,18,19,20,21]. López-Nicolás et al. [17] develop the control laws for three class optimal paths by using the entries in the homography matrix, while the optimal paths are generated under both the nonholonomic constraints and the field-of-view constraints [22]. Huang et al. [18] directly use the elements of the homography matrix building the error model, while two linear extended state observers are designed to compensate for the parameter uncertainties arising from the unknown 3D information and intrinsic parameters. Li et al. [19] propose a three-stage strategy including a rotation pointing to the goal, a straight line movement toward the goal, and a rotation regulating the desired pose. The intrinsic parameters estimated in the first stage are used in the latter two stages. Besides, Fang et al. [20] provide a geometric method for calibration of the camera principle point which uses the theorem that the parallel lines along the camera optical axis will meet in the principle point. This method is robust to radical distortion and the variation of the focal length. De Luca et al. [21] design a depth observer to estimate the depth online for regulating the mobile manipulator to desired configuration. This method can be extended to estimate the focal length of the camera.

Most visual servoing strategies for nonholonomic mobile robots assume the target remaining in the field-of-view during the regulation process. However, as the limited field-of-view of the pinhole camera, the vision constraint is an ineluctable problem of the mobile robot regulation. Chesi et al. [23] construct a switched approach consists of position-based rotational and translational control laws, and a backward motion for the manipulator, while the strategy is hard to extend to the mobile robot for the nonholonomic constraints. Murrieri et al. [24] present a hybrid-control approach deduced from five different Lyapunov functions with a polar coordinate presentation. The switching conditions are decided by 3D information of the feature points. Gans et al. [25] prove the controllability and stability of the control scheme for the optimal path in [22]. However, Salaris et al. demonstrate the optimal path in [22] is valid locally, which means the initial position should be close to the goal. So a complete synthesis for the whole plane of the motion is presented to extend the region of the initial point [26]. Then the vision constraints are considered not only in the horizontal directions but also in the vertical directions [27], and the $\epsilon$-optimal paths start from each point in the motion region are provided. Ma et al. [28] design a shortest path between the initial pose and desired pose by using a pan-tilt camera to increase the horizontal field-of-view and vertical field-of-view. Karimian and Tron [29] apply two control fields being tangent and normal to ellipsoids to solve the problem of navigation, where the normal field is utilized to adjust the distance between the mobile robot and landmarks to satisfy the field-of-view constraints. All above motion planning research work on the base path generated by a position-based strategy.

In this paper, a switched image-based visual servoing strategy is proposed for the nonholonomic mobile robots equipped with a limited field-of-view camera. Specifically, a pure backward motion is imposed on the mobile robot to get the principal point of the camera. Next, a pure rotational motion is applied to the robot to identify the scaled focal length in the x direction. Then the identified camera intrinsic parameters are used to structure the image features in a polar coordinate presentation which is similar to the 3D pose of the mobile robot in the polar coordinate. Subsequently, an adaptive non-smooth controller is designed under nonholonomic constraints. A switched strategy is designed for keeping the features in the field-of-view, which consists of two image-based control laws. Simulation results are given to show the practicality of the proposed approach. This work has no need for precise calibration of the camera in advance. The main contribution is that two image-based control laws are used to keep the features in view during the regulation of the mobile robot under nonholonomic constraints. During the regulation task, only the visual information from a fixed pinhole camera can be used which makes the proposed approach more practical.

The remainder of the paper is organized as follows. Section 2 formulates the visual stabilization problem with vision constraint in detail. Section 3 identifies the camera principle point and the scaled focal length in x direction step-by-step. Then the estimated parameters are used to build the system model in polar-coordinate. An adaptive non-smooth controller combined with a switched strategy are designed in Section 4. Simulation results in Section 5 shows the effectiveness of this approach.

2. Problem Formulation

The mobile robot with a limited field-of-view camera system and the coordinates are described in Figure 1. The frame ${\mathcal{F}}_c$ of the fixed onboard camera is consistent with the robot frame ${\mathcal{F}}_r$. So the camera is regarded as a part of the mobile robot. Consider a fixed frame ${\mathcal{F}}_w$ which is consistent with the robot frame ${\mathcal{F}}_r^{*}$ when the mobile robot is in the desired configuration. A right-handed frame is defined with origin O and x axis is parallel to the robot wheel axis, z axis is along the camera optical axis, y axis is orthogonal to the motion plane, respectively. The posture state of the mobile robot is described by $X\left(t\right)={(x\left(t\right),z\left(t\right),\theta \left(t\right))}^T$, where $\left(x\right(t),z(t\left)\right)$ are the position coordinates of the mobile robot in the Cartesian plane, and $\theta \left(t\right)$ is the orientation of the mobile robot with respect to z axis as shown in Figure 1. The kinematics of the mobile robot can be expressed as

$$\left\{\begin{array}{c}\dot{x}=vsin\theta \hfill \\ \dot{z}=vcos\theta \hfill \\ \dot{\theta }=w,\hfill \end{array}\right.$$

(1)

where $v,w$ are the linear and angular velocities that are used as the control inputs of the mobile robot.

${\mathcal{P}}_i\in {\mathcal{R}}^3,i=1,2,3,4$ are the static points in the world frame ${\mathcal{F}}_w$. In order to focus on the proposed method and simplify the system modeling, four marker points are used in this work while at least two points have the same height in the y axis direction but not in the camera optical axis plane to avoid the singular structure. In the actual scenario application, the extraction and matching of image points could be carried out using SIFT, SURF and ORB features instead of the marker points. Then the perspective projection geometry for point ${\mathcal{P}}_i$ is derived,

$$\left[\begin{array}{c}{s}_i\left(t\right)\\ 1\end{array}\right]=\frac{1}{Z_{ci}}\left[\begin{array}{cc}K& {\mathbf{0}}_{3\times 1}\end{array}\right]\left[\begin{array}{cc}{R}^T& -R^T{t}_r\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}{\mathcal{P}}_i\\ 1\end{array}\right],$$

(2)

where $s_i\left(t\right)=(u_i,v_i),i=1,2,3,4$ are the image coordinates. $Z_{ci}$ is the depth of the point ${\mathcal{P}}_i$ in the frame ${\mathcal{F}}_c$. R and $t_r$ defined as

$$R=\left[\begin{array}{ccc}c\theta & 0& s\theta \\ 0& 1& 0\\ -s\theta & 0& c\theta \end{array}\right],t_r={(x,0,z)}^T$$

(3)

are the rotation matrix and translation vector of the mobile robot with respect to the world frame, respectively. $s\theta$ and $c\theta$ are the abbreviations of $sin\theta$ and $cos\theta$, respectively. The camera intrinsic matrix K is

$$K=\left[\begin{array}{ccc}{f}_u& 0& u_0\\ 0& f_v& v_0\\ 0& 0& 1\end{array}\right],$$

where $f_u$ and $f_v$ are the scaled focal length in x and y directions, respectively. $(u_0,v_0)$ are the coordinates of the principle point in pixels. Calculating the derivative of the image point in equation (2) with respect to time yields

$${\dot{s}}_i\left(t\right)=\underset{J(s_i,Z_{ci})}{\underbrace{\left[\begin{array}{cc}\frac{u_i-u_0}{Z_{ci}}& -f_u-\frac{{(u_i-u_0)}^2}{f_u}\\ \frac{v_i-v_0}{Z_{ci}}& -\frac{(u_i-u_0)(v_i-v_0)}{f_u}\end{array}\right]}}\left[\begin{array}{c}v\\ w\end{array}\right],$$

(4)

where $J(s_i,Z_{ci})$ is the interaction matrix of an image point.

The desired image is taken at the desired configuration $X^{*}={(0,0,0)}^{T}m$ in advance. The desired image is used to guide the mobile robot from the current configuration to the desired configuration.

3. Self-Calibration of Camera Intrinsic Parameters

3.1. Self-Calibration of the Principle Point

According to the Theorem proposed [20], the image paths of the static feature points will meet in the principle point when the camera moves along the optical axis. To avoid the feature points escaping from the camera view, a pure backward motion is applied to the mobile robot. Four feature points are used so that four straight lines are calculated as

$${\mathcal{L}}_i:a_j{u}_i+b_j{v}_i+c_j=0,j=1,2,3,4.$$

(5)

Since all the straight lines pass through the principle point, the principle point $(u_0,v_0)$ can be calculated through

$${\mathcal{L}}_0:a_j{u}_0+b_j{v}_0+c_j=0,j=1,2,3,4.$$

(6)

Then the lines and the principal point can be obtained by solving the following geometric distance step by step:

$$(a_j,b_j,c_j)=argmin\sum _{i=1}^m{\parallel a_j{u}_i+b_i{v}_i+c_j\parallel }^2$$

(7)

and

$$(u_0,v_0)=argmin\sum _{j=1}^4{\parallel a_j{u}_0+b_j{v}_0+c_j\parallel }^2.$$

(8)

To solve problems (7) and (8), we use CVX which is a package for specifying and solving convex programs [30]. The result is $({\widehat{u}}_0,{\widehat{v}}_0)=(393.6890,285.9458)/pixels$ shown in Figure 2.

3.2. Self-Calibration of the Scaled Focal Length in x Direction

As the interaction matrix in (4) shows, pure rotation of the mobile robot do not depend on the depth but depends on the scaled focal length in x direction. Therefore, an focal length observer designed in [21] is exploited to the camera installed on the mobile robot. Let $\xi =(\overline{u},\overline{v},f_u,\frac{1}{f_u})$ be the state vector, where $\overline{u}=u-u_0,\overline{v}=v-v_0$ are the partially normalized image feature. Assume a pure rotational motion is applied to the mobile robot, the dynamic equations are expressed as

$$\dot{\xi }=\left[\begin{array}{c}-({\xi }_3+{\xi }_1^2{\xi }_4)\\ -{\xi }_1{\xi }_2{\xi }_4\\ 0\\ 0\end{array}\right]w,y=\left[\begin{array}{c}{\xi }_1\\ {\xi }_2\end{array}\right].$$

(9)

Let $\widehat{\xi }\in {\mathcal{R}}^4$ be the estimation of the state $\xi$. By defining $e=\xi -\widehat{\xi }$ as the estimate error vector, the nonlinear observer is designed as

$$\dot{\widehat{\xi }}=\left[\begin{array}{c}-({\widehat{\xi }}_3+y_1^2{\widehat{\xi }}_4)\\ -y_1{y}_2{\widehat{\xi }}_4\\ 0\\ 0\end{array}\right]w+\left[\begin{array}{c}{k}_1{e}_1\\ k_2{e}_2\\ -k_{3}w{e}_1\\ -k_4(y_1^{2}w{e}_1+y_1{y}_{2}w{e}_2)\end{array}\right]$$

(10)

with $k_1,k_2,k_3,k_4\in {\mathcal{R}}^{+}$ are positive parameters. Then we obtain the error dynamics

$$\dot{e}=\underset{H}{\underbrace{\left[\begin{array}{cccc}-k_1& 0& -w& -y_1^{2}w\\ 0& -k_2& 0& -y_1{y}_{2}w\\ k_{3}w& 0& 0& 0\\ k_4{y}_1^{2}w& k_4{y}_1{y}_{2}w& 0& 0\end{array}\right]}}e.$$

(11)

Since a pure rotational motion is imposed on the mobile robot, it ensures $w^2\ge 0$, $y_1^2{y}_2^2{w}^2\ge 0$. The matrix H is a Hurwitz matrix. Thus the exponential convergence of the error system can be guaranteed. The partially normalized image features are obtained by applying the estimated principle point from the previous subsection, which are used in the observer (10). Then the result is ${\widehat{f}}_u=830.5416$/pixels and the evolution is shown in Figure 3.

4. Image-Based Visual Servoing with Limited Field-of-View

4.1. System Model Development

After self-calibration of the camera parameters, a new state variable ${[{\eta }_1,{\eta }_2]}^T$ is defined as

$${\eta }_1=\frac{\overline{u}}{\overline{v}},{\eta }_2=f_u\frac{1}{\overline{v}}$$

(12)

to replace the states of (4). Correspondingly, the new desired state variables are defined as

$${\eta }_1^{*}=\frac{{\overline{u}}^{*}}{{\overline{v}}^{*}},{\eta }_2^{*}=f_u\frac{1}{{\overline{v}}^{*}}.$$

Taking the time derivative of the new state variables and utilizing (4), the kinematics model is obtained as

$$\left\{\begin{array}{c}{\dot{\eta }}_1=-{\eta }_{2}w\hfill \\ {\dot{\eta }}_2=-\frac{{\eta }_2}{Z_{ci}}v+{\eta }_{1}w=-\frac{f_u}{f_v{Y}_{ci}}v+{\eta }_{1}w.\hfill \end{array}\right.$$

(13)

Suppose the two points ${\mathcal{P}}_i$ and ${\mathcal{P}}_j$ have the same height in the y axis direction, i.e., $Y_{ci}=Y_{cj}\triangleq h>0$, the relationship between the current camera configuration and desired configuration can be deduced through the two points as

$${\mathcal{P}}_i^c-{\mathcal{P}}_j^c=R^T\left({\mathcal{P}}_i^d-{\mathcal{P}}_j^d\right)$$

(14)

with ${\mathcal{P}}^c=R^T({\mathcal{P}}^d-t_r)$ where ${\mathcal{P}}^c,{\mathcal{P}}^d$ denote the points in coordinate of the current camera configuration and the desired camera configuration, respectively. R and $t_r$ are the rotation matrix and translation vector defined in (3). It is clear that the new state variables have similar geometrical relations with the static points $\mathcal{P}$. The quantities defined in the image plane ${\eta }_{1ij}=\frac{{\overline{u}}_i}{{\overline{v}}_i}-\frac{{\overline{u}}_j}{{\overline{v}}_j},{\eta }_{2ij}=f_u\frac{1}{{\overline{v}}_i}-f_u\frac{1}{{\overline{v}}_j}$ have the following relation as

$$\left[\begin{array}{c}{\eta }_{1ij}\\ {\eta }_{2ij}\end{array}\right]=\left[\begin{array}{cc}c\theta & -s\theta \\ s\theta & c\theta \end{array}\right]\left[\begin{array}{c}{\eta }_{1ij}^{*}\\ {\eta }_{2ij}^{*}\end{array}\right].$$

(15)

Then the angle $\theta$ is recovered by the following equation: $sin\theta =({\eta }_{1ij}^{*}{\eta }_{2ij}-{\eta }_{2ij}^{*}{\eta }_{1ij})/({\eta }_{1ij}^{*2}+{\eta }_{2ij}^{*2})$. Therefore, the objective of the visual stabilization task becomes the construction of appropriate velocities $v,w$ to ensure that

$${\eta }_1\to {\eta }_1^{*},{\eta }_2\to {\eta }_2^{*},\theta \to 0.$$

(16)

In order to facilitate the subsequent controller design, the error signals are defined as follows

$$\left[\begin{array}{c}{e}_1\\ e_2\end{array}\right]=\left[\begin{array}{cc}c\theta & s\theta \\ -s\theta & c\theta \end{array}\right]\left[\begin{array}{c}{\eta }_1\\ {\eta }_2\end{array}\right]-\left[\begin{array}{c}{\eta }_1^{*}\\ {\eta }_2^{*}\end{array}\right]$$

(17)

and

$$e_0=\theta .$$

(18)

Considering the time derivative of the error signals and substituting (13) into them, the error system is obtained

$$\left\{\begin{array}{c}{\dot{e}}_0=w\hfill \\ {\dot{e}}_1=-cvs\theta \hfill \\ {\dot{e}}_2=-cvc\theta ,\hfill \end{array}\right.$$

(19)

where $c\triangleq \frac{f_u}{f_{v}h}>0$ is utilized to simplify the expression of (19).

The error system (19) has the similar formulations with the mobile robot kinematics model (1). To overcome the effects of the nonholonomic constraints on the mobile robot visual servoing system, a $\sigma$-process is applied to break the one-to-one correspondence of the system. The discontinuous coordinates transformation used [13,31] is introduced as follows

$$\left\{\begin{array}{c}\rho =\sqrt{e_1^2+e_2^2}\hfill \\ \alpha =atan\left(\frac{e_1}{e_2}\right)-e_0\hfill \\ \varphi =atan\left(\frac{e_1}{e_2}\right)\hfill \end{array}\right.$$

(20)

for building the system model. The new states in polar coordinates define a diffeomorphism in the region $\rho \ne 0$. Then taking the time derivative of these new states, the open-loop system model is developed as

$$\left\{\begin{array}{c}\dot{\rho }=-sign\left(e_2\right)cvcos\alpha \hfill \\ \dot{\alpha }=sign\left(e_2\right)\frac{c}{\rho }vsin\alpha -w\hfill \\ \dot{\varphi }=sign\left(e_2\right)\frac{c}{\rho }vsin\alpha .\hfill \end{array}\right.$$

(21)

From the open-loop system (21), it is clear that a singularity problem happens when the state $\rho$ vanishes. To deal with this situation, the control inputs in system (21) are designed to first maneuver the system to a small neighborhood of the desired value which means the mobile robot is close enough to the desired configuration. Then a proportional rotation is utilized to control the angle $\theta$ to zero. Thus the control objective turn to

$$\underset{t\to \infty }{lim}\rho \left(t\right)=0,\underset{t\to \infty }{lim}\alpha \left(t\right)=0,\underset{t\to \infty }{lim}\varphi \left(t\right)=0.$$

(22)

4.2. Control Design without Constraints

According to (17) and (18), the system states in polar coordinate can be obtained directly. There is an unknown constant c in the system (21), an adaptive controller is designed based on the Lyapunov Theory as following:

$$v=sign\left(e_2\right)k_{c1}\rho cos\alpha ,$$

(23)

$$w=k_{c2}\alpha +\widehat{c}{k}_{c1}\frac{sin\alpha cos\alpha }{\alpha }\left(\alpha +k_{c3}\varphi \right),$$

(24)

with the adaptive law of $\widehat{c}$ designed as

$$\dot{\widehat{c}}=k_a{k}_{c1}sin\alpha cos\alpha \left(\alpha +k_{c3}\varphi \right),$$

(25)

where $k_{c1},k_{c2},k_{c3},k_a\in {\mathcal{R}}^{+}$ are positive control parameters. Then substituting the control inputs (23) and (24) to the open-loop system (21), the closed-loop system can be obtained as

$$\left\{\begin{array}{c}\dot{\rho }=-c{k}_{c1}\rho {cos}^2\alpha \hfill \\ \dot{\alpha }=-k_{c2}\alpha +\tilde{c}{k}_{c1}sin\alpha cos\alpha -\widehat{c}{k}_{c1}{k}_{c3}\varphi \frac{sin\alpha cos\alpha }{\alpha }\hfill \\ \dot{\varphi }=c{k}_{c1}sin\alpha cos\alpha ,\hfill \end{array}\right.$$

(26)

where $\tilde{c}=c-\widehat{c}$ is the parameter estimation error. Note that there exists the limit law $li{m}_{\alpha \to 0}\frac{sin\alpha }{\alpha }=1$, such that there is no singularity problem for the closed-loop system (26) and the control law (24).

Theorem 1.

The proposed control law (23) and (24) with the parameter adaptive law (25) can drive the system states $\rho ,\alpha ,\varphi$ to zero in the sense that

$$\underset{t\to \infty }{lim}\rho \left(t\right)=0,\underset{t\to \infty }{lim}\alpha \left(t\right)=0,\underset{t\to \infty }{lim}\varphi \left(t\right)=0.$$

Proof of Theorem 1.

Choose a Lyapunov function candidate as follows

$$V=\frac{1}{2}{\rho }^2+\frac{1}{2}\left({\alpha }^2+k_{c3}{\varphi }^2\right)+\frac{1}{2k_a}{\tilde{c}}^2.$$

(27)

Taking the time derivative of the function V and then substituting the closed-loop system (26), we obtain

$$\begin{array}{cc}\hfill \dot{V}=& \rho \dot{\rho }+\alpha \dot{\alpha }+k_{c3}\varphi \dot{\varphi }-\frac{1}{k_a}\tilde{c}\dot{\widehat{c}}\hfill \\ \hfill =& -c{k}_{c1}{\rho }^2{cos}^2\alpha -k_{c2}{\alpha }^2+\tilde{c}{k}_{c1}\alpha sin\alpha cos\alpha -\widehat{c}{k}_{c1}{k}_{c3}\varphi sin\alpha cos\alpha \hfill \\ \hfill & +c{k}_{c1}{k}_{c3}\varphi sin\alpha cos\alpha -\tilde{c}{k}_{c1}sin\alpha cos\alpha (\alpha +k_{c3}\varphi )\hfill \\ \hfill =& -c{k}_{c1}{\rho }^2{cos}^2\alpha -k_{c2}{\alpha }^2.\hfill \end{array}$$

(28)

Since the constant parameter $c>0$, it is easy to deduce that

$$\dot{V}\le 0.$$

(29)

According to (27) and (29), it is easy to concluded that

$$\rho \left(t\right),\alpha \left(t\right),\varphi \left(t\right),\tilde{c}\left(t\right)\in {\mathcal{L}}_{\infty }$$

(30)

and to find out the point in the invariant set M

$$(\rho =0,\alpha =0)\in M.$$

(31)

Substituting (31) back to the closed-loop system (26) and parameter adaptive law (25), it is concluded that

$$\widehat{c}\varphi =0,\dot{\widehat{c}}=0.$$

(32)

From (30), we know that $\widehat{c}\in {\mathcal{L}}_{\infty }$. It is concluded that $\widehat{c}$ is a nonzero constant. Thus

$$\varphi =0.$$

(33)

According to above analysis, it is obvious that the invariant set M only consists of the equilibrium point being $(\rho =0,\alpha =0,\varphi =0,\widehat{c}=constant)\in M$. Based on LaSalle’s invariance principle [32], we can infer that the system states converge to the equilibrium point asymptotically. The Theorem 1 is proved. □

When the states of the closed-loop system converge to zero asymptotically, a singularity problem will happen. To deal with this problem that the state $\rho$ go to zero, an intuitive method is to set the linear velocity v to zero when the state $\rho$ is smaller than a threshold. The angular velocity is designed as a pure proportional controller $w=-k_w\theta$, with $k_w$ being a positive control gain. This is scheduled as the last stage.

4.3. A Switched Approach

The controller designed in the previous subsection should work under an ideal situation that the feature points are always in the field-of-view. The system is built based on partial 3D information. It cannot be guaranteed that the visual features remain in the field-of-view during the stabilization task of the mobile robot. Thus an image-based switched approach is proposed to ensure the features remain in view.

Suppose that the motion of the robot is on a plane, the image points only escape the field-of-view from the left and right sides. A heuristic approach is to set two bounds at the left and right boundaries. If any feature point falls within the boundary, the control process switches to the image-based controllers which are designed as following

$$\left\{\begin{array}{c}w=k_t\left(min\left(u_i\right)-min\left(u_i^{*}\right)\right)\hfill \\ v=-k_v{v}_i\left(v_i-v_i^{*}\right)\hfill \end{array}\right.$$

(34)

and

$$\left\{\begin{array}{c}w=k_t\left(max\left(u_i\right)-max\left(u_i^{*}\right)\right)\hfill \\ v=-k_v{v}_i\left(v_i-v_i^{*}\right),\hfill \end{array}\right.$$

(35)

where $v_i$ is the coordinate in v direction of the corresponding image point with the $min\left(u_i\right)$ or $max\left(u_i\right)$. $k_t,k_v\in {\mathcal{R}}^{+}$ are positive control parameters. The control laws (34) are applied when an image point falls into the boundary area on the left. And the control laws (35) are applied when an image point falls into the boundary area on the right. The purpose of both sets of controllers is to bring the image points near the boundary back to the center of the field-of-view. In addition, a zone needs to be set in the center of the image to ensure that the controller can switch back to (23) and (24) when the image points fall into that zone. Together with this simple switching mechanism, it is guaranteed that the feature points could be kept in the field-of-view during the stabilization task of the mobile robot under the control of (23) and (24).

5. Simulations

Two sets of simulation results are presented to validate the proposed image-based switched approach for mobile robots. The Egt-toolbox [33] is used to accomplish the simulation. The virtual camera parameters are set as $f_u=829.77,f_v=826.59$, $u_0=393.74$, $v_0=285.87$. Two static points are selected as ${\mathcal{P}}_1={[3,3,11]}^{T}m,{\mathcal{P}}_2={[-0.5312]}^{T}m$. The estimation error of the Principal point is less than 2.5 pixels, which is verified by multi-group experiments in [20]. Thus A random Gaussian noise with a deviation of 1.5 pixels, is added to the image points to verify the robustness.

The control parameters are shown in

Table 1. Control parameters.

${\mathit{k}}_{\mathit{c}1}$	${\mathit{k}}_{\mathit{c}2}$	${\mathit{k}}_{\mathit{c}3}$	${\mathit{k}}_{\mathit{a}}$	${\mathit{k}}_{\mathit{w}}$	${\mathit{k}}_{\mathit{t}}$	${\mathit{k}}_{\mathit{v}}$
0.3	0.3	1	5	1	0.001	0.0005

. These control parameters are utilized in the following two sets of simulations.

An initial pose of the mobile robot is selected as $X_0={[-2,-6,{10}^{\circ }]}^T$. Simulation results are shown in Figure 4 and Figure 5. In Figure 4a, the trajectories of two image points pass through the circular points at the starting and ending positions, respectively. The star points represent the desired image points. The circular points at the end positions coincide with the star points, which indicates that the mobile robot has reached the desired configuration. This also can be seen from the motion trajectory of the mobile robot in Figure 4b. The state curves of the mobile robot are shown in Figure 5. At almost 41 seconds, the mobile robot was very close to the desired position, then a pure rotation control was applied to the robot. The threshold of the state $\rho$ is set at $0.01$. In this case, no switching control occurs for the mobile robot. The feature points are always in the field-of-view during the stabilization task of the mobile robot, which is desirable. Next, a case that is not so ideal is considered.

In this case, an initial pose being $X_0={[2,-6,{10}^{\circ }]}^T$ is selected. Simulations are shown in Figure 6 and Figure 7. As shown in Figure 6a, despite several round trips, the feature trajectories remain within the image boundaries. In Figure 7, the movement process of the mobile robot is divided into four stages. In stage 1, the control laws (23) and (24) are used. When an image feature falls into the left border area or the right border area of the image, it switches to stage 2 or stage 3 correspondingly. The controllers (34) and (35) are applied in stage 2 and stage 3, respectively. If the state $\rho$ is smaller than the threshold, the last stage 4 is activated.

In this part, simulation results are provided to verify the effectiveness of the proposed strategy. According to above two sets of simulations, it is known that whether or not to start the switching process is related to the initial pose of the mobile robot. If the initial pose is good, the mobile robot can be driven to the desired pose quickly as shown in the first simulation. However, there is no basis for judging whether an initial pose is good or not good at present. This paper focus on the control strategy to keep two marker points in the field-of-view, which is a preliminary work. The efficiency of the control strategy is sacrificed to remain image features in view. Based on this work, the efficiency should be improved in the future work, which will be verified on a real mobile robot in an actual scenario.

6. Conclusions

A switching control approach with a polar coordinate based controller and two image-based controllers is proposed for the nonholonomic mobile robots. As the 3D angular information should be calculated for building the system model, the principle point and focal length in x direction of the camera are estimated by a self-calibration method in advance. A discontinuous coordinates transformation is applied to build the system model in polar coordinate. Then the asymptotical stability of the system is proven combined with a well designed adaptive controller. Afterward, two image-based controllers are designed to keep the features in the field-of-view during the visual stabilization task of the mobile robot. Simulation results demonstrate the proposed approach is effective. Although the image features could be kept in the field-of-view, the efficiency of the mobile robot is sacrificed with multiple round-trip movements. This will be considered in the future work.