A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned...

24
American Institute of Aeronautics and Astronautics 1 A Modeling, Simulation and Control Framework for Small Unmanned Multicopter Platforms in Urban Environments Corey A. Ippolito 1 , Kalmanje Krishnakumar 2 NASA Ames Research Center, Moffett Field, CA, 94035 Sebastian Hening 3 and Shankar Sankararaman 4 Stinger Ghaffarian Technologies Inc., Moffett Field, CA 94035 The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles grows, there is a pressing need for higher-fidelity simulation frameworks with accurate and validated dynamic models to ensure safe operation in complex environments, such as urban city-scapes, to support regulatory decision-making. In this paper we present a modeling, control, and simulation framework to support autonomy research on small unmanned multicopters. This framework includes dynamics based on a forward flight aircraft model derived from aerodynamic coefficients, an extensible mode-based flight management system, a flexible automatic flight control system, and an interactive environment model for urban environments. The system is built on the modular Reflection Architecture that facilitates development and testing of new research components for autonomous vehicle systems in both hardware and software. This paper presents an overview of the framework, derivation of models, overview of the flight hardware, and initial results. Nomenclature = Inertial frame = Body frame = Wind frame x n = Variable x is given in (inertial frame) x b = Variable x is given in (body frame) 2 = Rotation matrix (3x3) from body frame to inertial frame = Position of the vehicle’s CG (the origin of ) relative to the origin of and expressed in the inertial frame = gravity vector specified in the inertial frame 2 = instantaneous rotation matrix from body frame to inertial frame = inertia matrix of the vehicle relative to the body frame, which is assumed constant = 4x4 cross-product matrix for the quaternion vector q , = vehicle forces and moments, respectively = Body axis drag vector (3-elements) = Body axis drag vector = Cross-sectional area 2 = Rotation matrix (3x3) from inertial to body frame Δ = velocity in the inertial frame, difference between wind and body velocity, Δ = = i'th Motor inertia K t = torque proportionality constant (constant, scalar) = thrust from motor i = induced anchor voltage = Motor shaft rotation speed, in radian per second = Motor anchor current, in amperes I. Introduction HIS paper presents the modeling, simulation, and control architecture for general multicopter aircraft, supporting research into autonomy, guidance, navigation, and control (GN&C). NASA’s SAFE50 Project is currently conducting preliminary 1 Aerospace Scientist, NASA Ames Research Center, Moffett Field, CA 94035, AIAA Member. 2 Aerospace Scientist, NASA Ames Research Center, Moffett Field, CA 94035, AIAA Member. 3 Research Engineer, Stinger Ghaffarian Technologies, Inc., NASA Ames Research Center, Moffett Field, CA 94035. 4 Research Engineer, Stinger Ghaffarian Technologies, Inc., NASA Ames Research Center, Moffett Field, CA 94035. T Downloaded by NASA AMES RESEARCH CENTER on September 13, 2018 | http://arc.aiaa.org | DOI: 10.2514/6.2018-1915 2018 AIAA Modeling and Simulation Technologies Conference 8–12 January 2018, Kissimmee, Florida 10.2514/6.2018-1915 This material is declared a work of the U.S. Government and is not subject to copyright protection in the United States. AIAA SciTech Forum

Transcript of A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned...

Page 1: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

1

A Modeling, Simulation and Control Framework for Small

Unmanned Multicopter Platforms in Urban Environments

Corey A. Ippolito1 , Kalmanje Krishnakumar2

NASA Ames Research Center, Moffett Field, CA, 94035

Sebastian Hening3 and Shankar Sankararaman4

Stinger Ghaffarian Technologies Inc., Moffett Field, CA 94035

The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as

demand for using these vehicles grows, there is a pressing need for higher-fidelity simulation frameworks

with accurate and validated dynamic models to ensure safe operation in complex environments, such as

urban city-scapes, to support regulatory decision-making. In this paper we present a modeling, control,

and simulation framework to support autonomy research on small unmanned multicopters. This

framework includes dynamics based on a forward flight aircraft model derived from aerodynamic

coefficients, an extensible mode-based flight management system, a flexible automatic flight control

system, and an interactive environment model for urban environments. The system is built on the

modular Reflection Architecture that facilitates development and testing of new research components

for autonomous vehicle systems in both hardware and software. This paper presents an overview of the

framework, derivation of models, overview of the flight hardware, and initial results.

Nomenclature

ℱ𝑛 = Inertial frame

ℱ𝑏 = Body frame

ℱ𝑤 = Wind frame

xn = Variable x is given in ℱ𝑛 (inertial frame)

xb = Variable x is given in ℱ𝑏 (body frame)

𝑅𝑏2𝑛 = Rotation matrix (3x3) from body frame to inertial frame

𝑝𝑛 = Position of the vehicle’s CG (the origin of ℱ𝑏) relative to the origin of ℱ𝑛 and expressed in the inertial frame ℱ𝑛

𝑔𝑛 = gravity vector specified in the inertial frame

𝑅𝑏2𝑛 = instantaneous rotation matrix from body frame to inertial frame

𝐽𝑏 = inertia matrix of the vehicle relative to the body frame, which is assumed constant

�̃� = 4x4 cross-product matrix for the quaternion vector q

𝐹𝑏, 𝑀𝑏 = vehicle forces and moments, respectively

𝐹𝑑𝑟𝑎𝑔𝑏 = Body axis drag vector (3-elements)

𝑀𝑑𝑟𝑎𝑔𝑏 = Body axis drag vector

𝐴𝑥𝑠 = Cross-sectional area

𝑅𝑛2𝑏 = Rotation matrix (3x3) from inertial to body frame

Δ𝑉𝑛 = velocity in the inertial frame, difference between wind and body velocity, Δ𝑉𝑛 = 𝑣𝑤 − 𝑣𝑏

𝐽𝑀𝑖 = i'th Motor inertia

Kt = torque proportionality constant (constant, scalar)

𝑇𝑖 = thrust from motor i

𝑈𝐴 = induced anchor voltage

𝜔𝑀 = Motor shaft rotation speed, in radian per second

𝐼𝐴 = Motor anchor current, in amperes

I. Introduction

HIS paper presents the modeling, simulation, and control architecture for general multicopter aircraft, supporting research into

autonomy, guidance, navigation, and control (GN&C). NASA’s SAFE50 Project is currently conducting preliminary

1 Aerospace Scientist, NASA Ames Research Center, Moffett Field, CA 94035, AIAA Member. 2 Aerospace Scientist, NASA Ames Research Center, Moffett Field, CA 94035, AIAA Member. 3 Research Engineer, Stinger Ghaffarian Technologies, Inc., NASA Ames Research Center, Moffett Field, CA 94035. 4 Research Engineer, Stinger Ghaffarian Technologies, Inc., NASA Ames Research Center, Moffett Field, CA 94035.

T

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

2018 AIAA Modeling and Simulation Technologies Conference

8–12 January 2018, Kissimmee, Florida

10.2514/6.2018-1915

This material is declared a work of the U.S. Government and is not subject to copyright protection in the United States.

AIAA SciTech Forum

Page 2: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

2

investigations into high-density low-altitude unmanned aerial systems (UAS) operations in densely-populated urban environments 1-4. Several studies anticipate these environments to have a high economic growth potential and will see high anticipated demand

for use 5,6. Unfortunately, the regulatory frameworks for allowing these operations are not yet in place. These regulations must

establish an acceptable risk posture and assurance of safety for all stakeholders involved 7-10. Urban environments pose a significant

number of challenges towards concepts allowing safe and routine access to this airspace for unmanned aircraft, particularly in the

small to medium-size UAS categories. Given the current state of the UAS industry and UAS technologies, significant technological

advances may need to occur to allow for safe routine access, as these vehicles have shown to exhibit high failure rates and currently

pose an unacceptably high risk to people and property they overfly. The path towards establishing a regulatory framework likely

requires establishing UAS Traffic Management (UTM) system concepts that imposes functional, performance, and equipage

requirements on participating UAS vehicle systems 4. Before this framework is adopted, the UTM system concepts must be go

through a rigorous verification and validation (V&V) process to show the system will satisfy all stakeholders’ needs and

requirements. Given the large design space and complexity of the problem faced by the UTM system designers, trade studies and

competing architectures will need to be evaluated and selected as part of establishing an acceptable system concept. Evaluation of

proposed regulations would be facilitated through analysis with high-fidelity and validated simulations in conjunction with avionics

prototyping and flight test experimentation.

The simulation framework we present here is being developed to support evaluation of urban area autonomous vehicle

operations and traffic control concepts in high-density low-altitude urban environments. Small UAS operations in these

environments face numerous challenges that must be addressed and evaluated within the simulation framework. These challenges

include the following 1:

1. Low-altitude, autonomous, unmanned flight is inherently high risk. Lower altitudes allow pilots less time to respond

to emergencies. Remote operators of UAS lack the situational awareness that an onboard pilot would have, and

remote command and control links provide additional challenges. Autonomous flight operations require non-trivial

sophistication to address failures and off-nominal situations without human interaction.

2. Operations will occur in high-density mixed-use airspace. Various configurations of UAS with radically different

performance characteristics will need to safely interoperate with each other and with manned aircraft traffic.

3. Vehicles must operate in highly-constrained spaces within urban canyons. The constrained spaces require high levels

of precision and accuracy in the GN&C system for these vehicles. Urban canyons impede wireless communication

and impede satellite-based navigation. Communication line-of-sight and visual line-of-sight between the vehicle and

ground operator will not be possible for operations that go any significant distance away from the operator.

4. The environment is high-density, both in terms of manned and unmanned air traffic, and in terms of ground objects

that include people, property, and other high-valued assets.

5. The environment includes hazardous ambient conditions for small UAS operations, such as adverse weather,

precipitation, gusts, and local wind patterns.

6. The environment is highly dynamic with significant uncertainty. For instance, the location of people, automobiles,

and other ground objects in a city will not be known a priori. These dynamic objects move within the urban

environment in a manner that is highly unpredictable. Currently there are no databases providing 3-D geometry of

all static objects in a city that can provide assuredly safe autonomous navigation. The location of all hazardous

structures, such as power lines, are not well known. Unanticipated temporary structures may be present, such as

scaffolding, cloth lines, or tethered advertising balloons.

7. Small UAS are highly constrained in terms of size, weight, and power (SWaP).

8. Autonomous UAS pose a risk to objects and other vehicles. Failure rates of current technology is high. UAS systems

are brittle and often single-string designs. Autonomous UAS have limited ability to sense, understand, and respond

to the environment and to other vehicles. Separation assurance and collision avoidance will be difficult services to

provide. Assurance of safety for all objects and stakeholders will need to be demonstrated.

9. Technology capabilities and maturity of current UAS are insufficient for autonomous urban-area operations. This

includes issues with reliability, lack of standardization in vehicle sub-systems, lack of fault and failure tolerance,

lack of GN&C precision, GNSS-reliance for flight critical systems, inability to see and avoid other aircraft and

objects, limited onboard autonomy, a lack of verified fundamental flight dynamic models that capture sUAS behavior

in this environment, lack of robust survelliance and beyond line-of-site communications technologies, and a lack of

standardized concepts or systems level requirements for UAS to operate in these environments.

To support UTM systems-level concept evaluation and analysis, we present a general high-fidelity simulation framework for

evaluation and testing of vehicle-level concepts for general sUAS multicopter platforms operating in urban environments. The

system provides an end-to-end framework for autonomy design, simulation testing, and flight test experimentation. The simulation

environment features the following.

• An interactive simulated urban landscape, including building geometry and support for simulated onboard

environment sensors.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 3: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

3

• Static ground obstacles, such as power lines and billboards.

• Dynamic ground obstacles, such as cars and people.

• Validated high-fidelity vehicle models that incorporates wind dynamics, battery consumption, motor system

electrical effects, and propeller physics.

• A CFD-derived urban area wind model.

• End-to-end framework for autonomy design, simulation testing, and flight vehicle experimentation.

• Fully programmable control system which exposes inner-loop and outer-loop functions

• Modular and isolated design, allowing researchers to focus on single components and small code interfaces.

• Ability to simulate against both high-fidelity flight and low-fidelity flight models.

• Ability to quickly add new control system modes, structures, and autonomy algorithms.

• A reconfigurable plug-and-play simulation and embedded software systems. The software modules being simulated

are the same binary executable code used in flight vehicle configurations. Hardware-in-the-loop V&V of the flight

software in the simulation system provides the ability to quickly transfer simulation experiments to flight tests.

II. Vehicle Dynamics Modeling

A. Axis Frames

The inertial frame ℱ𝑖 is an earth-centered inertial reference frame, assumed here to be a rotating but non-translating frame.

The earth-fixed reference frame, ℱ𝑛, is an earth-fixed frame located at any convenient fixed point on the surface of the earth,

oriented with x-y-z aligned with the local east-north-up absolute directions, and translates and rotates with the Earth’s rotation.

The vehicle body frame, ℱ𝑏, is a body-fixed frame located at the CG of the vehicle with x-y-z aligned to right-forward-up. The

wind axis frame, ℱ𝑤, is a body-fixed frame positioned at the vehicle CG, with y pointing in the direction of the incoming total

wind velocity vector, rotated from ℱ𝑏 about the body x axis by the angle of attack and about the once-rotated body y axis. Generally,

the rotation from a frame A to another frame B will be denoted 𝑅𝐴2𝐵.

B. Vehicle Kinematics

The dynamics equations for the n-copter is of the form

�̇� = f(𝑥, 𝑢, 𝑡) (1)

The vehicle’s rigid-body state vector 𝑥 = [𝑝𝑛, 𝑣𝑛 , 𝑞, 𝜔𝑏]𝑇 is composed of 13 kinematic variables5 which will be augmented

with additional subsystem state variables below. Here, 𝑝𝑛(𝑡) ∈ ℝ3 is the position of the vehicle’s center of gravity (the origin of

ℱ𝑏) relative to the origin of ℱ𝑖 and expressed in the inertial frame ℱ𝑖. The velocity vector 𝑣𝑛(𝑡) ∈ ℝ3 is the velocity of the vehicle

expressed in the inertial frame. The vector 𝑞(𝑡) ∈ ℝ4 represents the normalized quaternion orientation of the vehicle’s non-

stationary frame ℱ𝑏 relative to ℱ𝑛. The vector 𝜔𝑏(𝑡) ∈ ℝ3 is the angular velocity of ℱ𝑏 relative to ℱ𝑛.

The kinematic relationships are given by the following relationships.

�̇�𝑛 = (ωe × pn) + 𝑣𝑛

�̇�𝑛 = 𝑔𝑛 − (ωe × (ωe × pn)) − (ωe × vn) +1

𝑚𝑅𝑏2𝑛𝐹𝑏

�̇� =1

2�̃�𝑞

�̇�𝑏 = 𝐽𝑏−1𝑀𝑏

(2)

The gravity vector 𝑔𝑛 is specified in ℱ𝑛, 𝑅𝑏2𝑛 is the instantaneous rotation matrix from body frame to inertial frame, and 𝐽𝑏 is

the inertia matrix of the vehicle relative to the body frame, which is assumed constant. The value �̃� is the 4x4 cross-product matrix

for the quaternion vector q. The vehicle forces and moments are denoted as 𝐹𝑏 and 𝑀𝑏, respectively. Earth’s rotation vector is

given by 𝜔𝑒 = [0 0 𝜔𝑥], where 𝜔𝑥 is the scalar value of Earth’s rotation rate. Note that the Coriolis 𝜔𝑒 terms can be neglected

when considering small distances, in which case ℱ𝑛 can be considered an inertial frame.

Given the wind vector 𝑣𝑤 and the vehicle velocity 𝑣, the relative wind velocity vector 𝑣𝑟𝑒𝑙 is given by

5 Note that quaternion orientation results in a non-minimal state vector representation.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 4: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

4

𝑣𝑟𝑒𝑙 = 𝑣 − 𝑣𝑤 (3)

The relationship between the wind axis ℱ𝑤 and the body axis ℱ𝑤 is given by the following, where 𝑅𝑧(𝛽) and 𝑅𝑦(𝛼) are

rotations about the z and y axis by side slide 𝛽 and attack angle 𝛼, respectively.

𝑅𝑏2𝑤 = 𝑅𝑧(𝛽)𝑅𝑦(−𝛼) (4)

C. Dynamic Forces and Moments

Forces and moments acting on the body are given in terms of dimensionless coefficients from aerodynamic (𝐹𝑎𝑏 and 𝑀𝑎

𝑏) and

propulsive (𝐹𝑝𝑏 and 𝑀𝑝

𝑏) sources, and are built up as follows. Additionally, contact forces are applied when the vehicle is in contact

with another body, given by 𝐹𝑐𝑏 and 𝑀𝑐

𝑏, as described in the collision and contact force section.

𝐹𝑏 = 𝐹𝑎𝑏 + 𝐹𝑝

𝑏 + 𝐹𝑐𝑏 = 𝑅𝑤2𝑏 [

𝑌−𝐷

𝐿] + 𝐹𝑝

𝑏 + 𝐹𝑐𝑏

𝑀𝑏 = 𝑀𝑎𝑏 + 𝑀𝑝

𝑏 + 𝑀𝑐𝑏

(5)

A simplified form for drag often presented in the literature for small multicopter UAS assumes aerodynamics are dominated

by a simple constant drag coefficient along each axis with a linear viscous drag model. Assuming drag is proportional to velocity

is appropriate at low Reynolds Numbers, around Re<100. Note that the Reynolds Number is given by 𝑅𝑒 = v𝐿/𝜈, where v is

velocity, L is a characteristic length, and 𝜈 is the kinematic viscosity. At standard sea-level atmospheric conditions, 𝜈 =1.460 × 10−5 𝑚2/𝑠, and if the characteristic length is on order of a meter, an Re=1000 roughly corresponds to a velocity of

0.015m/s. Drag models of this form, as expressed below, would only be valid near stationary-hover conditions with no relative

wind.

𝐹𝑎𝑏

𝑎𝑙𝑡𝑒𝑟𝑛𝑎𝑡𝑖𝑣𝑒 = [

𝑣𝑥𝑏 𝐶𝐷,𝑥

𝑣𝑦𝑏 𝐶𝐷,𝑦

𝑣𝑧𝑏 𝐶𝐷,𝑧

] (6)

Similarly, rotational motion often contains a dampening viscous rotational drag term, given as follows.

𝑀𝑎𝑏 = [

𝜔𝑥𝑏 𝐶𝑚𝑑,𝑥

𝜔𝑦𝑏 𝐶𝑚𝑑,𝑦

𝜔𝑧𝑏 𝐶𝑚𝑑,𝑧

] (7)

D. Initial Candidate Aerodynamic Model for Forward-Flight

One goal of this simulation system is to support higher-fidelity model dynamics, but a standard form for aerodynamic modeling

of small UAS systems was not found in the literature beyond the simplified model in equation (6). To support this research, we

hypothesize that a generalized aerodynamic build-up utilizing standard aerospace industry aerodynamic coefficient definitions

would provide an appropriate form for modeling a wider range of flight conditions, particularly forward-flight. The following was

implemented as a possible candidate aerodynamic model that achieves these goals. Verification and validation of a model of this

form through wind tunnel and flight test data is one ongoing goal of this research. The project continues to establish the

appropriateness of this candidate model, and the significance of these coefficients need to be established.

The following dimensionless damping rate derivate coefficients and acceleration derivate coefficients are included in our initial

model. The significance of these coefficients must be established for vehicles of this configuration and scale. A coefficient build-

up provides a number of advantages, including the ability to identify parameters through flight tests. The coefficients selected

include:

• Damping rate derivate coefficients: 𝐶𝑙𝑝, 𝐶𝑚𝑞

, 𝐶𝑛𝑟, 𝐶𝑙𝑟

, 𝐶𝑛𝑝, 𝐶𝐿𝑞

, 𝐶𝑌𝑝, 𝐶𝑌𝑟

• Acceleration derivative coefficients: 𝐶𝐿�̇� , 𝐶𝑚�̇�

The free-stream dynamic pressure �̅� is given by the following, where 𝜌 is the air density.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 5: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

5

�̅� =1

2𝜌|𝑣𝑟𝑒𝑙|2 (8)

The aerodynamic forces in equation (5) for forward-flight proposed of the following general form along the wind axis are given

by lift L, drag D, and side force Y. Similarly, aerodynamic moments in equation (5) are proposed of the following form, defined

by rolling moment l, pitching moment m, and yawing moment n.

𝐹𝑎𝑤 = [

𝑌−𝐷

𝐿] ; 𝑀𝑎

𝑤 = [𝑙

𝑚𝑛

] (9)

The wind-axis force coefficients are defined as follows, where an appropriate surface area S is selected for each platform for

dimensionalization of the non-dimensional coefficients.

𝐿 = �̅�𝑆𝐶𝐿

𝐷 = �̅�𝑆𝐶𝐷

Y=�̅�𝑆𝐶𝑌

(10)

Similarly, moment coefficients are dimensionalized by selection of appropriate characteristic lengths 𝑏𝑥 and 𝑏𝑦 selected along

the body x and y axes, respectively.

𝑙 = �̅�𝑆𝑏𝑦𝐶𝑙

𝑚 = �̅�𝑆𝑏𝑥𝐶𝑚

𝑛 = �̅�𝑆𝑏𝑦𝐶𝑛

(11)

The lift coefficient is currently composed of the following components.

𝐶𝐿 = 𝐶𝐿𝛼(𝛼) + �̇�

𝑏𝑥

2 𝑉𝑇

𝐶𝑙�̇�+ 𝑞

𝑏𝑥

2 𝑉𝑇

𝐶𝑙𝑞

𝐶𝐷 = 𝐶𝐷0 + 𝐶𝐿(+𝐶𝐷𝛼(𝛼) + 𝛽 𝐶𝐷𝛽

𝐶𝑌 = 𝐶𝑌𝛽(𝛽) + 𝑝

𝑏𝑦

2𝑉𝑇𝐶𝑌𝑝 + 𝑟

𝑏𝑦

2𝑉𝑇𝐶𝑌𝑟

(12)

The moment coefficients are built from the following equations

𝐶𝑙 = 𝛽 𝐶𝑙𝛽+ 𝑟

𝑏𝑦

2𝑉𝑇

𝐶𝑙𝑟+ 𝑝

𝑏𝑦

2𝑉𝑇

𝐶𝑙𝑝

𝐶𝑚 = 𝐶𝑚𝑜+ 𝛼 𝐶𝑚𝛼

+𝑏𝑥

2𝑉𝑇

𝐶𝑀𝑞+ �̇�

𝑏𝑥

2𝑉𝑇

𝐶𝑀�̇�

𝐶𝑛 = 𝛽𝐶𝑛𝛽+ 𝑝

𝑏𝑦

2𝑉𝑇

𝐶𝑛𝑝+ 𝑟

𝑏𝑦

2𝑉𝑇

𝐶𝑛𝑟

(13)

E. Propulsion and Motor System Dynamics

Propulsion forces and moments are computed for each motor i (𝐹𝑀𝑖

𝑏 and 𝑀𝑀𝑖

𝑏 , respectively) with i ranging from 1 to the number

of motors, NM.

𝐹𝑝𝑟𝑜𝑝𝑏 = ∑ 𝐹𝑀𝑖

𝑏𝑁𝑀

𝑖

𝑀𝑝𝑟𝑜𝑝𝑏 = ∑ 𝑀𝑀𝑖

𝑏𝑁𝑀

𝑖

(14)

This model utilizes the brushless electric motor model described in 17. The propulsive thrust force is given as a function of the

angular velocity of the motor (𝜔𝑀𝑖) and relative velocity of the wind over the motor (𝑣𝑖), of the form below.

𝐹𝑀𝑖

𝑏 = (𝐶𝑇0𝜔𝑀𝑖

2 + 𝐶𝑇1𝜔𝑀𝑖+ 𝐶𝑇2𝑣𝑖 ⋅ |𝑣𝑖|) �̂�𝑖

𝑏 (15)

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 6: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

6

Here, 𝑧𝑖𝑏 is the unit vector in the direction of positive thrust for motor i given in the body axis (typically 𝑧𝑖

𝑏=(0,0,1)). Propulsive

body moments result from two sources: the moment arm as a function of the motor location (largely about the body x and y axes),

and the motor torque about the motor axis (𝑧𝑖𝑏).

𝑀𝑀𝑖

𝑏 = (𝑟𝑖𝑏 × 𝐹𝑀𝑖

𝑏 ) + (𝑀𝑒 − 𝑀𝑚) �̂�𝑖𝑏 (16)

Here, 𝑟𝑖𝑏 is the position of motor i in the body frame. 𝑀𝑒 and 𝑀𝑚 are the electrical and mechanical torques, respectively.

Likewise, the motor angular velocity derivative is given by

�̇�𝑀𝑖 =1

𝐽𝑀𝑖

(𝑀𝑒 − 𝑀𝑚) (17)

𝐽𝑀𝑖 is the inertia of the motor hub and propeller. The electromagnetic torque for each motor is given by

𝑀𝑒 = 𝑘𝑡 𝐼 (18)

The motor torques are given as functions of the current I and applied voltage V as follows.

𝑀𝑚 = −(𝑘𝑡 𝐹𝑀𝑖

𝑏 + 𝑘𝑚𝜔𝑀𝑖)

𝐼 = 𝑥𝑥 + √𝑥𝑥2 +

𝑢 𝛼𝑀

𝑅𝐴

𝑥𝑥 =𝑢 𝑉𝑖𝑛𝛽𝑀 − 𝑘𝑡 𝜔𝑀𝑖

2 𝑅𝐴

(19)

Here, 𝑘𝑡 is the torque proportionality constant. Note that the velocity of air flow perpendicular to the propeller disc of motor i

(𝑣𝑖) can be expressed as follows.

𝑣𝑖 = 𝑧𝑖𝑏 ⋅ (𝑣𝑛 + (𝑤𝑛 × 𝑅𝑏2𝑛𝑟𝑖

𝑏) − 𝑣𝑤𝑛) (20)

F. Collision Restitution and Contact Force Modeling

Rigid body collisions are handled using two models, an impulse-based restitution model and a contact-force friction model 18,22. Additional information and discussion of various collision detection, restitution, and friction methods can be found in 19,20.

In this implementation, collision detection and penetration correction occur after each simulation time step, as shown in Figure 1

(a). Rigid bodies in simulation are assigned convex collision volume shapes that are defined in the object’s local coordinate system,

which translate and rotate with the rigid bodies. Each body is simulated forward in time by a short time step through integration

of their governing differential equation, as in equation (1), using a fourth-order Runge-Kutta integration scheme. Currently, the

system updates with a timestep of 0.01s, or 100Hz, which matches the update rate of the onboard flight control system rate group.

A geometric collision system then checks for any interpenetrating geometries. If objects are interpenetrating, the system identifies

the overlap, defines the collision coordinate frame, identifies what type of contact has occurred, and identifies a depth of penetration

value. This information is stored in a collision object which is created for each penetration occurrence. Penetrations are corrected

at the end of each time step iteratively by adjusting the position of bodies along the collision plane normal by the depth of

penetration.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 7: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

7

Figure 1. Simulation Update and Correction

Once all bodies are corrected for penetration, a collision restitution system computes the impulse needed to correctly handle

the collision on both colliding bodies. The geometry of a collision encounter is shown in Figure 2. The collision system classifies

collisions to be either a vertex-to-face contact or an edge-to-edge contact, which changes the definition of the collision frame.

Assigned to each body are restitution constants and Coulomb friction constants.

Figure 2. Collision Geometry

Collision restitution is a complex phenomenon that occurs on a time scale much smaller than the simulation time-step used for

updating the rigid bodies in this system. To accommodate this, restitution is computed and applied an instantaneous correction

using an impulse function 𝑓(𝑡). Conceptually, an impulse function is a limiting case of a pulse function with magnitude A and

time duration 𝑡∗, where 𝑡∗ becomes vanishingly small but the area defined under the pulse curve still remains a constant of A. The

function is defined as follows.

𝑓(𝑡) = {lim𝑡∗→0

(𝐴

𝑡∗) 𝑓𝑜𝑟 0 < 𝑡 < 𝑡∗

0 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒

(21)

Each collision object computes the restitution impulse that depends on if the objects are colliding, are at rest relative to each

other, or are moving apart. As shown in in Figure 2, the collision objects calculate the position vectors from the center of gravity

of the bodies to the collision point in world coordinates, 𝑟𝑎 and 𝑟𝑏, and the angular velocities 𝜔𝑎 and 𝜔𝑏 of the bodies in world

coordinates. The relative velocity of the collision points on each body along the collision frame normal, 𝑣𝑟𝑒𝑙− , before restitution is

applied, can be expressed as the following equation.

𝑣𝑟𝑒𝑙− = �̂� ⋅ [(𝑣𝑎

− + (𝜔𝑎 × 𝑟𝑎)) − (𝑣𝑏− + (𝜔𝑏 × 𝑟𝑏))] (22)

Note that the variable superscript – and + are utilized here to represent values before and after collision restitution, respectively.

The sign of the value 𝑣𝑟𝑒𝑙− dictates the type of collision that is occurring. Conceptually, if 𝑣𝑟𝑒𝑙

− < 0 and is negative, the bodies are

still moving towards each other and a collision is occurring that needs to be corrected. If 𝑣𝑟𝑒𝑙− > 0, the bodies are moving away

from each other, and no further constraints are needed. If 𝑣𝑟𝑒𝑙− =0, the bodies are maintaining contact, and a contact constraint is

added to the system.

If a collision has occurred (𝑣𝑟𝑒𝑙− < 0), a restitution impulse is calculated to correct the state of each body based on the collision

restitution parameters assigned to the bodies. We apply a Newtonian impact model where the resulting velocity normal to the

collision plane is given by the following.

Fb(ti)F'b(ti+1)

Fb(ti+1)

(a) Simulation Update Step (b) Penetration Correction Step

Áz

Áxy

BA

BB

Áz

eaBA

BB

eb

Vertex To Face Contact Edge To Edge Contact

P P

rA

PA

ea

BA

rB

PB

eb

xA

xB

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 8: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

8

𝑣𝑟𝑒𝑙+ = −𝜖 𝑣𝑟𝑒𝑙

(23)

The variable 𝜖 is the coefficient of restitution, where 0 ≤ 𝜖 ≤ 1. This parameter is a property of the material of the two bodies

interacting and is assigned to the body pairing. Note that a Newtonian impact model is a simple linear relationship that in practice

can add energy to the system. This formulation can be extended utilizing Poisson’s hypothesis, accounting for restitution occurring

over the compression phase of collision, or Strong’s hypothesis, where the 𝜖2 is given as the ratio of the work done by the normal

impulse during restitution and compression, as described in 24. Strong’s hypothesis has the added benefit for simulation of ensuring

energy is dissipated.

Utilizing the Newtonian impact model in (23), the magnitude of the collision impulse, A, can be expressed as the following

equation.

𝐴 = −(1 + 𝜖)𝑣𝑟𝑒𝑙

(1

𝑚𝑎+

1𝑚𝑏

) + �̂� ⋅ (𝐼𝑎−1(𝑟𝑎 × �̂�) × 𝑟𝑎) + �̂� ⋅ (𝐼𝑏

−1(𝑟𝑏 × �̂�) × 𝑟𝑏)

(24)

Here, 𝜖 is the collision restitution parameter assigned to the body pairing, 𝐼𝑎 and 𝐼𝑏 are the inertia matrices for each body, and

𝑚𝑎 and 𝑚𝑏 are the body masses. For a collision occurring at time 𝑡𝑖, applying the impulse along the collision normal results in the

following instantaneous change in the rigid body velocities and angular velocities.

𝑣𝑎+(𝑡𝑖) = 𝑣𝑎

−(𝑡𝑖) +𝐴

𝑚𝑎�̂�(𝑡𝑖)

𝜔𝑎+(𝑡𝑖) = 𝜔𝑎

−(𝑡𝑖) + 𝐼𝑎−1(𝑠𝑎 × 𝐴�̂�(𝑡𝑖))

(25)

Similarly, for body B, the restitution impulse results in the following change to the vehicle state.

𝑣𝑏+(𝑡𝑖) = 𝑣𝑏

−(𝑡𝑖) +𝐴

𝑚𝑏�̂�(𝑡𝑖)

𝜔𝑏+(𝑡𝑖) = 𝜔𝑏

−(𝑡𝑖) − 𝐼𝑏−1(𝑠𝑏 × 𝐴�̂�(𝑡𝑖))

(26)

For collisions where restitution occurs (𝑣𝑟𝑒𝑙− <0), a viscous friction correction is applied to the impulse. When two bodies are

maintaining contact (𝑣𝑟𝑒𝑙− =0), the system creates a persistent force object that applies forces to all connected bodies while the

contact condition is maintained. A contact group is a group of bodies that are connected through a set of persistent contact points.

For any contact, the contact condition is the following, where 𝑣𝑟𝑒𝑙∗ is a small value that represents the velocity at which the system

believes a contact has been lost.

|𝑣𝑟𝑒𝑙𝑖| < 𝑣𝑟𝑒𝑙

∗ (27)

For each contact group, contact forces are computed simultaneously for all bodies and contacts. Tangential forces are calculated

through a Coulomb friction model and applied to the rigid body objects in future simulation update steps. These forces will continue

to be applied to the bodies until contact is no longer maintained, at which time the contact object is destroyed. The normal forces

at each contact 𝒇𝑛 are computed across the contact group, and the resulting tangential velocities 𝑢𝑡 are calculated. At each contact,

a friction force in the tangential direction 𝒇𝑡 is applied given by the following.

𝒇𝑡 = −𝜇𝑘 ‖𝒇𝑛‖ �̂� 𝑖𝑓 𝑢𝑡 ≠ 0

‖𝒇𝑡‖ < −𝜇𝑠 ‖𝒇𝑛‖ �̂� 𝑖𝑓 𝑢𝑡 = 0

(28)

For the static friction case, 𝑢𝑡 = 0, a solution for 𝒇𝑡 is found across all contacts in the contact group such that the constraints

at the contact points are maintained. In this equation, the tangential velocity 𝑢𝑡 is the magnitude along the tangential direction

vector �̂�, a coefficient of static and kinetic friction 𝜇𝑠 and 𝜇𝑘, and the force vector along the normal and tangential directions 𝒇𝑛

and 𝒇𝑡. The resulting computed friction force at contact i, 𝐹𝑓𝑖, is applied at the contact point to each body in the contact group.

Returning to (5), and utilizing the terms in Figure 2, the terms 𝐹𝑐𝑏 and 𝑀𝑐

𝑏 can then be found for all contact forces i that are applied

to the body.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 9: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

9

𝐹𝑓𝑖= 𝒇𝑛 + 𝒇𝑡

𝐹𝑐𝑏 = ∑ 𝐹𝑓𝑖

𝑖

𝑀𝑐𝑏 = ∑ ((𝑥𝑎𝑖

− 𝑝𝑎𝑖) × 𝐹𝑓𝑖

)

𝑖

(29)

G. Model Summary for Non-Linear Round-Earth Simulation Model

The following equations summarize the multicopter flight dynamics model derived in this paper.

• Kinematics and Dynamics

�̇�𝑛 = (ωe × pn) + 𝑣𝑛

�̇�𝑛 = 𝑔𝑛 − (ωe × (ωe × pn)) − (ωe × vn) +1

𝑚𝑅𝑏2𝑛𝐹𝑏

�̇� =1

2�̃�𝑞

�̇�𝑏 = 𝐽𝑏−1𝑀𝑏

(2)

• Forces and Moments

𝐹𝑏 = 𝐹𝑎𝑏 + 𝐹𝑝

𝑏 + 𝐹𝑐𝑏 = 𝑅𝑤2𝑏 [

𝑌−𝐷

𝐿] + 𝐹𝑝

𝑏 + 𝐹𝑐𝑏

𝑀𝑏 = 𝑀𝑎𝑏 + 𝑀𝑝

𝑏 + 𝑀𝑐𝑏

(5)

• Propulsive Forces and Engine Model

𝐹𝑝𝑟𝑜𝑝𝑏 = ∑ 𝐹𝑀𝑖

𝑏𝑁𝑀

𝑖

𝑀𝑝𝑟𝑜𝑝𝑏 = ∑ 𝑀𝑀𝑖

𝑏𝑁𝑀

𝑖

(14)

𝐹𝑀𝑖

𝑏 = (𝐶𝑇0𝜔𝑀𝑖

2 + 𝐶𝑇1𝜔𝑀𝑖+ 𝐶𝑇2𝑣𝑖 ⋅ |𝑣𝑖|) �̂�𝑖

𝑏 (15)

𝑀𝑀𝑖

𝑏 = (𝑟𝑖𝑏 × 𝐹𝑀𝑖

𝑏 ) + (𝑀𝑒 − 𝑀𝑚) �̂�𝑖𝑏 (16)

�̇�𝑀𝑖 =1

𝐽𝑀𝑖

(𝑀𝑒 − 𝑀𝑚) (17)

• Aerodynamic Forces and Moments

𝐶𝐿 = 𝐶𝐿𝛼(𝛼) + �̇�

𝑏𝑥

2 𝑉𝑇𝐶𝑙�̇� + 𝑞

𝑏𝑥

2 𝑉𝑇𝐶𝑙𝑞

𝐶𝐷 = 𝐶𝐷0 + 𝐶𝐷𝛼(𝛼) + 𝛽 𝐶𝐷𝛽

𝐶𝑌 = 𝐶𝑌𝛽(𝛽) + 𝑝

𝑏𝑦

2𝑉𝑇𝐶𝑌𝑝 + 𝑟

𝑏𝑦

2𝑉𝑇𝐶𝑌𝑟

(12)

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 10: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

10

𝐶𝑙 = 𝛽 𝐶𝑙𝛽+ 𝑟

𝑏𝑦

2𝑉𝑇

𝐶𝑙𝑟+ 𝑝

𝑏𝑦

2𝑉𝑇

𝐶𝑙𝑝

𝐶𝑚 = 𝐶𝑚𝑜+ 𝛼 𝐶𝑚𝛼

+𝑏𝑥

2𝑉𝑇

𝐶𝑀𝑞+ �̇�

𝑏𝑥

2𝑉𝑇

𝐶𝑀�̇�

𝐶𝑛 = 𝛽𝐶𝑛𝛽+ 𝑝

𝑏𝑦

2𝑉𝑇

𝐶𝑛𝑝+ 𝑟

𝑏𝑦

2𝑉𝑇

𝐶𝑛𝑟

(13)

III. Simulation Architecture Framework

The system is implemented in NASA’s Reflection Architecture. The Reflection Architecture is a component-based plug-and-

play architecture for autonomy and embedded vehicle systems research. Reflection allows components to be compiled into reusable

executable libraries, exposing variables for data passing and methods for remote invocation. A real-time scripting language,

ReflectionScript, provides users the ability to establish configurations, load modules, and interact with modules through their

interfaces. Reflection provides complete code isolation between modules, removing any compile-time and link-time dependencies,

and allowing module binaries to be written and compiled once, then tested and reused without any further modifications. Reflection

provides a distributed resource mirroring scheme that allows multi-threaded execution of module functions and methods that is free

from resource-sharing issues, such as race condition and deadlock, while providing assured real-time deterministic timing

execution.

Four major configurations were created to establish a workflow path from simulation to flight testing, as shown in Figure 3.

Configuration (a) loads all components of the simulation onto a single workstation and a single multi-threaded execution instance

of Reflection. This is the primary configuration for simulation testing. Configuration (b) is an intermediate configuration which

separates the flight system and ground control station components into different execution instances communicating over virtual

communication links. The virtual links are either a virtual serial interface or a TCP/UDP/IP interface. This configuration allows

testing of the wireless link communication modules and ground control interfaces. Delays, latency, drop-outs, and bandwidth

limitations on the air-to-ground link can be evaluated, while still having the benefits of the full simulation visualization interface.

Configuration (c) is another intermediate configuration which moves the flight system modules to the target embedded flight system

hardware, including all vehicle and environment simulation modules. Currently the flight system hardware targeted is a Linux-

based Raspberry Pi single-board computer with an Emlid Navio 2 flight control hardware unit. A Microhard Nano n920 wireless

modem is utilized for air-to-ground communication which features an RS-232 serial communication interface. This configuration

still utilizes a simulated vehicle and emulation of the flight system hardware, though output of the simulation can drive the hardware

actuators. The fourth configuration (d) is the final flight testing configuration. The simulation components onboard the flight

system are removed and replaced with hardware interfaces.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 11: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

11

Figure 3. Configuration Summary for Stand-Alone Simulation, COM, HILS, and Flight Testing

In order to maintain an embedded flight system build that is as close as possible to the simulated system, a series of

ReflectionScript files were created to load various segments of the simulation, controller, and dynamics. Each script can be used

interchangeably to support the designed configurations in Figure 3. A challenge for moving easily between simulation and

hardware is the fact that the interfaces will be different. To address this issue, the facade design pattern 23 was utilized to create a

standard interface for various subsystems. The script tree is summarized in Figure 4. The initial definition of the facades allows

each script to operate independently of the simulation or hardware components.

Desktop Workstation

Desktop Workstation / Ground Station CPU

Desktop Workstation

Simulation Visualization

Configuration

Flight System Emulation Configuration

Ground Station

Configuration

Scene VisualizationFlight Control

Displays / GUIs

Flight Control

SystemSimulation

Hardware

Emulation

(a) Single-Workstation Simulation Configuration

Scene VisualizationFlight Control

Displays / GUIs

Flight Control

SystemSimulation

Hardware

Emulation

RP2/Navio/Linux

Flight System Emulation Configuration

Ground Station

Configuration

Flight Control

Displays / GUIs

Flight Control

SystemSimulation * Hardware Emulation

Desktop Workstation / Ground Station CPU

RP2/Navio/Linux

Flight Control System (FCS) Configuration

Ground Station

Configuration

Flight Control

Displays / GUIs

Flight Control

System

FCS Hardware

Interfaces

TCP/UDPLink

Serial Uplink

Serial Downlink

(b) Single-Workstation Multi-Instance Configuration

(c) Embedded HILS Configuration (d) Flight System Configuration

Serial Uplink

Serial Downlink

Serial Uplink

Serial Downlink

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 12: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

12

Figure 4. Configuration Script Tree

A customized Ground Control System (GCS) was created to support autonomy research, control system development and flight

testing. A screenshot of the ground control system interface is shown in Figure 5. At the top of the screen a multicopter autopilot

control panel (MACP) allows users to engage any specific mode in the flight control system in flight. For instance, a heading hold

mode can be engaged by selecting the “CMD_HDG_SELECT” mode and dialing in a specific heading (currently dialed into a

compass heading of 164 degrees). The MACP allows GCS operators to engage or disengage the Flight Management System

(FMS), or to uplink command targets from a pilot joystick. Control stick values are shown in the window on the top left, which

include pilot hardware stick inputs, virtual mouse commands, and the resulting aircraft flight control system stick output. Mode

indications are shown in the Primary Flight Display (PFD) display on the bottom right. The PDF displays current state as measured

by the onboard GPS-augmented Inertial Navigation System (INS/GPS). Additional displays, such as a strip charts or programmable

buttons, can be added depending on the needs for a particular configuration.

DesktopSim.rfs GCS.rfs VehicleSim.rfs

Add control system

(fs/_addcontroller.rff)

(fs_addplanner.rff )

Add simulation (sim/

_addoctocopter.rff)

Add joystick (gs/

_addjoystick.rff)

Define inteface facades

(_definefacades.rff)

Create Flight Displays (gs/

_addFlightDisplays.rf f)

Add map displays

(gs/_addMapDisplay.rff)

Create Simulink Interface (gs/

_addSimulink.rff)

Create modem

(gs/_addmodem_GCS.rf f)

Add joystick (gs/

_addjoystick.rff)

Add control system

(fs/_addcontroller.rff)

(fs_addplanner.rff )

Add simulation (sim/

_addoctocopter.rff)

Define inteface facades

(_definefacades.rff)

Create SceneVis (sim/

_addSceneVis.rf f)

Create Collision Objects (sim/

_addCollision.rff)

Set up simulated scenes

(sim/_addSceneVisIndy.rff, sim/

_addSceneVisIndyExtraModels.rff )

Create simulated sensors

(_addSceneVis_Cameras.rff,

_addSceneVis_LIDAR.rf f)

Create Flight Displays (gs/

_addFlightDisplays.rf f)

Add map displays

(gs/_addMapDisplay.rff)

Create Simulink Interface (gs/

_addSimulink.rff)

Create modem

(gs/_addmodem_FS.rf f)

(_addModem_FS_RouteUplink.rff)

FlightSystem.rfs

Add control system

(fs/_addcontroller.rff)

(fs_addplanner.rff )

Add vehicle system interfaces (fs/

_addvehicleinterfaces.rff )

Define inteface facades

(_definefacades.rff)

Create modem

(gs/_addmodem_FS.rf f)

(_addModem_FS_RouteUplink.rff)

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 13: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

13

Figure 5. Custom Ground Control Station Interface

The software components that support the ground control station are shown in Figure 6 below. The Reflection components are

supported on Windows, Linux, and Mac OS. Each module component compiles to an executable DLL that can be validated and

tested independently before testing within a target configuration. ReflectionScript configurations remove any compile-time

dependency on the module any particular configuration, allowing modules to be compiled once and tested with the same binary

executable used for flight. The vehicle sensor facade autopilot facade provide a uniform interface definition that allows these

scripts to be used any configuration. Each module is written in C/C++ with a supporting library that facilitates cross-compilation

on the various supported platforms.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 14: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

14

Figure 6. Ground Control Station Components and Data Routing Diagram

IV. Flight Management and Flight Control System

The FMS system is a programmable state machine execution engine. Commands are constructed by inputting appropriate

states to guide the autopilot through a mission. A command is completed when it’s associated state triggers transition based on

specified criteria. In each state, the FMS can issue control commands to the FCS controller. Transition and commands are

programmable to allow for arbitrary mission execution sequence, allowing flexibility and a wide range of complex functionality in

constructing missions and supporting research, but at the cost of added complexity that may not be warranted for vehicle operators.

For instance, for conventional waypoint navigation, each state has an associated from and to waypoint that provides input to the

FCS; The state is programmed to engage the track-to modes and heading control modes in the FCS; The command state is

programmed to transition to the next state when the waypoint is reached within a certain radius or other conditions are met.

Arbitrary execution sequences can also be created to construct complex autonomous mission sequences. For instance, in a simple

urban city navigation example, the vehicle might use a GPS-free relative navigation strategy. Users can program the FMS to

perform a lateral wall-following mode based on a distance-to-wall sensor while maintaining altitude and ground speed, then

transitioning to a heading hold when the wall sensor no longer reports a wall, and then switch back to the wall following state until

some terminal state, such as a given number of city blocks have been traversed.

The FCS module is a traditional continuous mode-based control system. The core modes that are currently implemented are

shown in Figure 9. Each one of these modes can be engaged automatically by the FMS.

Figure 7. Core FCS Modes

The control system diagram is summarized in Figure 9. The core flight system integrates a simple cascade of controllers that

are based on proportional-integral-differential (PID) controllers. This core implementation was selected for the ease of

implementation, robustness, and is similar to many commercial and hobby systems in the market. The FCS is designed to be

extensible. Additional flight control modes are currently in development that build on these modes or replace the cascades entirely.

m_posNEU_m[3]

m_eulerRPY_rad[3]

m_headingTrue_rad

(m_posNEU_ft[2]]

m_linearVel_fps[3]

m_rateEulerRPY_radps[3]

m_status_isEngaged

m_sensorInput_airspeed_mps

m_sensorInput_pos_east_ENU_m

m_sensorInput_pos_north_ENU_m

m_sensorInput_pos_up_ENU_m

m_sensorInput_roll_rad

m_sensorInput_pitch_rad

m_sensorInput_heading_rad

m_out_prevWpPos_ENU_m

m_out_nextWpPos_ENU_m

m_out_nextWpSpd_mps

m_status_nWaypoints

m_status_isEngaged

m_status_currentWaypoint

m_status_timeInCommand_se

m_status_WaypointCommand

m_in_isEngagedFMS

m_in_isEngagedAP

m_in_IsBypassEngaged

m_in_hdgModeSelect

m_in_thrustModeSelect

m_in_latModeSelect

m_in_lonModeSelect

m_in_hdgSelected_rad

m_in_altSelected_m

m_in_matlabMotorCmds_m1p1

m_in_pilotSticksAERT[4]

m_out_hdgPreSelect_rad

m_out_hdgSelected_rad

m_out_hdgModePreSelect

m_out_hdgModeSelect

m_out_altPreSelect_m

m_out_altSelected_m

m_out_thrustModePreSelect

m_out_thrustModeSelect

m_out_latModeSelect

m_out_lonModeSelect

m_out_IsFMSEngaged

m_out_isAPEngaged

m_out_isBypassEngaged

Var: fAP(Autopilot Facade)

m_output_motor0_m1p1

m_output_motor1_m1p1

m_output_motor2_m1p1

m_output_motor7_m1p1

m_eulerRoll_rad

m_eulerPitch_rad

m_eulerYaw_rad

m_rollRate_radps

m_pitchRate_radps

m_yawRate_radps

m_latMode

m_lonMode

m_hdgMode

m_thrMode

m_apCmdHeading_rad

m_cmdAlt_MSL_m

m_cmdVertSpd_fps

m_cmdPitch_rad

m_cmdRoll_rad

m_bearingToPrevWP_rad

m_bearingToNextWP_rad

m_isEngagedFMS

m_isEngagedAP

Var: objFMSDLL: octocopter_fmc.dll

m_FMSEngaged

m_FMSLatMode

m_FMSLonMode

m_FMSPreSelectAlt_f t

m_FMSPreselectIAS

m_FMSPreselectMach

m_FMSSelectedAlt_ft

m_FMSSelectIAS

m_FMSSelectMach

m_PreselectHeading_rad

m_SelectHeading_rad

m_PreselectAlt_ft

m_SelectedAlt_ft

m_battVoltage_V

m_battSOC_pcnt

m_roll_rad

m_pitch_rad

m_heading_rad

m_baroAlt_ft

m_radarAlt_ft

m_latMode

(..)

m_lonMode

m_speedMode

m_SelectHeading_rad

m_SelectedAlt_ft

m_SelectedVS_fps

m_fdDesiredPitch_rad

m_fdDesiredRoll_rad

m_compass_VOR1Heading_deg

m_compass_VOR2Heading_deg

m_isAPEngaged

m_isFMSEngaged

Var: objPFDDLL: md11_pfd.dll

m_apCmdHeading_rad

m_posEast_ft

m_posNorth_ft

m_posUp_ft

m_eulerPitch_rad

m_eulerRoll_rad

m_eulerYaw_rad

Var: objMap_v1DLL: (objmap)

Var: fVS(Vehicle Sensor Facade)

Var: objACPDLL: octocopter_cp.dll

Var: objMap (Moving Map Disp)DLL: objMap

Var: objJoyPanelDLL: joypanel.dll

m_dt_sec

m_dataset1_..

m_dataset0_..(gui)

m_dataset2_..(a/p)

m_ouputdata_[. .]_m1p1

Var: objJoyDLL: directplay.dll

m_dt_sec

m_analogBuffer[ ]

m_buttonBuffer[]

(This module supports serialized robust

air/ground communication across the

wireless modems)

Var: objModemDLL: serialstream.dll

Var: objSimulinkDLL: simulink.dll

Lateral Modes

LATMODE_STICKCMD

LATMODE_ROLLZERO

LATMODE_ROLLSTICKCMD

LATMODE_ROLLCMD

LATMODE_GNDSPDHOLD

LATMODE_TRACKTOWP

Longitudinal Modes

LONMODE_STICKCMD

LONMODE_PITCHCMD

LONMODE_GNDSPDHOLD

Heading Modes

HDGMODE_STICK_N_CMD

HDGMODE_HDG_ZERO

HDGMODE_STICK_HDG_CMD

HDGMODE_HDG_CMD

Thrust Modes

THRMODE_STICK_THR_CMD

THRMODE_VSPD_CMD

THRMODE_ALT_AGL_HOLD

THRMODE_ALT_MSL_HOLD

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 15: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

15

Once a control mode is implemented, the modes are available for the FMS to utilize in the command state machine, or available

for the GCS operator to engage through the MCP in simulation or flight testing.

The core embedded flight control system configuration is shown in Figure 8. The vehicle sensor facade and autopilot facade

provide the interface to the simulation or flight hardware components. Both the FCS and GCS configurations include a TCP/IP-

based Matlab Simulink interface that can be used for real-time testing in simulation or in flight. Simulink real-time control is

engaged through the MACP. Additional autonomy components are added to this configuration as needed, depending on the flight

or simulation test goals.

Figure 8. Core Flight Control System Components and Data Routing Graph

m_posENU_ft[3]

m_altMSL_ft

m_altAGL_ft

m_eulerRPY_rad[3]

m_rateEulerRPY_radps[3]

m_headingTrue_rad

m_indicatedAirspeed_kts

m_linearVel_fps[3]

m_flightPathAngle_rad

m_lat itude_rad

m_longitude_rad

m_motorSpeed_radps[8]

m_motorCurrent_A[8]

m_motorBatVolt_V

m_cmdHeading_rad

m_cmdAlt_MSL_m

m_cmdVertSpd_fps

m_cmdPitch_rad

m_cmdRoll_rad

m_bearingToPrevWP_rad

m_bearingToNextWP_rad

m_latMode

m_lonMode

m_hdgMode

m_thrMode

m_isEngagedFMS

m_isEngagedAP

m_cmdVSticksAERT[4]

m_FMS_nWaypoints

m_FMS_currentWPID

m_FMS_timeInCommand_sec

[m_in_matlabMotorCmds_m1p1]

Var: fAP(Autopilot Facade)

m_motor0_ucmd_0p1

m_motor1_ucmd_0p1

m_motor2_ucmd_0p1

m_motor3_ucmd_0p1

m_motor4_ucmd_0p1

m_motor5_ucmd_0p1

m_motor6_ucmd_0p1

m_motor7_ucmd_0p1

m_motor[X]_InputVoltage_V

Var: objOctoDLL: octocopter.dll

m_sensorInput_airspeed_mps

m_sensorInput_pos_east_ENU_m

m_sensorInput_pos_north_ENU_m

m_sensorInput_pos_up_ENU_m

m_sensorInput_roll_rad

m_sensorInput_pitch_rad

m_sensorInput_heading_rad

m_status_nWaypoints

m_status_isEngaged

m_status_currentWaypoint

m_status_timeInCommand_sec

m_status_WaypointCommand

m_out_prevWpPos_ENU_m

m_out_nextWpPos_ENU_m

m_out_nextWpSpd_mps

Var: objFMSDLL: octocopter_fmc.dll

m_outputs_motorCmds_m1p1[0]

m_outputs_motorCmds_m1p1[..]

m_outputs_motorCmds_m1p1[7]

m_sense_roll_rad

m_sense_pitch_rad

m_sense_hdg_rad

m_sense_motorSpeed_radps[10]

m_sense_motorCurrent_A[10]

m_intrm_LMNFcmd[3]

m_intrm_Ucmds_m1p1[4]

[m_states_latMode]

[m_states_lonMode]

[m_states_hdgMode]

[m_states_thrMode]

m_intrm_hdgCmd_rad

m_intrm_altCmd_MSL_m

m_intrm_rollCmd_rad

m_intrm_pitchCmd_rad

m_outputs_bearingToPrevWP_rad

m_outputs_bearingToNextWP_rad

m_intrm_aglCmd_m

m_in_prevWpPos_ENU_m

m_in_nextWpPos_ENU_m

m_in_nextWpSpd_mps

m_inputs_isEngaged

m_inputs_isBypassEngaged

[m_states_hdgMode]

[m_states_thrMode]

[m_states_latMode]

[m_states_lonMode]

m_inputs_hdgSelectCmd_rad

m_inputs_altSelectCmd_MSL_m

m_inputs_matlabMotorCmds_m1p1[8]

m_inputs_stick[.. ]_m1p1

Var: objAPDLL: octocopter_control.dll

m_output_motor0_m1p1

m_output_motor1_m1p1

m_output_motor2_m1p1

m_output_motor7_m1p1

m_posENU_m[3]

m_velUVW_ba_mps[3]

m_eulerRPY_rad[3]

m_angRatePQR_ba_radps[3]

m_motorCurrent_A[8]

Var: objSimulinkDLL: simulink.dll

Var: fVS(Veh. Sensor Facade)

m_in_isEngagedAP

m_in_IsBypassEngaged

m_in_hdgModeSelect

m_in_thrustModeSelect

m_in_latModeSelect

m_in_lonModeSelect

m_in_hdgSelected_rad

m_in_altSelected_m

m_in_matlabMotorCmds_m1p1

m_in_pilotSticksAERT[4]

Var: fAP(Autopilot Facade)

CONTINUED

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 16: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

16

Figure 9. Automatic Flight Control System Diagram

Mca

Lcmd

PID LcmdferrS

fest

frefRoll Cmd

Ref Filter

(2nd order)

Roll

Cmd

Limiter

Lateral Modes

LATMODE_STICKCMD

LATMODE_ROLLZERO

LATMODE_ROLLSTICKCMD

LATMODE_ROLLCMD

LATMODE_GNDSPDHOLD

LATMODE_TRACKTOWP

fcmd_hold

fcmd_gndspd

Mwa2lhv

Vg

ndre

f_lh

v[2

]

Vgndcmd_wp_wa[2]

S

Vgnd _lhv_est[2]

Verr[2]

Roll to Moment (L)

GndSpd to Roll

PID

Vgnd Cmd

Ref Filter

(2nd order)

VGnd

Cmd

Limiter

Vg

ndre

f_w

a[2

]

stickLat_m1p1K

L Cmd

Limiter

Mcm dPID McmdqerrS

qest

qrefPitch Cmd

Ref Filter

(2nd order)

Pitch

Cmd

Limiter

qcmd_hold

qcmd_gndspdS

Vgnd _lhv_est[2]

Verr[2]

Pitch to Moment (M)GndSpd to Pitch

PID

Stick Cmd Filter

(1st order)KMstickcmd

M Cmd

Limiter

Vgndref_lhv_y

Vgndref_lhv_x

Longitudinal Modes

LONMODE_STICKCMD

LONMODE_PITCHCMD

LONMODE_GNDSPDHOLD

Heading Modes

HDGMODE_STICK_N_CMD

HDGMODE_HDG_ZERO

HDGMODE_STICK_HDG_CMD

HDGMODE_HDG_CMD

Stick Cmd Filter

(1st order)KNcmd

Ncmd

N Cmd

Limiter

PIDyerr

yest

Yaw to Moment (N)

yrefHdg Cmd

Ref Filter

(2nd order)

Ncmd

Hdg

Cmd

Limiter

Yaw

Error

Control Allocation

(u=Mca*[LMNFz] )

Fzcmd

U[1..8]

Lref

Nref

Mref

stickLon_m1p1

StickRdr_m1p1

Stick to L

Kst icklat2L

Kst lon

Stick to Rate

Stick to Rate

KstickRdr2N

Lcmd

Thrust Modes

THRMODE_STICK_THR_CMD

THRMODE_VSPD_CMD

THRMODE_ALT_AGL_HOLD

THRMODE_ALT_MSL_HOLD

hcmd_agl

Alt Cmd Ref

Filter

(2nd order)

hcmd_msl

Alt.

Cmd

Limiter

href

Altitude Error

to Thrust

h_agl_est

S

MSL/AGL

h_msl_est

MSL/AGL

herr

PIDVert

Speed

Limiter

dvspdS

vspdref

vspdcmd

ALT/VSPD

PID

VSPD to FZ

vspdref

S

Fzcmd

StickThr_m1p1

Fz Cmd

Limiter

(0,+1)

m_motorCmds[8]

m_inputGearCmd (0,1) m_outGearServoCmd_pwmConvert

m1p1 to

pwm

Top: LATMODE_STICKCMD

Bot: Others

Rate

Limiter

w/(s+w);

M1p1

Limiter ;

PWM

convert

Normalize

(-1,+1)

Rate

Limiter

w/(s+w)

Kstthr

y=(x+1)/2

-1

Note: +stick fwd

means -pitch down

Note: +stick right

means +roll ri ght

Note: +stick is

rudder right,

+rotat ion about Z

U0

U1

U2

U3

U4

U5

U6

U7

=

-S L N T

-L S -N T

-L -S N T

-S -L -N T

S -L N T

L -S -N T

L S N T

S L -N T

L

M

N

Z

Control Allocation Matrix

fCmd=0

fCmd

rollCmdLimit_rad

qCmd=0

Lim it range from -

PI to +P I

safeHoldAlt_m_agl

AGL

LimiterAGLCmdLim itM in_m

AGLCmdLim itMax_m

PID

AGL to Fz

aglErrS

aglCmd fzcmd

fzCmdM in_0p1

Ksticklat2Roll

KfCmdStick

m_int rm .

hdgCmd_rad

StickRdr_m1p1

m_params.

Kst ickRdr2Hdg

Stick to HDG

K

m_inputs.headingCmd_rad

m_inputs.prevWP_ENU

m_inputs.nextWP_ENU

Calculate

Track-To

XtrackErr

xtrackerr Xtrack to

Roll

rollcmd

A/C Position

LATMODE_TRACKTOW PLATMODE_GNDSPD

Calculate

Track-To

HdgCmd

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 17: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

17

A core flight sensor being utilized in this project is a 3D scanning Light Detection and Ranging (LIDAR) system. Support for

simulation and in flight is shown in the following configuration diagram. In hardware, a LIDAR interface component provides

input into a voxel-based mapping module that combines sensor information from various sources. For real-time performance, two

high-priority rate groups (threads) are triggered when data packets are available on the hardware interface ports. When data is

available, these modules receive the data, time-stamp it, and translate it into data that can be used by other modules in Reflection.

In simulation, the LIDAR interface is supported through a custom physics engine in CGL (Component Graphics Library), a NASA

developed software package. A lower-priority and lower-frequency rate group (objRG_LidarProc) schedules the updates for sensor

and environment model updates. A separate rate group (objRG_Planner) operates at an even lower frequency that schedules the

autonomous planners and decision-making components. Various planners provide suggested inputs and trajectories to a decision-

making module, which evaluates the plans based on additional criteria, and if needed, provides a new plan to the FMS.

Visualization support was added for the simulated LIDAR environment, as shown in Figure 12.

Figure 10. LIDAR and Sonar Simulation through Ray-Casting and Volumetric Collision Detection

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 18: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

18

Figure 11. LIDAR Processing and Integration with Planning and Decision-Making

Flight simulation configurations were developed for stand-alone testing and HILS testing with the following configuration.

The FMS (objFMS) and controller (objAP) modules provide motor commands to the flight dynamics module (objOcto) which was

derived above. An IMU emulator converts the simulation output into readings that would be measured by onboard sensors, which

includes injecting noise, latency, and errors.

OnUpdate()

Simulat ionUpdate()

Var: objPhysics

DLL: physicsengine.dll

m_peSpace

m_peSpaceQuery

Var: phyCLEnvironment

DLL: physicsengine.dll

Var: phyCLGround

DLL: physicsengine.dll

objPhysics.

CreateCollisionObject(0)

Var: clLidarQueryDLL: T3DScanQuery::physicsengine.dll

m_inMp2w

m_inMsc2p

m_ptrResultHitLocs_scs

m_ptrResultHitDists_m

m_resultMsc2wc[16]

m_resultDistArray[..]

m_resultHitLocations_scs[. .][3]

m_resultHitLocations_wcs[.. ][3]

CreateLineScanQuery(0)

Var: objOctoDLL: octocopter.dll

Ml2w

SetTransformMode_UseInMp2w()

SetNumberOfRays_Total_MaxPerQuery(...)

SetVert icalScanParams(.. .)

SetRange(.. .)

SetPosition_pa(. ..)

SetEulerRotRPY_rad_pa(...)

SetTotalScanAngle_deg(360)

SetTimeBetweenScans_sec(...)

DelayScanUntilTime_sec(2)

Var: objRG_LiDARProcDLL: (RateGroup)

Sim

Var: objVMDLL: voxelmap.dll

m_pLidarDataPtr_dist_sc

m_vehiclePosENU_m[3]

m_vehicleEulerRPY_rad[3]

m_pVoxelMap

(one time)

Var: svLIDARFDLL: LineScanVisObject (scenevis.dll)

m_Mp2w

m_posENU_ft[3]

m_altAGL_ft

m_eulerRPY_rad[3]

m_rateEulerRPY_radps[3]

m_headingTrue_rad

m_indicatedAirspeed_kts

m_linearVel_fps[3]

m_flightPathAngle_rad

m_lat itude_rad

m_longitude_rad

m_motorSpeed_radps[8]

m_motorCurrent_A[8]

m_motorBatVolt_V

Var: fVS(Veh. Sensor Facade)

UpdateMapWithLiDARData()

Var: objLidarDLL: velolidar.dll

Update_Blocking()()

Flight H/W

m_scanDist1_m[

m_scanDist2_m[

m_scanAzimuth1_rad

m_scanAzimuth2_rad

m_scanTime_(?)

(?other scan data?)

Var: objPlanner1DLL: (astarplanner.dll)

m_vehiclePos_m[3]

m_pVoxelMap

m_pResultingPlan

GeneratePlan()

Var: objDecisionMakerDLL: (decmaker.dll)

m_pPlan1

m_pPlan2

EvaluatePlans()

set through SetVoxelMapPtr()

Var: objSceneVisDLL: scenevis.dll

Var: objRG_PlannerDLL: (RateGroup)

Rate: Continuous Loop in flight

1Hz in simulation

Rate: TBD (period of ~30sec)

Var: objRG_IMUDLL: (RateGroup)

Rate: Continuous Loop

Var: objRG_GPSDLL: (RateGroup)

Rate: Continuous Loop

Var: objLidar(todo)

Update_Blocking()

Var: objGPS(todo)

Update_Blocking()

Flight Hardware

Configuration Only

(one time)

Var: objPlanner2DLL: (rsstplanner.dll)

m_vehiclePos_m[3]

m_pVoxelMap

m_pResultingPlan

GeneratePlan()

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 19: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

19

Figure 12. Voxel-Based Real-Time Mapping. Simulated (left), Hardware (right), and Navigation Near Buildings with

Real-time Trajectory Generation (bottom).

Voxel-based processing is one method for supporting higher-level understanding of the environment. A powerline

reconstruction engine was implemented and tested in this system to identify powerlines at the periphery of the sensor’s field. This

system reconstructed the location of powerlines utilizing the voxel map and the raw LIDAR data. One result is shown in Figure 13.

More details on powerline reconstruction are provided in the references 24.

Figure 13. LIDAR and Voxel-Based Powerline Detection

A layout of the autonomy architecture is shown in Figure 14. Simultaneous localization and mapping (SLAM) through onboard

LIDAR and vision facilitate navigation in GPS-degraded or denied environments to provide a robust vehicle state estimate. The

path planning and decision-making modules utilize the environment voxel map and generate a trajectory in terms of an FMS

command sequence. The FMS module controls the FCS to ensure compliance with the trajectory. The main flight control system

rate group and planning system rate group are shown, with flight control currently running at 100Hz.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 20: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

20

Figure 14. Overview of Timing and Functions in the Autonomous Control System

V. DJI S1000 Octocopter Vehicle and Model

A simulation of a NASA octocopter was created using the dynamics model derived above. This system builds on a DJI S1000+

octocopter aircraft platform. The vehicle platform in shown in Figure 15 below (left). The simulated vehicle is shown on the right.

The simulated vehicle’s rendering model includes actuable propellers and landing gear. The flight test platform has been flown at

Moffett Airfield and Crows Landing Airfield in California in support of this research.

Figure 15. NASA S1000 Octocopter Platform. Field testing in NASA Roverscape (left)

and simulation model (right).

Legend

Decision Making

Module

LIDAR

Interface

Environment

Mapping Module

(VoxelMap)

Sensor Data

(distances)

Env.

Map

Inter-Vehicle

Communication

(IVC)

Other Vehicles

(P,V)(eg, ADS-B)

Path Planning

SystemTrajectories (1..n)

Trajectory Generation Adjustments

Trajectory

Flight

Management

System (FMS)

Accepted

Trajectory

Module

Communication Requirement

(Real-Time Data Flow)

Periodic Timer

Update Sequence

Env.

Map

Executive

Flight Control

System (FCS)

Vehicle

Actuator

Facade (fVA)

Vehicle

Sensor

Facade (fVS)

Autopilot

Facade (fAP)

Vehicle

ActuatorsVehicle

Sensors

Facade

Hardware

Air/Ground

Modem

LIDAR

Hardware

External Source

Other

Vehicles

Sensor

Data

GCS

Vehicle

State, x

Ground

Commands

Update

Planning

Rate

Group

FCS

Rate

GroupUpdate

Vehicle

Sensor

Hardware

Vehicle

Actuator

Hardware

FCS

CmdsActuator

Cmds

Augmented INS

(LIDAR/GNSS/IMU/

Air Data)

SLAM

Processing

Vision

Hardware

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 21: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

21

The hardware configuration for the vehicle is shown in Figure 16. For experimental testing of the avionics and flight systems,

the system is designed with a backup autopilot system that can give or take control to a primary research autopilot through an

external multiplexor switch, should any issues be encountered in flight.

Figure 16. Component Diagram for S1000 Avionics. Proposed Rev B hardware configuration, in development.

Figure 17. Flight Tracks during Path/Trajectory Conformance Testing at Moffett Field, CA.

Radio Modem3DR 915MHz Radio

Pololu 4-ChRC ServoMultiplexer

Pololu 4-ChRC ServoMultiplexer

M1 ESC

M2 ESC

M3 ESC

M4 ESC

M5 ESC

M6 ESC

M7 ESC

M8 ESCSignal

Power

PowerDistribution

Board

Primary FCS BatteryLiPo, 6S1P, 15Ah22.2VDC (18.0-25.2V)

GND

+Vbat

GND

+Vbat

MotorDJI 4114 Pro

MotorDJI 4114 Pro

MotorDJI 4114 Pro

MotorDJI 4114 Pro

MotorDJI 4114 Pro

MotorDJI 4114 Pro

MotorDJI 4114 Pro

MotorDJI 4114 Pro

Out1

Out2

Out3

Out4

Out5

Out6

Out7

Out8Signal

Power

Signal

Power

Signal

Power

Signal

Power

Signal

Power

Signal

Power

Sig

Pwr Out

Out

Out

Out

Out

Out

Out

Out

RC ReceiverDJI A2 Autopilot Control System

AIL

ELE

THR

RUD

AUX

BND/DAT

THR

AIL

ELE

RDR

GEAR

AUX1

AUX2

Remote 1

Remote 2

M1

M2

M3

M4

M5

M6

M7

M8

OUT1

OUT2

OUT3

OUT4

Jumper OUT

(fail to master)

SEL

M1

M2

M3

M4

S1

S2

S3

S4

OUT1

OUT2

OUT3

OUT4

Jumper OUT

(fail to master)

Flight Control SystemEmLid Navio+

5V

RP

IB

EC

PP

M1

31

21

11

09

G

P

S

SP1

UART

I2C

Power

Opto-Isolator(Electrodynamics EDR?)

IN1

IN2

IN3

IN4

IN5

IN6

IN7

IN8

OUT1

OUT2

OUT3

OUT4

OUT5

OUT6

OUT7

OUT8

Power RegulatorCastle BEC Pro 20A

13A cont/5.2VDC

Out2

Out1

Input 5.2VDC

- + S

- + S

SEL

M1

M2

M3

M4

S1

S2

S3

S4

GPSAntenna (MCX)

GPS ANT

8 (

RC

1)

7 6 5 4 3 2 1 (

RC

8)

Power Regulator and Monitor (V/I)

HobbyPower, 1-7S, 3A

Pwr+Sen

PassThruInput

Fuse20A,5VDC

SwitchDC/20A

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 22: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

22

Figure 18. NASA Flight Test Team, August 2015.

H. Environment Model

An urban landscape rendering and collision model was created for large section of downtown Indianapolis. The vehicle and

urban models are shown Figure 19. Additional onboard sensors are supported in this framework, including cameras, sonar, and

LIDAR.

Figure 19. Out the window rendering of simulated urban scape.

A large-scale computational fluid dynamics (CFD) wind field study was performed over the Indianapolis geometry and

incorporated into the simulation. The CFD analysis was performed at NASA Ames Research Center. A visualization of the wind

field is shown in Figure 20. The wind field provides input into the dynamics model through the wind axis frame.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 23: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

23

Figure 20. CFD Wind Field Visualized over Indianapolis Model.

Figure 21. Wind-Field Incorporated into Simulation

VI. Conclusion

This paper has provided details on a model, simulation, and flight vehicle testing framework that supports research investigation

into advanced autonomy for small unmanned aircraft. The core simulation modules and hardware have been established and are

currently being extended in support of research for advanced concepts of fully autonomous urban environment operations.

Development continues on this framework as the research progresses to evaluate requirements for safe and fully autonomous

operations for this class of vehicle. This includes extension to large-scale multi-vehicle scenarios, and incorporation of more

advanced planning, decision-making, environment processing, and control system algorithms.

Acknowledgments

The authors would like to thank our collaborators and colleagues in the NASA UAS Traffic Management (UTM) and the

NASA SAFE50 projects.

References

1. Ippolito, C. A. (2017). Safe Autonomous Flight Environment for the Notional First and Last 50 Feet ( SAFE50 ) Project Toward UAS Operations in High-Density Low-Altitude Urban Environments. Retrieved from https://nari.arc.nasa.gov/.

2. Krishnakumar, K. (2017). Safe and Autonomous Drones for Urban Flight. Retrieved from

https://ntrs.nasa.gov/search.jsp?R=20160014638 3. Krishnakumar, K., Kopardekar, P., Ippolito, C., Melton, J. E., Stepanyan, V., Sankararaman, S., & Nikaido, B. (2017). Safe Autonomous

Flight Environment (SAFE50) for the Notional Last 50 ft of Operation of 55 lb Class of UAS. In AIAA 2017 SciTech Forum (pp. 1–7). Grapevine,

Texas. http://doi.org/10.2514/6.2017-0445 4. Kopardekar, P. (2017). Unmanned Aircraft Systems Traffic Management (UTM) Safely Enabling UAS Operations in Low-Altitude

Airspace.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15

Page 24: A Modeling, Simulation and Control Framework for Small ... · The market for small unmanned multicopter vehicle platforms has been growing rapidly, and as demand for using these vehicles

American Institute of Aeronautics and Astronautics

24

5. Atkins, E. M., Donato, P. F. A. Di, Civil, N., Agency, A., & Anac, B. (2016). Low-Altitude Rural to Urban Unmanned Aircraft System

Operations, (15), 1–13. http://doi.org/10.1002/9780470686652.eae1139

6. Atkins, E., Khalsa, A., & Groden, M. (2009). Commercial Low-Altitude UAS Operations in Population Centers. 9th AIAA Aviation

Technology, Integration, and Operations Conference (ATIO), (September). http://doi.org/10.2514/6.2009-7070 7. Atkins, E.M., 2010, June. Risk identification and management for safe UAS operation. In Systems and Control in Aeronautics and

Astronautics (ISSCAA), 2010 3rd International Symposium on (pp. 774-779). IEEE.

8. Ancel, Ersin, Francisco M. Capristan, John V. Foster, and Ryan C. Condotta. "Real-time Risk Assessment Framework for Unmanned Aircraft System (UAS) Traffic Management (UTM)." In Air Transportation Integration & Operations (ATIO)–Aerospace Traffic Management

(ATM) Conference. 2017.

9. Neogi, Natasha A., Kelly J. Hayhurst, Jeffrey M. Maddalon, and Harry A. Verstynen. "Some impacts of risk-centric certification requirements for UAS." In Unmanned Aircraft Systems (ICUAS), 2016 International Conference on, pp. 1003-1012. IEEE, 2016.

10. Atkins, E. M. (2014). Autonomy As an Enabler of Economically - Viable , Beyond - Line - of - Sight , Low - Altitude Uas Applications

With Acceptable Risk, (May), 1–12. 11. Bulusu, V., Sedov, L., & Polishchuk, V. (2017). Extended Abstract : Noise Estimation for future large-scale small UAS Operations. In

NOISE-CON 2017. Grand Rapids, Michigan.

12. Bulusu, V., & Sengupta, R. (2017). Capacity Estimation for Low Altitude Airspace, (June), 1–15. http://doi.org/10.2514/6.2017-4266 13. Jung, J., Souza, S. N. D., Johnson, M. A., Ishihara, A. K., Modi, H. C., Nikaido, B., & Hasseeb, H. (2016). Applying Required Navigation

Performance Concept for Traffic Management of Small Unmanned Aircraft Systems. ICAS 2016 30th Congress of International Council of the

Aeronautical Sciences, 1–13. Retrieved from https://ntrs.nasa.gov/search.jsp?R=20160011496 14. Ren, L., Castillo-Effen, M., Yu, H., Johnson, E., Nakamura, T., Yoon, Y., & Ippolito, C. A. (2017). Small unmanned aircraft system

(sUAS) trajectory modeling in support of UAS traffic management (UTM). In 17th AIAA Aviation Technology, Integration, and Operations

Conference, 2017. 15. Souza, S. D., Ishihara, A., & Nikaido, B. (2016). Feasibility of Varying Geo-Fence around an Unmanned Aircraft Operation based on

Vehicle Performance and Wind. In Digital Avionics Systems Conference (DASC), 2016 IEEE/AIAA 35th, Pp. 1-10. IEEE, 2016., 1–10.

16. Carl Russell, Jaewoo Jung, G Willink, B Glasner. Wind Tunnel and Hover Performance Test Results for Multicopter UAS Vehicles. Presented at the AHS 72nd Annual Forum, West Palm Beach, FL, May 16-19, 2016.

17. Meyer, J., Sendobry, A., Kohlbrecher, S., Klingauf, U., & Von Stryk, O. (2012). Comprehensive simulation of quadrotor UAVs using ROS and Gazebo. Lecture Notes in Computer Science (including Subseries Lecture Notes in Artificial Intelligence and Lecture Notes in

Bioinformatics), 7628 LNAI, 400–411. doi:10.1007/978-3-642-34327-8_36

18. Baraff, David, and Andrew Witkin. Dynamic simulation of non-penetrating flexible bodies. Vol. 26, no. 2. ACM, 1992. 19. Jiménez, Pablo, Federico Thomas, and Carme Torras. "3D collision detection: a survey." Computers & Graphics 25, no. 2 (2001): 269-

285.

20. Gilardi, G., and I. Sharf. "Literature survey of contact dynamics modeling." Mechanism and machine theory 37, no. 10 (2002): 1213-1239. 21. Stronge, W. J. "Comment: collision with friction; part B: Poisson’s and Stronge’s hypotheses." Multibody System Dynamics 24, no. 1

(2010): 123-127.

22. Featherstone, R., 2014. Rigid body dynamics algorithms. Springer. 23. Gamma, Erich, R Helm, R Johnson, and J Vlissides. "Design patterns. Elements of Object-Oriented Software.” Addison Wesley, Reading,

MA (1995).

24. Ippolito, Corey, Kalmanje Krishnakumar, and Sebastian Hening. "Preliminary results of powerline reconstruction from airborne LiDAR

for safe autonomous low-altitude urban operations of small UAS." In SENSORS, 2016 IEEE, pp. 1-3. IEEE, 2016.

Dow

nloa

ded

by N

ASA

AM

ES

RE

SEA

RC

H C

EN

TE

R o

n Se

ptem

ber

13, 2

018

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

8-19

15