Monocular distance estimation with optical flow maneuvers and efference copies: a stability-based strategy

Before going into why a single gain is only stable for a given range of heights, first the equations of motion will be described. Let us start with the model in [6]. In the notation of a state space model:

$$\begin{eqnarray}&&\dot{{\bf{x}}}(t)=A{\bf{x}}(t)+B{\bf{u}}(t),\end{eqnarray} \tag{ 3 }$$

where the state vector ${\bf{x}}={[z(t),{v}_{z}(t)]}^{T}$, and ${\bf{u}}(t)={u}_{z}(t)$, so that:

$$\begin{eqnarray}&&[\begin{array}{c}\dot{z}(t)\\ \dot{{v}_{z}}(t)\end{array}]=[\begin{array}{cc}0 & 1\\ 0 & 0\end{array}][\begin{array}{c}z(t)\\ {v}_{z}(t)\end{array}]+[\begin{array}{c}0\\ 1\end{array}]{u}_{z}(t).\end{eqnarray} \tag{ 4 }$$

This model assumes that u_z = a_z, which effectively implies that any possible gravity is subsumed under the u_z-term:

$$\begin{eqnarray}&&{a}_{z}=-g+\displaystyle \frac{{u}_{z}^{\prime }}{m}={u}_{z},\end{eqnarray} \tag{ 5 }$$

where ${u}_{z}^{\prime }=m({a}_{z}+g)$ is the actual upward thrust generated by the robot in Newton. It is common in control system theory to subsume factors such as gravity and the mass into u_z, as the calculation of ${u}_{z}^{\prime }$ is straightforward and does not depend on any state variables.

Equation (5) is only valid if the control force is the only force acting on the robot. In a vacuum environment this can be approximately correct, e.g. for a moon landing. However, a robot flying in the air will undergo additional accelerations. Therefore, in this article, the drag force will be modeled, which depends on the robot's movement relative to the air:

$$\begin{eqnarray}&&{f}_{D}={\rm{sign}}({v}_{{\rm{air}}})\displaystyle \frac{1}{2}\rho {C}_{D}{{Av}}_{{\rm{air}}}^{2},\end{eqnarray} \tag{ 6 }$$

where:

$$\begin{eqnarray}&&{v}_{{\rm{air}}}={v}_{{\rm{wind}}}-{v}_{z},\end{eqnarray} \tag{ 7 }$$

and ${\rm{sign}}({v}_{{\rm{air}}})$ indicates the directionality of f_D along the z-axis. This leads to an additional acceleration:

$$\begin{eqnarray}&&{a}_{z}=-g+\displaystyle \frac{{u}_{z}^{\prime }+{f}_{D}}{m},\end{eqnarray} \tag{ 8 }$$

in which f_D is a time-varying value involving a non-linear function of v_z and an uncontrolled variable ${v}_{{\rm{wind}}}$.

Now, let us start from the visual observable ${\vartheta }_{z}$:

$$\begin{eqnarray}&&{\vartheta }_{z}=\displaystyle \frac{{v}_{z}}{z}.\end{eqnarray} \tag{ 9 }$$

If ${\vartheta }_{z}$ is differentiated over time, it results in:

$$\begin{eqnarray}&&\dot{{\vartheta }_{z}}=\displaystyle \frac{{a}_{z}}{z}-\displaystyle \frac{{v}_{z}^{2}}{{z}^{2}}.\end{eqnarray} \tag{ 10 }$$

Using equation (9):

$$\begin{eqnarray}&&\dot{{\vartheta }_{z}}=\displaystyle \frac{{a}_{z}}{z}-{\vartheta }_{z}^{2}.\end{eqnarray} \tag{ 11 }$$

Assuming a_z as in equation (8):

$$\begin{eqnarray}&&\dot{{\vartheta }_{z}}=\displaystyle \frac{-g+\displaystyle \frac{{u}_{z}^{\prime }+{f}_{D}}{m}}{z}-{\vartheta }_{z}^{2}.\end{eqnarray} \tag{ 12 }$$

Differentiating this formula with respect to ${u}_{z}^{\prime }$ gives:

$$\begin{eqnarray}&&\displaystyle \frac{\partial \dot{{\vartheta }_{z}}}{\partial {u}_{z}^{\prime }}=\displaystyle \frac{1}{{mz}}.\end{eqnarray} \tag{ 13 }$$

Equation (13) shows that a change in thrust has a larger effect on the change in $\dot{{\vartheta }_{z}}$ closer to the landing surface than far away (independently of external accelerations)¹ Thus, a control gain for regulating ${\vartheta }_{z}$ that leads to a satisfactory control performance far away from the landing surface, will lead to very large effects on $\dot{{\vartheta }_{z}}$ and hence indirectly on ${\vartheta }_{z}$ close to the landing surface. Although theoretically one could use $\frac{\partial \dot{{\vartheta }_{z}}}{\partial {u}_{z}^{\prime }}$ directly for distance estimation ($z=\frac{\partial {u}_{z}^{\prime }}{\partial \dot{{\vartheta }_{z}}}\frac{1}{m}$), in practice this value is extremely noisy, since it involves a partial derivative with the noisy value $\dot{{\vartheta }_{z}}$.

In this subsection, it will be shown that the larger influence of u_z on ${\vartheta }_{z}$ at lower heights eventually leads to instability at a specific height. For the analysis, the following constant divergence control law is studied, using just a proportional gain K_z as in [6]:

$$\begin{eqnarray}&&{u}_{z}={K}_{z}({\vartheta }_{z}^{*}-{\vartheta }_{z}),\end{eqnarray} \tag{ 14 }$$

where it is easy to see that in a noiseless, delayless system:

$$\begin{eqnarray}&&\underset{{K}_{z}\to \infty }{\mathrm{lim}}\displaystyle \frac{{u}_{z}}{{K}_{z}}=({\vartheta }_{z}^{*}-{\vartheta }_{z})=0,\end{eqnarray} \tag{ 15 }$$

and hence ${\vartheta }_{z}={\vartheta }_{z}^{*}$. Real systems always have some noise and delay. Below, it will be shown that just discretizing the control with zero-order-hold (ZOH) will already introduce an instability into the system.

Let us assume the state space model of equation (4), ignoring drag for the moment. The corresponding observation is:

$$\begin{eqnarray}&&y={\vartheta }_{z}=\displaystyle \frac{{v}_{z}}{z},\end{eqnarray} \tag{ 16 }$$

which is a nonlinear function. Linearizing the state space model gives:

$$\begin{eqnarray}{\rm{\Delta }}y(t) & = & C{\rm{\Delta }}{\bf{x}}(t)+D{\rm{\Delta }}{\bf{u}}(t)\\ & = & [\begin{array}{cc}\displaystyle \frac{-{v}_{z}}{{z}^{2}} & \displaystyle \frac{1}{z}\end{array}]{[\begin{array}{cc}{\rm{\Delta }}z(t) & {\rm{\Delta }}{v}_{z}(t)\end{array}]}^{\top },\end{eqnarray} \tag{ 17 }$$

so that the state space model matrices are:

$$\begin{eqnarray}A=[\begin{array}{cc}0 & 1\\ 0 & 0\end{array}]\ \ B=[\begin{array}{c}0\\ 1\end{array}]\ \ C=[\begin{array}{cc}\displaystyle \frac{-{v}_{z}}{{z}^{2}} & \displaystyle \frac{1}{z}\end{array}]\ \ D=[0].\end{eqnarray} \tag{ 18 }$$

As mentioned, the continuous system without noise and delay is stable (equation (15)). Here, the discretized system is studied, which has the following state space model matrices corresponding to the continuous ones in equation (18):

$$\begin{eqnarray}{\rm{\Phi }} & = & [\begin{array}{cc}1 & T\\ 0 & 1\end{array}]\ \ {\rm{\Gamma }}=[\begin{array}{c}\displaystyle \frac{{T}^{2}}{2}\\ T\end{array}]\\ C & = & [\begin{array}{cc}\displaystyle \frac{-{v}_{z}}{{z}^{2}} & \displaystyle \frac{1}{z}\end{array}]\ \ D=[0],\end{eqnarray} \tag{ 19 }$$

where T is the discrete time step in seconds. The transfer function of the open loop system can be determined to be:

$$\begin{eqnarray}&&G(w)=C{({wI}-{\rm{\Phi }})}^{-1}{\rm{\Gamma }},\end{eqnarray} \tag{ 20 }$$

$$\begin{eqnarray}&&=\displaystyle \frac{({zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2})w-{zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2}}{{z}^{2}{(w-1)}^{2}},\end{eqnarray} \tag{ 21 }$$

where we use w as the Z-transform variable, since z already represents height. The feedback transfer function is:

$$\begin{eqnarray}&&G(w)=\displaystyle \frac{{K}_{z}(({zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2})w-{zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2})}{{z}^{2}{(w-1)}^{2}+{K}_{z}(({zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2})w-{zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2})}.\end{eqnarray} \tag{ 22 }$$

Equation (22) shows that, given a gain K_z and time step T, the dynamics and stability of the system depend both on the height z and velocity v_z.

Figure 2 is the root locus plot of G(w). For the Z-transform any pole in the unit circle is a stable pole. The different elements of the plot can be found back in equation (22). The numerator indicates a single finite zero at:

$$\begin{eqnarray}&&{w}_{0}=\displaystyle \frac{{zT}+\displaystyle \frac{1}{2}{v}_{z}{T}^{2}}{{zT}-\displaystyle \frac{1}{2}{v}_{z}{T}^{2}}.\end{eqnarray} \tag{ 23 }$$

Given $z\gt 0$, $T\gt 0$, and ${v}_{z}\lt 0$, w₀ is positive and due to the typically small T values slightly smaller than 1. There is also a negative infinite zero. Since the denominator of equation (22) is a second order equation in w, G(w) has two poles, both of which are stable. For K_z = 0, the poles are located at w = 1. As the gain K_z increases, one pole moves toward the finite zero and one toward the infinitely negative zero. This latter pole is interesting, because as soon as it passes $w=-1$, the system becomes unstable. Setting $w=-1$ in the denominator and equating it with 0 gives an equation for the unstable gain value K_z:

$$\begin{eqnarray}&&{K}_{z}=\displaystyle \frac{2}{T}z.\end{eqnarray} \tag{ 24 }$$

Zoom In Zoom Out Reset image size

This implies that given a specific gain K_z and time step T, there is always a height at which the control system will become unstable:

$$\begin{eqnarray}&&z=\displaystyle \frac{1}{2}{K}_{z}T.\end{eqnarray} \tag{ 25 }$$

When using a fixed gain, a tradeoff has to be made between the control performance at a larger height (requiring a large gain) and at lower heights (requiring a small gain). Please remark that the instability at lower altitudes is exactly what is observed for robotic systems performing constant divergence landings!

In this subsection it is shown that also for a model with wind, the instability of the system given a K_z depends on z. Including wind leads to a changed formula for $\dot{{v}_{z}}$ (see equations (6) and (8)):

$$\begin{eqnarray}\dot{{v}_{z}} & = & {u}_{z}+{\rm{sign}}({v}_{{\rm{wind}}}-{v}_{z})\displaystyle \frac{1}{2\;m}\rho {C}_{D}A\\ & & {({v}_{{\rm{wind}}}-{v}_{z})}^{2},\end{eqnarray} \tag{ 26 }$$

where from now on β will stand in for the constant $\displaystyle \frac{1}{m}\rho {C}_{D}A$. In order to obtain a linear state space model, this equation is linearized to obtain:

$$\begin{eqnarray}{\rm{\Delta }}{\dot{v}}_{z} & = & {\rm{\Delta }}{u}_{z}-({\rm{sign}}({v}_{{\rm{wind}}}-{v}_{z})\\ & & \times \beta ({v}_{{\rm{wind}}}-{v}_{z})){\rm{\Delta }}{v}_{z},\end{eqnarray} \tag{ 27 }$$

where the factor multiplied with ${\rm{\Delta }}{v}_{z}$ is a constant in the linearized model (${v}_{{\rm{wind}}}$ and v_z are given the values at the linearization point). In order to avoid cluttering the formulas, this constant is represented by $p={\rm{sign}}({v}_{{\rm{wind}}}-{v}_{z})\beta ({v}_{{\rm{wind}}}-{v}_{z})$ (where $p\gt 0$). This leads to the following continuous, linear state space model:

$$\begin{eqnarray}A & = & [\begin{array}{cc}0 & 1\\ 0 & -p\end{array}]\ \ B=[\begin{array}{c}0\\ 1\end{array}]\ \ C=[\begin{array}{cc}\displaystyle \frac{-{v}_{z}}{{z}^{2}} & \displaystyle \frac{1}{z}\end{array}]\\ D & = & [0],\end{eqnarray} \tag{ 28 }$$

which results in the Z-transform matrices:

$$\begin{eqnarray}{\rm{\Phi }}=[\begin{array}{cc}1 & \displaystyle \frac{(1-{e}^{-{pT}})}{p}\\ 0 & {e}^{-{pT}}\end{array}]\ \ {\rm{\Gamma }}=[\begin{array}{c}\displaystyle \frac{T}{p}-\displaystyle \frac{(1-{e}^{-{pT}})}{{p}^{2}}\\ \displaystyle \frac{(1-{e}^{-{pT}})}{p}\end{array}]\end{eqnarray} \tag{ 29 }$$

$$\begin{eqnarray}C=[\begin{array}{cc}\displaystyle \frac{-{v}_{z}}{{z}^{2}} & \displaystyle \frac{1}{z}\end{array}]\ \ D=[0],\end{eqnarray} \tag{ 30 }$$

and a rather involved transfer function H(w). The root locus plot of H(w) is very similar to that of G(w), also with a negative infinite zero. Following the same procedure as for the 'vacuum' model, setting $w=-1$ and equating the denominator with 0, gives:

$$\begin{eqnarray}&&{K}_{z}\ =\frac{(2{p}^{2}+2{p}^{2}{e}^{{pT}}){z}^{2}}{(2{e}^{{pT}}-2-{Tp}-{{Tpe}}^{{pT}}){v}_{z}+(2{{pe}}^{{pT}}-2p)z}\begin{array}{l}.\end{array}\end{eqnarray} \tag{ 31 }$$

Equation (32) includes many instances of p that depend on v_z and ${v}_{{\rm{wind}}}$, and also contains a term in the denominator with v_z. This suggests that there is no fixed linear relation between K_z and z. However, closer inspection shows that the term $(2{e}^{{pT}}-2-{Tp}-{{Tpe}}^{{pT}})\approx 0$. Rearranging terms, this leads to:

$$\begin{eqnarray}&&{K}_{z}=\displaystyle \frac{2{p}^{2}+2{p}^{2}{e}^{{pT}}}{2{{pe}}^{{pT}}-2p}z,\end{eqnarray} \tag{ 32 }$$

which still depends on p. It turns out that the fraction that is multiplied with z is almost identical to the fraction $\displaystyle \frac{2}{T}$. Indeed, solving equation (32) for different variable settings gives almost identical results to equation (24).

The previous subsections discussed discretized models for which unstable K_z exists, and where the limit case can be analytically expressed as a function of z. In order to show that continuous systems encounter the same type of phenomenon, figure 3 shows the root locus plot of a continuous system with wind, a delay of ${\rm{\Delta }}t=0.03\;{\rm{s}}$ and ${c}^{2}=0.01$. The delay introduces two zeros in the (unstable) right-hand plane of this continuous root locus plot. A few values of K_z are plotted, where the poles get an imaginary component and where they cross the imaginary axis (shown for z = 10 m and z = 1 m).

Zoom In Zoom Out Reset image size

The instability discussed in section 3.2 is induced by the robot itself. Before the system gets unstable, it will start to show oscillations. This can be seen for instance in figure 3. When K_z = 24.3, the control system's poles will start to have an imaginary component and hence oscillate around the desired value from z = 10 m downward. In the field of aerospace, such self-induced oscillations are a well-known problem, and referred to as pilot-induced oscillations (PIO).

The two most important properties of self-induced oscillations are that (1) there is a phase shift between the observations and control inputs in the order of 90–180^◦, and (2) the oscillations are of a significant magnitude. Let us have a look at a simulated landing to see how this phenomenon occurs for the studied constant divergence landings. For the simulations, a ZOH-model is assumed with a time step T = 0.03 s, $\displaystyle \frac{1}{2}\rho {C}_{D}A=0.5$, a mass of 1 kg, and a delay of ${\rm{\Delta }}t=0.15\;{\rm{s}}$ (as in section 3.4)² . The left part of figure 4 shows a landing for K_z = 20, ${z}_{0}=10$, ${v}_{z0}=-1$, ${c}^{2}=0.1$, in wind still conditions (${v}_{{\rm{wind}}}=0$). It shows that the landing goes smoothly, until the very end at $\approx 0.5\;{\rm{m}}$ at which point the robot's height starts to oscillate.

Zoom In Zoom Out Reset image size

The right part of figure 4 shows the main relevant variables, zooming in at the few seconds before landing (until t = 26 s the variables do not vary much). The dark yellow dashed line is the ground-truth ${\vartheta }_{z}={v}_{z}/z$, while the magenta solid line is its delayed version $\hat{{\vartheta }_{z}}$ as observed by the robot. The blue dotted line represents the thrust ${u}_{z}^{\prime }$. Toward the end of the landing ${\vartheta }_{z}$ starts deviating significantly from its reference point −0.1 (indicated with the thin dotted magenta line). It even obtains positive values, which means that the robot goes up. This itself could be used for the detection of instability, were it not that a wind gust can also move the robot upward, causing a positive ${\vartheta }_{z}$ at a larger height. The problem here is that the effects of the thrust changes have an ever larger and quicker effect on the relative velocity when getting closer to the landing surface. This response will get so quick that the robot starts getting in resonance with its own delay, as testified by the phase shift between ${\vartheta }_{z}$ and $\hat{{\vartheta }_{z}}$. The robot starts thrusting up while going up and thrusting down while going down.

The two properties of self-induced oscillations (phase shift and magnitude) can be captured with the covariance between the ground-truth ${\vartheta }_{z}$ and the thrust ${u}_{z}^{\prime }$. The right part of figure 4 shows $\mathrm{cov}({u}_{z}^{\prime },{\vartheta }_{z})$ as determined over a window of the last W = 20 time steps (black dashed line):

$$\begin{eqnarray}&&{\mathrm{cov}}_{t}({u}_{z}^{\prime },{\vartheta }_{z})=\displaystyle \sum _{i=t-W+1}^{t}({u}_{z}^{\prime }(i)-\bar{{u}_{z}^{\prime }})({\vartheta }_{z}(i)-\bar{{\vartheta }_{z}}).\end{eqnarray} \tag{ 33 }$$

During the smooth part of the trajectory $\mathrm{cov}({u}_{z}^{\prime },{\vartheta }_{z})$ is a small negative number: when the robot descends too fast (${\vartheta }_{z}$ too negative) the robot thrusts more up, and vice versa. Towards the end of the landing the observable ${\vartheta }_{z}$ starts to change too quickly for the control system and $\mathrm{cov}({u}_{z}^{\prime },{\vartheta }_{z})$ becomes positive: if the robot descends too fast (${\vartheta }_{z}$ too negative) the robot will thrust less up, and thus it starts inducing oscillations.

How can robots detect self-induced oscillations without access to the ground truth ${\vartheta }_{z}$? There have been several investigations on the automatic, on board detection of self-induced oscillations [7, 10, 34, 38, 47]. A typical detection method involves a fast Fourier transform (FFT) of the observations [7, 38], looking for the specific frequency band related to the self-induced oscillations. In this article, in the interest of a computationally efficient and straightforward method, we will investigate the use of the $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})$ as a proxy for $\mathrm{cov}({u}_{z}^{\prime },{\vartheta }_{z})$. In figure 4, $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})$ is shown with a black solid line. It is always negative, as ${u}_{z}^{\prime }={K}_{z}({\vartheta }_{z}^{*}-\hat{{\vartheta }_{z}})$, and hence mostly captures the magnitude of the oscillations. The $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})$ can be used in conjunction with ${\vartheta }_{z}$ for detecting the onset of oscillations. The black line in the left part of figure 4 shows the first point during the landing at which $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})\leqslant -0.1$ and $\hat{{\vartheta }_{z}}\geqslant 0.01$.

In order to test the hypothesis that the onset of oscillations is related to the magnitude of the gain and the height of the robot, simulation runs were performed for gains ${K}_{z}\in \{10,30,50\}$ and for various types of wind ${v}_{\mathrm{wind}}\in \{-3,-2.5,\ldots ,2.5,3\}$ m/s. Furthermore, ${z}_{0}=10$, ${v}_{z0}=-1$, ${c}^{2}=0.05$. The left part of figure 5 shows the results of this experiment, with the different K_z on the x-axis and the corresponding z at which self-induced oscillations are detected on the y-axis. Each K_z additionally is represented with a different marker, and the markers are colored according to the wind velocity, from blue (−3 m/s) to red (3 m/s). The black dashed line is a linear least-squares fit through the points, with parameters $z=0.04{K}_{z}-0.1$.

Zoom In Zoom Out Reset image size

There are three main observations. First, as the linear fit shows, higher gains result in oscillations further away from the landing surface. Hence, a fixed gain will result in self-induced oscillations around a certain height. The detection of these oscillations can be used for instance for triggering landing responses (such as the leg extension by fruitflies). Second, a considerable wind difference between −3 to 3 m/s leads to a relatively small (for MAVs) difference of $\approx 0.50\;{\rm{m}}$ in the height at which self-induced oscillations occur. Third, the colors show that there is a slight correlation between the detection height and the wind velocity. Analyzing the landings with wind shows that for the higher wind velocities, the control system has more difficulties tracking ${\vartheta }_{z}^{*}$, leading to a steady-state error. This suggests that introducing an integrator term could resolve this problem. The results of a PI-controller, with K_z as the P-gain and I_z = 1 as the I-gain, are shown in the right part of figure 4. Since the PI-controller can cancel the steady-state error, the reference ${\vartheta }_{z}^{*}$ is tracked much better. As expected, self-induced oscillations still occur, and the detection heights are much closer together. Moreover, inspection of the marker colors shows that there is no obvious relation anymore between the wind velocity and the detection height. In order to keep in line with the system that was analyzed theoretically, for the remainder of the article we will focus on the P-controller setup with gain K_z.

In the previous subsection, the feasibility of detecting self-induced oscillations for distance estimation was shown. However, two factors were not modeled that could have a significant influence on the practicality of the approach.

First, during a typical landing outdoors the wind is not constant. The wind can vary and sudden wind gusts can occur. In the simulation, wind gusts are added to the simulation in the form of a sine function:

$$\begin{eqnarray}&&{v}_{{\rm{gust}}}=W{\rm{sin}}({at}),\end{eqnarray} \tag{ 34 }$$

where W determines the magnitude of the gusts and a the period.

Second, it is unlikely that a robot's command signal (and hence efference copy) is equal to ${u}_{z}^{\prime }$. A command signal ${u}_{z}^{\prime\prime }$ (in any unit, e.g. the commanded RPM of a robot's propellors or the flapping frequency of an insect) is related to ${u}_{z}^{\prime }$ (in Newton) with an actuator effectiveness function: ${u}_{z}^{\prime }=f({u}_{z}^{\prime\prime })$. In flying robots such as rotorcraft or flapping wing vehicles the function f, in turn, depends on the air flow. Here, a rotorcraft is assumed and the relation between actuator effectiveness and air flow is modeled according to findings on propellors in [44]. Specifically, the following formula was used:

$$\begin{eqnarray}&&{u}_{z}^{\prime }\leftarrow {\rm{max}}\{{u}_{z}^{\prime }-{{bv}}_{{\rm{air}}}{u}_{z}^{\prime }-{{cv}}_{{\rm{air}}},0\},\end{eqnarray} \tag{ 35 }$$

so that both the offset and slope of the efficiency change over different air velocities.

The left part of figure 6 shows a landing with a rather extreme scenario, in which the average wind velocity is 0 m/s, W = 4, a = 1, b = 0.5, and c = 0.5. This means that per every π seconds, the wind varies from −4 to 4 m/s! The red line in the figure is the height over time, the black line indicates the height at which self-induced oscillations are detected. The right part shows the results over many landings with the same settings but different wind velocities, from −3 m/s (blue) to 3 m/s (red), to which the gusts are added during each landing. The black dashed line is a linear fit through all the detection heights for all gains K_z.

Zoom In Zoom Out Reset image size

The results show that despite the very gusty conditions and varying actuator efficiency, there is still a positive linear relation between z and K_z. The uncertainty is slightly higher though than for a constant wind and actuator efficiency, especially for larger gains/heights. It is important to emphasize that the actuator efficiency function f does not have to be known. Assuming a linear actuator efficiency function and using that the fit should pass through $({K}_{z},z)=(0,0)$, a single landing with a fixed gain in principle suffices to 'calibrate' the linear relation between z and K_z.

In principle, the detection of self-induced oscillations could be sufficient for flying robots (or insects) to determine when they are close to their landing target. This allows for the triggering of a final landing procedure and hence is behaviorally very relevant. However, it would be extremely useful if the robot could determine height at any distance to the landing surface.

Surprisingly, the proposed stability-based strategy for height estimation does not require the robot to land! Equation (25) (derived for the vacuum condition) does not depend on v_z, nor on c². This suggests a strategy for determining the height around hover: the robot can set ${\vartheta }^{*}=0$ and change its gain K_z so that the control loop starts exhibiting small self-induced oscillations. The gain at such a point is then a stand-in for the height.

Specifically, a control law can regulate $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})$ by continuously adapting the gain K_z. This leads to the following adaptive gain control setup. An inner loop uses ${u}_{z}={K}_{z}({\vartheta }_{z}^{*}-{\vartheta }_{z})$, while an additional outer loop controls K_z:

$$\begin{eqnarray}&&{K}_{z}(t)={K}_{z}^{\prime }(t)-(P\ {K}_{z}^{\prime }(t)){e}_{\mathrm{cov}}(t),\end{eqnarray} \tag{ 36 }$$

$$\begin{eqnarray}&&{K}_{z}^{\prime }(t)\leftarrow {K}_{z}^{\prime }(t-T)-(I\ {K}_{z}^{\prime }){e}_{\mathrm{cov}}(t-T),\end{eqnarray} \tag{ 37 }$$

$$\begin{eqnarray}&&{e}_{\mathrm{cov}}(t)=\mathrm{cov}{({u}_{z}^{\prime },\hat{{\vartheta }_{z}})}^{*}-\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})(t),\end{eqnarray} \tag{ 38 }$$

where $P,I\in [0,1]$ are a proportional and integral gain for the outer loop control, both relative to the current ${K}_{z}^{\prime }$. The reason for this is that larger heights involve higher speeds, so that K_z should be changed more quickly for larger ${K}_{z}^{\prime }$ than for smaller ${K}_{z}^{\prime }$. The adaptive gain control is represented as a control diagram in figure 7.

Zoom In Zoom Out Reset image size

This strategy has been tested in simulation for $z\in \{2,4,\ldots ,10\}$ and ${v}_{\mathrm{wind}}\in \{-1,0,1\}$. The simulation stops when $| {e}_{\mathrm{cov}}| \lt 0.005$. Figure 8 shows for each simulation run the last gain and height with a marker. The markers are again color-coded for the wind from blue (−1 m/s) to red (1 m/s). The black dashed line shows a linear least squares fit.

Zoom In Zoom Out Reset image size

The main result is that this method indeed gives an approximately linear relationship between z and K_z, with linear fit $z=0.07{K}_{z}+0.1$. Although the wind influences the hover altitude (since it is not accounted for in the initial thrust) it again hardly influences the results.

Is there a possibility of landing and continuously estimating the distance to the surface? Figure 8 suggests that there is. If the robot descends while keeping $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})$ at a fixed negative value, then the gain K_z will directly represent the height z during the landing. This strategy of 'landing on the edge of oscillation' can use the exact same adaptive gain control as explained above, but then with ${c}^{2}\gt 0$.

Simulations of this strategy have been performed for $z\in \{5,6,\ldots ,10\}$. The implementation of the strategy starts with a hover maneuver (${c}^{2}=0$) with K_z = 50, I = 0.005, and P = 0.15. If in hover the condition $\mathrm{cov}{({u}_{z}^{\prime },\hat{{\vartheta }_{z}})}^{*}=-0.05$ is met, the landing is commenced. During landing ${c}^{2}=0.05$, $\mathrm{cov}{({u}_{z}^{\prime },{\vartheta }_{z})}^{*}=-0.05$.

Figure 9 shows the K_z versus z during the landing phase. The markers are color-coded for the starting height z₀ in the landing phase, from blue (10 m) to green (5 m). Each starting point is indicated with a red cross. After switching from ${c}^{2}=0$ to ${c}^{2}=0.05$, it takes some time for the outer loop to compensate, which causes K_z to vary at the start of the landing phase. After that, all K_z are linearly related to z, with linear fit $z=0.11{K}_{z}-0.1$.

Zoom In Zoom Out Reset image size

The experimental setup is shown in figure 10. A Parrot AR drone 2.0 is used, running the Paparazzi open source autopilot software on board [22, 36]³ . This means that the Paparazzi autopilot is in full control of the AR drone, processing the sensors on board. The AR drone is equipped with an IMU (accelerometers, gyros, magnetometer, pressure sensor), and a downward pointing camera and sonar. For the purpose of the experiments, the vertical control loop implements the fixed and adaptive gain controls explained in the previous sections, and hence only uses the relative velocity estimates coming from the vision process, which is further explained below. The sonar is logged for analysis purposes. The horizontal position is stabilized as much as possible. In the indoor experiments, an Optitrack motion tracking system measures the drone's X- and Y-position at 120 Hz with millimeter precision. Paparazzi then performs the outer loop control on the basis of these measurements. In the outdoor experiments, where the drone is subject to much larger disturbances from the wind, a human pilot attempts to keep the drone in the same horizontal position by means of manual control.

Zoom In Zoom Out Reset image size

The vision process is designed as follows. The images are processed with a rather standard vision pipeline available in Paparazzi. Maximally 40 corners are detected with the method in [40], and these corners are tracked to the next image with the Lucas–Kanade optical flow algorithm [31]. The optical flow divergence is estimated by means of the distances between tracked corners in two subsequent images. From all possible pairs of optical flow vectors, 50 pairs are sampled with replacement. Per pair of optical flow vectors, the image distance in the previous image ${d}_{t-{\rm{\Delta }}t}$ and the image distance in the current image d_t (see figure 11) are used for a single estimate i of the divergence:

$$\begin{eqnarray}&&{D}_{i}={d}_{t-{\rm{\Delta }}t}/({d}_{t}-{d}_{t-{\rm{\Delta }}t}).\end{eqnarray} \tag{ 39 }$$

A global divergence estimate $\hat{D}$ is obtained by taking the median of estimates D_i, $i\in \{1,2,\ldots ,50\}$. With the help of the frame rate (which is on average 25 Hz) and the camera's field of view ($47.5^\circ$ in the horizontal direction), this estimated divergence is transformed to the estimate of the $\hat{{\vartheta }_{z}}$ expressed in ${s}^{-1}$.

Zoom In Zoom Out Reset image size

The estimated relative velocity $\hat{{\vartheta }_{z}}$ is low-pass filtered over time and then used in a control loop ${u}_{z}^{\prime }={K}_{z}({\vartheta }_{z}^{*}-\hat{{\vartheta }_{z}})$, where ${u}_{z}^{\prime }$ is the vertical thrust command. Please note that in the real-world experiments, the thrust command is bounded to the interval ${u}_{z}^{\prime }\in [0.6\;M,0.9\;M]$, where M is the maximal thrust that can be commanded to the propellors. The program allows both fixed gain control and adaptive gain control as in section 5.2. Experiments were performed in an indoor and an outdoor environment, shown in figure 12. The main parameters used in the experiments have been determined empirically for the used AR drone 2.0 and will be mentioned along with the relevant experiment.

Zoom In Zoom Out Reset image size

First, it is verified that a fixed-gain constant divergence landing results in instability at different heights. The results of this test for ${K}_{z}\in \{0.5,1,2\}$ (green, blue, and red lines) and ${c}^{2}=0.1$ are shown in figure 13. The left plot shows the height over time for nine such landings. Dotted lines indicate the first point at which $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})\leqslant -1.0$, set to trigger the drone's automatic landing procedure. This value is larger than in simulation to prevent too-early landing triggering. The reason is that the real drone does not only have a delay in the observations, but also noise and it is subject to larger disturbances and uncertainty in the actuation. The main observation from the figure is that also for the real robot, higher gains lead to an instability at larger heights. The right part of figure 13 shows a marker for each point $({K}_{z},z)$ at which self-induced oscillations are detected, and a corresponding linear fit, with parameters $z=1.01{K}_{z}-0.2$. The mean absolute error when using this function for height estimation is 0.11 m.

Zoom In Zoom Out Reset image size

The second finding to verify is the determination of height around hover. For the real robot, $\mathrm{cov}{({u}_{z}^{\prime },{\vartheta }_{z})}^{*}=-0.025$ is employed, with I = 0.25 and P = 10. During the hover experiment, the robot has been sent to different heights, subsequently activating the adaptive gain control. In order to get an impression of what the adaptive control does, let us first zoom in on the relevant variables during a short part of the flight, shown in figure 14. In this part of the flight, the drone has just ascended to ∼2 m (see top left plot), while it has an initial gain of K_z = 1.0 (bottom left). The drone starts to regulate its relative velocity to ${\vartheta }_{z}^{*}=0$ (top right), and initially there are no self-induced oscillations as witnessed by the $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})\approx 0$ (bottom right). The low covariance makes the gain ${K}_{z}^{\prime }$ increase over time. This gradually causes ${\vartheta }_{z}$ to show ever larger oscillations, which leads to more negative $\mathrm{cov}({u}_{z}^{\prime },\hat{{\vartheta }_{z}})$. Around t = 114.5 s, the covariance reaches and overshoots its set point, and the gain ${K}_{z}^{\prime }$ starts stabilizing around 1.6 in order to keep the covariance around −0.025. The question now is at what values ${K}_{z}^{\prime }$ will stabilize at other heights, and whether there is indeed a linear relationship between z and K_z.

Zoom In Zoom Out Reset image size

The middle row of figure 15 shows the result of having the drone hover at different heights with $\mathrm{cov}{({u}_{z}^{\prime },{\vartheta }_{z})}^{*}=-0.025$. The left plot shows a short time period during which the drone is continuously adjusting its gain (K_z and ${K}_{z}^{\prime }$ shown with the solid and dashed green lines, respectively), while attempting to keep the same height (z is represented by the red line). The right plot summarizes the results of five of such periods. The small blue markers represent the instantaneous $({K}_{z},z)$-pairs, while each large red marker is the average $(\bar{{K}_{z}},\bar{z})$ over each period. The dashed line is a linear least squares fit through the red markers. Although more noisy than in simulation, the relation between z and K_z seems to be roughly linear, as predicted by the theoretical derivation. When using the linear fit $\hat{z}=0.93{K}_{z}+0.3$ as a height estimation function, the median absolute error of all observations is $| z-\hat{z}| =0.15$ m, while the mean absolute error is 0.26 m.

Zoom In Zoom Out Reset image size

The third finding to verify is the possibility to land on the edge of oscillation. The AR drone is sent to a height of 3.5 m. Then, the adaptive gain control is started, at first with ${c}^{2}=0$. As soon as $| {e}_{\mathrm{cov}}| \lt 0.005$, the landing phase starts. The bottom row of figure 15 shows the results for this test. The bottom left plot shows K_z, ${K}_{z}^{\prime }$, and z over time during a single landing on the edge of oscillation. The right plot shows the points $(z,{K}_{z})$ of this and two other landings, where different experiments have different markers and colors. The dashed line is a linear fit through all points. Although the results are quite noisy, again a roughly linear relation appears, with as linear fit $z=0.68{K}_{z}+0.5$. In this case, the median absolute error of all observations is $| z-\hat{z}| =0.24$ m, while the mean absolute error is 0.29 m.

Outdoor experiments have been performed to confirm that the method also works in an outdoor environment with wind and wind gusts. The right part of figure 12 shows the outdoor experimental setup. The ground surface consisted of small stones, which already provide ample texture. Some beach toys were added to the scene to also ensure visible texture at larger heights. The bottom left part of figure 16 shows the summary of the results from the hover experiment. Just as indoors, there is a roughly linear relationship between the height z as measured by the sonar and the gain K_z as determined with the adaptive gain control. The median and mean absolute errors in height of the fit are both 0.31 m. The bottom right part of figure 16 shows the results from three landings on the edge of oscillation, again showing higher gains at larger heights. The median absolute error in height of the fit is 0.32 m, the mean is 0.40 m.

Zoom In Zoom Out Reset image size

Videos of some of the experiments can be seen at⁴ .

The presented strategy forms a novel hypothesis on how flying insects such as fruitflies and honeybees can estimate distances, viz. by means of the detection of self-induced oscillations. In [6] a different, thrust-based strategy for distance estimation was introduced, and its implications for landing insects discussed. In part, the implications of these strategies overlap. They both explain how flying insects can trigger landing responses at a particular given distance, such as fruitflies that extend their legs just before landing (e.g. [5]). However, there is a significant difference in the strategies, which has its consequences for the explanation of flying insects' behavior. A few notable differences in this respect will be highlighted below.

A first difference is in the possible explanation of the hover phase honeybees exhibit just before touchdown [13]. The hover phase could in principle be explained as a maneuver that is triggered by a distance measurement, for instance by means of the thrust-based strategy of [6]. However, the proposed strategy suggests instead that the hover mode is an intrinsic property of optical flow control and is part of the distance estimation itself. As the honeybee gets closer to the surface, its control gain will start becoming too high. This will result in self-induced oscillations and lead to a situation of zero divergence, i.e. hover. In this light, it is interesting to remark the characteristic little 'bump' in the trajectory in figure 4. Similar bumps can be observed in figure 4 in [13] (for almost straight landings), and in figure 5 in [42] (for grazing landings).

A second difference is that the stability-based strategy explicitly allows for the observation that a visual stimulus itself can trigger the landing responses, even for tethered insects [4, 43]. The reason for this is that the strategy depends on the control stability and hence the system bandwidth. A standard way to measure the system bandwidth is to make a Bode plot that maps the system's frequency response. At too-high frequencies, the phase shift between the quickly changing observation and the subsequent control input will become larger and the magnitude of the control input lower. If the magnitude of the triggered control input is large enough, a purely visual stimulus may be detected as a self-induced oscillation.

Concerning flying robots, the novel proposed distance estimation strategy provides a novel way to perceive distances with a single vision sensor. This may turn out to be essential for tiny flying robots such as the Robobee [33], for which any type of sensor is a significant payload [20, 23, 39]. For slightly larger drones such as 25 g 'pocket drones' (e.g. [11]), the finding is also immediately relevant. For instance, currently the lightest, fully autonomously flying robot is the 20 g DelFly Explorer [46], which uses a 4 g stereo vision system with onboard processing to avoid obstacles in its environment. The proposed stability-based strategy to distance estimation may provide a bigger potential to even lighter, single-camera solutions, also for obstacle avoidance. In addition, it can provide scale to monocular navigation methods such as visual odometry [12, 17]. Even for MAVs on the order of ∼1 kg or larger, the method may still be useful. It allows us to estimate potentially large distances without compromising the MAVs' payload capability.

However, in order for stability-based distance estimation to live up to its potential, some issues need to be further investigated. The experiments have shown the strategy to work in an onboard processing scheme with a Parrot AR drone 2.0 with a median height estimation error in the order of ∼0.3 m. As a proof of concept this suffices, but it is desirable to improve the accuracy of the measurement. This seems quite possible by further improving both the inner loop divergence control (which now only involved a P-gain) and the outer loop adaptive control. Moreover, other, more reliable ways of detecting self-induced oscillations may be created. Such improvements could not only lead to more accurate distance estimates, but also to smoother divergence control. If high-performance, but smooth landing trajectories are the goal, the gain should always be slightly below the critical gain value leading to self-induced oscillations.

Let us start from the observation:

$$\begin{eqnarray}&&y={\vartheta }_{x}=\displaystyle \frac{{v}_{x}}{z},\end{eqnarray} \tag{ 43 }$$

Since both the horizontal and vertical axes are involved, the state space model will have four variables. Linearizing the state space model gives:

$$\begin{eqnarray}{\rm{\Delta }}y(t)=[\begin{array}{cccc}\displaystyle \frac{-{v}_{x}}{{z}^{2}} & 0 & 0 & \displaystyle \frac{1}{z}\end{array}]\\ \quad \times \ {[\begin{array}{cccc}{\rm{\Delta }}z(t) & {\rm{\Delta }}{v}_{z}(t) & {\rm{\Delta }}x(t) & {\rm{\Delta }}{v}_{x}(t)\end{array}]}^{\top },\end{eqnarray} \tag{ 44 }$$

so that the state space model matrices are:

$$\begin{eqnarray}A & = & [\begin{array}{cccc}0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\end{array}]\ \ B=[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}]\\ C & = & [\begin{array}{cccc}\displaystyle \frac{-{v}_{x}}{{z}^{2}} & 0 & 0 & \displaystyle \frac{1}{z}\end{array}]\ \ D=[0].\end{eqnarray} \tag{ 45 }$$

Again, the discretized system is studied, which has the following state space model matrices corresponding to the continuous ones in equation (45):

$$\begin{eqnarray}{\rm{\Phi }} & = & [\begin{array}{cccc}1 & T & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & T\\ 0 & 0 & 0 & 1\end{array}]\ \ {\rm{\Gamma }}=[\begin{array}{c}0\\ 0\\ \displaystyle \frac{{T}^{2}}{2}\\ T\end{array}]\\ C & = & [\begin{array}{cccc}\displaystyle \frac{-{v}_{z}}{{z}^{2}} & 0 & 0 & \displaystyle \frac{1}{z}\end{array}]\ \ D=[0],\end{eqnarray} \tag{ 46 }$$

where T is the discrete time step in seconds. The transfer function of the open loop system can be determined to be:

$$\begin{eqnarray}&&G(w)=C{({wI}-{\rm{\Phi }})}^{-1}{\rm{\Gamma }},\end{eqnarray} \tag{ 47 }$$

$$\begin{eqnarray}&&=\displaystyle \frac{T}{z(w-1)},\end{eqnarray} \tag{ 48 }$$

where we use w as the Z-transform variable, since z already represents height. The feedback transfer function is:

$$\begin{eqnarray}&&G(w)=\displaystyle \frac{{K}_{x}T}{z(w-1)(\displaystyle \frac{{K}_{x}T}{z(w-1)}+1)}.\end{eqnarray} \tag{ 49 }$$

Equation (49) shows that, given a gain K_x and time step T, the dynamics and stability of the system depend on the height z.

Setting $w=-1$ in the denominator and equating it with 0 gives:

$$\begin{eqnarray}&&-2z(\displaystyle \frac{{K}_{x}T}{-2z}+1)=0,\end{eqnarray} \tag{ 50 }$$

which leads to an equation that expresses the height at which the control system will become unstable in terms of the unstable gain value K_x:

$$\begin{eqnarray}&&z=\displaystyle \frac{1}{2}{K}_{x}T,\end{eqnarray} \tag{ 51 }$$

which is exactly the same formula as for the vertical movements studied in section 4.

It is instructive to also look at what a possible thrust-based method would look like for the horizontal motion direction. For this, let us again start from the observation ${\vartheta }_{x}$, which is differentiated over time:

$$\begin{eqnarray}&&\dot{{\vartheta }_{x}}=\displaystyle \frac{{a}_{x}}{z}-\displaystyle \frac{{v}_{x}{v}_{z}}{{z}^{2}}=\displaystyle \frac{{a}_{x}}{z}-{\vartheta }_{x}{\vartheta }_{z},\end{eqnarray} \tag{ 52 }$$

leading to:

$$\begin{eqnarray}&&z=\displaystyle \frac{{a}_{x}}{(\dot{{\vartheta }_{x}}+{\vartheta }_{x}{\vartheta }_{z})}.\end{eqnarray} \tag{ 53 }$$

Equation (53) shows that the height can be estimated with the help of a horizontal accelerometer measurement and the observables ${\vartheta }_{z}$, ${\vartheta }_{x}$, and the time derivative $\dot{{\vartheta }_{x}}$ (as was done in [25]).

If the relative velocity ${\vartheta }_{x}$ (sometimes referred to as 'ventral flow') is kept constant, then $\dot{{\vartheta }_{x}}=0$, and:

$$\begin{eqnarray}&&z=\displaystyle \frac{{a}_{x}}{{\vartheta }_{x}{\vartheta }_{z}},\end{eqnarray} \tag{ 54 }$$

where in a vacuum environment, a_x can be assumed to be u_x. A constant ventral flow and divergence landing would then again lead to a way to estimate z, as u_x is known and ${\vartheta }_{x}$ and ${\vartheta }_{z}$ can be assumed equal to constants.

Interestingly, also this horizontal motion case does not allow for distance estimation while staying at the same height (${\vartheta }_{z}=0$). The reason for this can be seen when rearranging equation (54):

$$\begin{eqnarray}&&{a}_{x}=z({\vartheta }_{x}{\vartheta }_{z}),\end{eqnarray} \tag{ 55 }$$

which shows that ${\vartheta }_{z}=0$ implies a_x = 0. Indeed, if ${\vartheta }_{z}=0$, ${\vartheta }_{x}$ can only be constant if there is no horizontal acceleration.