Fast, Autonomous Flight in GPS-Denied and Cluttered Environments

1 Introduction

In recent times, there has been an explosion of research on micro-aerial vehicles (MAVs), ranging from low-level control [Lee et al., 2010] to high-level, specification-based planning [Wolff et al., 2014]. One class of MAVs, the quadrotor, has become popular in academia and industry alike due to its mechanical and control simplicity, high maneuverability and low cost of entry point compared to other aerial robots [Karydis and Kumar, 2016]. Indeed, there have been numerous applications of quadrotors to fields such as Intelligence, Surveillance and Reconnaissance (ISR), aerial photography, structural inspection [Özaslan et al., 2016], robotic first responders [Mohta et al., 2016], and cooperative construction [Augugliaro et al., 2014] and aerial manipulation [Thomas et al., 2014]. Such recent advances have pushed forward the capabilities of quadrotors.

Most of these works rely on the availability of ground truth measurements to enable smooth robot performance. Ground truth can be provided by motion capture systems in lab settings, GPS in outdoor settings, or by appropriately placed special tags in known environments. However, real-world environments are dynamic, partially-known and often GPS-denied. Therefore, there is need to push further on developing fully autonomous navigation systems that rely on onboard sensing only in order to fully realize the potential of quadrotors in real-world applications. Our work aims at narrowing this gap.

Specifically, the focus of this paper is to provide a detailed description of a quadrotor system that is able to navigate at high speeds between a start and a goal pose (position and orientation) in cluttered 3D indoor and outdoor environments while using only onboard sensing and computation for state estimation, control, mapping and planning. The motivation for this problem comes from the recently announced DARPA Fast Lightweight Autonomy program

¹¹1http://www.darpa.mil/program/fast-lightweight-autonomy. The main challenge in creating such small, completely autonomous MAVs is due to the size and weight constraints imposed on the payload carried by these platforms. This restricts the kinds of sensors and computation that can be carried by the robot and requires careful consideration when choosing the components to be used for a particular application. Also, since the goal is fast flight, we want to keep the weight as low as possible in order to allow the robot to accelerate, decelerate and change directions quickly.

Any robot navigation system is composed of the standard building blocks of state estimation, control, mapping and planning. Each of these blocks builds on top of the previous ones in order to construct the full navigation system. For example, the controller requires a working state estimator while the planner requires a working state estimator, controller and mapping system. Initial works on state estimation for aerial robots with purely onboard sensing used laser rangefinders as the sensing modality due to the limited computational capacity available on the platforms [Bachrach et al., 2009, Achtelik et al., 2009, Grzonka et al., 2009]. Due to the limitations of laser scan matching coupled with limited computational capability, these works were limited to slow speeds. Since the small and lightweight laser rangefinders that could be carried by the aerial robots only measured distances in a single plane, these methods also required certain simplifying assumptions about the environment, for example assuming 2.5D structure. As the computational power grew and more efficient algorithms were proposed, it became possible to use vision for state estimation for aerial robots [Achtelik et al., 2009, Blösch et al., 2010]. Using cameras for state estimation allowed flights in 3D unstructured environments and also faster speeds [Shen et al., 2013]. As the vision algorithms have improved [Klein and Murray, 2007, Mourikis and Roumeliotis, 2007, Jones and Soatto, 2011, Forster et al., 2014, Mur-Artal et al., 2015, Bloesch et al., 2015] and the computational power available on small computers has grown, cameras have now become the sensor of choice for state estimation for aerial robots. We use the visual odometry algorithm described in [Forster et al., 2017] for our platform due to the fast run time, ability to use wide angle lenses without requiring undistortion of the full image, allowing the use of multiple cameras to improve robustness of the system and incorporation of edgelet features in addition to the usual point features.

The dynamics of the quadrotor are nonlinear due to the rotational degrees of freedom. In the control design for these robots, special care has to be taken in order to take this nonlinearity into account in order to utilize the full dynamics of the robot. Most early works in control design for quadrotors

[Bouabdallah et al., 2004, Bouabdallah and Siegwart, 2005, Escareno et al., 2006, Hoffmann et al., 2007, Bouabdallah and Siegwart, 2007] used the small angle approximation for the orientation controller to convert the problem into a linear one and proposed PID and backstepping controllers to stabilize the simplified system. Due to the small angle assumption, these controllers are not able to handle large orientation errors and have large tracking errors for aggressive trajectories. A nonlinear controller using quaternions (instead of Euler angles) was developed in [Guenard et al., 2005] where the quadrotor was commanded to follow velocity commands. [Lee et al., 2010] defined an orientation error metric directly in the SO(3) space and proposed a globally asymptotic controller that can stabilize the quadrotor from large position and orientation errors. Our controller is based upon this work and has good tracking performance even when following aggressive trajectories.

It has been shown that the trajectory generation for multi-rotor MAVs can be formulated as a Quadratic Program (QP) [Mellinger and Kumar, 2011]. Since the quadrotor is a differentially flat system, the trajectory can be optimized as an $n^{t h}$ order polynomial parameterized in time [Mellinger and Kumar, 2011]. Generating a collision-free trajectory is more complicated, in which additional constraints for collision checking are required. Using Mixed Integer optimization methods to solve this problem has been discussed in [Mellinger et al., 2012] and recently other approaches have been proposed to remove the integer variables and solve the QP in a more efficient way [Richter et al., 2016, Deits and Tedrake, 2015b, Watterson and Kumar, 2015]. Our pipeline uses a linear piece-wise path from a search-based planning algorithm to guide the convex decomposition of the map to find a safe corridor in free space as described in [Liu et al., 2016]. The safe corridor is formed as linear equality constraints in the QP for collision checking. We also consider dynamic constraints on velocity, acceleration and jerk in the QP to ensure that the generated trajectory does not violate the system’s dynamics. In order to increase the safety, we propose a modified cost functional in the trajectory generation step such that the generated trajectory will be close to the center of the safe corridor.

We couple our trajectory generation method described above with a receding horizon method [Bellingham et al., 2002] for replanning. As the robot moves, we only keep a local robot centric map and use a local planner to generate the trajectory. The main reasons behind this approach are: first, updating and planning in global map is expensive; second, the map far away from the robot is less accurate and less important. Since we are using a local planning algorithm, dead-ends are a well-known challenge. In order to efficiently solve this, we build a hybrid map consisting of a 3D local map and a 2D global map and our planner searches in this hybrid map to provide a globally consistent local action.

The purpose of a navigation system is to enable a robot to successfully traverse from a start pose to a goal pose in either a known or an unknown environment. The problem of navigating in an unknown environment is especially difficult, because in addition to having good state estimation and control, the robot needs to build an accurate map as it moves, and also generate collision-free trajectories quickly in the known map so that the replanning can be done at a high rate as the robot gets new sensor data. The initial works on navigation in unknown environments with quadrotors used offboard computation in order to run the planning due to the limited computation capability available on the platforms. [He et al., 2008] presented a navigation system that uses a known map and takes the localization sensor model into account when planning so as to avoid regions that would lead to bad localization quality. [Grzonka et al., 2009] demonstrated a quadrotor system that is able to localize and navigate in a known map using laser rangefinders as the main source of localization and mapping. Both of these transferred the sensor data to an offboard computer for processing. With a more powerful computer onboard the robot, [Bachrach et al., 2011] were able to run a scan matching based localization system and the position and orientation controllers on the robot while the planner and a SLAM system to produce a globally consistent map ran on an offboard computer. [Shen et al., 2011] was the first to demonstrated a full navigation system running onboard the robot without a known map of the environment. Since then, multiple groups have demonstrated similar capabilities [Valenti et al., 2014, Schmid et al., 2014].

In this paper, we describe our navigation system that allows a quadrotor to go from a starting position to a goal location while avoiding obstacles during the flight. We believe this is one of the first systems that is capable of fast aerial robot navigation through cluttered GPS-denied environments using only onboard sensing and computation. The navigation system has been tested thoroughly in the lab and in real world obstacle-rich environments that were set up as part of the DARPA FLA program.

This paper is organized as follows. In Section 2 we describe our platform and the design decisions made in order to choose the current configuration. In Section 3, we describe our estimation and control algorithms. In this section, we also elaborate upon our sensor fusion methodology that is crucial to get good state estimates in order to control the robot. Section 4 describes our mapping, planning and trajectory generation modules. In Section 5 we show results from various experiments performed in order to benchmark and test our full navigation system. Finally, we conclude in Section 6 with some discussion about the results and give some directions of future work that would help improve our system.

2 System Design

In this section we describe our overall system design. Specifically, we discuss platform design considerations, describe our computation, sensing and communication modules, and highlight critical software architecture components that enable the system to operate smoothly.

Figure 1: Our robot configuration showing the platform with stereo cameras and a nodding lidar.

2.1 Platform Design

The guiding principle in the design of the platform was fast and agile flight. The desired capability of the platform was to be able to reach speeds of more than $20 m / s$ while avoiding obstacles. This leads to a secondary and stronger requirement that the platform has to be able to stop from those speeds within typical sensor detection distances, which are around $20$ – $25$ $m /$ . This implies that the platform should be capable of accelerations of up to $10 m / s^{2}$ . Reaching such high accelerations while maintaining the altitude requires a thrust-to-weight ratio of at around $1.5$ . In order to have some margin for control during these high acceleration phases, we searched for an off-the-shelf platform that had sufficient thrust to provide a thrust-to-weight ratio of more than $2.0$ when fully loaded. This included an expected sensing and computation payload of up to $1 k g /$ and a battery sufficient for desired flight time of around $5 m i n /$ . A list of various commercially available options is shown in Table 1.

Platform	Frame	Battery	All-up	Max Thrust	Thrust/Weight
	(kg)	(kg)	(kg)	(kgf)	Ratio
3DR X8+	1.855	0.817 (4S)	3.672	10.560	2.876
DJI F550 + E310	1.278	0.600 (4S)	2.878	5.316	1.847
DJI F550 + E600	1.494	0.721 (6S)	3.215	9.600	2.986
DJI F450 + E310	0.826	0.400 (3S)	2.226	3.200	1.438
DJI F450 + E600	0.970	0.721 (6S)	2.691	6.400	2.378

Table 1: Specifications of different commercially available off the shelf platforms. We expect a sensing and computation payload of approximately

1 k g /

, which has been added in the All-up mass. The mass of the battery is based upon the recommended battery for each platform.

Based on the survey of the available platforms, we selected the platform configuration consisting of the DJI Flamewheel 450 base along with the DJI E600 motors, propellers and speed controllers since it closely matches our performance requirement. Each of the E600 motor and propeller combination has a rated maximum thrust of approximately $1.6 k g f$ . This leads to total thrust of around $6.4 k g f$ for our quadrotor configuration. For the low-level controller, we selected the Pixhawk [Meier et al., 2015] which is a popular open-source autopilot. The main reason behind choosing the Pixhawk is that the firmware is open-source and customizable, giving us the capability of easily modifying or adding low-level capabilities as desired. In comparison, most of the commercially available autopilot boards are usually black boxes with an interface to send commands and receive sensor data. The base platform consisting of the F450 frame, E600 propulsion system and the Pixhawk has a mass of approximately $1.1 k g /$ . Adding the sensing and computation payload leads to a platform weight of $2.1 k g /$ without the battery. In order to achieve the flight time requirement, we need to select the correct battery taking into account that the maximum total mass of the platform should be below $3 k g /$ .

Figure 2: Histogram of specific energy values for a set of 36 6S rated hobby grade lithium polymer batteries.

The batteries used in MAVs are based on lithium polymer chemistry due to their high energy and power densities. The DJI E600 propulsion system requires a 6S battery, i.e. a battery with rated voltage of approximately $22.2 V /$ . Given that, the main design choice available is the battery capacity. Typical hobby grade lithium polymer batteries have specific energy values are around $130$ – $140$ $W h / k g$ (Fig. 2). The power required to hover for quadrotors is approximately $200 W / k g$ [Mulgaonkar et al., 2014], so for a platform with a total mass between $2.5$ – $3$ $k g /$ , the power consumption would be $500$ – $600$ $W /$ . Assuming an overall efficiency of around $60 %$ , [Theys et al., 2016] going from the supplied power from the battery to the mechanical power output at the propellers, the energy capacity of the battery for a $5 m i n /$ flight time needs to be approximately $69.4$ – $83.3$ $W h /$ . In practice, we never use the full capacity of the battery in order to preserve the life of the battery and also to have some reserve capacity for unforeseen circumstances. If we only use $80 %$ of the rated capacity of the battery, it leads to a required battery energy capacity of $86.8$ – $104.1$ $W h /$ . Using the average specific energy value of $135 W h / k g$ , we expect the mass of the battery to be between $0.64$ – $0.77$ $k g /$ which fits in well with our total mass budget. Based on the available battery capacities, we selected batteries with capacities of $88.8 W h /$ and $99.9 W h /$ in order to provide some flexibility in terms of having higher performance or higher endurance.

In order to power all the sensors and the computer onboard the robot, we required regulated power supplies for $12 V /$ and $5 V /$ . We designed a custom power distribution board, shown in Fig. 3, consisting of a power conditioning circuit, DC-DC converters, power connectors, and a battery monitor. The board is capable of providing filtered $12 V /$ and $5 V /$ supply at a maximum of $120 W /$ each. In addition to power management, for weight saving reasons, the board replaces the top plate of the standard commercially available configuration, forming an integral part of the robot frame.

2.2 Sensing, Computation and Communication

The robot needs to navigate through cluttered 3D environments with purely on-board sensing and computation. This requires the correct selection of sensors and the onboard computer in order to be able to perform the desired task while keeping the mass low. The two tasks that the robot has to perform which require proper sensor selection are state estimation and mapping. The two solutions for state estimation for MAVs are either vision based or lidar based. For unstructured 3D environments, the vision based systems have been more successful that lidar based ones, so we decided on using cameras as our primary state estimation sensors. More details about why the stereo configuration was selected are provided in Section 3. In addition to the cameras, we added a downward pointing lidar (Garmin Lidar-Lite) and a VectorNav VN-100 IMU for state estimation. The VN-100 IMU is also used to trigger the capture from the cameras in order to have time synchronization between the cameras and IMU.

The situation for mapping is a bit different. Current vision based dense mapping algorithms are either not accurate enough or too computationally expensive to run in real time, so lidar based mapping is still the preferred choice for MAVs. In order to keep our weight low, we decided to use a Hokuyo 2D lidar instead of a heavy 3D lidar. We still required a 3D map for planning, so we decided to mount the 2D lidar on a one degree of freedom nodding gimbal as shown in Fig. 4.

Figure 4: Our mapping solution consisting of a 2D lidar mounted on a nodding gimbal.

In order to handle all the computations for estimation, control, mapping and planning onboard the robot, we selected the Intel NUC i7 computer. This single board computer is based on the Intel i7-5557U processor and supports up to 16GB of RAM and an M.2 SSD for storage. This provides sufficient computing power to run our full software stack on the robot without overloading the CPU and also gives us ample amount of storage for recording sensor data for long flights. While the robot is flying, we need to have a communication link in order to monitor the status of the various modules running on the robot. We wanted a link that has good bandwidth, so that during development we can stream the sensor data back to the base station, but also good range so that we do not loose the link when running long range (up to $200 m /$ ) experiments. In addition, since we use ROS as our software framework, having a wireless link that behaves like a wireless local area network was preferred in order to be able to use the standard ROS message transport mechanism. Based on these requirements, we selected the Ubiquity Networks Picostation M2 for the robot side and the Nanostation M2 for the base station. These are high power wireless radios that incorporate Ubiquity Networks’ proprietary airMAX protocol, which improves latency and reliability for long range wireless links compared to the 802.11 protocol, which was designed mainly for indoor use. The Picostation is the smaller and lighter of the two, weighing at around $50 g /$ (after taking off the outer plastic case) compared to $400 g /$ for the Nanostation. This lower weight comes with the compromise of lower transmit power and lower bandwidth, but the performance was sufficient for our purpose, providing a bandwidth of more than $50 M b p s$ up to distances of $200 m /$ .

2.3 Software Architecture

Figure 5: A high level block diagram of our system architecture.

Any big system requires all of the individual components to work together in order to allow the full system to function properly. Figure 5 shows a high level block diagram of our system illustrating the different components and how they are connected to each other. The software components in our system can be grouped under four categories: Estimation, Control, Mapping and Planning. Each of these is in turn separated into smaller parts, and we use ROS as the framework for all the high level software running on the robot. ROS is chosen because it provides a natural way to separate each component into its own package allowing distributed development and ease of testing and debugging. Each executable unit in a ROS system is called a node and different nodes communicate with each other using message passing. In this way, a ROS system can be thought of as a computational graph consisting of a peer-to-peer network of nodes processing and passing data among them. One convenient feature of this system is that the nodes can be run on different computers, since the message passing uses the TCP transport, which allows us to run a subset of the nodes on the robot while the remaining can be run on a workstation computer making it easier to analyze and debug problems leading to a faster development phase. We also benefit from the whole ROS ecosystem of tools and utilities that have been developed in order to perform routine but useful tasks when developing a system such as tools for logging and playing back the messages passed between nodes or tools to visualize the data being sent between nodes.

3 Estimation and Control

There has been a lot of research in recent times on visual and visual-inertial odometry for MAVs with a variety of proposed algorithms [Klein and Murray, 2007, Mourikis and Roumeliotis, 2007, Jones and Soatto, 2011, Forster et al., 2014, Mur-Artal et al., 2015, Bloesch et al., 2015]

. The algorithms can be classified based on the number and type of cameras required into three groups: monocular, stereo, or multi-camera. There are also algorithms using depth cameras but these cameras don’t work well outdoors with sunlight so we do not consider them. An overview of the advantages and disadvantages of the algorithms is shown in Table

2. Looking at these, it is clear that the multi-camera setup would be the most preferred but the software complexity is still a hurdle in terms of real-world usage. Monocular algorithms have received a lot of research attention in the last few years and have improved to a level that they can be reliably used as the only source of odometry for a MAV system. One problem of the monocular algorithms is that they require an initialization process during which the estimates are either not available or are not reliable. In comparison, the stereo algorithms can be initialized using a single frame making them much more robust in case the algorithm needs to be reinitialized while flying if, for example, there is a sudden rotation. One more advantage of using stereo cameras is that in the extreme case that stereo matching is not possible due to features being too far away, we can use the input from only one of the cameras from the stereo pair and treat it as a monocular camera setup.

	Monocular	Stereo	Multi-camera
Mechanical complexity	Low	Medium	Low
Software complexity	Medium	Low-Medium	High
Robustness	Low-Medium	Medium	High
Feature distance	High	Medium-High	High

Table 2: Advantages and disadvantages of different visual odometry algorithms.

3.1 Visual Odometry

3.1.1 Overview of SVO

To estimate the six degree-of-freedom motion of the platform, we use the Semi-direct Visual Odometry (SVO) framework proposed in [Forster et al., 2017]. SVO combines the advantages of both feature-based methods, which minimize the reprojection error of a sparse set of features, anddirect methods, which minimize photometric error between image pixels. This approach estimates frame-to-frame motion of the camera by first aligning the images using a sparse set of salient features (e.g. corners) and their neighborhoods in the images, estimating the 3D positions of the feature points with recursive Bayesian depth estimation, and finally refining the structure and camera poses with optimization (e.g. bundle adjustment). Our efficient implementation of this approach is capable of estimating the pose of a camera frame in as little as 2.5 milliseconds, while achieving comparable accuracy to more computationally intensive methods.

Figure 6: A high level diagram of the SVO software architecture.

The system is decomposed into two parallel threads: one for estimating camera motion, and one for mapping the environment (see Fig. 6). The motion-estimation thread proceeds by first performing a sparse image alignment between the two most recent frames, then obtaining sub-pixel feature correspondence using direct methods on patches around each sparse feature, and finally pose refinement on the induced reprojection error.

A brief overview of the notation used in describing the method is presented here. The intensity image from camera C at timestep $k$ is denoted by $I\tiny{C}k:Ω\tiny{C}⊂R2↦R$ , where $Ω\tiny{C}$ is the image domain. A point in 3D space $ρ \in R^{3}$ maps to image coordinates $u \in R^{2}$ through the camera projection model, $u = π (ρ)$ , which we assume is known. Given the inverse scene depth $0 p t > 0$ for a pixel $u∈R\tiny{C}k$ , the position of the corresponding 3D point is obtained using the back-projection model $ρ = π_{0}^{- 1} p t (u)$ . We denote $R\tiny{C}k⊆Ω$ the set of pixels for which the depth is known at time $k$ in camera C. The position and orientation of the world frame W with respect to the $k^{th}$ camera frame is described by the rigid body transformation $Tk\tiny{W}∈SE(3)$ . A 3D point $\tiny{W}ρ$ that is expressed in world coordinates can be transformed to the $k^{th}$ camera frame using: $kρ=Tk\tiny{W} \tiny{W}ρ$ .

3.1.2 Motion Estimation

Figure 7: Changing the relative pose $T_{k, k - 1}$ between the current and the previous frame implicitly moves the position of the reprojected points in the new image $u_{i}^{'}$ . Sparse image alignment seeks to find $T_{k, k - 1}$ that minimizes the photometric difference between image patches corresponding to the same 3D point (blue squares).

Consider a body frame B that is rigidly attached to the camera frame C with known extrinsic calibration $T\tiny{C}\tiny{B}∈SE(3)$ (see Fig. 7). Our goal is to estimate the incremental motion of the body frame $Tkk−1≐T\tiny{B}k\tiny{B}k−1$ such that the photometric error is minimized:

T⋆kk−1=argminTkk−1∑u∈¯R\tiny{C}k−112∥rI\tiny{C}u(Tkk−1)∥2ΣI,

(1)

where the photometric residual $rI\tiny{C}u$ is defined by the intensity difference of pixels in subsequent images $I\tiny{C}k$ and $I\tiny{C}k−1$ that observe the same 3D point $ρ_{u}$ :

rI\tiny{C}u(Tkk−1)≐I\tiny{C}k(π(T\tiny{C}\tiny{B}Tkk−1 ρu))−I\tiny{C}k−1(π(T\tiny{C}\tiny{B} ρu)).

(2)

The corresponding 3D point $ρ_{u}$ for a pixel with known depth, expressed in the reference $B_{k - 1}$ frame, is computed by means of back-projection:

ρu=T\tiny{B}\tiny{C} π−10pt(u),∀ u∈R\tiny{C}% k−1.

(3)

Sparse image alignment solves the non-linear least squares problem in Eq. (1) with $R\tiny{C}k−1$ corresponding to small patches centered at features with known depth, using standard iterative non-linear least squares algorithms.

In the next step, we relax the geometric constraints given by the reprojection of 3D points and perform an individual 2D alignment of corresponding feature patches. In order to minimize feature drift over the camera trajectory, the alignment of each patch in the new frame is performed with respect to a reference patch from the frame where the feature was first extracted. However, this step generates a reprojection error, representing the difference between the projected 3D point and the aligned feature position.

We detect both edge and corner features, representing points of strong gradient in the image. Feature alignment in the image minimizes the intensity difference of a small image patch $P$ that is centered at the projected feature position $u^{'}$ in the newest frame $k$ with respect to a reference patch from the frame $r$ where the feature was first observed. For corner features, the optimization computes a correction $δ u^{⋆} \in R^{2}$ to the predicted feature position $u^{'}$ that minimizes the photometric cost:

	${u^{'}}^{⋆}$	$=u′+δu⋆,withu′=π(T\tiny{C}\tiny{B} Tkr T\tiny{B}\tiny{C} π−10pt(u))$		(4)
	$δ u^{⋆}$	$=argminδu∑Δu∈P12∥∥I\tiny{C}k(u′+δu+Δu)−I\tiny{% C}r(u+AΔu)∥∥2,$

where $Δ u$ is the iterator variable that is used to compute the sum over the patch $P$ . For features on edges, we therefore optimize for a scalar correction $δ u^{⋆} \in R$ in the direction of the edge normal $n$ to obtain the corresponding feature position ${u^{'}}^{⋆}$ in the newest frame:

	${u^{'}}^{⋆}$	$= u^{'} + δ u^{⋆} \cdot n, with$		(5)
	$δ u^{⋆}$	$=argminδu∑Δu∈P12∥∥I\tiny{C}k(u′+δu⋅n+Δu)−I\tiny{C}r(u+AΔu)∥∥2.$

In the previous step, we established feature correspondence with subpixel accuracy, but this feature alignment violated the epipolar constraints and introduced a reprojection error $δ u$ , which is typically well below 0.5 pixels. Therefore, in the last step of motion estimation, we refine the camera poses and landmark positions $X={Tk\tiny{W},ρi}$ by minimizing the squared sum of reprojection errors:

	$X^{⋆} = arg min X$	$∑k∈K∑i∈LCk12∥u′⋆i−π(T\tiny{C}% \tiny{B} Tk\tiny{W} ρi)∥2$		(6)
	$+$	$∑k∈K∑i∈LEk12∥nTi(u′⋆i−π(T\tiny{C}\tiny{B} Tk\tiny{W} ρi))∥2$

where $K$ is the set of all keyframes in the map, $L_{k}^{C}$ the set of all landmarks corresponding to corner features, and $L_{k}^{E}$ the set of all edge features that were observed in the $k^{th}$ camera frame.

While optimization over the whole trajectory in Eq. (6) with bundle adjustment results in higher accuracy, for MAV motion estimation, it is sufficient to only optimize the latest camera pose and the 3D points separately, which permits more efficient computation.

3.1.3 Mapping

In the derivation of motion estimation, we assumed that the depth at sparse feature locations in the image was known. Here, we describe how the mapping thread estimates this depth for newly detected features, assuming that the camera poses are known from the motion estimation thread.

The depth at a single pixel is estimated from multiple observations by means of a recursive Bayesian depth filter. When the number of tracked features falls below some threshold, a new keyframe is selected and new depth filters are initialized at corner and edge features in that frame. Every depth filter is associated to this reference keyframe $r$ , and the initial depth uncertainty is initialized with a large value. For a set of previous keyframes as well as every subsequent frame with known relative pose ${I_{k}, T_{k r}}$ , we search for a patch along the epipolar line that has the highest correlation via zero mean sum of squared differences (see Fig. 8). From the pixel with maximum correlation, we triangulate the depth measurement ${~ 0 p t}_{i}^{k}$ , which is used to update the depth filter. If enough measurements have been obtained such that uncertainty in the depth is below a certain threshold, we initialize a new 3D point at the estimated depth in our map, which subsequently can be used for motion estimation (see system overview in Fig. 6).

Figure 8: Probabilistic depth estimate ${^0 p t}_{i}$ for feature $i$ in the reference frame $r$ . The point at the true depth projects to similar image regions in both images (blue squares). Thus, the depth estimate is updated with the triangulated depth ${~ 0 p t}_{i}^{k}$ computed from the point $u_{i}^{'}$ of highest correlation with the reference patch. The point of highest correlation lies always on the epipolar line in the new image.

We model the depth filter according to [Vogiatzis and Hernández, 2011] with a two dimensional distribution: the first dimension is the inverse depth $0 p t$ [Civera et al., 2008], while the second dimension $γ$

is the inlier probability. Hence, a measurement

{~ 0 p t}_{i}^{k}

is modeled with a Gaussian + Uniform

mixture model distribution: an inlier measurement is normally distributed around the true inverse depth

0 p t_{i}

while an outlier measurement arises from a uniform distribution in the interval

[0 p t_{i}^{min}, 0 p t_{i}^{max}]

p ({~ 0 p t}_{i}^{k} | 0 p t_{i}, γ_{i}) = γ_{i} N ({~ 0 p t}_{i}^{k} ∣ ∣ 0 p t_{i}, τ_{i}^{2}) + (1 - γ_{i}) U ({~ 0 p t}_{i}^{k} ∣ ∣ 0 p t_{i}^{min}, 0 p t_{i}^{max}),

(7)

where $τ_{i}^{2}$

the variance of a good measurement that can be computed geometrically by assuming a disparity variance of one pixel in the image plane

[Pizzoli et al., 2014]. We refer to the original work [Vogiatzis and Hernández, 2011] and the [Forster et al., 2017] for more details.

3.1.4 Implementation Details

Figure 9: Visual odometry with multiple rigidly attached and synchronized cameras. The relative pose of each camera to the body frame $T\tiny{B}\tiny{C}j$ is known from extrinsic calibration and the goal is to estimate the relative motion of the body frame $T_{k k - 1}$ .

Our system utilizes multiple cameras, so consider a camera rig with $M$ cameras (see Fig. 9). We assume that the relative pose of the individual cameras $c \in C$ with respect to the body frame $T\tiny{C}\tiny{B}$ is known from extrinsic calibration. To generalize sparse image alignment to multiple cameras, we simply need to add an extra summation in the cost function of Eq. (1):

T⋆kk−1=argminTkk−1∑\tiny{C}∈C∑u∈¯R\tiny{C}k−112∥rI\tiny{C}u(Tkk−1)∥2ΣI.

(8)

The same summation is necessary in the bundle adjustment step to sum the reprojection errors from all cameras. The remaining steps of feature alignment and mapping are independent of how many cameras are used, except that more images are available to update the depth filters. An initial map is computed during initialization using stereo matching.

We additionally apply motion priors within the SVO framework by assuming a constant velocity relative translation prior ${~ p}_{k k - 1}$ and a relative rotation prior ${~ R}_{k k - 1}$ from a gyroscope. We employ the motion prior by adding additional terms to the cost of the sparse image alignment step:

$T_{k k - 1}^{⋆} = arg min T_{k k - 1}$		(9)
	$+ \frac{1}{2} ∥ p_{k k - 1} - {~ p}_{k k - 1} ∥_{Σ_{p}}^{2}$
	$+ \frac{1}{2} ∥ log ({~ R}_{k k - 1}^{T} R_{k k - 1})^{\lor} ∥_{Σ_{R}}^{2},$

where the covariances $Σ_{p}, Σ_{R}$ are set according to the uncertainty of the motion prior, the variables $(p_{k k - 1}, R_{k k - 1}) ≐ T_{k k - 1}$ are the current estimate of the relative position and orientation (expressed in body coordinates B), and the logarithm map

maps a rotation matrix to its rotation vector.

We apply the sparse image alignment algorithm in a coarse-to-fine scheme, half-sampling the image to create an image pyramid of five levels, and use a patch size of $4 \times 4$ pixels. The photometric cost is then optimized at the coarsest level until convergence, starting from the initial condition $T_{k k - 1} = I_{4 \times 4}$ , before continuing at the finer levels to improve the precision.

In the mapping thread, we divide the image in cells of fixed size (e.g., $32 \times 32$ pixels). For every keyframe a new depth-filter is initialized at the FAST corner [Rosten et al., 2010]

with highest score in the cell, unless there is already a 2D-to-3D correspondence present. In cells where no corner is found, we detect the pixel with highest gradient magnitude and initialize an edge feature. This results in evenly distributed features in the image. To speed up the depth-estimation we only sample a short range along the epipolar line; in our case, the range corresponds to twice the standard deviation of the current depth estimate. We use a

8 \times 8

pixel patch size for the epipolar search.

We refer the reader to the original paper [Forster et al., 2017] for further details about both the approach and its performance.

3.2 Sensor Fusion

We have multiple sensors on the platform, each providing partial information about the state of the robot. Moreover, the sensors provide output at different rates, for example, we run the stereo cameras at $40 H z /$ while the downward pointing distance sensor runs at $20 H z /$

. We need to merge these pieces of partial information into a single consistent estimate of the full state of the robot. The typical method used for such sensor fusion tasks is some variant of the Kalman filter. The quadrotor is a nonlinear system due to its rotational degrees of freedom. This requires the use of either an Extended Kalman filter (EKF) or an Unscented Kalman filter (UKF). The UKF has the advantage of better handling the system nonlinearities with only a small increase in computation, so we chose the UKF for our system. Fig

10 shows the inputs and outputs of the UKF module running on the robot. The state vector used in the UKF is,

x = {[\begin{matrix} p^{T} & {˙ p}^{T} & ϕ & θ & ψ & b_{a}^{T} & b_{ω}^{T} \end{matrix}]}^{T}

where $p$ is the world-frame position of the robot, $˙ p$ is the world-frame velocity, $ϕ$ , $θ$ and $ψ$ are the roll, pitch and yaw respectively, $b_{a}$ is the accelerometer bias while $b_{ω}$ is the gyroscope bias. We use the ZYX convention for representing the rotations in terms of the Euler angles $ϕ$ , $θ$ and $ψ$ . The Euler angle representation was chosen for representing the orientation primarily because of its simplicity. The well-known problem of gimbal lock when using Euler angles is not an issue in this case since the desired and expected roll and pitch of the robot is always less than $90 ° /$ .

Figure 10: Data flow diagram of the UKF used on the robot.

The UKF consists of a prediction step which uses the IMU data as the input and multiple update steps, one for each of the other sensors. The update step is performed whenever the corresponding sensor measurement arrives. The prediction step is nonlinear since the accelerometer and gyroscope measurements are in the body frame while the position and velocity in the state are in the world frame, which requires the transformation of the measured quantities from body to world frame using the estimated orientation.

Given that the state at iteration $k$ , $x_{k}$ (dimension $n$ ), has mean ${¯ x}_{k}$ and covariance $P_{k}$ , we augment it with the process noise (dimension $p$ ) having mean ${¯ v}_{k}$ and covariance $Q_{k}$ , creating the augmented state $x_{k}^{a}$ and covariance matrix $P_{k}^{a}$ ,

{¯ x}_{k}^{a} = [\begin{matrix} {¯ x}_{k} {¯ v}_{k} \end{matrix}], P_{k}^{a} = [\begin{matrix} P_{k} & 0 0 & Q_{k} \end{matrix}]

Then, we generate a set of sigma points by applying the Unscented transform [Julier et al., 1995] to the augmented state,

$X_{0}^{a} (k)$	$= {¯ x}_{k}^{a}$	(10)
$X_{i}^{a} (k)$	$= {¯ x}_{k}^{a} + \sqrt{(L + λ) P_{k}^{a}} i = 1, \dots, L$
$X_{i}^{a} (k)$	$= {¯ x}_{k}^{a} - \sqrt{(L + λ) P_{k}^{a}} i = L + 1, \dots, 2 L$

where $L = n + p$ is the dimension of the augmented state and $λ$ is a scaling parameter [Wan and Merwe, 2000].

These sigma points are then propagated through the process model with the accelerometer and gyroscope measurements as input.

X_{i}^{x} (k + 1 | k) = f (X_{i}^{x} (k), u (k), X_{i}^{v} (k))

where $X_{i}^{x}$ is the state part of the augmented state while $X_{i}^{v}$ is the process noise part. The process model, $f (x_{k}, u_{k}, v_{k})$ , for our system is given by

	$u_{k} = {[a_{m e a s}^{T} ω_{m e a s}^{T}]}^{T} v_{k} = {[v_{a}^{T} v_{ω}^{T} v_{b_{a}}^{T} v_{b_{ω}}^{T}]}^{T}$
	$a = a_{m e a s} - b_{a} + v_{a}$
	$ω = ω_{m e a s} - b_{ω} + v_{ω}$
	$pk+1=pk+˙pk\dift˙pk+1=˙pk+(Rka−g)\diftRk+1=Rk(I3+[ω]×\dift)bak+1=bak+vba\diftbωk+1=bωk+vbω\dift$

where $R_{k} = R (ϕ_{k}, θ_{k}, ψ_{k})$ is the rotation matrix formed by using the ZYX convention for the Euler angles while $v_{a}$ , $v_{ω}$ , $v_{b_{a}}$ and $v_{b_{ω}}$ are the individual process noise terms.

From the transformed set of sigma points, $X_{i}^{x} (k + 1 | k)$ , we can calculate the predicted mean and covariance,

	${¯ x}_{k + 1 \| k} = 2 L \sum i = 0 w_{i}^{m} X_{i}^{x} (k + 1 \| k)$
	$P_{k + 1 \| k} = 2 L \sum i = 0 w_{i}^{c} [X_{i}^{x} (k + 1 \| k) - {¯ x}_{k + 1 \| k}] {[X_{i}^{x} (k + 1 \| k) - {¯ x}_{k + 1 \| k}]}_{k + 1 \| k}^{T}$

where $w_{i}^{m}$ and $w_{i}^{c}$ are scalar weights [Wan and Merwe, 2000].

Whenever a new sensor measurement, $y_{k + 1}$ , arrives, we run the update step of the filter. First we generate a new set of sigma points in the same way as done during the prediction step, (10), with the augmented state and covariance given by,

{¯ x}_{k + 1 | k}^{a} = [\begin{matrix} {¯ x}_{k + 1 | k} {¯ n}_{k} \end{matrix}], P_{k + 1 | k}^{a} = [\begin{matrix} P_{k + 1 | k} & 0 0 & R_{k} \end{matrix}]

where ${¯ n}_{k}$ is the mean of the measurement noise and $R_{k}$ is the covariance. The generated sigma points are then used to generate the predicted measurement using the measurement function $h (x, n)$ ,

	$Y_{i} (k + 1 \| k) = h (X_{i}^{x} (k + 1 \| k), X_{i}^{n} (k + 1 \| k))$
	${¯ y}_{k + 1 \| k} = 2 L \sum i = 0 w_{i}^{m} Y_{i} (k + 1 \| k)$
	$P_{y y} = 2 L \sum i = 0 w_{i}^{c} [Y_{i} (k + 1 \| k) - {¯ y}_{k + 1 \| k}] {[Y_{i} (k + 1 \| k) - ¯ y_{k + 1 \| k}]}_{i}^{T}$

And finally the state is updated as follows,

	$P_{x y} = 2 L \sum i = 0 w_{i}^{c} [X_{i}^{x} (k + 1 \| k) - {¯ x}_{k + 1 \| k}] {[Y_{i} (k + 1 \| k) - {¯ y}_{k + 1 \| k}]}_{i}^{T}$
	$K = P_{x y} P_{y y}^{- 1}$
	${¯ x}_{k + 1} = {¯ x}_{k} + K (y_{k + 1} - ¯ y_{k + 1 \| k})$
	$P_{k + 1} = P_{k + 1 \| k} - K P_{y y} K^{T}$

Note that for each sensor input to the UKF except the IMU, which is used for the prediction step, there is a separate measurement function, $h (x, n)$ , and the full update step is performed, with the corresponding measurement function, when an input is received from any of those sensors.

The attitude filter running on the Pixhawk is a simple complementary filter which can take an external reference orientation as an input. This allows us to provide the estimate from the UKF to the Pixhawk in order to improve the orientation estimate on the Pixhawk. This is important for good control performance since the orientation controller running on the Pixhawk uses the Pixhawk’s estimate of the orientation while our control commands are calculated using the UKF estimates. Without an external reference being sent to the Pixhawk, the orientation estimates on the Pixhawk can be different from the UKF which would lead to an incorrect interpretation of the control commands.

3.3 Control

The controller used for the robot has the cascade structure, as shown in Figure 5, which is has become standard for MAVs. In this structure, we have an inner loop controlling the orientation and angular velocities of the robot while an outer loop controls the position and linear velocities. In our case, the inner loop runs at a high rate ( $400 H z /$ ) on the Pixhawk autopilot while the outer loop runs at a slightly slower rate ( $200 H z /$ ) on the Intel NUC computer.

At every time instance, the outer loop position controller receives a desired state, which consists of a desired position, velocity, acceleration and jerk, from the planner and using the estimated state from the UKF, computes a desired force, orientation and angular velocities which are sent to the orientation controller. The inner loop orientation controller receives these and computes the thrust and moments required to achieve the desired force and orientation. These are then converted into individual motor speeds that are sent to the respective motor controllers.

The controller formulation we use is based on the controller developed in [Lee et al., 2010] with some simplifications. The thrust command of the position controller is calculated as,

	$e_{p o s} =^p - p_{d e s}, e_{v e l} =^˙ p - {˙ p}_{d e s}$
	$f = m (- k_{p o s} e_{p o s} - k_{v e l} e_{v e l} + g e_{3} + {¨ p}_{d e s})$
	$T h r u s t = f \cdot^R e_{3}$		(11)

where $e_{3} = {[\begin{matrix} 0 & 0 & 1 \end{matrix}]}^{T}$ and $^R$ is the rotation matrix which converts vectors from body frame to world frame calculated using the estimated roll, pitch and yaw. The desired attitude is calculated as,

	$b_{2, d e s} = {[\begin{matrix} - sin ψ_{d e s}, & cos ψ_{d e s}, & 0 \end{matrix}]}^{T}$
	$b3=f\normf,b1=b2,des×b3\normb2,des×b3,b2=b3×b1$
	$R_{d e s} = [\begin{matrix} b_{1}, & b_{2}, & b_{3} \end{matrix}]$		(12)
	${˙ b}_{2, d e s} = {[\begin{matrix} - cos ψ_{d e s} {˙ ψ}_{d e s}, & - sin ψ_{d e s} {˙ ψ}_{d e s}, & 0 \end{matrix}]}^{T}$
	$˙b3=b3×˙f\normf×b3,˙b1=b1×˙b2,des×b3+b2,des×˙b3\normb2,des×b3×b1,˙b2=˙b3×b1+b3×˙b1$
	${[Ω_{d e s}]}_{\times} = R_{d e s}^{T} {˙ R}_{d e s}$		(13)

Note that here we have to define $b_{2, d e s}$ based on the yaw instead of defining $b_{1, d e s}$ as done in [Mellinger and Kumar, 2011] due to the different Euler angle convention, we use the ZYX convention while they used ZXY.

The thrust and attitude commands, from (11), (12) and (13), are then sent to the Pixhawk autopilot through mavros²²2https://github.com/mavlink/mavros. The attitude controller running on the Pixhawk takes these commands and converts them to commanded motor speeds. First, using the $R_{d e s}$ and the estimate of the current orientation, $^R$ , we calculate the desired moments as follows,

	${[e_{R}]}_{\times} = \frac{1}{2} (R_{d e s}^{T}^R - {^R}^{T} R_{d e s}), e_{Ω} = Ω - {^R}^{T} R_{d e s} Ω_{d e s}$
	$M = - k_{R} e_{R} - k_{Ω} e_{Ω}$

Then, from the desired thrust and moments, we can calculate the thrust required from each propeller which allows us to compute the desired motor speed as shown in [Michael et al., 2010].

4 Mapping and Planning

Our navigation system consists of five parts as shown in Fig. 11. In this section, we discuss the mapping, planner and trajectory generation threads.

Figure 11: Our navigation framework. A desired goal $g$ is sent to the planner at the beginning of the task. The planner generates a path, $P_{τ}$ , using the map, $M_{τ}$ , and sends it to the trajectory generator. The trajectory generator converts the path into a trajectory, $Φ_{τ}$ , and sends it to the receding horizon controller. The controller then derives the desired state $x_{des}$ at $200 H z /$ from this trajectory which is sent to the robot controller. The input $m$ to the mapping block denotes the sensor measurements.

4.1 Mapping

We have mounted a LIDAR on a servo such that we can generate a 3D voxel map by rotating the laser. Updating the map and planning using the 3D global map are both computationally expensive and in addition, with noise and estimation drift, the global map can be erroneous. Hence we utilize a local mapping technique that generates a point cloud around current robot location (Fig. 12). This local point cloud, $M^{c}$ , has fixed size and fine resolution and is used to build a 3D occupancy voxel map, $M^{l}$ , centered at current robot location. Since the local map only records the recent sensor measurements with respect to current robot location, the accumulated error in mapping is small. We also generate a coarse 2D map, $M^{w}$ , in global frame in order to solve the dead-end problem caused by local planning. We call this map the “global information map” since it contains two pieces of information: one is the known and unknown spaces so that we know which part has been explored, the other is the location of walls detected from $M^{c}$ that the robot cannot fly over.

(a) Local point cloud $M^{c}$ ( $$ 40 m $ \times $ 20 m $ \times $ 4 m $$ ).

4.2 Planner

We use $A^{⋆}$ to plan a path in a hybrid graph $G (V, E)$ that links the voxels in both local 3D map $M^{l}$ and global information map $M^{w}$ (result is shown in Fig. 12(c)). We can efficiently derive the path $P$ in local map that is globally consistent. Fig. 13 shows an example of using this method to solve the dead-end corridor problem.

4.3 Trajectory Generation

In this subsection, we are going to introduce the trajectory generation method given the map $M$ and a prior path $P$ . The trajectory generation process is shown in Fig. 14. Through regional inflation, a safe corridor is found in $M$ that excludes all the obstacle points. As the intermediate waypoints in $P$ can be close to the obstacles, we shift the intermediate waypoints towards the center of safe corridor. The new path $P^{⋆}$ and the safe corridor are used to generate the trajectory.

Figure 14: Trajectory generation process, which can be treated as a black box (dashed rectangle) as in Fig. 11. The inputs are a path $P$ and a discrete map $M$ , output is the dynamically feasible trajectory $Φ$ .

4.3.1 Regional Inflation by Line Segments

Inspired by IRIS in [Deits and Tedrake, 2015a], we developed the algorithm to dilate a path in free space using ellipsoids. For each line segment in the path $P$ , we generate a convex polyhedron that includes the whole segment but excludes any occupied voxel in $M$ through two steps:

Grow ellipsoid for each line segment (Fig. 15(a))
Inflate the ellipsoid to generate the polyhedron (Fig. 15(b))

The ellipsoid is described as $ξ_{r} (E, d) = {E ~ x + d | ∥ ~ x ∥ \leq r}$ which is the projection of a unit sphere with radius $r$ into $R^{3}$ . A polyhedron $C$ is the intersection of $m$ half-planes: $C = {x | A_{j}^{T} x \leq b_{j}, j = 1 \dots m}$ . The half-plane is found at the intersection of ellipsoid with the closest obstacle point $x_{r}$ as shown in equation (14).

Aj=\difξr\difx∣∣∣x=xr=2E−1E−T(xr−d),bj=ATjxr

(14)

(a) Grow ellipsoid for each line segment.

4.3.2 Path Modification

The original path from the planner can be close to the obstacles. Although the trajectory generation does not require the final trajectory to go through the intermediate points, the path affects the route of the trajectory. The path modification step aims to modify the original path away from the obstacle by keeping the intermediate waypoints in the middle of a safe corridor. We use a bisector plane that passes through the waypoint $p$ , this plane intersects with both polyhedra that connected through $p$ . The point $p$ is moved to the centroid of the intersection polygon (Fig. 15(c)).

4.3.3 Trajectory Optimization

We formulate the trajectory generation as an optimal control problem as shown in equation (15). Compared to the standard formulation of this optimal control problem, we add a second term in the cost function which is the square of the distance between the trajectory $Φ_{i}$ and the line segment $l_{i} : {a_{i} x + b_{i} y + c_{i} z + d_{i} = 0}$ . This distance cost is weighted by a factor $ϵ$ . Fig. 16 shows the affect on the trajectory by changing this weighting factor. Thus, we can control the shape of a trajectory to keep it close to the modified path $P^{⋆}$ and away from obstacles. This process increases the safety of the trajectory such that the robot will not get too close to obstacles.

argminJ=\bigintsssT0\norm\dif4Φ(t)\dift42\dift+ϵ(l⋅Φ(t)\norml)2s.t.Φ(0)=p0,Φ(T)=pf,Φ(t)∈C3˙Φ(t)≤vmax,¨Φ(t)≤amax,...Φ(t)≤jmax

(15)

4.3.4 Continuous Optimization

To to be able to compute Equation 15 in real time, we have chosen to represent $Φ$ as an $n^{t h}$ order polynomial spline. Each spline segment $Φ_{j}$ takes time $Δ_{j}$ such that $\sum_{j} Δ_{j} = T$ . For our experiments, we have found that $n = 7$ provides good performance for trajectory optimization and that increasing $n$ produces similar trajectories, but with longer computational time.

For this section, we will assume that our trajectory has $d$ dimensions, $m$ segments and is represented by an $n$ th order polynomial. Also we define the indices: $i = 0, \dots, n$ , $j = 1, \dots, m$ ,and $k = 1, \dots, d$ . Therefore we can write the $k$ th dimension of $Φ_{i}$ with respect to coefficients $α_{i j k}$ and basis functions $p_{i}$ .

Φ_{j k} (t) = \sum i α_{i j k} p_{i} (t)

(16)

We have chosen $p_{i}$ to be shifted Legendre polynomials which are represented with a unit time scaling:

s = \frac{t - q \sum j = 0 Δ_{j}}{Δ_{q + 1}}

(17)

For whichever $q \in 1, \dots, m$ makes $s \in [0, 1]$

\dif4\difskpi+4(t)=(−1)ii∑l=0(l)i(l+ii)(−s)l

(18)

This leaves $4$ basis polynomials undefined, which we just use:

p_{i} (t) = s^{i} for i = 0, \dots, 3

(19)

This definition results in a cost which is quadratic in $α$

\bigintsssT0\norm\dif4Φ(t)\dift42\dift=∑ijkα2ijkΔ−7j2k+1

(20)

For the other constraints are concerned with constraining $Φ$ at different times. We note that the evaluation of any derivative of $Φ_{i}$ is linear with respect to $α$

\difq\diftqΦjk(t)=(Δj)q∑iαijk⋅\difq\difsqpi(s)

(21)

To ensure continuity of our spline up to $3$ derivatives, we need to add the following constraints for $j = 2.. (m - 1)$ , $q = 0..3$ , and $k = 1.. d$ :

(Δj)q∑iαijk⋅\difq\difsqpi(1)=(Δj+1)q∑iαi(j+1)k⋅\difq\difsqpi(0)

(22)

The start and end constraints:

(Δ1)q∑iαi1k⋅\difq\difsqpi(0)=\difq\diftqp0(Δm)q∑iαimk⋅\difq\difsqpi(1)=\difq\diftqpf

(23)

For the inequality constraints and the centering part of the cost functional, we use the sub-sampling method proposed by [Mellinger and Kumar, 2011]. Along the interval, we can select $g$ points at which to sample the trajectory. In practice we found that $g = 10$ points worked well and they could be sample uniformly within $s_{g} \in [0, 1]$ or by using Chebyshev sampling. The $a_{v k}$ and $b_{v}$ come from the polyhedra found in 4.3.1 for $q = 0$ and $L_{1}$ bounds for $q > 0$ [Boyd and Vandenberghe, 2004].

\difq\diftq∑kΦjk(sg)avk≤bv

(24)

To compute the centering part of the cost, we could use a Gaussian quadrature, but found that rectangular integration worked fine [Press, 1992].

∑jgϵ(l⋅Φ(sg)\norml)2

(25)

This results in the following QP in $α$

\begin{matrix} min α & α^{T} Q α & Eqn. ??? + Eqn. ??? s . t & A α = b & Eqn. ??? and Eqn. ??? C α \leq d & Eqn. ??? \end{matrix}

(26)

Choosing $Δ_{j}$ is critical to the feasibility of Equation (26) and the quality of the resultant trajectory. To choose the $Δ_{j}$ we use the times we get by fitting a trapezoidal velocity profile through the segments, so the time per segment is based on the length of each line segment $l_{j}$ in the path through the environment.

5 Experimental Results

5.1 Estimation benchmarking

The main task for the robot is to fly long distances to a goal point, so the estimation accuracy is very important. The drift in the estimator should be low so that the robot reaches the desired goal. In order to test the accuracy and drift in the estimator, we flew the robot in the motion capture space in the lab. The robot was flown manually along an aggressive trajectory reaching speeds of up to $4 m / s$ and accelerations of $4 m / s^{2}$ . The plots of the estimated and ground-truth position and velocity as shown in Figure 17. As can be seen from the figure, the final drift after more than $60 m /$ of flight is less than $0.6 m /$ , giving us a drift of around $1 %$ . Note that there is almost no drift in the Z-axis due to the use of the downward pointing distance sensor, which gives us an accurate height estimate.

The SVO framework was deployed on our MAV system for visual odometry using a forward-facing stereo camera configuration and onboard computation. As a demonstration of the accuracy of the motion estimation, some high-speed maneuvers were flown manually in a warehouse environment. The MAV accelerated aggressively along a $50 m /$ straight aisle in the warehouse, braked aggressively to a stop, and then returned to the starting location at a moderate speed. Figure 18 shows several onboard camera images marked up with the features that SVO is tracking, as well as the sparse map of 3D points in the environment that were mapped during this trajectory. During this trial, the MAV reached a maximum speed of over $15 m / s$ , as shown in Fig. 19 even with such an aggressive flight, SVO only incurs around $2 m /$ of position drift over the more than $100 m /$ trajectory.

(a) Onboard image with features during hover

5.2 Real World Tests

The quadrotor navigation system described in this paper has been tested extensively in the lab environment as well as in multiple real-world environments. The system has been used on our entry for the first test of the DARPA Fast Lightweight Autonomy (FLA) program and was able to successfully navigate multiple obstacle courses that were set up. The rules of the FLA program do not allow any human interaction after the robot is airborne, so the runs described in this section were fully autonomous.

The test environment was constructed so as to simulate the inside of a warehouse. There were two aisles separated by scaffolding of around $5 m /$ height with tarps on the back of the scaffolding and boxes placed on the shelves. The total length of the test course was around $65 m /$ while the width of each of the aisles, in between the scaffoldings, was $3 m /$ . Different types of obstacles such as a scaffolding tower or scissor lifts were placed along the aisles in order to test the obstacle avoidance performance of the robot. The minimum clearance between the obstacle in the aisle and the scaffolding on the side was set to be $2.1 m /$ . As a reference, the rotor tip-to-tip diameter of the platform is $0.76 m /$ . An example of the obstacles along the aisle is shown in Figure 20.

(a) An example obstacle course. The goal was to get to the other end of the aisle. The different types of obstacles along the length of the aisle can be seen.

Different types of obstacle courses were set up using the aisles and the obstacles in order to challenge the robot. The simplest task was to just go straight down an empty aisle, while the most complicated ones involved changing aisles due to the first aisle being blocked in the middle. The only prior information available for each task was the type of obstacles to expect along the course, but the actual layout of the test course was unknown. The goal position was specified as a bearing and a range from the starting position at the start of each run. A particular task was deemed complete only when we were able to complete three successful runs of the task with different obstacle course layouts, thus ensuring that our system can work robustly. In the following, we describe some of the specific tasks and also show results of our runs through them.

5.2.1 Slalom

In the slalom task, the obstacles in an aisle were arranged in a manner such that robot is forced to move in a zigzag manner along the aisle, going to the right of the first obstacle, then left of the second, one and so on. Figure 21 shows the result of one of our runs. Since there was no ground truth position data available, the only way to judge the performance of the system is to compare the map created by the robot with a map of the real obstacle course. From the figure, we can see that the projected map (in grey) matches the actual obstacle course layout (in black) showing the accuracy of our estimation and control algorithms.

Figure 21: One of our runs for the slalom task. In black we show the actual obstacle course layout. The hollow obstacles in the aisle are similar to the tower shown in Figure 20 while the filled black ones are scissor lifts. The gray regions are the projection of map created by the robot onto the 2D plane. The robot starts near the opening on the left and has to reach the target represented by the black rectangle on the right. The path of the robot shows it moving in a zigzag fashion in order to avoid the obstacles. Each grid cell is $5 m /$ $\times$ $5 m /$ .

5.2.2 Aisle change with $45 ° /$ transition

In this task, the robot was required to change from the first aisle to the second one since the first aisle was blocked in the middle. The opening between the first and second aisles was constructed such that the robot could move diagonally along a $45 ° /$ line from the first aisle into the second aisle. Figure 22 shows one of our runs for this task. The robot successfully completes the transition and starts moving along the second aisle. Note that the goal was still in line with the first aisle, so the robot is always looking to move towards the left in order to get closer to the goal. This causes it to remain in the left part of the second aisle as is observed in the figure. After crossing the second aisle, the robot moves back to the left to reach the goal. Again we can see that the projected map (in grey) matches the actual course layout (in black).

Figure 22: One of our runs for the aisle change with $45 ° /$ transition task. In black we show the actual obstacle course layout. The small filled black objects along the aisles are short obstacles that the robot could fly over. The gray regions are the projection of map created by the robot onto the 2D plane. The robot starts near the opening on the left and has to reach the target represented by the black rectangle on the right. Each grid cell is $5 m /$ $\times$ $5 m /$ .

5.2.3 Aisle change with $90 ° /$ transition

This task was just a more challenging variation of the previous one. Here the aisle change required the robot to move sideways (see Figure 23). We were able to reach the goal, but our system did not perform very well for this task. As can be seen from the figure, the state estimate had small jumps and drifted during the transition between the aisles. There is some position drift but the main issue is the drift in yaw. Since the distance to the goal is large, even small drifts in yaw correspond to large position errors when the robot reaches the goal. The main reason for this drift was that when moving sideways in front of the obstacle during the transition, the vision system lost all the tracked features in the image and as it entered the second aisle, got new features which were far from the camera since it was looking along the aisle. Since the new features were far from the camera, they could not be triangulated accurately and hence caused bad estimates from the vision system. During this phase, there were a number of jumps in the output of vision system and hence led to drifts in the state estimate. As the robot started moving forward after the transition, the vision system was able to triangulate more features along the corridor and get good estimates again. This issue does not occur for the $45 ° /$ transition case since the robot is able to see some part of the second aisle when moving diagonally and hence already has well triangulated features when it is completes the transition into the second aisle. One way to help with this issue would be to make the robot orient itself such that it is always facing the direction along which it is moving.

Figure 23: One of our runs for the aisle change with $90 ° /$ transition task. In black we show the actual obstacle course layout. The small filled black objects along the aisles are short obstacles that the robot could fly over. The gray regions are the projection of map created by the robot onto the 2D plane. The robot starts near the opening on the left and has to reach the target represented by the black rectangle on the right. Each grid cell is $5 m /$ $\times$ $5 m /$ .

5.3 High speed flight

In order to test the high speed capability of the system, we performed a test run in an aisle with no obstacles. The goal provided to the robot was to go straight for a distance of $65 m /$ . We were able to fly at speeds of up to $7 m / s$ and reach the desired goal position. A plot of the desired and estimated position and velocity is shown in Figure 24, which shows that the performance of our controller is good enough to track such aggressive trajectories. The initial section of the plots, from $0$ – $4$ $s /$ , is the autonomous takeoff and the forward trajectory begins at $t =$ $4 s /$ . There was no source of ground truth during the test but based on the expected location of the goal, the net drift in the position estimates was less than $2 m /$ .

6 Discussion and Conclusion

In this work, we developed a system that allows a quadrotor to navigate autonomously in GPS-denied and cluttered environments. Our navigation system consists of a set of modules that work together in order to allow the robot to go from a starting position to a specified goal location while avoiding obstacles on the way. After developing our system, we found the following points especially important in order to successfully build such a system:

Modular architecture: During our development process, each of the modules were separately developed. This was made possible by defining proper interfaces between the modules and using message passing to communicate among them. We used ROS as the framework for all the software running on the robot since it was designed to solve this exact problem. This separation of the modules allowed most of the planner development to happen in a simulator while the estimation and control modules were being developed. This accelerated the development since different modules could be implemented and tested in parallel.
Sensor selection: The choice of sensors used for estimation and mapping plays an important role in determining the robustness of the system. As shown in Table 2, there are various advantages and disadvantages of different camera configurations for visual odometry. We selected a stereo configuration for our system since it provides increased robustness over a monocular camera setup, which is gaining popularity among the research community due to its minimalistic nature and also allows us to have simpler algorithms compared to multi-camera systems. The use of a dedicated height sensor makes it possible to maintain altitude even when there is drift in our visual odometry, allowing the robot to safely fly without hitting the ground or going too high. However, the downward pointing height sensor has jumps in the measurement when the robot goes over obstacles and this has to be properly taken care of in the sensor fusion module. For mapping, instead of using a fixed lidar, we mounted it on a servo in order to sweep it up and down allowing us to create 3D maps and navigate in 3D environments with obstacles above and below the robot. With a fixed lidar we would not have been able to safely avoid all the obstacles that we encountered during the tests.
Local map for planning: Using a local map for planning instead of a global map was an crucial decision in the design of our planner. The problem with creating a global map is that we need to explicitly maintain global consistency by making use of loop closures to eliminate drifts. By comparison, the local map approach helps the planner tolerate drifts in the state estimation since the drift is small in the short period of time that the local map is constructed in. This can be seen clearly in Figure 23 where there is large drift in the yaw but the robot is still able to reach the goal. This also helps in reducing the computational complexity and allows us to run the planner at a higher rate. Faster replanning reduces the latency between an obstacle being seen by the mapping system and the robot reacting to it, thus improving the robustness of the system.

In addition to these positive points, we learned some lessons during the tests in the warehouse environment.

Drift in the visual odometry: The tests in the warehouse environment involved flying long trajectories while constantly moving forward. Since the floor of the building was smooth and did not have texture, very few image features could be detected on it. This led to most of our image features coming from the obstacles to the side and front of the robot. We even picked up edgelet features from structures on the ceiling of the building. Thus a large part of the image features were at a large distance from the robot. In order to get good depth estimates of these far-away features, either the stereo baseline needs to be large or there needs to be sufficient parallax between the feature observations due to the motion between frames. We were limited to a $0.2 m /$ stereo baseline due to the size of the robot. When moving along the long aisles, the image features were mainly in front of the robot, which sometimes led to insufficient parallax to get good depth estimates for the features. Due to the poor depth estimates for some of the features, the visual odometry was not able to detect the correct scale of the motion between frames, which led to drift in the estimates. This caused a failure to reach the goal in some cases. One solution to this, that we are already looking into, is to have a more tightly coupled visual odometry system where the accelerometer measurement is also used in order to provide another source of scale for the visual odometry system.
Local map size: One factor that prevents us from reaching high speeds is the size of the map used for planning. Since we want to generate dynamically feasible trajectories for the robot, we have to take into account the maximum acceleration that the robot can safely achieve. Also, in order to guarantee safety, we have to plan trajectories such that the robot comes to a halt at the end of the known map since there can be undiscovered obstacles just outside the map. Thus, the combination of a map size and maximum acceleration puts a limit on the maximum speed that the robot can reach. The main factor limiting our local map size is the time required to plan in that map. The majority of the time in each planning step is taken by the $A^{*}$ algorithm, which is used to find a path through the hybrid graph (as described in Section 4
). In order to reduce this time, we are looking into using better heuristics for
$A^{*}$ and other techniques such as Jump Point Search [Harabor and Grastien, 2011] which can significantly speed up the graph search.

In conclusion, we have presented a solution that consists of all the modules that are required for a robot to autonomously navigate in an unknown environment. The system has been designed such that all the sensing and computation occur onboard the robot. Once the robot has been launched, there is no human interaction necessary for the robot to navigate to the goal.

The system has been thoroughly tested in the lab as well as in the warehouse environment that was set up as part of the DARPA FLA program. Our robot was able to successfully navigate the various obstacle courses that were specifically designed to challenge the navigation system. The only input from the operator for each run was the goal position relative to the starting position. In fact, during some of the runs, we even lost the communication link between the base station (which was only used for monitoring purposes) and the robot, due to the long distance and the large scaffolding structures in between, but the robot kept on going and successfully completed the task.

The final goal is to be able to fly at speeds of around $20 m / s$ through cluttered environments, and we believe that it would require more work in all the individual modules that make up the system. In estimation, we need to reduce the drift that the visual odometry system experiences when flying fast while following long trajectories. In control, we need to incorporate aerodynamic effects such as drag, which become increasingly important when flying fast. In the mapping part, the nodding lidar solution needs to be replaced by one which provides a denser representation of the environment in order to detect small obstacles reliably. And finally, the planning subsystem needs to be sped up in order to allow us to use a larger map for planning and also to allow faster replanning in order to make the system more robust. As these developments are made, we would be able to incorporate them into our system due to the modular architecture, thus providing a strong foundation for future research.

Acknowledgments

We gratefully acknowledge support from DARPA grants HR001151626/HR0011516850.

References

[Achtelik et al., 2009] Achtelik, M., Bachrach, A., He, R., Prentice, S., and Roy, N. (2009). Stereo vision and laser odometry for autonomous helicopters in gps-denied indoor environments. volume 7332, pages 733219–733219–10.
[Augugliaro et al., 2014] Augugliaro, F., Lupashin, S., Hamer, M., Male, C., Hehn, M., Mueller, M. W., Willmann, J., Gramazio, F., Kohler, M., and D’Andrea, R. (2014). The Flight Assembled Architecture installation: Cooperative construction with flying machines. IEEE Control Systems Magazine, 34(4):46–64.
[Bachrach et al., 2009] Bachrach, A., He, R., and Roy, N. (2009). Autonomous flight in unknown indoor environments. International Journal of Micro Air Vehicles, 1(4):217–228.
[Bachrach et al., 2011] Bachrach, A., Prentice, S., He, R., and Roy, N. (2011). RANGE – Robust Autonomous Navigation in GPS-Denied Environments. Journal of Field Robotics, 28(5):644–666.
[Bellingham et al., 2002] Bellingham, J., Richards, A., and How, J. P. (2002). Receding horizon control of autonomous aerial vehicles. In Proceedings of the 2002 American Control Conference (IEEE Cat. No.CH37301), volume 5, pages 3741–3746 vol.5.
[Bloesch et al., 2015] Bloesch, M., Omari, S., Hutter, M., and Siegwart, R. (2015). Robust visual inertial odometry using a direct ekf-based approach. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 298–304.
[Blösch et al., 2010] Blösch, M., Weiss, S., Scaramuzza, D., and Siegwart, R. (2010). Vision based mav navigation in unknown and unstructured environments. In 2010 IEEE International Conference on Robotics and Automation, pages 21–28.
[Bouabdallah et al., 2004] Bouabdallah, S., Murrieri, P., and Siegwart, R. (2004). Design and control of an indoor micro quadrotor. In Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004 IEEE International Conference on, volume 5, pages 4393–4398 Vol.5.
[Bouabdallah and Siegwart, 2005] Bouabdallah, S. and Siegwart, R. (2005). Backstepping and sliding-mode techniques applied to an indoor micro quadrotor. In Proceedings of the 2005 IEEE International Conference on Robotics and Automation, pages 2247–2252.
[Bouabdallah and Siegwart, 2007] Bouabdallah, S. and Siegwart, R. (2007). Full control of a quadrotor. In 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 153–158.
[Boyd and Vandenberghe, 2004] Boyd, S. and Vandenberghe, L. (2004). Convex Optimization. Cambridge University Press, New York, NY, USA.
[Civera et al., 2008] Civera, J., Davison, A., and Montiel, J. (2008). Inverse Depth Parametrization for Monocular SLAM. IEEE Trans. Robotics, 24(5).
[Deits and Tedrake, 2015a] Deits, R. and Tedrake, R. (2015a).

	$P_{x y} = 2 L \sum i = 0 w_{i}^{c} [X_{i}^{x} (k + 1 \| k) - {¯ x}_{k + 1 \| k}] {[Y_{i} (k + 1 \| k) - {¯ y}_{k + 1 \| k}]}_{i}^{T}$
	$K = P_{x y} P_{y y}^{- 1}$
	${¯ x}_{k + 1} = {¯ x}_{k} + K (y_{k + 1} - ¯ y_{k + 1 \| k})$
	$P_{k + 1} = P_{k + 1 \| k} - K P_{y y} K^{T}$

Fast, Autonomous Flight in GPS-Denied and Cluttered Environments

Experiments in Fast, Autonomous, GPS-Denied Quadrotor Flight

Neuromodulated Patience for Robot and Self-Driving Vehicle Navigation

Toward Agile Maneuvers in Highly Constrained Spaces: Learning from Hallucination

Autonomous Mobile Robot Navigation in Uneven and Unstructured Indoor Environments

Robust Collision-free Lightweight Aerial Autonomy for Unknown Area Exploration

Autonomous Cooperative Multi-Vehicle System for Interception of Aerial and Stationary Targets in Unknown Environments

How ornithopters can perch autonomously on a branch

1 Introduction

2 System Design

2.1 Platform Design

2.2 Sensing, Computation and Communication

2.3 Software Architecture

3 Estimation and Control

3.1 Visual Odometry

3.1.1 Overview of SVO

3.1.2 Motion Estimation

3.1.3 Mapping

3.1.4 Implementation Details

3.2 Sensor Fusion

3.3 Control

4 Mapping and Planning

4.1 Mapping

4.2 Planner

4.3 Trajectory Generation

4.3.1 Regional Inflation by Line Segments

4.3.2 Path Modification

4.3.3 Trajectory Optimization

4.3.4 Continuous Optimization

5 Experimental Results

5.1 Estimation benchmarking

5.2 Real World Tests

5.2.1 Slalom

5.2.2 Aisle change with $45 ° /$ transition

5.2.3 Aisle change with $90 ° /$ transition

5.3 High speed flight

6 Discussion and Conclusion

Acknowledgments

References

Fast, Autonomous Flight in GPS-Denied and Cluttered Environments

Related Research

Experiments in Fast, Autonomous, GPS-Denied Quadrotor Flight

Neuromodulated Patience for Robot and Self-Driving Vehicle Navigation

Toward Agile Maneuvers in Highly Constrained Spaces: Learning from Hallucination

Autonomous Mobile Robot Navigation in Uneven and Unstructured Indoor Environments

Robust Collision-free Lightweight Aerial Autonomy for Unknown Area Exploration

Autonomous Cooperative Multi-Vehicle System for Interception of Aerial and Stationary Targets in Unknown Environments

How ornithopters can perch autonomously on a branch

1 Introduction

2 System Design

2.1 Platform Design

2.2 Sensing, Computation and Communication

2.3 Software Architecture

3 Estimation and Control

3.1 Visual Odometry

3.1.1 Overview of SVO

3.1.2 Motion Estimation

3.1.3 Mapping

3.1.4 Implementation Details

3.2 Sensor Fusion

3.3 Control

4 Mapping and Planning

4.1 Mapping

4.2 Planner

4.3 Trajectory Generation

4.3.1 Regional Inflation by Line Segments

4.3.2 Path Modification

4.3.3 Trajectory Optimization

4.3.4 Continuous Optimization

5 Experimental Results

5.1 Estimation benchmarking

5.2 Real World Tests

5.2.1 Slalom

5.2.2 Aisle change with 45°/ transition

5.2.3 Aisle change with 90°/ transition

5.3 High speed flight

6 Discussion and Conclusion

Acknowledgments

References

5.2.2 Aisle change with $45 ° /$ transition

5.2.3 Aisle change with $90 ° /$ transition