Contact Inertial Odometry: Collisions are your Friends

Abstract

Autonomous exploration of unknown environments with aerial vehicles remains a challenge, especially in perceptually degraded conditions. Dust, fog, or a lack of visual or LiDAR-based features results in severe difficulties for state estimation algorithms, which failure can be catastrophic. In this work, we show that it is indeed possible to navigate in such conditions without any exteroceptive sensing by exploiting collisions instead of treating them as constraints. To this end, we present a novel contact-based inertial odometry (CIO) algorithm: it uses estimated external forces with the environment to detect collisions and generate pseudo-measurements of the robot velocity, enabling autonomous flight. To fully exploit this method, we first perform modeling of a hybrid ground and aerial vehicle which can withstand collisions at moderate speeds, for which we develop an external wrench estimation algorithm. Then, we present our CIO algorithm and develop a reactive planner and control law which encourage exploration by bouncing off obstacles. All components of this framework are validated in hardware experiments and we demonstrate that a quadrotor can traverse a cluttered environment using an IMU only. This work can be used on drones to recover from visual inertial odometry failure or on micro-drones that do not have the payload capacity to carry cameras, LiDARs or powerful computers.

T. Lew and T. Emmei—Both authors contributed equally to this manuscript.

Video: Experimental results are available at https://youtu.be/AGyu9tkhSLk.

Appendices

A Contact Position Estimation

1.1 A.1 Analytical Solution

This section presents the analytical solution of the contact point estimation method presented in Sect. 2.2. For conciseness, we denote $\tilde{F}_e^z:=(F_e^z-m_t g)$. The positions of the estimated contact forces on the left and right wheels are decomposed into two sets of equations depending on whether $F_y<0$ or $F_y\ge 0$.

First, if $F_y < 0$, the solution for the position of the contact point on the left wheel is given as

$$\begin{aligned} p^l_{x}&=\, \frac{-a_1c_1 -\mathrm {sgn}(F_e^x)|b_1|\sqrt{R^2(a_1^2+b_1^2)-c_1^2}}{a_1^2+b_1^2}\end{aligned}$$

(16a)

$$\begin{aligned} p^l_{z}&=\, \frac{-a_1c_1 - \mathrm {sgn}(\tilde{F}_e^z)|a_1|\sqrt{R^2(a_1^2+b_1^2) -c^2}}{a_1^2+b_1^2}, \end{aligned}$$

(16b)

with $a_1 = M_{e}^x + L\tilde{F}_e^z$, $b_1 = M_{e}^z - LF_{e}^x$ and $c_1 = 2LM^l_w$. Using these equations, the estimated contact point on the right wheel is derived as

$$\begin{aligned} p^r_{x}&=\, \frac{-A_1C_1 - \mathrm {sgn}(F_e^x)|B_1|\sqrt{r^2(A_1^2+B_1^2) -C_1^2}}{A_1^2+B_1^2}\end{aligned}$$

(17a)

$$\begin{aligned} p^r_{z}&=\, \frac{-A_1C_1 - \mathrm {sgn}(\tilde{F}_e^z)|A_1|\sqrt{R^2(A_1^2+B_1^2)-C_1^2}}{A_1^2+B_1^2}, \end{aligned}$$

(17b)

with $A_1 = (- L^2\tilde{F}_e^{z2} + F_{e}^yp^l_zL\tilde{F}_e^z + M_{e}^{x2} + F_{e}^yp^l_zM_{e}^x)$, $B_1 = M_{e}^xM_{e}^z + L^2F_{e}^x\tilde{F}_e^z + LM_{e}^xF_{e}^x + LM_{e}^z\tilde{F}_e^z + 2LM_\mathrm{w}^lF_{e}^y + M_{e}^zF_{e}^yp^l_z - LF_{e}^xF_{e}^yp^l_z$ and $C_1 = - 2LM_{e}^xM_\mathrm{w}^r - 2L^2M_\mathrm{w}^r\tilde{F}_e^z$.

On the other hand, if $F_e^y \ge 0$, the position of the contact point on the left and right wheels are computed as

$$\begin{aligned} p^r_{x}&=\, \frac{-a_2c_2 -\mathrm {sgn}(F_e^x)|b_2|\sqrt{R^2(a_2^2+b_2^2)-c_2^2}}{a_2^2+b_2^2}\end{aligned}$$

(18a)

$$\begin{aligned} p^r_{z}&=\, \frac{-a_2c_2 - \mathrm {sgn}(\tilde{F}_e^z)|a_2|\sqrt{R^2(a_2^2+b_2^2) -c^2}}{a^2+b^2}\end{aligned}$$

(18b)

$$\begin{aligned} p^l_{x}&=\, \frac{-A_2C_2 - \mathrm {sgn}(F_e^x)|B_2|\sqrt{R^2(A_2^2+B_2^2) -C_2^2}}{A_2^2+B_2^2}\end{aligned}$$

(18c)

$$\begin{aligned} p^l_{z}&=\, \frac{-A_2C_2 - \mathrm {sgn}(\tilde{F}_e^z)|A_2|\sqrt{R^2(A_2^2+B_2^2)-C_2^2}}{A_2^2+B_2^2}, \end{aligned}$$

(18d)

with $a_2 = M_{e}^x - L\tilde{F}_e^z$, $b_2 = M_{e}^z + LF_{e}^x$, $c_2 = -2LM^r_w$, $A_2 = (M_{e}^x - L\tilde{F}_e^z)(M_{e}^x + L\tilde{F}_e^z + F_{e}^yp^r_z)$, $B_2 = M_{e}^xM_{e}^z + L^2F_{e}^x\tilde{F}_e^z - LM_{e}^xF_{e}^x - LM_{e}^z\tilde{F}_e^z - 2LM_\mathrm{w}^rF_{e}^y + M_{e}^zF_{e}^yp^r_z + LF_{e}^xF_{e}^yp^r_z$ and $C_2 = 2LM_{e}^xM_\mathrm{w}^l - 2L^2M_\mathrm{w}^l\tilde{F}_e^z$ .

1.2 A.2 Results: Contact Point Estimation

Experiments are conducted to validate our proposed contact point estimation method. The results are shown in Fig. 12.

Fig. 12.

Estimated contact point positions in rolling mode, with ground truth indicated by straight lines. The height of the contact point ($\mathrm {p_z^l},\mathrm {p_z^r}$) is negative since the origin is set to the center of the robot wheels. Each data point corresponds to a different experiment.

In each experiment, the hybrid vehicle Rollocopter is driven on a flat ground and collided frontally with a fixed box-shaped obstacle with a height of 15cm. From the known dimensions of the box and of the wheels, it is possible to determine the true contact point position. As shown in Fig. 12, the estimated positions $p^i_x$ and $p^i_z$ are close to the their true value for each one of the 6 experiments, validating our approach. This method could be used in future work to determine whether the contact point is caused by a collision in front of or behind the vehicle, or if it is detected due to rough terrain, providing additional useful information, e.g. for path planning.

B Derivation of Nonholonomic Model

This section derives additional nonholonomic constraints for the rolling mode of the hybrid vehicle.

1.1 B.1 Derivation of Nonholonomic Constraint

To derive (5), we first express the velocities at the contact points on the ground as

$$\begin{aligned} \boldsymbol{v_c^l}&= \begin{pmatrix} v_x \\ v_y \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ 0\\ \omega _z \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ L \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ \omega _y {+} \gamma _l\\ 0 \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ 0 \\ -R \end{pmatrix} {=} \begin{pmatrix} v_x {-} L\omega _z {-} R(\omega _y{+}\gamma _l)\\ v_y\\ 0 \end{pmatrix} \end{aligned}$$

(19a)

$$\begin{aligned} \boldsymbol{v_c^r}&= \begin{pmatrix} v_x \\ v_y \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ 0\\ \omega _z \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ -L \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ \omega _y {\,{+}\,}\gamma _r\\ 0 \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ 0 \\ -R \end{pmatrix} {=} \begin{pmatrix} v_x {+} L\omega _z {-} R(\omega _y{+} \gamma _l)\\ v_y\\ 0 \end{pmatrix} . \end{aligned}$$

(19b)

Assuming that the wheels of the hybrid vehicle remain in contact with the ground and that no slip occurs, As long as the wheels keep contact with the ground and no slip occurs, $\mathbf {v}_c^l=\mathbf {v}_c^r = \mathbf {0}$ hold. Therefore, (19) can be equivalently expressed as

$$\begin{aligned} v_x = \frac{R}{2}(\gamma _r + \gamma _l+2\omega _y),\ \ v_y = 0,\ \ \omega _z = \frac{R}{2L}(\gamma _r - \gamma _l) , \end{aligned}$$

(20)

which is equivalent to (5).

1.2 B.2 Derivation of Force Estimation for Rolling Mode

In this section, we show how to take into account nonholonomic constraints in our force estimation method. First, the nonholonomic motion of the hybrid vehicle implies that

$$\begin{aligned} v_y,v_z,\dot{v}_y,\omega _x = 0 . \end{aligned}$$

(21)

Also, by including the left and right wheel rolling resistance forces $F_d^l,F_d^r$ acting in the rolling direction, the dynamics of the hybrid vehicle in (3) are rewritten as

$$\begin{aligned} m_t\dot{v}_x&=F_{in}^x+F_{e}^x-F_{d}^l - F_{d}^r \end{aligned}$$

(22a)

$$\begin{aligned} m_tv_x\omega _z&=F_{e}^y \end{aligned}$$

(22b)

$$\begin{aligned} (I_t^z+2m_wL^2)\dot{\omega }_z&=(M_{in}^z+M_{e}^z-L(F_{d}^r-F_{d}^l)). \end{aligned}$$

(22c)

Furthermore, by analyzing the dynamics of each wheel in Eq. (3c), we have

$$\begin{aligned} I_\mathrm{w}(\dot{\gamma }_i + \dot{\omega }_y)= RF_{d}^i. \end{aligned}$$

(23)

Therefore, using the nonholonomic constraint (5) to replace $v_x,\dot{v}_x,\omega _z,\dot{\omega }_z$ and the previous result to replace $F_d^i$, the equations above are rewritten as

$$\begin{aligned} \left( \frac{m_tR}{2} + \frac{2I_\mathrm{w}}{R}\right) (\dot{\gamma }_r+\dot{\gamma }_l+2\dot{\omega _y}) =F_{in}^x + F_{e}^x \end{aligned}$$

(24a)

$$\begin{aligned} \frac{m_tR^2}{4L}(\gamma _r-\gamma _l)(\gamma _r+\gamma _l+2\omega _y)=F_{e}^y. \end{aligned}$$

(24b)

Also, (22c) can be rewritten by replacing $F_d^l,F_d^r,\dot{\omega }_z$ using Eqs. (23) and (5) as

$$\begin{aligned} \left( \frac{R}{2L}I_t^z+m_wLR+\frac{I_\mathrm{w}L}{R}\right) (\dot{\gamma }_r - \dot{\gamma }_l)= M_{in}^z+ M_{e}^z . \end{aligned}$$

(25)

Finally, the external wrench $\{F_{e}^x ,F_{e}^y, M_{e}^z \}$ can be computed as

$$\begin{aligned} F_{e}^x&= \left( \frac{m_tR}{2} + \frac{2I_\mathrm{w}}{R}\right) (\dot{\gamma }_r + \dot{\gamma }_l +2\dot{\omega }_y) -F_{in}^x\end{aligned}$$

(26a)

$$\begin{aligned} F_{e}^y&= \frac{m_tR^2}{4L}(\gamma _r-\gamma _l)(\gamma _r+\gamma _l+2\omega _y)\end{aligned}$$

(26b)

$$\begin{aligned} M_{e}^z&= \left( \frac{R}{2L}I_t^z+m_wLR+\frac{I_\mathrm{w}L}{R}\right) (\dot{\gamma }_r - \dot{\gamma }_l)-M_{in}^z . \end{aligned}$$

(26c)