Fast and Agile Vision-Based Flight with Teleoperation and Collision Avoidance on a Multirotor

TL;DR

This paper introduces a multirotor system capable of fast, safe, and agile autonomous and teleoperated flight in GPS-denied environments, utilizing visual-inertial navigation and real-time collision avoidance.

Contribution

It presents a novel integrated system combining visual-inertial state estimation, collision-free trajectory planning, and teleoperation for high-speed multirotor flight in unstructured environments.

Findings

Autonomous flight speeds exceeded 12 m/s.

System enables safe teleoperation at 10 m/s.

Validated in outdoor field experiments.

Abstract

We present a multirotor architecture capable of aggressive autonomous flight and collision-free teleoperation in unstructured, GPS-denied environments. The proposed system enables aggressive and safe autonomous flight around clutter by integrating recent advancements in visual-inertial state estimation and teleoperation. Our teleoperation framework maps user inputs onto smooth and dynamically feasible motion primitives. Collision-free trajectories are ensured by querying a locally consistent map that is incrementally constructed from forward-facing depth observations. Our system enables a non-expert operator to safely navigate a multirotor around obstacles at speeds of 10 m/s. We achieve autonomous flights at speeds exceeding 12 m/s and accelerations exceeding 12 m/s^2 in a series of outdoor field experiments that validate our approach.

Figures

Tables

Table 1. Table 1 : Frame Classification

Frame Type	Definition	Initialization Condition for Frame $𝑭$
KF	Sensor frames to which subsequent	$T({\mathrm{𝐾𝐹}}_{i-1},F)>{\alpha }_k$
(Anchor)	SFs and BFs are registered
SF	Sensor frames that provide novel information	$T({\mathrm{𝑆𝐹}}_{i-1},F)>{\alpha }_s\left|\right|$
	about the vehicle surroundings	$H({\mathrm{𝑆𝐹}}_{i-1},F)>{\beta }_s$
	Sensor frames that are registered for one time
BF	step to accommodate the dynamic changes

Table 2. Table 2 : Experiment descriptions and parameters.

Experiment	Description	$𝐓$ (s)	${𝐯}_x^{\text{max}}$ (m/s)	${\omega }^{\text{max}}$ (rad/s)	$r$ (m)
Outdoor-1	High speed teleoperation outside	$2.2$	$12.0$	$2.0$	N/A
Outdoor-2,3	Aggressive teleoperation with collision avoidance outside	$2.0$	$5.0$	$1.0$	$0.8$
Indoor-1,2	Aggressive teleoperation with collision avoidance in a dimly lit garage	$1.5$	$3.0$	$1.5$	$0.4$
Outdoor-4	High speed, aggressive teleoperation with collision avoidance outside	${1.3}^{\ast }$	$7.0$	$2.0$	$0.5$
Outdoor-5	High speed, aggressive teleoperation with collision avoidance outside	${1.5}^{\ast }$	$10.0$	$2.0$	$0.2$
$𝐓$ denotes the duration of the motion primitive, ${𝐯}_x^{\text{max}}$ is the maximum desired speed,
${\omega }^{\text{max}}$ is the maximum yaw rate, and $r$ is the collision radius.
∗Motion primitive duration increased adaptively as a linear function of the desired velocity change

Table 3. Table 3 : Execution time (ms) and std. dev. per iteration for safe teleoperation during some of our experiments

Experiment	Trajectory Generation	Trajectory Pruning	Local Map Generation
Outdoor-2	$0.47\pm 0.072$	$29.37\pm 24.03$	$5.72\pm 5.54$
Outdoor-3	$0.48\pm 0.071$	$23.65\pm 15.42$	$5.84\pm 7.95$
Indoor-1	$0.50\pm 0.125$	$8.34\pm 5.19$	$11.67\pm 14.83$
Indoor-2	$0.51\pm 0.219$	$15.73\pm 7.23$	$12.71\pm 16.78$

Table 4. Table 4 : CPU usage (%, out of a total available 600%) and std. dev. on a 6 core NVIDIA TX2

Experiment	Safe Teleoperation	VINS Mono	Total
Outdoor-2	$32.92\pm 17.59$	$67.45\pm 10.87$	$395.72\pm 43.53$
Outdoor-3	$26.70\pm 19.98$	$56.19\pm 19.47$	$359.89\pm 60.87$
Indoor-1	$57.38\pm 21.25$	$64.21\pm 3.42$	$450.40\pm 43.14$
Indoor-2	$58.76\pm 24.09$	$63.82\pm 6.41$	$456.47\pm 36.83$

Equations

$$\mathbf{\gamma }({\mathbf{a}}_k,\tau )$$

$$\displaystyle\ \mathrm{s.t.}\$$

$$\dot{\mathbf{\gamma }}({\mathbf{a}}_k,T)=\dot{\mathbf{x}}({\mathbf{a}}_k,T)$$

$${\mathbf{\gamma }}^{(j)}({\mathbf{a}}_k,T)=0\text{for }j=2,3,4$$

Full text

1 Introduction

Autonomous aerial vehicles with onboard vision-based sensing and planning have been shown to be capable of performing fast and agile maneuvers. However, long-horizon planning required to achieve a task has proven to be a challenge, particularly with limited onboard compute. We propose a fully integrated vision-based multirotor platform that allows minimally trained operators to teleoperate the vehicle at high speeds with agility, while remaining safe around obstacles in unstructured outdoor environments. At high speeds, the environment around the vehicle changes quickly, and is subject to dynamic obstacles and lighting conditions. Our multirotor architecture integrates the following to achieve agile and collision-free flight in these scenarios: 1) an extension of motion primitive-based teleoperation Yang et al. (2018) to generate jerk-continuous local trajectories, a crucial component to prevent instability in agile flights, and 2) efficient local mapping for collision avoidance purposes using a KD-tree.

Many prior works in high speed flight exploit the field-of-view (FOV) of stereo cameras for fast collision checking for autonomous flights, including Florence et al. (2016, 2018), where trajectories are constrained to be inside the FOV of the depth sensor with a max range of $10$ m. In Matthies et al. (2014), trajectories generated by RRT∗ are checked for collisions directly in the disparity space. Lopez and How (2017) presents aggressive flight on a $1.2$ kg MAV, achieving a velocity of $3$ m/s. These methods achieve fast collision checking by circumventing the need to construct a local map and checking for collisions in the sensor’s FOV. This however, limits the range of motions the vehicle can perform. Approaches with local map generation using a laser range finder Chen et al. (2016) and a monocular RGB camera Dey et al. (2016) have been shown to achieve maximum velocities of $1.8$ m/s and $1.5$ m/s respectively, but data processing limits the update rate of the local maps. In our approach, we give a minimally trained operator full control of the vehicle and show that fast and agile flights can be achieved with a human-in-the-loop while maintaining safety.

We perform a series of high speed collision avoidance trials in both indoor and outdoor environments with untrained operators. In our experiments, our hexarotor attains speeds exceeding $12$ m/s and accelerations exceeding $12$ m/s2. We are able to safely avoid obstacles at speeds up to $10$ m/s and accelerations of $8$ m/s2, while retaining a local map.

2 Technical Approach

2.1 Smooth Motion Primitive-Based Teleoperation

Aggressive multirotor flights demand large angular velocities and large angular accelerations, which are directly related to the jerk and snap of the reference position Mellinger and Kumar (2011). Thus to avoid incurring large tracking error due to discontinuous trajectories, we extend forward-arc motion primitives Yang et al. (2018) to generate trajectories that retain differentiability up to jerk and continuity up to snap. From the resulting trajectories, we can calculate desired vehicle attitude, angular velocity, and angular acceleration for use as feedforward terms in the controller.

The motion primitives are parameterized as follows. We define a local frame $\mathcal{L}$ to be a fixed $z$-axis aligned frame, taken at a snapshot in time. The motion primitive definition will be provided in the local frame at the time at which an input is issued, and can be freely transformed into a fixed global frame or body frame for control purposes. An action $\mathbf{a}$ specified by a continuous external input, such as a joystick, generates a single motion primitive. For a multirotor, we define an action as $\mathbf{a}=\{v_{x},v_{z},\omega\}$ in the local frame at which the input is issued, where $v_{x}$ is the $x$-velocity (i.e., the forward velocity of the vehicle), $v_{z}$ is the $z$-velocity, and $\omega$ is the yaw rate. Let $\mathbf{x}$ denote a vector containing the position and yaw of the vehicle, i.e. $\mathbf{x}=[x,\;y,\;z,\;\theta]$. Then, the endpoint velocities are defined by the unicycle model Pivtoraiko et al. (2009). The unicycle model dynamics are given by \dot{\mathbf{x}}(\mathbf{a},\tau)=[v_{x}\cos(\omega\tau)\quad v_{x}\sin(\omega\tau)\quad v_{z}\quad\omega]\mbox{{}^{\top}}, where ${\tau\in[0,T]}$. The position endpoints are unconstrained and we enforce all higher order derivatives above velocity to be zero.

A regeneration step $k$ occurs when a new input is received from the joystick, or the previous trajectory $\bm{\gamma}(\mathbf{a}_{k-1},T)$ finishes executing. Alternatively, a fixed regeneration rate can be chosen in order to accommodate changes in the environment for collision avoidance. Suppose regeneration step $k$ occurs at time $t_{k}$. Then, a library of dynamically feasible motion primitives is generated in the local frame $\mathcal{L}_{t_{k}}$ specified by the reference state at time $t_{k}$, i.e. $\mathbf{x}_{\text{ref}}(t_{k})=\bm{\gamma}(\mathbf{a}_{k-1},t_{k}-t_{k-1})$, given a set of discretized actions $\mathbf{a}_{k}$. Each motion primitive $\bm{\gamma}$ is a vector of four $8^{\text{th}}$ order polynomials that specify the trajectory along the position coordinates $x$, $y$, $z$ and yaw coordinate $\theta$. Given an action $\mathbf{a}_{k}$ at regeneration step $k$, each motion in the motion primitive library is generated in frame $\mathcal{L}_{t_{k}}$ according to

[TABLE]

where $\{\cdot\}^{(j)}$ specifies the $j^{\text{th}}$ time derivative. Note that all constraints are appropriately transformed into $\mathcal{L}_{t_{k}}$. The duration of the trajectory $T$ and the maximum $x$, $z$, and yaw velocities are specified according to the desired operational range. We further allow the operator to freely rotate the motion primitive library with respect to $\mathcal{L}_{t_{k}}$’s $z$-axis to allow for sideway slalom motions.

The result of having snap-continuous trajectories (see Fig. 1) ensures that we have smoothness in error dynamics, thus minimizing instabilities and tracking error.

2.2 Pruning & Trajectory Selection.

At every time step, a family of motion primitives, called the motion primitive library, is created. The motion primitive library is constructed by discretizing the continuous input along each action dimension, such that each action $\mathbf{a}_{i}\in A$ is selected from a convex set $A:=\{\mathbf{a}\in[\mathbf{a}_{\text{min}},\mathbf{a}_{\text{max}}]\}$ with size $N_{1}\times N_{2}\times\dots\times N_{n}$ where $N_{i}$ is the dimension of the space of each input. An example of the discretization is shown in Fig. 2.

At every time step, the operator input is mapped to the closest input in the action space, as defined by the Euclidean norm. A priority queue that minimizes input distance from the selected input $\mathbf{a}_{\text{joystick}}$ to each input in the action space $\mathbf{a}_{i}\in A$ is used to iterate through the action space until a feasible, collision-free trajectory is found. This results in having the operator input mapped to the feasible motion primitive in the library that is parameterized by the closest discretized action, i.e. $\bm{\gamma}(\mathbf{a}_{i})=\operatorname*{argmin}\left\|\mathbf{a}_{\text{joystick}}-\mathbf{a}_{i}\right\|$. A trajectory is deemed collision-free if the minimum distance between any point along the trajectory and the surrounding environment is above the sum of the vehicle size and an operator specified collision radius. Algorithm 1 describes teleoperation with reactive collision avoidance in detail.

The effect of this pruning algorithm is that the vehicle exhibits natural behavior in the presence of obstacles. If a pillar is in front of the vehicle, then the vehicle chooses a motion primitive some angle away and avoids the obstacle. If a wall is present, then the vehicle will choose linear velocities that gradually decrease until the vehicle is stopped.

2.3 Local Map Representation using KD-Trees

We present a local mapping framework that generates a spatially consistent local map of the robot surroundings represented as voxel grids. The local map is generated by retaining only the depth sensor measurements obtained at poses that lie in the vicinity of the vehicle’s current pose. This enables trajectories to span in the space observed by all of the retained sensor measurements. Sequential sensor measurements obtained using a stereo imaging sensor often contain redundant information about the surroundings of the robot. The novelty of information in an incoming sensor measurement at the resolution of the voxel grids is dictated by the rotation and translation displacement of the robot between the current frame and the previous frames. To enforce spatial locality, we dynamically select anchor frames and transform subsequent sensor measurements that contain novel information about the surroundings in to the anchor frames. In order to efficiently incorporate only novel information in the local map, we classify each incoming sensor measurement into one of the following categories: a KeyFrame (KF), a SubFrame (SF), or a BufferFrame (BF) (see Table LABEL:table:frame_classification). The local map (L) is updated in a locally consistent coordinate frame according to the type of sensor frame ($\textit{F}\in\{\text{{KF}, {SF}, {BF}}\}$).

A sensor measurement that is more than $\alpha_{k}$ meters away from the current KF is classified as a KF. Sensor measurements that are not new KFs, but are either more than $\alpha_{s}$ meters in position or $\beta_{s}$ degrees in heading away from the previous SF, are classified as SFs. Sensor measurements that are neither KFs nor SFs are classified as BFs, which do not contain sufficient novel information about the surroundings, but are registered to L to account for dynamic changes in the scene. BFs are in L only for the iterations in which they are observed.