Vision-based maze navigation for humanoid robots

Abstract

We present a vision-based approach for navigation of humanoid robots in networks of corridors connected through curves and junctions. The objective of the humanoid is to follow the corridors, walking as close as possible to their center to maximize motion safety, and to turn at curves and junctions. Our control algorithm is inspired by a technique originally designed for unicycle robots that we have adapted to humanoid navigation and extended to cope with the presence of turns and junctions. In addition, we prove here that the corridor following control law provides asymptotic convergence of robot heading and position to the corridor bisector even when the corridor walls are not parallel. A state transition system is designed to allow navigation in mazes of corridors, curves and T-junctions. Extensive experimental validation proves the validity and robustness of the approach.

Appendix

In this section we analyze the perturbtion terms in Eq. (11) to prove that they are vanishing and locally Lipschitz around the equilibrium point $(x_m, x_v)=(0, 0)$.

Computing the time derivative of the visual features expressions (6) and (8) in the case of non-parallel corridor walls and using the unicycle model (1) and the control designed on the nominal system (9) we obtain, for the middle point $x_m$ the closed-loop dymanics

$$\begin{aligned} \dot{x}_m = -k_p x_m + p_m = -k_p x_m + A_m + B_m + C_m + D_m, \end{aligned}$$

where the perturbation $p_m$ is composed by the terms

$$\begin{aligned} A_m= & {} \sigma \left( x_m \omega + k_2 v \right) \nonumber \\&\frac{1}{d k_1} \frac{x_v \left( h \tan \gamma \sec \theta - (x_v/k_1) x \right) - k_1 x}{1 - (\sigma /d) \left( y- (x_v/k_1) x \right) }, \nonumber \\ B_m= & {} E_m\tan ^2\theta (k_3 \omega + k_2 v \tan \theta ), \nonumber \\ C_m= & {} E_m \tan \theta \left( 2 x_m \sec ^2 \theta \omega + k_2 v \right) , \nonumber \\ D_m= & {} E_m\theta \left( k_3 + k_2 \sec ^3\theta (y - d/\sigma )\right) \omega , \end{aligned}$$

(12)

with

$$\begin{aligned} E_m = \frac{\sigma ^2}{1-\sigma ^2 \tan ^2 \theta }. \end{aligned}$$

To show that the perturbation is vanishing we should invert the map $(x,\theta )\rightarrow (x_m, x_v)$ so as to express the perturbation term as a function of $(x_m, x_v)$ only. However, in Proposition 1 we have shown that at the equilibrium $(x_m, x_v)^T=(0, 0)^T \Rightarrow (x, \theta )^T=(0, 0)^T$ except on points of the circle with center $P_V$, the intersection point of the two corridor guidelines, and radius $r_V=\frac{h}{\tan \gamma }$.

These points, however, cannot be stable equilibria of the closed loop dynamics, as also shown in Proposition 1. In addition, considering the robot footprint with respect to the corridor width, these points can hardly be reached in practical situations. Hence, by ignoring these points, we can evaluate the perturbation terms at the equilibrium of the nominal system by setting $(x_m, x_v)=(0, 0)$ and $(x, \theta )=(0,0)$.

A quick inspection of Eq. (12) provides evidence that $p_m$ is null at the equilibrium point $(x_m, x_v) = (0, 0)$, implying that the perturbation induced by the non-parallel corridor guidelines on the closed loop nominal dynamics is non-persistent.

The perturbation term is composed by sums and products of functions that are locally Lipschitz around the equilibrium of the nominal system with the exception of the terms $A_m$ and $E_m$ presenting, respectively, the following singularities.

• $y-y_V = \frac{x_v}{k_1}x$: this singularity cannot be met around the equilibrium point since this would imply that the robot is very close to the point $P_V$ at the intersection point of the corridor guidelines, a situation physically impossible;

• $\tan \theta = \pm 1/\sigma$: is verified if the robot heading is perpendicular to the corridor walls, a situation not possible around the origin of the system if $\sigma \ne 0$.

Finally, given the terms in (12), $p_m$ can be easily expressed as $p_m = \sigma \tilde{p}_m.$

To prove that the perturbation of the vanishing point closed-loop dynamics is non-persisting and locally Lipstchiz at the equilibrium of the nominal system we proceed in a way analogous to the case of the middle point. The closed loop dynamics of $x_v$ in case of non-parallel guidelines is

$$\begin{aligned} \dot{x}_v = f_v(x_m,x_v) + D_v + E_v = f_v (x_m,x_v) + p_v, \end{aligned}$$

where $f_v (x_m,x_v)$ represents the nominal closed-loop dynamics in Eq. (10), while the perturbation $p_v$ is composed of the following terms

$$\begin{aligned} E_v= & {} \sigma ^2 \frac{1}{k_1 d^2} \left( \frac{ x_v (h \tan \gamma \sec \theta - (x_v / k_1) x) - k_1 x}{1 - (\sigma /d) \left( y- (x_v/k_1) x \right) } \right) ^2 \omega \\ D_v= & {} \frac{(\dot{B}_v-\dot{A}_v C_v)(1+C_v)-\dot{C}_v(A_v+B_v)}{(1+C_v)^2} \end{aligned}$$

where

$$\begin{aligned} A_v= & {} k_1 \tan \theta ,\\ \dot{A}_v= & {} k_1 (\tan ^2\theta +1 ) \omega ,\\ B_v= & {} \sigma k_1 \frac{1}{d} (x - y \tan \theta ),\\ \dot{B}_v= & {} -\frac{\sigma }{d}y \dot{A}_v,\\ C_v= & {} \sigma \frac{1}{d}\left( h\tan \gamma \sec \theta - x \tan \theta - y\right) ,\\ \dot{C}_v= & {} \sigma \frac{1}{d} \sec \theta \left( \left( h \tan \gamma s_\theta - x \right) \sec \theta \omega - v \right) . \end{aligned}$$

As in the previous case, we can prove that the perturbation vanishes if we set $(x_m, x_v)=(0, 0)$ and $(x, \theta )=(0, 0)$. Also in this case, the perturbation term has some singularities. In particular, the term $E_v$ presents the same singularity as $A_m$ in Eq. (12) discussed above. The term $1+C_v$ at the denominator of $D_v$ is null if the following equation is verified

$$\begin{aligned} (y_V - y)\cos \theta - x \sin \theta = - h \tan \gamma . \end{aligned}$$

The simple geometric construction in Fig. 18 shows that the term on the left represents the signed distance of the robot to the line orthogonal to the optical axis (directed as the robot heading) and passing through the point $P_V$ where the corridor guidelines intersect. This distance becomes negative, and eventually equal to $h\tan \gamma$, if the robot crosses this line. Around the equilibrium point this would mean that the robot intersects this line close to $P_V$, again a non-operative condition.

Fig. 18

Geometric interpretation of the $D_v$ singularity

We can then state that around the equilibrium the perturbation term $p_v$ is locally Lipschitz being given by sums and products of locally Lipschitz functions. Analogously to the perturbation of the nominal middle point dynamics, $p_v$ can be written as $p_v = \sigma \tilde{p}_v$.

Wrapping up, the perturbation terms generated by the non-parallel wall corridor condition have been shown to be vanishing and locally Lipschitz around the equilibrium of the nominal dynamics. Furthermore, the perturbation term is proportional to the perturbation parameter $\sigma$ representing the corridor walls relative slope.