Autonomous navigation of micro aerial vehicles using high-rate and low-cost sensors

Abstract

The combination of visual and inertial sensors for state estimation has recently found wide echo in the robotics community, especially in the aerial robotics field, due to the lightweight and complementary characteristics of the sensors data. However, most state estimation systems based on visual-inertial sensing suffer from severe processor requirements, which in many cases make them impractical. In this paper, we propose a simple, low-cost and high rate method for state estimation enabling autonomous flight of micro aerial vehicles, which presents a low computational burden. The proposed state estimator fuses observations from an inertial measurement unit, an optical flow smart camera and a time-of-flight range sensor. The smart camera provides optical flow measurements up to a rate of 200 Hz, avoiding the computational bottleneck to the main processor produced by all image processing requirements. To the best of our knowledge, this is the first example of extending the use of these smart cameras from hovering-like motions to odometry estimation, producing estimates that are usable during flight times of several minutes. In order to validate and defend the simplest algorithmic solution, we investigate the performances of two Kalman filters, in the extended and error-state flavors, alongside with a large number of algorithm modifications defended in earlier literature on visual-inertial odometry, showing that their impact on filter performance is minimal. To close the control loop, a non-linear controller operating in the special Euclidean group SE(3) is able to drive, based on the estimated vehicle’s state, a quadrotor platform in 3D space guaranteeing the asymptotic stability of 3D position and heading. All the estimation and control tasks are solved on board and in real time on a limited computational unit. The proposed approach is validated through simulations and experimental results, which include comparisons with ground-truth data provided by a motion capture system. For the benefit of the community, we make the source code public.

Appendices

Appendix A: Quaternion conventions and properties

We use, as in Solà (2015), the Hamilton convention for quaternions. If we denote a quaternion ${}^G\mathbf{q}_L$ representing the orientation of a local frame L with respect to a global frame G, then a generic composition of two quaternions is defined as

$$\begin{aligned} {}^G\mathbf{q}_{C}\,=\,{}^G\mathbf{q}_{L}\otimes {}^L\mathbf{q}_{C} ={}^G\mathbf{Q}^{+}_{L}\,{}^L\mathbf{q}_{C}\,=\,{}^L\mathbf{Q}^{-}_{C}\,{}^G\mathbf{q}_{L}, \end{aligned}$$

(46)

where, for a quaternion $\mathbf{q}=[w,x,y,z]^\top$, we can define $\mathbf{Q}^{+}$ and $\mathbf{Q}^{-}$ respectively as the left- and right-quaternion product matrices,

$$\begin{aligned} \mathbf{Q}^{+} = \begin{bmatrix} w&-x&-y&-z \\ x&w&-z&y \\ y&z&w&-x \\ z&-y&x&w \end{bmatrix}~,~ \mathbf{Q}^{-} = \begin{bmatrix} w&-x&-y&-z \\ x&w&z&-y \\ y&-z&w&x \\ z&y&-x&w \end{bmatrix}\,. \end{aligned}$$

(47)

In the quaternion product, we notice how the right-hand quaternion is defined locally in the frame L, which is specified by the left-hand quaternion. Vector transformation from a local frame L to the global G is performed by the double product

$$\begin{aligned} {}^G\mathbf{v} = {}^G\mathbf{q}_{L}\otimes {}^L\mathbf{v}\otimes ({}^G\mathbf{q}_{L})^* = {}^G\mathbf{q}_{L}\otimes {}^L\mathbf{v}\otimes {}^L\mathbf{q}_{G}, \end{aligned}$$

(48)

where we use the shortcut $\mathbf{q}\otimes \mathbf{v} \equiv \mathbf{q}\otimes [0,\mathbf{v}]^\top$ for convenience of notation.

Throughout the paper, we note $\mathbf{q}\{x\}$ the quaternion and $\mathbf{R}\{x\}$ the rotation matrix equivalents to a generic orientation x. A rotation $\varvec{\theta }= \theta \mathbf u$, of $\theta$ radians around the unit axis $\mathbf u$, can be expressed in quaternion and matrix forms using the exponential maps

$$\begin{aligned} \mathbf{q}\{{\varvec{\theta }}\} =&\, e^{{\varvec{\theta }}/2} = \begin{bmatrix} \cos (\theta /2) \\ \mathbf{u}\sin (\theta /2) \end{bmatrix} \xrightarrow [\theta \rightarrow 0]{} \begin{bmatrix} 1 \\ {\varvec{\theta }}/2 \end{bmatrix} \,, \end{aligned}$$

(49)

$$\begin{aligned} \mathbf{R}\{{\varvec{\theta }}\} =&\, e^{[{\varvec{\theta }}]_\times } = \mathbf{I} \!+\! \sin \theta [\mathbf{u}]_\times \!+\! (1 \!-\! \cos \theta )[\mathbf{u}]_\times ^2\nonumber \\&\xrightarrow [\theta \rightarrow 0]{} \mathbf{I} \!+\! [{\varvec{\theta }}]_\times \end{aligned}$$

(50)

We also write $\mathbf{R}\,=\,\mathbf{R}\{\mathbf{q}\}$, according to

$$\begin{aligned} {\mathbf{R}\{\mathbf{q}\} = \begin{bmatrix} w^2\!+\!x^2\!-\!y^2\!-\!z^2&2(xy-wz)&2(xz + wy) \\ 2(xy + wz)&w^2\!-\!x^2\!+\!y^2\!-\!z^2&2(yz - wx) \\ 2(xz - wy)&2(yz + wx)&w^2\!-\!x^2\!-\!y^2\!+\!z^2 \end{bmatrix}} \end{aligned}$$

(51)

Finally, the time-derivative of the quaternion is

$$\begin{aligned} \dot{\mathbf{q}} = \frac{1}{2}{\varvec{\Omega }}({\varvec{\omega }})\mathbf{q} = \frac{1}{2}{} \mathbf{q}\otimes {\varvec{\omega }}\,, \end{aligned}$$

(52)

with ${\varvec{\omega }}$ the angular velocity in body frame, and ${\varvec{\Omega }}$ the skew-symmetric matrix defined as

$$\begin{aligned} {\varvec{\Omega }}({\varvec{\omega }}) \triangleq \mathbf{Q}^-({\varvec{\omega }}) = \begin{bmatrix} 0&-{\varvec{\omega }}^\top \\ {\varvec{\omega }}&-[{\varvec{\omega }}]_\times \end{bmatrix}. \end{aligned}$$

(53)

Appendix B: Filter transition matrices

We detail the construction of the filter transition matrix for the three involved integrals: ESKF nominal—(22), ESKF error—(23), and EKF true—(25) kinematics. For each case, we need to define the matrix $\mathbf{A}$ as the Jacobian of the respective continuous-time system, and build the transition matrix $\mathbf{F}_N$ as the truncated Taylor series (20), i.e.,

$$\begin{aligned} \mathbf{F}_N = \sum _{n=0}^N \frac{1}{n!}{} \mathbf{A}^n\Delta t^n = \mathbf{I}+\mathbf{A}\Delta t +\frac{1}{2\,!}{} \mathbf{A}^2\Delta t^2 +\cdots \end{aligned}$$

In the following paragraphs, we detail the matrices $\mathbf{A}$ for each case, and some examples of their first powers up to $n=3$. The reader should find no difficulties in building the powers of $\mathbf{A}$ that have not been detailed, and the transition matrices $\mathbf{F}_N$ using the Taylor series above.

The Jacobian $\mathbf{A}=\partial f(\mathbf{x},\delta \mathbf{x},\cdot )/\partial \delta \mathbf{x}$ of the ESKF’s continuous time error-state system f() (18) using GE is,

$$\begin{aligned} \mathbf{A} = \begin{bmatrix} 0&\mathbf{I}&0&0&0 \\ 0&0&\mathbf{V}&-\mathbf{R}&0 \\ 0&0&0&0&-\mathbf{R} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix}\,, \end{aligned}$$

(54)

with $\mathbf{V} = -[\mathbf{R}(\mathbf{a}_S-\mathbf{a}_b)]_\times$. Its powers are,

$$\begin{aligned} \mathbf{A}^2 = \begin{bmatrix} 0&0&\mathbf{V}&-\mathbf{R}&0 \\ 0&0&0&0&-\mathbf{V}{} \mathbf{R} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix} ~,~ \mathbf{A}^3 = \begin{bmatrix} 0&0&0&0&-\mathbf{V}{} \mathbf{R} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix}\,, \end{aligned}$$

and $\mathbf{A}^{n} = \mathbf{0}$ for $n>3$. For LE we have

$$\begin{aligned} { \mathbf{A} = \begin{bmatrix} 0&\mathbf{I}&0&0&0 \\ 0&0&\mathbf{V}&-\mathbf{R}&0 \\ 0&0&{\varvec{\Theta }}&0&-\mathbf{I} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix}, \mathbf{A}^2 = \begin{bmatrix} 0&0&\mathbf{V}&-\mathbf{R}&0 \\ 0&0&\mathbf{V}{\varvec{\Theta }}&0&-\mathbf{V} \\ 0&0&{\varvec{\Theta }}^2&0&-{\varvec{\Theta }} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix},\cdots } \end{aligned}$$

with $\mathbf{V} = -\mathbf{R}[\mathbf{a}_S-\mathbf{a}_b]_\times$, and ${\varvec{\Theta }} = -[{\varvec{\omega }}_S-{\varvec{\omega }}_b]_\times$.

The Jacobians $\mathbf{A}=\partial f(\mathbf{x},\cdot )/\partial \mathbf{x}$ of the continuous-time EKF true—(16) and ESKF nominal—(17) systems are equal to each other, having

$$\begin{aligned} { \mathbf{A} = \begin{bmatrix} 0&\mathbf{I}&0&0&0 \\ 0&0&\mathbf{V}&-\mathbf{R}&0 \\ 0&0&\mathbf{W}&0&\mathbf{Q} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix} ~,~ \mathbf{A}^2 = \begin{bmatrix} 0&0&\mathbf{V}&-\mathbf{R}&0 \\ 0&0&\mathbf{V}{} \mathbf{W}&0&\mathbf{V}{} \mathbf{Q} \\ 0&0&\mathbf{W}^2&0&\mathbf{W}{} \mathbf{Q} \\ 0&0&0&0&0 \\ 0&0&0&0&0 \\ \end{bmatrix}} \,,\cdots \end{aligned}$$

where $\mathbf{V}$, $\mathbf{W}$ and $\mathbf{Q}$ are defined by

$$\begin{aligned} \mathbf{V}&= \frac{\partial \mathbf{R}\{\mathbf{q}\}\,(\mathbf{a}_S-\mathbf{a}_b)}{\partial \mathbf{q}} \end{aligned}$$

(55a)

$$\begin{aligned} \mathbf{W}&= \frac{\partial \frac{1}{2}\mathbf{q}\otimes ({\varvec{\omega }}_S-{\varvec{\omega }}_b)}{\partial \mathbf{q}} \end{aligned}$$

(55b)

$$\begin{aligned} \mathbf{Q}&= \frac{\partial \frac{1}{2}\mathbf{q}\otimes ({\varvec{\omega }}_S-{\varvec{\omega }}_b)}{\partial {\varvec{\omega }}_b}, \end{aligned}$$

(55c)

and are developed hereafter. For the first Jacobian $\mathbf{V}$ it is convenient to recall the derivative of a rotation of a vector $\mathbf a$ by a quaternion $\mathbf{q}=[w,x,y,z]^\top =[w,\mathbf{v}]^\top$ with respect to the quaternion,

$$\begin{aligned} \mathbf{V}(\mathbf{q},\mathbf{a})&\triangleq \frac{\partial \mathbf{R}\{\mathbf{q}\}\,\mathbf{a}}{\partial \mathbf q} = \frac{\partial (\mathbf{q}\otimes \mathbf{a}\otimes \mathbf{q}^*)}{\partial \mathbf q}\\&= { 2\big [w \mathbf{a} \!+\! \mathbf{v}\!\times \!\mathbf{a} ~\big |~ \mathbf{v}{} \mathbf{a}^\top \!-\! \mathbf{a}{} \mathbf{v}^\top \!+\! \mathbf{a}^\top \mathbf{v}{} \mathbf{I}_3 \!-\! w[\mathbf{a}]_\times \big ] \nonumber }\,, \end{aligned}$$

(56)

having therefore

$$\begin{aligned} \mathbf{V} = \mathbf{V}(\mathbf{q},\,\mathbf{a}_S - \mathbf{a}_b)~. \end{aligned}$$

(57)

For the Jacobian $\mathbf{W}$ we have from (52)

$$\begin{aligned} \mathbf{W} = \frac{1}{2}{\varvec{\Omega }}({\varvec{\omega }}_S-{\varvec{\omega }}_b)~, \end{aligned}$$

(58)

with $\varvec{\Omega }({\varvec{\omega }})$ the skew-symmetric matrix defined in (53).

Finally, for the Jacobian $\mathbf{Q}$ we use (46), (47) and (49) to obtain

$$\begin{aligned} \mathbf{Q} = - \frac{1}{2} \begin{bmatrix} -x&-y&-z \\ w&-z&y \\ z&w&-x \\ -y&x&w \end{bmatrix}. \end{aligned}$$

(59)