Rapid Quadrotor Navigation in Diverse Environments using an Onboard Depth Camera

SSRR · 2024

Best Paper Award

How do we leverage a depth camera and an IMU to enable quadrotor navigation through a diverse set of environments?

Jonathan Lee Abhishek Rathod Kshitij Goel John Stecklein Wennie Tabib

Search and rescue environments exhibit challenging 3D geometry (e.g., confined spaces, rubble, and breakdown), which necessitates agile and maneuverable aerial robotic systems. Because these systems are size, weight, and power (SWaP) constrained, rapid navigation is essential for maximizing environment coverage. Onboard autonomy must be robust to prevent collisions, which may endanger rescuers and victims. Prior works have developed high-speed navigation solutions for autonomous aerial systems, but few have considered safety for search and rescue applications. These works have also not demonstrated their approaches in diverse environments. We bridge this gap in the state of the art by developing a reactive planner using forward-arc motion primitives, which leverages a history of RGB-D observations to safely maneuver in close proximity to obstacles. At every planning round, a safe stopping action is scheduled, which is executed if no feasible motion plan is found at the next planning round. The approach is evaluated in thousands of simulations and deployed in diverse environments, including caves and forests. The results demonstrate a 24% increase in success rate compared to state-of-the-art approaches.

Figures

**Motivating Hazards** (a) Tight spaces, (b) high amounts of particulate matter such as dust and (c) thin obstacles such as branches are hazards found in search and rescue environments. To aid search teams, autonomous aerial systems must rapidly navigate these dangers without posing additional risks for rescuers or victims. This paper proposes a rapid quadrotor navigation system, which uses forward-arc motion primitives and a forward-facing depth camera to achieve speeds up to 6 m/s in cluttered environments. Safety is achieved by executing a safe stopping action when no feasible action is found. Experiments are conducted in diverse environments, including caves and forests. A video of these experiments may be found at https://youtu.be/tk8vUot0gD4

**System Diagram** System diagram of the navigation algorithm. Given depth images and odometry, NanoMap [7] is used for collision avoidance and a library of forward-arc motion primitives is generated for motion planning. To maintain safety, collision-free trajectories are scheduled such that a feasible stopping action is always available within the known free space.

**Trajectory Smoothness** Derivatives of the scheduled trajectory are continuous up to snap and smooth up to jerk. The planning strategy ensures the robot stops in a safe region.

**Motion Primitive Scheduling** Illustrative example of trajectory scheduling with the motion primitive library. The motion primitives in the library are shown in gray. The cost of each primitive is evaluated by computing the Euclidean distance between the endpoint and goal (shown as a pink triangle). The endpoints are shown as dots colored from purple to blue, where more pink indicates closer to the goal. Motion primitives that are in collision are pruned. The primitive with lowest cost (shown in dark gray) is selected for execution. The selected primitive segment is scheduled from times [tp, 2tp) and the stopping primitive is scheduled from [2tp, 2tp + T).

**Simulation Environments** (a)–(c) illustrate a subset of the simulation environments used to validate the proposed approach.

**Planner Success Rates** Planner success rate and failure modes across the environments detailed in Table I. The proposed method (Forward-Arc) achieves the highest success rate and lowest collision rate compared to the baseline reactive planners [3, 7]. A total of 1350 trials are run (450 for each approach).

**Mine Environment Trajectories** Cross-section of the mine environment with overlays of 5 trajectories for each planning approach. The start and goal locations are indicated in blue and magenta, respectively. ForwardArc reaches the goal in all 5 trials. RAPPIDS collides in 4 trials and times out in 1 trial. Florence collides in 1 trials and times out in 4 trials.

**Trajectory Overlays** Overlay of 5 trajectories for each planner on the ground truth point cloud (vmax = 3m/s and ρ = 0.075 obstacles/m2). Forward-Arc takes the shortest path towards the goal compared to the baselines.

**Density and Speed Study** (a) success rate and collision rate matrices for simulations that vary obstacle densities and speeds. The Forward-Arc approach has higher rates of success at higher obstacle densities and speeds compared to the baseline approaches. (b) distribution of planning performance metrics from successful simulation trials across varying obstacle densities and speeds. Note that a log scale is used for the control effort plots. Forward-Arc reaches the objective with lower average flight time, path length, and control effort metrics compared to the baseline approaches.

**Hardware Platform** The aerial robot used in hardware experiments is equipped with a D455 and NVIDIA Orin AGX.

**Test Environments and Velocity Profiles** Left: flight arena, forest, and cave test environments. Right: reference trajectory velocity profiles for flights 10, 7 and 4, respectively, from Table II. The maximum achieved speed across all trials is 6 m/s.

**Forest Flight Test** Outdoor obstacle avoidance test under tree canopy (forest flight 7 in Fig. 11 and Table II). Top: VINS trajectory overlayed on terrain map (low speeds are shown in red and high speeds in yellow). Camera frustums are visualized at four time steps where the robot maneuvers to avoid obstacles. Middle: Onboard RGB image from RealSense D455. Bottom: Robot and forward-arc motion primitive library (red: non-safe primitives, blue: safe primitives, yellow: selected primitive, cyan: stopping primitive).

Acknowledgments

This work was supported in part by the U.S. Army Research Office and the U.S. Army Futures Command under Contract No. W519TC-23-C-0031. The authors would like to thank E. Burkholder for field testing support. The authors would also like to thank K. Bailey and T. Miller for facilitating experiments at the cave in Kentucky.

BibTeX

@inproceedings{rapid-quadrotor-navigation-ssrr-2024,
  title={Rapid Quadrotor Navigation in Diverse Environments using an Onboard Depth Camera},
  author={Jonathan Lee, Abhishek Rathod, Kshitij Goel, John Stecklein, and Wennie Tabib},
  booktitle={IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2024},
  year={2024}
}