### Inhalt des Dokuments

# Motion Tracking for Autonomous Search and Rescue Drones

### From Fachgebiet Regelungssysteme TU Berlin

### Abstract

The Searchwing project aims to build an autonomous low-cost drone specifically designed to support search and rescue (SAR) missions on the Mediterranean sea and support private rescue organizations to help migrants in distress. These drones rely on accurate real-time motion tracking. We derive new methods for non-restrictive calibration and sensor fusion that meet the special demands of operation on the high seas. Among others, we address the challenge of inertial sensor calibration on moving sea vessels by proposing novel optimization-based schemes for the estimation of accelerometer and gyroscope bias errors in such non-restrictive motion settings. In contrast to previous bias estimation methods, the proposed algorithms do not rely on stationary periods to estimate the parameters but leverage the fact that the strapdown-integrated velocities remain bounded if biases are accurately compensated.

### People involved

- Ricardo Martínez (Universidad Politécnica de Madrid)
- Thomas Seel
- Dustin Lehmann
- Daniel Laidig
- The Searchwing Team

## Description

Despite the great efforts made by civilian rescuers, every year hundreds of people still die on the Mediterranean Sea in their attempt to reach Europe. The Searchwing project is a group of volunteers from Augsburg and Berlin that supports the NGOs that search migrants in distress on the Mediterranean waters. The aim of the project is to build a fully automated drone with a 100 km range that extends the daily search area by a factor of 2. In Figure 2, the mission structure is illustrated.

To meet the mission requirements, the drone makes use of accurate sensor fusion and real time motion tracking. The autopilot runs an Extended Kalman Filter that combines information from 2 IMUs, GPS and barometer, and includes accelerometer and gyroscope calibration routines. The built-in calibration methods rely on resting periods, which are unrealistic to happen on a moving vessel. Therefore, we propose two optimization-based schemes for gyroscope and accelerometer bias estimation that are non-restrictive in terms of motion and are applicable on moving vessels. A description of the two proposed methods is briefly provided hereafter.

We consider two different coordinate systems: A body frame, which is aligned with the sensor axes, and an inertial reference frame, which is assumed to be fixed and stationary and defined by the initial arbitrary position of the IMU at the beginning of the motion. We use a simplified sensor model that accounts for gyroscope and accelerometer biases as the dominating sources of measurement errors.

### Method for gyroscope bias estimation

The specific forces on the body frame can be represented by the sum of a gravity vector and a vector representing the free acceleration, which relates to the changes of velocity of the object. Consider accelerometer readings of an IMU that moves arbitrarily but within reasonable acceleration and velocity bounds. The trajectory of the measured acceleration vectors is then contained in a sphere centered around the gravity vector, as illustrated in Figure 3.

The diameter of the sphere can be determined in three steps: (1) strapdown integration of the measured angular rates, (2) transformation of the measured acceleration to the inertial reference frame, (3) calculating the maximum distance between any pair of the free acceleration vectors. This diameter is upper bounded in the absence of bias errors. In the presence of gyroscope bias, however, the maximum pairwise distance grows with time due to integration drift. Therefore, we can determine the gyroscope bias from arbitrary motions by minimizing the defined diameter over sufficiently large time windows.

### Method for simultaneous bias estimation

Consider now the same arbitrary motion with reasonable velocity and acceleration bounds as before. For such motions, integrating the free acceleration in the inertial frame of reference yields valid velocity estimates at least on short time scales. In the presence of accelerometer bias, however, the velocities get large quickly due to integration drift, and this divergence will get even faster when gyroscope bias is present. We exploit these facts to find the true bias values by minimizing the maximum velocity changes over a range of time intervals from a given measurement.

## Experimental validation

We evaluate the methods in two different setups. First we consider the on-ship calibration scenario of the Searchwing drone, and secondly, we explore the benefits of sensor bias compensation in arbitrary motions with an orientation estimation filter.

### On-ship calibration scenario

The methods were tested in 3 different datasets that simulate the on-ship calibration procedure on a smartphone IMU. The datasets represent an attempt to emulate the user inputs for calibration on a moving vessel. In each of them the IMU is assumed to stand for a period of approximately 20 seconds with each of its sides on an even surface aboard the vessel. With respect to the motion of that surface, three different scenarios are considered, which are described below:

- Static: The IMU is placed in 6 different orientations, one at each face, while the surface remains still. This dataset approximates the IMU motion on very calm waters.
- Unimodal: The IMU position oscillates vertically with an amplitude of approximately 2 meters and with small lateral displacements, while each of its six faces points approximately up for approximately 20 seconds. This recording simulates the impact of the waves that makes the boat move along the vertical.
- Bimodal: The IMU position oscillates vertically with an amplitude of several meters in addition to another smaller and faster vertical oscillation and small lateral displacements, while each of its six faces points approximately up for approximately 20 seconds. This represents the most challenging scenario.

For all the motions, an average error of less than 0.05°/s for gyroscope bias and less than 0.04 m/s ^{2} is achieved using simultaneous bias estimation. The static case yields lower accuracy, and the results confirm the importance of properly exciting all IMU axes so that the biases of all axes become observable. Pure gyroscope optimization yields an error of less than 0.2°/s for static data and unimodal motion, and 0.7 °/s error for the bimodal motion. In comparison with EKF-based methods, the proposed simultaneous-optimization-based method performs more accurately and robustly in challenging datasets.

### On-the-run bias compensation

We evaluate the proposed methods as an extension to an orientation estimation filter to simultaneously estimate sensor biases on-the-run. To do so, we have chosen a diverse dataset comprising translational, purely rotational and arbitrary motion at different speeds. In order to analyze the effect of bias compensation on the orientation estimates, we remove residual gyroscope biases and add known bias values between 0.8 and 1.2 °/s. Then, we apply an orientation estimation filter together with ground-truth orientation from an optical device and evaluate the inclination error by stages of 10 s, 20 s, 30 s...etc.

The analysis showed improvements on the inclination estimation of at least 20 % (between 1 - 3 °) for all the analyzed motions, even when very fast motion or disturbances were applied. In some of the motions, improvements of 50 % or more was achieved. We highlight the importance of aligning all axes with local gravity so that all gyroscope biases become identifiable. Figure 6 illustrates an example of the calculated RMSE in a fast arbitrary motion.

## Conclusions and Future Research

The proposed optimization-based methods are found to be suitable for bias estimation on the high seas. We expect that the usage of the methods has a beneficial impact on navigation accuracy, especially during the critical take-off and landing phase. We emphasize also the potential use of the methods as an on-the-fly calibration extension for orientation estimation filters. Our current and future research are concerned with the adaption of the methods to an online setting and the optimization of the computational load.

### Related Publications

- T. Seel, S. Ruppin.
**Eliminating the Effect of Magnetic Disturbances on the Inclination Estimates of Inertial Sensors**. In*Proc. of 20th IFAC World Congress (to appear in IFAC-PapersOnLine)*, pages 1–6, Toulouse, France, 2017. - Bibtex
**Author :**T. Seel, S. Ruppin**Title :**Eliminating the Effect of Magnetic Disturbances on the Inclination Estimates of Inertial Sensors**In :**In*Proc. of 20th IFAC World Congress (to appear in IFAC-PapersOnLine)*,**Date :**2017

- Kai Brands.
**Development and Validation of Long-Time Stable Magnetometer-free Inertial Lower-Body Motion Tracking**. Master Thesis, TU Berlin, Germany, 2020. - Bibtex
**Author :**Kai Brands**Title :**Development and Validation of Long-Time Stable Magnetometer-free Inertial Lower-Body Motion Tracking**In :**Master Thesis, TU Berlin, Germany,**Date :**2020