Angle-based homing from a reference image set using the 1D trifocal tensor

Abstract

This paper presents a visual homing method for a robot moving on the ground plane. The approach employs a set of omnidirectional images acquired previously at different locations (including the goal position) in the environment, and the current image taken by the robot. We present as contribution a method to obtain the relative angles between all these locations, using the computation of the 1D trifocal tensor between views and an indirect angle estimation procedure. The tensor is particularly well suited for planar motion and provides important robustness properties to our technique. Another contribution of our paper is a new control law that uses the available angles, with no range information involved, to drive the robot to the goal. Therefore, our method takes advantage of the strengths of omnidirectional vision, which provides a wide field of view and very precise angular information. We present a formal proof of the stability of the proposed control law. The performance of our approach is illustrated through simulations and different sets of experiments with real images.

1 Appendix

1.1 1.1 Global asymptotic stability

Theorem 1

The system under the proposed control law (11), (12) is globally asymptotically stable if $k_{\omega }>k_{v}\cdot \pi /d_{min}.$

Proof

We will use Lyapunov techniques (Khalil 2001) to analyze the stability of the system. We define the following definite positive candidate Lyapunov function:

$$\begin{aligned} V(\mathbf{x}, t) = \frac{\rho ^2}{2} + \frac{(\alpha _{CG}-\alpha _{CG}^d)^{2}}{2}, \end{aligned}$$

(13)

where $\rho$ is the distance between the current and goal positions, and $\mathbf{x}$ is the state of the system, determined by $\rho$ and $\alpha _{CG}.$ The two state variables we use are a suitable choice, since we are only interested in reaching the goal position, regardless of the final orientation of the robot. As can be seen, both $V$ and $\dot{V}$ are continuous functions.

We note at this moment that the equilibrium in our system occurs at the two following points: $(\rho ,\alpha _{CG})=(0,0)$ and $(\rho ,\alpha _{CG})=(0,\pi ),$ which correspond to the situations where the robot reaches the goal moving forwards or backwards, respectively. In order to account for the multiple equilibria, in the following we use the global invariant set theorem (Slotine and Li 1991) to prove the asymptotic stability of the system.

What we need to show is that $V$ is radially unbounded and $\dot{V}$ is negative semi-definite over the whole state space. It is straightforward that $V(\mathbf{x})$ is radially unbounded, given that $V(\mathbf{x})\rightarrow \infty$ as $\Vert \mathbf{x}\Vert \rightarrow \infty .$ Next, we prove that the derivative $\dot{V}(\mathbf{x})$ is negative definite. For our chosen candidate Lyapunov function, this derivative is as follows:

$$\begin{aligned} \dot{V} = \rho \, \dot{\rho } + (\alpha _{CG} - \alpha _{CG}^d) \, \dot{\alpha }_{CG}. \end{aligned}$$

(14)

We will suppose that the vehicle on which the control method is to be implemented is a nonholonomic unicycle platform. The dynamics of the system as a function of the input velocities is then given, using the derivatives in polar coordinates with the origin at the goal, by $\dot{\rho }=-v \cos (\alpha _{CG})$ and $\dot{\alpha }_{CG}=-\omega + v \sin (\alpha _{CG})/\rho .$ Using the control velocities (11), (12) we obtain

$$\begin{aligned} \dot{V}&= -k_{v} \, \rho \, \cos ^{2}(\alpha _{CG}) \, \frac{1}{n}\sum _{i=1}^{n} S_{i} - k_{\omega }(\alpha _{CG} - \alpha _{CG}^d)^{2} \nonumber \\&\!\! + (\alpha _{CG} - \alpha _{CG}^d) \cdot \sin \alpha _{CG} \, \frac{k_{v}}{\rho } \, \cos \alpha _{CG}\, \cdot \frac{1}{n}\sum _{i=1}^{n} S_{i}.\qquad \quad \end{aligned}$$

(15)

By definition $\rho \ge 0$ and $S_i \ge 0.$ It is then straightforward to see that the first and the second term of (15) are negative definite. However, the third term can be positive. The interpretation is that for the system to be stable, the convergence speed provided by the angular velocity has to be higher than the convergence speed given by the linear velocity. Otherwise, the angular error is not corrected fast enough and the robot will move following spirals around the goal. Still, the stability can be guaranteed if the control gains are selected properly. From (15) we can see that it is guaranteed that $\dot{V}<0$ if the following inequality holds:

$$\begin{aligned} |k_{\omega } \cdot (\alpha _{CG} - \alpha _{CG}^d)| > |\sin (\alpha _{CG}) \cos (\alpha _{CG})\, \frac{k_{v}}{\rho } \cdot \frac{1}{n}\sum _{i=1}^{n} S_{i}|.\nonumber \\ \end{aligned}$$

(16)

This is equivalent to the following condition on the angular velocity gain:

$$\begin{aligned} k_{\omega } > \left|\frac{\sin (\alpha _{CG})}{(\alpha _{CG} - \alpha _{CG}^d)}\cos (\alpha _{CG})\cdot k_{v}\cdot \frac{1}{n}\sum _{i=1}^{n} \frac{S_{i}}{\rho }\right|. \end{aligned}$$

(17)

We aim to find an upper bound to the right side of (17). We start by analyzing the first fraction. Since $\alpha _{CG}^d$ is equal to either $0$ or $\pi ,$ and $\sin (\alpha _{CG})=-\sin (\alpha _{CG}-\pi ),$ we have:

$$\begin{aligned} \left|\frac{\sin (\alpha _{CG})}{(\alpha _{CG} - \alpha _{CG}^d)}\right|= \left|\frac{\sin (\alpha _{CG})}{(\alpha _{CG})}\right| \le 1, \end{aligned}$$

(18)

as $\sin (\alpha _{CG})/\alpha _{CG}$ is a sinc function, whose maximum absolute value occurs at $\alpha _{CG}=0$ and equals 1. We now look for a bound to the $S_{i}/\rho$ term in (17). The angular sector $S_{i}$ seen from reference view $i$ has a value lying in the interval $0 \le S_{i} \le \pi .$ We will study two subintervals separately:

• $0 \le S_{i} \le \pi /2.$ Applying the law of sines on the triangle defined by vertices $C,\,G$ and $R_{i}$ in Fig. 5, the addend in (17) corresponding to reference view $i$ becomes:

  $$\begin{aligned} \frac{S_{i}}{\rho }=\frac{S_{i}}{\sin (S_{i})}\cdot \frac{\sin (\widehat{CR_{i}G})}{d_{i}} \le \frac{\pi }{2\cdot d_{min}} \end{aligned}$$

  (19)

     The first fraction of the product in (19) is a function of $S_{i}$ whose value equals 1 at $S_{i}=0$ and increases monotonically to a value of $\pi /2$ at $S_{i}=\pi /2,$ which is the limit of the interval we are considering. Since the second fraction has an upper bound equal to $1/d_{min},$ the product of the two is upper-bounded by $\pi /(2\cdot d_{min}).$

• $\pi /2 < S_{i} \le \pi .$ In this case, $\rho >d_{i},$ and an upper bound is readily found for the addend in (17) corresponding to reference view $i$:

  $$\begin{aligned} \frac{S_{i}}{\rho } \le \frac{\pi }{d_{min}}. \end{aligned}$$

  (20)

Thus, the contribution of each of the reference views to the sum is upper-bounded by the higher of the two bounds in (19) and (20), which is $\frac{\pi }{d_{min}}.$ The mean of all the individual contributions is therefore bounded by this value, i.e.:

$$\begin{aligned} \frac{1}{n}\sum _{i=1}^{n} \frac{S_{i}}{\rho } \le \frac{\pi }{d_{min}}, \end{aligned}$$

(21)

and inequality (17) becomes:

$$\begin{aligned} k_{\omega } > \frac{k_{v}\cdot \pi }{d_{min}}. \end{aligned}$$

(22)

$\square$

1.2 1.2 Local exponential stability

Proposition 1

The system under the proposed control law (11), (12) is locally exponentially stable.

Proof

We analyze the behavior of the system locally, i.e. assuming the orientation of the robot has already been corrected ($\alpha _{CG}=\alpha _{CG}^d$). The dynamics of the distance from the goal for the unicycle vehicle considered is then given by:

$$\begin{aligned} \dot{\rho }=-v\cos \alpha _{CG}=-k_{v}\frac{1}{n}\sum _{i=1}^{n} S_{i}. \end{aligned}$$

(23)

Now, taking into account that $S_{i}\ge \sin S_{i}$ in all the interval of possible values ($0\le S_{i}\le \pi$), we have:

$$\begin{aligned} \dot{\rho }\le \frac{-k_{v}}{n}\sum _{i=1}^{n} \sin S_{i}=-\left[\frac{k_{v}}{n}\sum _{i=1}^{n}\frac{\sin (\widehat{CR_{i}G})}{d_{i}}\right]\cdot \rho . \end{aligned}$$

(24)

It can be readily seen, looking at Fig. 5, that for any given current position $C$ of the robot, $\sin (\widehat{CR_{i}G})$ will be greater than zero for at least one $R_{i}$ as long as there are at least three reference views (including the goal) and their locations are not collinear. Thus, there exists a positive value $\lambda _{min}$ such that

$$\begin{aligned} \frac{k_{v}}{n}\sum _{i=1}^{n}\frac{\sin (\widehat{CR_{i}G})}{d_{i}} \ge \lambda _{min} > 0. \end{aligned}$$

(25)

From (24) and (25) it can be concluded that the local convergence to the target state is bounded by an exponential decay, i.e. the system is locally exponentially stable. $\square$