On the VINS Resource-Allocation Problem for a Dual-Camera, Small-Size Quadrotor

Abstract

In this paper, we present a novel resource-allocation problem formulation for vision-aided inertial navigation systems (VINS) for efficiently localizing micro aerial vehicles equipped with two cameras pointing at different directions. Specifically, based on the quadrotor’s current speed and median distances to the features, the proposed algorithm efficiently distributes processing resources between the two cameras by maximizing the expected information gain from their observations. Experiments confirm that our resource-allocation scheme outperforms alternative naive approaches in achieving significantly higher VINS positioning accuracy when tested onboard quadrotors with severely limited processing resources.

This work was supported by the Air Force Office of Scientific Research (FA9550-10-1-0567) and the National Science Foundation (IIS-1111638).

Appendix A

In order to compute the expected information matrices in (7), we start by deriving the measurement Jacobian $\mathbf {H}_i$, appearing in (4), at time step $k'$. Consider a feature i, observed by the camera s, $s\in \{f,d\}$, whose position, $\mathbf {p}_i$, with respect to the camera frame $\{C_s^{k'}\}$, is:

$$\begin{aligned} {}^{{\scriptscriptstyle {C}}_s^{k'}}\mathbf {p}_i = \begin{bmatrix} x_i \\ y_i \\ z_i \end{bmatrix} = \begin{bmatrix} \rho _i\sin \theta _i\cos \phi _i \\ \rho _i\sin \theta _i\sin \phi _i \\ \rho _i\cos \theta _i \end{bmatrix} \end{aligned}$$

(11)

where $[x_i, y_i, z_i]^T$ and $[\phi _i, \theta _i, \rho _i]^T$ are the feature’s Cartesian and spherical coordinates, respectively. The camera measures the perspective projection of feature i:

$$\begin{aligned} \mathbf {z}=\pi \left( {}^{{\scriptscriptstyle {C}}_s^{k'}}\mathbf {p}_i\right) + \mathbf {n}_i = \begin{bmatrix} \frac{x_i}{z_i} \\ \frac{y_i}{z_i}\end{bmatrix} + \mathbf {n}_i, \ \ \ \ {}^{{\scriptscriptstyle {C}}_s^{k'}}\mathbf {p}_i = {}^{{\scriptscriptstyle {C}}_s^{k'}}_{{\scriptscriptstyle {C}}_s^k}\mathbf {R} ({}^{{\scriptscriptstyle {C}}_s^{k}}\mathbf {p}_i - {}^{{\scriptscriptstyle {C}}_s^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_s^{k'}}) \end{aligned}$$

(12)

where $\mathbf {n}_i$ is the measurement noise and ${}^{{\scriptscriptstyle {C}}_s^{k}}\mathbf {p}_i$ denotes the feature’s position with respect to the first-observing camera frame, $\{C_s^{k}\}$, at time step k, while ${}^{{\scriptscriptstyle {C}}_s^{k'}}_{{\scriptscriptstyle {C}}_s^k}\mathbf {R}$ and ${}^{{\scriptscriptstyle {C}}_s^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_s^{k'}}$ represent the rotation matrix and translation vector, respectively, between the camera frames at the corresponding time steps k and $k'$. Based on (12), the measurement Jacobian with respect to the camera’s position is:

$$\begin{aligned} \mathbf {H}_i = \frac{\partial \pi \left( {}^{{\scriptscriptstyle {C}}_s^{k'}}\mathbf {p}_i \right) }{\partial {}^{{\scriptscriptstyle {C}}_s^{k'}}\mathbf {p}_i} \frac{\partial {}^{{\scriptscriptstyle {C}}_s^{k'}}\mathbf {p}_i}{\partial {}^{{\scriptscriptstyle {C}}_s^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_s^{k'}}} = -\frac{1}{\rho _i\cos \theta _i} \begin{bmatrix}1&0&-\tan \theta _i\cos {\phi }_i \\ 0&1&-\tan \theta _i\sin {\phi }_i\end{bmatrix} {}^{{\scriptscriptstyle {C}}_s^{k'}}_{{\scriptscriptstyle {C}}_s^k}\mathbf {R} \end{aligned}$$

(13)

which leads to the following information matrix:

$$\begin{aligned} \frac{1}{\sigma _s^2}\mathbf {H}^{{\scriptscriptstyle {T}}}_i\mathbf {H}_i = \frac{1}{\sigma _s^2\rho ^2_i \cos ^{2}{\theta _i}} {}^{{\scriptscriptstyle {C}}_s^{k'}}_{{\scriptscriptstyle {C}}_s^k}\mathbf {R}^{{\scriptscriptstyle {T}}} \begin{bmatrix} 1&0&-\tan {\theta _i}\cos {\phi _i} \\ 0&1&-\tan {\theta _i}\sin {\phi _i} \\ -\tan {\theta _i}\cos {\phi _i}&-\tan {\theta _i}\sin {\phi _i}&\tan ^{2}{\theta _i} \end{bmatrix} {}^{{\scriptscriptstyle {C}}_s^{k'}}_{{\scriptscriptstyle {C}}_s^k}\mathbf {R} \end{aligned}$$

(14)

By employing the assumptions about the features’ distribution in (6), and substituting (14) into (4), yields:

(15)

Note that $\mathbf {H}_i$ in (13), and hence in (15), is expressed with respect to the position state, ${}^{{\scriptscriptstyle {C}}_s^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_s^{k'}}$, of the camera s [see (13)]. Therefore, and since we chose the system’s state to comprise the downward-camera’s position, ${}^{{\scriptscriptstyle {C}}_d^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_d^{k'}}$, the expected information gain from the corresponding feature observations is obtained by directly setting $s=d$ in (15), i.e.,

(16)

On the other hand, the forward-camera’s measurement Jacobian also depends on the extrinsics of the two cameras, i.e.,

$$\begin{aligned} \mathbf {H}_j = \frac{\partial \pi \left( {}^{{\scriptscriptstyle {C}}_f^{k'}}\mathbf {p}_i \right) }{\partial {}^{{\scriptscriptstyle {C}}_f^{k'}}\mathbf {p}_i} \frac{\partial {}^{{\scriptscriptstyle {C}}_f^{k'}}\mathbf {p}_i}{\partial {}^{{\scriptscriptstyle {C}}_f^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_f^{k'}}} \frac{\partial {}^{{\scriptscriptstyle {C}}_f^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_f^{k'}}}{\partial {}^{{\scriptscriptstyle {C}}_d^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_d^{k'}}}, \ \ \ \ \text {where} \ \ \ \frac{\partial {}^{{\scriptscriptstyle {C}}_f^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_f^{k'}}}{\partial {}^{{\scriptscriptstyle {C}}_d^{k}}\mathbf {p}_{{\scriptscriptstyle {C}}_d^{k'}}} = {}^{{\scriptscriptstyle {C}}_f}_{{\scriptscriptstyle {C}}_d}\mathbf {R} \end{aligned}$$

(17)

results from the geometric relationship between the two cameras across time steps k and $k'$. By comparing (17) to (13), the forward-camera’s Jacobian is obtained by first setting $s=f$ in (13), and then multiplying it, from the right, with the extrinsic-calibration rotation matrix ${}^{{\scriptscriptstyle {C}}_f}_{{\scriptscriptstyle {C}}_d}\mathbf {R}$. Consequently, the expected information gain from the forward camera becomes:

(18)

Lastly, employing the geometric relationship ${}^{{\scriptscriptstyle {C}}_f^{k'}}_{{\scriptscriptstyle {C}}_f^{k}}\mathbf {R} {}^{{\scriptscriptstyle {C}}_f}_{{\scriptscriptstyle {C}}_d}\mathbf {R} = {}^{{\scriptscriptstyle {C}}_f}_{{\scriptscriptstyle {C}}_d}\mathbf {R} {}^{{\scriptscriptstyle {C}}_d^{k'}}_{{\scriptscriptstyle {C}}_d^{k}}\mathbf {R}$ in (18) results in the expression for shown in (7).