Abstract
Our goal in this research is to develop and integrate three-dimensional
collision-avoidance and navigation with physical agents, such as micro-air vehicles (MAVs). We build
on prior techniques based on reciprocal velocity obstacles (RVOs) for collision-free
navigation and use the ArduCopter quad-rotor helicopters, designed by 3D Robotics, as the
physical agents. MAVLink, a communications protocol for micro-air vehicles, is used with the
Mission Planner ground-station software providing hardware- and software-in-the-loop
capabilities to interface between the quadcopters and the RVO software. We also extend
RVO-based 3D collision algorithms to take into account kinematic and dynamic constraints,
including bounds on the turn radius, maximum altitude, maximum velocity, etc., and
highlight the results using our simulator.
Introduction
Research in robot motion planning has become enormously popular in recent years,
demonstrated by such projects as Google Chauffeur, the software powering the company’s
self-driving car, Big Dog and other products by Boston Dynamics, and most recently Amazon
Prime Air, which aims to deliver packages using eight-rotor helicopters. Research in
two-dimensional motion planning for agents confined to the ground and moving only in the x-y
plane with one rotational (turn) angle is much more mature than its younger counterpart of
motion planning in three dimensions. Motion planning for micro-air vehicles (MAVs) in a
three-dimensional workspace have a configuration space denoted R3 x SO(3), meaning they
are capable of achieving configurations in the three translational components (x, y, z) as well
are said to have six degrees of freedom.
Let us define the meaning of autonomous. In Merriam-Webster’s dictionary, it is
defined as existing or acting separately from other things or people. With this definition of
autonomy we cannot use the term to accurately describe the vast majority of existing
unmanned aircraft systems (UAS), if any at all, at the current state of the art. These UAS do
not act and form decisions independently of things or people. Rather they are unmanned or
remotely piloted, meaning that although a human is not physically in the cockpit of the
aircraft, a human has either pre-programmed the UAS’s flight path and cleared the airspace
to ensure its success, or a human is piloting the UAS from a ground station as if it were a
videogame. The long-term goal of our research is to develop a UAS that is capable of
collecting data about its environment and, given a goal destination, planning a path that
avoids static and dynamic obstacles and that obeys the kinematic, dynamic, and stability
constraints of the micro-air vehicle to successfully reach its goal destination.
Previous Work
The basis for much of this work comes from previous work in the GAMMA (Geometric
Algorithms for Modeling, Motion, and Animation) group at UNC Chapel Hill. We use an
implementation of collision-free navigation using reciprocal velocity obstacles (RVO),
defined as follows:
𝑅𝑉𝑂𝐵𝐴(𝑣𝐵, 𝑣𝐴) = {𝒗′𝐴|2𝒗′
𝐴− 𝒗𝐴 ∈ 𝑉𝑂𝐵𝐴(𝒗𝑩)}
The reciprocal velocity obstacle 𝑅𝑉𝑂𝐵𝐴(𝑣
𝐵, 𝑣𝐴) of an agent B to an agent A contains all
velocities for agent A that are the average of the current velocity vA and a velocity inside the
velocity obstacle 𝑉𝑂𝐵𝐴(𝑣
The RVO2 (Reciprocal Velocity Obstacle) library was developed by researchers in
GAMMA for collision avoidance in real-time multi-agent simulations. We use the extension
of this library, RVO2-3D, which differs from RVO2 in that it does not allow for static obstacles
(or any obstacles except other known agents) and it is applied in three dimensional
workspaces rather than two-dimensional workspaces. Using this library, the user specifies
the number of agents in the environment as well as the initial and goal positions, radius, and
preferred velocity for each agent. The implementation assumes that all agents can sense
their own position, radius, and velocity as well as those of all other agents in the
environment. At each timestep, each individual agent computes a collision-free motion plan
based on the optimal reciprocal collision avoidance (ORCA) algorithm.
In ORCA, for any given pair of agents each agent takes half the responsibility for
avoiding collisions, assuming the aforementioned parameters are known and other agents
are using the same logic for avoiding collisions. A given agent computes a half-plane of
velocities (in velocity-space) for each of the other agents in the environment that it might
take in order to avoid collisions with those agents. The agent then decides the best course of
action by taking the intersection of all these half-planes in velocity-space, guaranteeing a
smooth and collision-free trajectory for at least a given time window [Snape et al, 2010].
In Navigating Multiple Simple-Airplanes in 3D Workspace [Snape and Manocha 2010],
the authors introduce the simple airplane model which computes collision-free and
oscillation-free motion using ORCA in three-dimensions while also accounting for kinematic
and dynamic constraints. This paper also introduces the concept of variable reciprocity in
avoiding collisions. The size, mass, payload, and countless other factors of fixed-wing aircraft
reciprocity, aircraft that experience fewer constraints on motion have a higher responsibility
to avoid collisions since they have a larger space of velocities to choose from than an aircraft
with more constraints they might be avoiding collisions with. Since quadrotor helicopters,
or quadcopters, have different kinematic and dynamic constraints than a simple fixed-wing
aircraft, we extend this approach to account for the variations in physical constraints of
quadcopters.
System Formulation
Towards the goal of autonomous navigation -- meaning real-time, collision-free path
planning with no intervention from a human pilot or external controller -- we propose a
system of three components: a ground station, an arbitrary number of physical agents, and
telemetry communications between the ground and air components. Figure 1 illustrates a
potential configuration of two quadcopters, each with a corresponding ground station
performing path computation, that broadcast information about their position, velocity, etc.
to any subscribing ground stations.
There are good path planning techniques that have been successfully tested on
physical systems in two-dimensional workspaces, but not many have been tested on physical
systems in three-dimensional workspaces outside of simulation. In this work we modify one
such algorithm proven successful on physical systems in 2D, Reciprocal Velocity Obstacles
(RVO), to perform automatic path computation for quadcopters, the physical agents. In RVO,
agents assume equal responsibility for avoiding collisions in a pairwise manner. It computes
the position and velocity of each nearby agent, and computes a set of velocities for each agent
set that results in minimal change from the current velocity and the resulting trajectory
would also satisfy the kinematic and dynamics constraints.
The software library RVO2 is one of the implementations of the ORCA algorithm,
which has been applied to the iRobot Create mobile robots to successfully demonstrate
collision-free and oscillation-free navigation for physical agents on a two dimensional-plane.
We use the RVO2-3D software library, a 3D extension of collision avoidance for objects in a
three-dimensional workspace. The existing implementation assumes each agent can
correctly sense its own radius, position, and velocity as well as those of all other agents in
the environment. It accounts for only four degrees of freedom -- the translation components
x, y, z and one rotation angle, yaw -- and ignores the two remaining rotation angles, pitch and
roll. Our goal is to modify this implementation to account for the dynamic and control
constraints specific to quadcopters.
Figure 1
We chose to use a quadcopter (quad-rotor helicopter) as the form-factor for physical
agents. We chose this over other aerial vehicle options, i.e. fixed-wing or helicopter, because
of their stability (achieved by electronic control systems and sensors), comparatively low
price, small size, agile maneuverability, and ability to be flown indoors and outdoors. For
these reasons, quadcopters are an extremely popular platform for research in unmanned
aerial systems.
After extensive research into different manufacturers of these quadcopters, we chose
the ArduCopter Quad C Frame by 3D Robotics. This option was very inexpensive compared
to other vehicles designed and sold for research purposes and had an optional electronics
kit which provided us with a wireless telemetry kit, ArduPilot Mega 2.6 autopilot, power
module, and GPS unit in addition to the frame, propellers, motors, and some spare parts. The
ArduCopter allows a load capacity of 400 grams, more than double the average load capacity
of less expensive quadcopters on the market (i.e. the Parrot ARDrone 2.0).
The ArduPilot Mega (APM) autopilot suite consists of three components: the
hardware, the firmware, and the software. The hardware is the physical autopilot unit
manufactured by 3D Robotics, which contains embedded systems and peripheral sensors
such as accelerometers within a small hardware package (Figure 2). The ArduPilot acts as
the eyes and ears of the system. It transmits information about the vehicle’s position, speed,
flight progress, etc. through the radio module on board the vehicle through, which is
Figure 2
The ArduPilot Mega 2.6 (APM) combines all the systems and sensors needed for an autopilot into a small, modular package.
The firmware runs on top of the hardware, which we loaded onto the APM during
setup. There are firmware options for any of the supported aerial vehicles; in our case the
option was “QuadCopter,” but we could use the same APM unit with another type of vehicle,
such as an Octocopter, Hexacopter, Fixed-wing, or land rover, simply by loading the provided
firmware for that vehicle (Figure 3).
Figure 3
The software component of the APM suite, called Mission Planner, serves as the user’s
interface to the hardware system. Initial set-up, configuration, and testing (including
selecting and loading firmware onto the vehicle) is done through this interface. Mission
Planner allows planning of missions by selection and design of waypoints or more detailed
flight plans as well as post-mission analysis using full automatic data logs (Figure 4).
Figure 4
Mission Planner allows definition of flight plans via point-and-click selection or loading of waypoints.
Communication between the ground station and quadcopters is performed using the
MAVLink Micro-Air Vehicle Communication Protocol. MAVLink is a lightweight, header-only
message marshalling library. These messages are transmitted and received by the ground
station and quadcopters using interchangeable ground and air radio telemetry modules
manufactured by 3D Robotics. The quadcopter transmits information about its position,
module in the form of MAVLink packets. The GPS module, shown in Figure 5, contains a
digital compass and has a theoretical accuracy between two and three meters.
Figure 5
The image on the left shows the interchangeable ground and air radio telemetry modules used for communications between the ground station and quadcopters. The image on the right shows the GPS module with digital compass.
Sensing and Uncertainty
There are many factors we assume to be negligible in simulation that we must
consider in reality, such as the influence of wind and drag on the flight of a quadcopter. Many
challenges arise as a result of error and uncertainty introduced when we move from the
perfect world of simulation to the unpredictable physical world due to use of various
sensors. In our simulation, RVO2-3D assumes that each agent can correctly measure its own
position and velocity as well those of all other agents in the environment. This assumption is
acceptable in simulated environments but accurate positioning becomes a challenge with
real sensors that have noise and error. For example, the accuracy of the quadcopter’s
additional methods must be incorporated to mitigate the uncertainty of positioning. In our
physical system we must also account for the latency of communication between the
quadcopters and their respective ground stations in order to ensure a new velocity can be
computed and received by the quadcopter’s autopilot before the potential collision occurs.
Another challenge present in the physical world but absent from simulation is the
quadcopter battery duration. Electronic stabilization and control of four motors combined
with the quadcopter’s small and lightweight form factor requires significant power
consumption, so the expected battery life must be accounted for in the lifetime of the path
computation from initial to goal positions.
Preliminary Results
In order to test the path planning computation and our various modifications to the
existing RVO2-3D implementation in simulation, we created a visualization of the simulation
using Ogre3D, an open-source graphics engine. We created a test simulation of our own
system with two quadcopters, represented by the blue spheres in Figure 5, whose initial
positions are at opposite ends of a sphere. The goal point of each quadcopter is on the
antipodal position of the sphere, to which they must autonomously compute a collision-free
and oscillation-free path. We include various parameters, such as agent separation (the
distance between a given pair of quadcopters) in a detail panel in the upper left, also shown
Figure 6
where they must choose new velocities to avoid an imminent collision with each other. The third image shows the agents after they have successfully avoided collisions with each other and have begun to move away from the center of the sphere to their goal location.
Quadcopters achieve motion by varying the rotation speed, and thus the lift forces,
produced by each of the four motors that control the propellers. Increasing or decreasing the
rotational speed of all four motors simultaneously results in a motion in the up and down
direction, respectively. Creating a differential in thrust between each of the four motors will
tilt the entire body in the direction resulting from that particular control configuration. Thus
quadcopters achieve configurations in three-dimensional workspaces with six-degrees of
freedom by a combination of rotational speeds of the individual motors and subsequent lift
forces at each rotor. In some configurations where these thrust forces are unbalanced, the
quadcopter is unable to maintain stability and may return to its original configuration if
disturbed or even be overcome by the gravitational force downward and crash. The
trajectory of this dynamic system is expressed by differential equations relating the
quadcopter body and inertial earth frame of reference through a rotation matrix.
Towards the goal of incorporating these dynamic and control constraints for
quadcopters into the RVO2-3D implementation, we have incorporated restrictions on the
velocities that need to be taken into account during path computation. We modify the
implementation to ensure the computed velocities are not only collision- and oscillation-free
but also within the range of achievable altitudes and velocities of the ArduCopter. We also
specify a generous time horizon -- the minimum amount of time in seconds for which a new
agent's velocities that are computed by the simulation are safe with respect to other agents
ground station and quadcopter. We limit the yaw angle, the only rotational angle accounted
for by the current implementation, to ensure stable configurations. We incorporate the
uncertainty of the GPS into the specification of the agents’ radii to form a “safety bubble”
around the agents as they move through the environment.
Ongoing Work
Often with hardware we find that the theoretical specifications reported by the
manufacturer varies significantly from actual values measured in experiment. For example,
3D Robotics claims the radio telemetry modules support links 4.5 kilometers long and a GPS
accuracy between two and three meters, but in practice this is probably much different.
Other factors, such as battery life, tend to vary significantly from the reported values and
must be measured through repeated tests in practice. Before we can integrate the ground
station software and quadcopters together into a single system, we must first measure these
values and account for them in our path computation.
Once this testing is complete, we can integrate the three components of the system—
ground station running RVO2-3D, quadcopters, and radio telemetry—by establishing a
publish-and-subscribe framework between the various ground stations and micro-air
vehicles. Improving accuracy and reducing noise in measurements for values like agent
position and velocity that RVO2-3D relies on to compute collision-free paths is an ongoing
challenge. We must continue to explore potential methods of guaranteeing accuracy in these
measurements in order to ensure the automatic navigation of these vehicles is as safe and
reliable as possible. The current system formulation only accounts for avoiding collisions
work we aim to extend this formulation to avoid collisions with unknown static and dynamic
obstacles in the environment, not just other quadcopters, by incorporating technologies such
