See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/269403644

UAV Rendezvous .: From Concept to Flight

Test

Conference Paper · October 2012

CITATIONS READS
5 1,700

3 authors:

Daniel Briggs Wilson Ali Haydar Göktoǧan

The University of Sydney The University of Sydney
12 PUBLICATIONS 178 CITATIONS 108 PUBLICATIONS 1,122 CITATIONS

SEE PROFILE SEE PROFILE

Salah Sukkarieh
The University of Sydney
311 PUBLICATIONS 9,807 CITATIONS

SEE PROFILE

All content following this page was uploaded by Daniel Briggs Wilson on 11 December 2014.

The user has requested enhancement of the downloaded file.

 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

UAV Rendezvous: From Concept to Flight Test

Daniel B. Wilson, Ali Haydar Göktoǧan and Salah Sukkarieh

The Australian Centre for Field Robotics (ACFR)
The University of Sydney, Australia
[d.wilson, a.goktogan, s.sukkarieh] at acfr.usyd.edu.au

Abstract
The algorithms onboard Unmanned Aerial Vehi-
cles are increasing in complexity and thus require a
well established concept to flight test development
process to accelerate deployment and avoid loss
of aircraft. We present a modular multi-UAV al-
gorithm development framework which combines
design, simulation, performance evaluation and de-
ployment in a single, high level graphical environ-
ment. The framework is then utilised to develop a
heuristic direct search algorithm for rendezvous to
leader-follower formation. Fixed-wing simulation
and quadrotor flight test results validate the ren-
dezvous algorithm. Figure 1: A screenshot from the multi-UAV simulation visu-
alisation.
1 Introduction
Driven by technological advances, Unmanned Aerial Vehi- facilitates automatic C++ code generation for deployment on
cles (UAV) are increasing in complexity, particularly in terms a UAV. The hierarchical and modular nature of the graphical
of the algorithms being implemented. With increased com- environment, coupled with standard interfaces, enables seam-
plexity comes a higher probability of unforeseen problems, less integration at a central location. This is particularly use-
errors and uncertain system performance. Ideally, all algo- ful in a research environment where collaboration is key and
rithm testing would occur during flight testing since this en- participation is dynamic.
vironment is most representative of the actual UAV deploy- As a minimum, a UAV simulation consists of an aircraft
ment. Such an approach is highly time consuming and will model and a control scheme to generate control inputs to
inevitably lead to loss of aircraft. Instead, a majority of prob- achieve mission goals [Jung and Tsiotras, 2007]. Our system
lems should be identified and system performance evaluated extends this by simulating guidance, navigation and control
in a laboratory environment where rapid testing and iteration (GNC) algorithms, modelling sensor noise, bias and latency
can occur. Consequently, a robust and well established sim- as well as modelling actuator and environmental characteris-
ulation to flight testing framework is critically important for tics. Visualisation was used to provide the user with intuitive
UAV algorithm development. feedback as shown in Fig. 1. The modular architecture, com-
To this end, we have created a modular development frame- bined with standardised interfaces allows interchangeable ve-
work which enables rapid, iterative algorithm development by hicle dynamic models, GNC algorithms and sensor models.
combining algorithm design, simulation, performance evalu- Configuration files are used to modify specific vehicle aero-
ation and implementation in a single high level graphical en- dynamic, geometric and propulsive coefficients, as well as
vironment. This approach avoids time consuming and error levels of sensor uncertainty and severity of environmental dis-
prone conversion between development environments and al- turbances. In this way, numerous UAV system configurations
lows iterative development to occur offline. The framework and algorithms can be quickly benchmarked against one an-
was developed using Matlab’s Simulink [MATLAB, 2011] other using quantitative performance data.
because it provides a level of abstraction higher than tradi- To demonstrate the rapid development process, rendezvous
tional programming languages such as C++ and Java. It also to leader-follower formation is considered. As a nonholo-
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

nomic multi-UAV problem, fixed-wing rendezvous is com- space.

plex by nature, so a high level development process is es- UAV algorithm development environments of varying rel-
sential. The problem can be formulated as an optimisation, evance have been proposed in the past. A single UAV simula-
where the objective is to select a point on the leader’s path that tion framework was developed in [Jung and Tsiotras, 2007],
minimises the error between the leader and follower arrival although their focus was on aircraft stability derivative identi-
times to a satisfactory threshold. These minima correspond to fication, they provide useful details about their simulation ar-
points where rendezvous is feasible. Given the unconstrained chitecture. [Saunders and Beard, 2010] created a Hardware-
leader’s paths, there are potentially many feasible rendezvous in-the-loop (HIL) simulation for vision algorithm prototyp-
points. We wish to identify the earliest occurrence of these ing. The algorithms were written in C/C++ and simulated
minima. The vehicles are nonholonomic and the leader’s using a Matlab wrapper. This methodology avoids conver-
path is unconstrained so the derivative of the arrival time er- sion from simulation to autopilot firmware but is more time
ror function in intractable to calculate. Therefore, traditional consuming and less flexible than developing in the simulation
convex optimisation techniques such as gradient descent can- environment directly, which is what is done in our work. A
not be employed. Instead, we utilise a heuristic direct search multi-UAV simulation for cooperative control algorithm de-
method which is considered to be derivative free [Lewis et al., velopment was described in [Rasmussen et al., 2005]. This
2000]. Once the rendezvous point has been identified, the cor- work provides a nice environment for early algorithm proto-
responding minimum length path is generated for the follower typing and simulation but does not facilitate automatic code
to navigate. Fixed-wing simulation results show the develop- generation for deployment.
ment process identifying an algorithm shortcoming which is
then resolved before flight testing occurs. Flight tests using 3 Rendezvous
two quadrotors to emulate fixed-wings then demonstrate the The problem being considered in this work is fixed-wing
rendezvous algorithm being successfully deployed on UAVs UAV rendezvous to leader-follower formation. We first for-
using the same development framework. The modularity and mulate the problem and outline the constraints and assump-
flexibility of the framework is evident by the seamless transi- tions. A minimum-length path planning method to guide
tion from simulation to flight test and fixed-wing to quadrotor. the follower to rendezvous is then presented. We then pro-
The paper is organised as follows. First, the rendezvous pose a rendezvous point selection algorithm that utilises the
problem is formulated and the algorithmic solution explained. minimum-length path planning method. Finally, a method for
The algorithm development framework which is used to de- handling algorithm divergence is presented.
sign, simulate, evaluate and flight test the rendezvous algo-
rithm is then described. Simulation and flight test results then
demonstrate the successful implementation of the develop-
ment process. Finally, conclusions and suggestions for future
work are offered.

2 Related Work
Previous works related to this work fall into two categories,
robot rendezvous and UAV simulation. [Croft et al., 1998]
develop an optimal rendezvous point selection algorithm for
robot interception with a moving target. The algorithm as-
sumes that the robot travel time, as a function of rendezvous
completion time, can be approximated by a polynomial and
is therefore differentiable. This assumption is not valid in
our work since the calculation of the derivative is intractable.
Evolutionary optimisation techniques have been employed
for path planning applications, but they take a long time to
escape local minima [Jia, 2004]. Techniques exist to con-
strain the execution time to around 5 seconds with a desktop
computer [Nikolos et al., 2003], though this is not suitable
for real-time implementations. [Park et al., 2004] solve a Figure 2: The rendezvous problem formulation.
similar rendezvous problem by choosing a fixed rendezvous
point and then minimising the arrival time error by increasing
the length of the follower’s path. We propose a more optimal 3.1 Problem Formulation
solution by only considering minimum-length paths and us- We consider the rendezvous to leader-follower formation
ing the leader’s complete path as the rendezvous point search problem in the Cartesian plane as shown in Fig. 2. A
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

leader with initial state [xl (0), yl (0), ψl (0)]T denoted ql (0) !
and fixed velocity vl , is following a predefined path γl . A fol- −1 vf ψ̇(t)
φf (t) = tan (7)
lower with initial state qf (0) and fixed velocity, vf , aims to g
plan and follow a path γf to rendezvous with the leader. The
follower goal configuration qg ∈ γl , is a distance d behind the 3.2 Generating Minimum-Length Paths
leader as described by Eq 1. It is assumed that γl is known The Minimum Principle [McGee et al., 2005], dictates that
by the follower apriori and the leader is uncooperative in the the time-optimal path consists of intervals of maximum rate
sense that it will not deviate from γl to aid in rendezvous. turns and straight lines. This idea was originally proposed by
  Dubins [Dubins, 1957] who proved that in the absence of ob-
d stacles, a curvature constrained shortest path from a starting
qg = ql Tr − (1)
vl state q0 , to an end state q1 , consists of, at most, three seg-
ments. Each of these segments must be either a straight line
The terminal leader-follower constraint dictates that at the
or an arc with radius ρmin . The resulting Dubins path types
rendezvous time, Tr , the follower must approach (xg , yg ) in
consist of right turns R, left turns L and straight segments S.
the same direction as the leader, so qg includes a specific ψg .
The minimum-length path was calculated by constructing
The follower must also approach from behind the leader so
two circles of radius ρmin at q0 and q1 to represent initial and
that d > 0 and the velocity of the leader and follower at qg
final left and right turns of maximum rate as shown in Fig. 3.
must be equal. For rendezvous to be deemed successful, the
The tangents between each of these four circles were then
two aircraft must arrive at the rendezvous configuration si-
calculated to yield sixteen possible paths. Eight paths were
multaneously, i.e. the arrival time error, Te must be zero. In
eliminated by checking the traversability of the path at the
practice, |Te | shall be below a threshold, ε, of problem spe-
start of the tangent. A further four were eliminated using the
cific rendezvous accuracy.
same method at the end of the tangent. The shortest of the
d final four paths was selected as the minimum-length path. A
Tl = t(ql (0), qg ) + (2) detailed explanation of this method can be found in [Hota and
vl
Ghose, 2009]
Tf = t(qf (0), qg ) (3) 100
q0
q
1
|Te | = |Tf − Tl | < ε (4) 50
Minimum−length path α
t( . , . ) is the flight time between two configurations and Tl δ
and Tf are the global leader and follower rendezvous point ar- 0

rival times respectively. The objective is to find qg which min-

imises the global time-to-rendezvous Tr and satisfies Eq 4.
ρmin
y

−50
Each UAV is modelled as a Dubins vehicle [Dubins, 1957], θ
with fixed forward speed and constant altitude. −100

ẋ(t) = v sin ψ(t)

ẏ(t) = v cos ψ(t) (5) −150

ψ̇(t) = u(t)
−200
The turn radius R(t) in γf shall satisfy the turn radius con- −250 −200 −150 −100 −50 0 50 100
x
straint Eq. 6 for a maximum bank angle, φmax .

vf2
Figure 3: A minimum-length path connecting q0 and q1 with
ρmin = g tan φmax a minimum turn radius.
(6)
R(t) ≥ ρmin , ∀t ∈ [0, Tr ] This path planning strategy is applicable to a problem
with the specific constraints and assumptions outlined in Sec-
The rendezvous point selection algorithm must only know tion 3.1 and can be changed to suit different vehicle kinemat-
the time taken for the follower to fly to the goal point, so more ics and problem constraints.
complicated path planning strategies and velocity profiles can
be incorporated without any change in the rendezvous point 3.3 Rendezvous Point Selection
selection algorithm. To guide the follower along the path, For rendezvous to be feasible, the leader and follower must
ψ˙f (t) shall be controlled by adjusting the bank angle φf (t) arrive at the selected rendezvous point simultaneously. It fol-
using the relationship in Eq 7. lows then that the rendezvous point selection problem can be
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

formulated as an minimisation of |Te | to a satisfactory thresh- flight time to this point, Tinit is then calculated and becomes
old as shown in Eq 4. A further constraint dictates that in the first point to be sampled in the search space. Tinit is cal-
the presence of multiple feasible rendezvous points, the ear- culated to avoid searching over the space where rendezvous
liest point is selected to ensure Tr is optimal. Tr is chosen is known to be impossible.
as the search space because it is one dimensional and ql can Next, the goal point qg1 ∈ γl corresponding to the initial
be expressed as a function of Tr . The nonholonomic vehicle sample point is determined. The follower’s minimum-length
and terminal rendezvous constraints in this problem make Te path to qg1 is then constructed and the flight time to this point,
as a function of qg non-differentiable and discontinuous with Tf1 , calculated. The arrival time error, Te1 is then inspected
often many local minima. Therefore, traditional optimisation to determine whether Eq 4 is satisfied. If it is, qg1 is the ren-
techniques such as gradient descent cannot be employed. Ide- dezvous point, otherwise the next sample point is calculated
ally, one would search over some interval of Tr , find the set by adding half Te1 to the current sample point. If divergence
of qg that satisfies Eq 4 and then choose qg that corresponds is detected or the maximum number of iterations have been
to the smallest Tr . This approach would be highly inefficient reached, the loop is stopped. A summary of the rendezvous
and an appropriate search space is difficult to define. Instead, point selection algorithm is provided in algorithm 1.
we propose an iterative direct search method with a problem
specific heuristic. This heuristic dictates that the search shall 3.4 Divergence
begin at Tr = Tinit which is the minimum time taken for the Divergence checking was necessary to ensure algorithm sta-
follower to fly to any points on γl . Additionally, the search bility, particularly near discontinuities which are inherent to
shall propagate Tr in the positive direction. This exploits two the problem. This can occur for small changes in Tr when
fundamental properties of the problem, R → S → L or L → S → R paths fail to exist [Xuan-
Nam et al., 1994] and when step heading changes exist in γl .
• regardless of the kinematic and rendezvous constraints, Divergence checking was performed by inspecting the evolu-
Tr must always be greater than the time taken for the tion of Tei to ensure it was decreasing. When it occurs, the
follower to fly from qf (0) to the closest point on γl algorithm oscillates between two feasible paths where neither
• the first qg that satisfies Eq 4 when propagating Tr is the satisfies Eq 4. When divergence is detected, the path corre-
optimal rendezvous point sponding to a max[Tei ≤ 0] is selected. This is because a pos-
itive Te is impossible to eliminate since this corresponds to
the follower arriving at qg late, even with a minimum-length
path. It is, however, possible to eliminate a negative Te , where
Algorithm 1 Calculate rendezvous point
the follower arrives early, by increasing ρmin and hence Tf .
qinit = min(s(qf (0), γl )) This was done by exploiting the linear relationship between
s(q (0),γ )
Tinit = f vf l minimum path length and ρmin . First, a new path γfnew was
Tl1 = Tinit calculated between qf (0) and ql (tl ) using ρ = ρmin + . The
for i = 1 → maxIterations do
new minimum turn radius ρnew to achieve the desired path
qgi = ql (Tli − vdl )
γfi = minLengthPath(qf (0), qgi , ρmin )
length s(γf ) − vf Te was calculated through interpolation.
s(γ ) Algorithm
An 1 representation can be found in algorithm 2.
algorithmic
Tfi = vffi
Tei = Tli − Tfi
if |Tei | < ε then
Te = Tei Algorithm 2 Calculate ρnew to account for discontinuity
qg = qgi if divergence then
end loop γnew = minLengthPath(qf (0), qg , ρmin + )
else if divergence then sdesired = s(γf ) −
 Te 
sdesired −s(γf )
Te = max[Tei ≤ 0] ρnew = ρmin +  s(γ new )−s(γf )
qg = qgi−1 end if
end loop
else
T
Tli+1 = Tli + 2ei
end if
end for 4 Algorithm Development Framework
To transition the rendezvous algorithm from concept to flight
test in an error free and timely manner, an algorithm develop-
The algorithm is initialised by calculating the point, qinit ment framework was created. Matlab Simulink tools were
which corresponds to the minimum Euclidean distance be- utilised to encapsulate UAV algorithm design, simulation,
tween qf (0) and γl , where s( . , . ) is the length of the prob- evaluation and deployment in a single high level graphical
lem specific, minimum length path. The follower’s expected environment as shown in Fig. 4. The framework comprised
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

three main component, modelling and simulation (M&S), al- 4.2 Algorithm Deployment
gorithm deployment and visualisation. The M&S component The algorithm deployment module contained all algorithms
created a realistic virtual world for the algorithms to operate to be simulated and then automatically converted to C/C++
within and allowed quick evaluation. The algorithm deploy- code for deployment. This approach avoids a time consuming
ment component was designed such that custom algorithms and error prone conversion from the graphical environment to
could be simulated within the virtual world and automatically embedded C++ code and ensures that the deployed algorithm
converted to C/C++ code without any change in the module. is functionally equivalent to the simulated algorithm. In addi-
Visualisation was used to provide the user with an intuitive tion to the rendezvous algorithm, this module contained GNC
understanding of the behaviour of the algorithm. A detailed algorithms to generate appropriate aircraft inputs, given high
description of each component is provided in this section. level mission objectives and noisy sensor measurements.
Vehicle state estimates were obtained from noisy sensor
4.1 Modelling and Simulation data using an extended Kalman Filter (EKF) in a loosely cou-
Flight Dynamic Model pled GPS/INS arrangement. The 13 dimensional state vector
The flight dynamic model (FDM) implements a nonlinear is given by,
6DOF fixed-wing aircraft model with equations that can be
found in [Jung and Tsiotras, 2007]. The inputs to the FDM, x = [P T V T QT ωbT ] (10)
[δf δe δa δr δt ]T , are deflections of the flaps, elevator, where ωb = [pb qb rb ] is the IMU gyro bias which in-
T

ailerons, rudder and throttle respectively as well as the wind, cludes both the scale factor error and time-varying bias in a
[wN wE wD ]T . At each time step, the FDM utilises the in- single term. The inertial navigation process model is given in
puts to propagate the previous vehicle state to the true current Eq 13, where the inputs are the IMU measurements ā and ω̄.
state which is given by, na and nω are the accelerometer and gyro measurement noise
terms.
x = [P T V T aT ω T QT Va ] (8)
ā = ã − na (11)
where P = [X Y Z]T and V = [vN vE vD ]T are the po-
sition and velocity in the navigation frame, a = [ax ay az ]T b
ω̄ = ω̃ib − ωb − nω (12)
is the acceleration in the body frame, ωIB
B
= [p q r]T is the
angular velocity in the body frame, Q = [q0 q1 q2 q3 ]T is the
attitude quaternion and Va is the airspeed.    
Ṗ V
 V̇  Cbn ā − (2ωie
n
+ ωenn
) × V + g
ẋ =  =
 Q̇   1

 (13)
Sensors Ω̃
2 ω̄ Q
ω˙b n ωb
The GPS measurements were modelled with position and ve-
locity Gauss-Markov noise correlation and transport delay. A full derivation of the INS mechanisation equations can
The IMU model included white noise, bias, cross-coupling, be found in [Rogers, 2007]. The rotation matrix, Cbn , is used
mounting pose and the gravity vector from the 1984 World to transform between the body frame and navigation frame
Geodetic System (WGS84 geoid model) [NIMA, 1984]. The and is given below,
magnetometer was modelled by rotating the 2010 World
Magnetic Model (WMM2010) [NOAA, 2012] magnetic vec-  
tor to the body frame and adding white noise. Static pres- 0.5 − q22 − q32 q1 q2 − q0 q3
q1 q3 + q0 q2
n 
Cb = 2 q1 q2 + q0 q3 q2 q3 − q0 q1 
0.5 − q12 − q32
sure was calculated using the International Standard Atmo-
sphere (ISA) [Government, 1976] model, given the height q1 q3 − q0 q2 0.5 − q12 − q22
q2 q3 + q0 q1
about mean sea level (MSL) and white noise. Dynamic pres- (14)
sure utilised Eq 9, given the ISA air density, ρisa , Va and white Ω̃ω̄ is a skew matrix composed of bias corrected gyro mea-
noise. surements,
 
0 −ω̄p −ω̄q −ω̄r
1 ω̄p
pdyn = ρisa Va2 (9) 0 ω̄r −ω̄q 
2 Ω̃ω̄ = 
ω̄q −ω̄r
 (15)
0 ω̄p 
Actuators ω̄r ω̄q −ω̄p 0
The actuators were modelled with saturation limits and rate The gyro bias is modelled as a random walk where nωb is
saturation limits. Time lag was modelled with first order dy- a zero-mean Gaussian random variable. GPS, barometer and
namics. This block also converted from raw servo commands magnetometer measurements are used to observe the system.
to control surface deflections. The EKF measurement model is given below,
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

States Sensors
0.4008 Roll
Sensors
0.04439 Pitch 1
IServos_in Actuators Controls VelW Air_data
z AirData
0.4254 Yaw
Mach
−0.5911 Lat
Actuators Ang Acc
2.639
Lon
Euler Euler
97.84 Alt Euler
21.92 AeroCoeff
Vn
9.816 Set wind_out Winds PropCoeff
Ve Pace
0.6305 EngCoeff
Vd Create Wind
0.233 Simulation Pace
1 sec/sec Mass STOP
ax
−0.3132
Euler ECEF
ay Stop Simulation
−10.9
Sensors MSL if hits ground
−0.335 az
Air_data p 0 RST AGL
0.06021
q Reset REarth
0.1113
9.99e+004 r AConGnd

349.6 psta Flight Dynamic Model

Visualisation
287.4 pdyn

2.659e+004 OAT
IAds_in IAds_out
−2.442e+004 Hx AngAccel
−4.467e+004 Hy IMagnet_in IMagnet_out

24.03 IServos_out
Hz
IImu_in IImu_out
0.02347 Va
Sensors
0.06677
IGps_in IGps_out
Aircraft states
Guidance, Navigation & Control Sensor Models

Figure 4: Simulink model for one complete UAV simulation.

ailerons, elevator, rudder and throttle to achieve the bank

    angle, altitude, airspeed and vertical velocity targets set by
Pgps Pk−N + Cbnk−N r̃gps + nPgps the guidance. This was done with proportional-integral-
 Vgps  Vk−N + C n ω̄k−N × r̃gps + nVgps  derivative (PID) controllers and feedforward to account for
y=  
 pbaro  = 
bk−N 

pisa + npsta aircraft aerodynamic and geometric properties.
Hmag (Cbn )T Hwmm + nH
(16) 4.3 Visualisation
where Pk−N and Vk−N are position and velocity vectors in Visualisation utilised Flightgear v2.6, which is an open-
the navigation frame, delayed by N samples due to GPS la- source flight simulator developed and maintained by Curt Ol-
tency. r̃gps is the position vector connecting the GPS antenna son [Olson, 2012]. Two instances of Flightgear ran on a re-
to the IMU, ω̄k−N is the time delayed gyro measurement and mote computer. The FlightGear interface transmitted position
Cbnk−N is the time delayed rotation matrix. pisa is the ISA and attitude information for each UAV to the remote com-
pressure at the current MSL altitude and Hwmm is the mag- puter via UDP. Multiplayer mode was enabled to allow both
netic vector as per the WMM2010. nPgps , nVgps , npsta and nH aircraft to be visible in a single window as shown in Fig. 1.
are the measurement noise terms for the GPS position, GPS In the future, visual feedback from the visualisation could be
velocity, static pressure and magnetometer respectively [Shin, used to simulate vision sensors.
2005].
Given the estimated state of the vehicle, guidance algo- 5 Experiments
rithms generated low level commands to achieve high level The successful and rapid transition of the rendezvous algo-
mission goals. For rendezvous to leader-follower forma- rithm from concept to flight has been demonstrated in sim-
tion, the leader guidance simply follows a predefined path, ulation and flight tests. We have performed a set of vir-
whereas the follower implements the rendezvous-point selec- tual simulations of two simulated fixed wing UAVs perform-
tion algorithm to generate and follow a kinematically feasible ing rendezvous using the algorithm development framework.
path to rendezvous. The same framework was further demonstrated where two
Low level control algorithms generated input to the quadrotors performed the same mission in a real flight test.
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

5.2 Flight Tests

Table 1: Initial conditions and rendezvous errors for simula-
tion and flight tests. Rendezvous flight tests occurred in the multi-vehicle test
Sim 1 Sim 2 Flight 1 Flight 2 bed at the Center for Advanced Aerospace Technolo-
d 30m 30m 0.7m 0.7m gies (CATEC). Two Ascending Technologies Hummingbird
vcmd 25ms−1 25ms−1 0.8ms−1 1.2ms−1 quadrotors [Ascending, 2012] flew within a 15x15x5m arena
altcmd 100m 100m 1.5m 1.5m while the position and orientation was observed by a VICON
ρmin 80m 80m 1.0m 1.5m motion capture system [Vicon, 2012]. Numerical differentia-
replan off on off on tion was used to estimate the linear velocity.
derror 15.73m 4.12m 0.83m 0.07m Given the Vicon state estimate for each quadrotor, the
ψerror 3.74◦ 2.36◦ 5.92◦ 5.02◦ guidance module calculated bank angle, altitude and veloc-
ity commands at 100Hz for the leader to follow its predefined
path and the follower to navigate the calculated path to ren-
The simplicity with which the algorithm was implemented on dezvous. An additional fixed-wing emulation layer was im-
fundamentally different platforms is a testament to the mod- plemented after the guidance module to account for the kine-
ularity and flexibility of the development framework. matic differences between fixed-wing aircraft and quadrotors.
In each test, leader and follower path guidance was The conversion is given in Eq 19 where z is the commanded
achieved using a nonlinear guidance logic as proposed altitude, vcmd is the velocity, ψ(t) is the inertial heading and
by [Park et al., 2004]. This algorithm commands a lateral θ(t) is the heading. Two Zigbee transceivers transmitted
acceleration, ascmd using Eq 17. the resulting attitude and thrust commands to the quadrotors
where onboard stabilisation systems handled the low level
v2 control.
ascmd = 2 sin η (17)
L1
   
z altcmd
where L1 is the distance to a lookahead point on the refer-  vcmd   vcmd 
ence path, v is the aircraft’s velocity and η is the difference   R  (19)
ψ(t) =  ψ̇(t) dt
between the line-of-sight (LOS) angle ψLOS to the lookahead R
θ(t) ψ̇(t) dt
point and the aircraft’s heading. L1 is used to tune the al-
gorithm, where smaller values result in tighter tracking of Where,
the path and larger values result in less aggressive, smoother
tracking. The lookahead point was calculated geometrically g tan φ(t)
by finding the intersections of the path and a circle with radius ψ̇(t) = (20)
vf
L1 about the UAV’s position. The commanded lateral accel-
eration was converted to a bank angle, φfcmd , using Eq 18. In each experiment, a leader flew a 6x6m square path with
1.5m radius circular corners of radius 1.5m at a fixed altitude
ascmd of 1.5m as shown in Fig. 6(b). A follower was randomly po-
φfcmd = (18)
vf sitioned within the arena, initially with zero velocity and a
fixed altitude of 1m to avoid collisions with the leader. When
5.1 Simulation initiated by an operator, the follower flew forward until its
Simulations of the rendezvous algorithm were performed us- velocity was equal to that of the leader. At this point, the
ing fixed-wing UAVs. Results from an initial test are shown rendezvous algorithm was enabled to continuously replan fol-
in Fig. 5(a). Here, the flight path plot shows poor tracking lower reference paths.
of the reference path which lead to a longer flight time than Results from a flight test where replanning was not used
anticipated and a rendezvous separation error of 15.73m. Re- are shown in Fig. 6(a). Here, a final separation error of 0.83m
vision of the rendezvous algorithm incorporated replanning was observed. In contrast, Fig. 6(b) shows a test where the
to account for the poor guidance. Figure 5(b) shows results algorithm was modified to include replanning. The final sep-
where the modified algorithm was able to account for these aration error for this test was 0.07m, an improvement of al-
guidance inaccuracies and result in a much more accurate fi- most 12 times. Similarly to simulation, final heading errors
nal rendezvous separation, in the order of four times better. were comparable. This comparison is provided to highlight
The early identification of this problem highlighted a funda- the benefit of simulation and to demonstrate a situation where
mental advantage of the development framework because the a potential problem was identified and resolved before flight
problem was able to be resolved in a laboratory environment testing occurred. A summary of the initial conditions and ren-
without wasting time in the field or risking aircraft. A sum- dezvous accuracy is provided in Table 1.
mary of simulation results and initial conditions can be found The results presented in this section demonstrate a com-
in Table 1. plex, multi-UAV algorithm being simulated and flight tested
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

300

Distance (m)
200 Separation
300 ql(0)
d
qf(0) 100

200 γl 0
0 5 10 15 20 25
γf
3

Heading (rad)
100 γ ref
f
2 ψ (t)
f
x (m)

ψ
1 g
0
0
0 5 10 15 20 25

−100

Velocity (m/s)
26
vl(t)

−200 24 vf(t)

0 100 200 300 400 500 600 700 22

0 5 10 15 20 25
y (m) Time (s)
(a) Simulation 1, replanning off.

600
Distance (m)

300
400 Separation
d
200
200 0
0 5 10 15 20 25
Heading (rad)

100 2
ψf(t)
x (m)

0
ψg
q (0)
0 l −2
qf(0)
0 5 10 15 20 25
γl
−100
Velocity (m/s)

γ 26
f
vl(t)
γfref
24 v (t)
−200 f

22
0 100 200 300 400 500 600 700 0 5 10 15 20 25
y (m) Time (s)
(b) Simulation 2, replanning on.

Figure 5: Leader and follower fixed-wing simulation data from a rendezvous manoeuvre. Replanning in (a) has allowed the
heading and separation error to be low at rendezvous, even with inaccurate velocity command tracking and guidance errors.
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

10

Distance (m)
10 ql(0) Separation
qf(0) 5 d
9 γl
0
γf 0 2 4 6 8 10 12
8
γf ref 5

Heading (rad)
7 ψf(t)
x (m)

0
ψg
6
−5
0 2 4 6 8 10 12
5 1

Velocity (m/s)
v (t)
l

4 0.8 vf(t)
vcmd
3 0.6
3 4 5 6 7 8 9 10 11 12 0 2 4 6 8 10 12
y (m) Time (s)
(a) Flight 1, replanning off.
6
6m
Distance (m)

10 ql(0) 4 Separation
qf(0) d
2
9
γl
1.5m 0
γf 0 2 4 6 8 10
8
γf ref 5
Heading (rad)

ψf(t)
7
x (m)

0
ψg

6
−5
0 2 4 6 8 10
5
Velocity (m/s)

1.4 vl(t)

4 vf(t)
1.2
vcmd
3 1
3 4 5 6 7 8 9 10 11 12 0 2 4 6 8 10
y (m) Time (s)
(b) Flight 2, replanning on.

Figure 6: Leader and follower quadrotor flight test data from a rendezvous manoeuvre with and without replanning. The final
heading error is low without replanning but the final separation error is high as a consequence of inaccurate velocity command
tracking.
 Proceedings of Australasian Conference on Robotics and Automation, 3-5 Dec 2012, Victoria University of Wellington, New Zealand.

using the same high level graphical framework. The frame- [Jung and Tsiotras, 2007] Dongwon Jung and Panagiotis
work was able to identify and resolve a potential problem Tsiotras. Modelling and hardware-in-the-loop simulation
in simulation, before flight testing occurred. Successful ren- for a small unmanned, 2007.
dezvous to leader-follower configuration was demonstrated in [Lewis et al., 2000] Robert Michael Lewis, Virginia Torc-
both simulation and flight tests from a variety of initial condi- zon, Michael, and Michael W. Trosset. Direct search meth-
tions. The very different path geometries and flight velocities ods: Then and now. Journal of Computational and Applied
between the simulation and flight test demonstrates the algo- Mathematics, 124:191–207, 2000.
rithm working successfully within a wide range of conditions.
[MATLAB, 2011] MATLAB. version 7.12.0 (R2011a). The
MathWorks Inc., Natick, Massachusetts, 2011.
6 Conclusions and Future Work
[McGee et al., 2005] Timothy G. McGee, Stephen Spry, and
A modular development framework for rapid, iterative al- J. Karl Hedrickz. Optimal path planning in a constant wind
gorithm development has been created by combining sim- with a bounded turning rate, 2005.
ulation, performance evaluation and implementation into a
[Nikolos et al., 2003] I. K. Nikolos, K. P. Valavanis, N. C.
single, graphical environment. The framework was demon-
strated through the simulation and flight testing of a multi- Tsourveloudis, and A. N. Kostaras. Evolutionary algo-
UAV rendezvous algorithm. Advantages of the development rithm based offline/online path planner for uav naviga-
process were immediately clear when the simulation was able tion. Systems, Man, and Cybernetics, Part B: Cybernetics,
to identify and resolve a design problem, before flight testing IEEE Transactions on, 33(6):898–912, 2003.
occurred. [NIMA, 1984] NIMA. Department of defense world geode-
Future work will utilise the framework to flight test the ren- tic system 1984, its definition and relationship with local
dezvous algorithm on fixed-wing aircraft and develop new geodetic systems., 1984.
multi-UAV cooperative algorithms. Computer vision based [NOAA, 2012] NOAA. The world magnetic model, 2012.
navigation will also be incorporated to develop relative navi-
[Olson, 2012] Curt Olson. Flightgear flight simulator, 2012.
gation algorithms.
[Park et al., 2004] Sanghyuk Park, John
Acknowledgments Deyst, and Jonathan P. How. A new nonlinear guidance
logic for trajectory tracking, 2004.
The authors thank Miguel Ángel Trujillo Soto for his assis-
[Rasmussen et al., 2005] S. J. Rasmussen, J. W. Mitchell,
tance during the experimentation and CATEC for the use of
P. R. Chandler, C. J. Schumacher, and A. L. Smith. In-
the test bed facility. This work is supported by the Australian
troduction to the multiuav2 simulation and its application
Research Council (ARC) Centre of Excellence programme.
to cooperative control research, 8-10 June 2005.
[Rogers, 2007] Robert M. Rogers. Applied Mathematics in
References
Integrated Navigation Systems. AIAA, 3rd edition, 2007.
[Ascending, 2012] Ascending. Hummingbird quadrotor,
[Saunders and Beard, 2010] Jeffery Saunders and Randal
2012.
Beard. Uas flight simulation with hardware-in-the-loop
[Croft et al., 1998] Elizabeth A. Croft, R. G. Fenton, and testing and vision generation. J. Intell. Robotics Syst.,
Beno Benhabib. Optimal rendezvous-point selection for 57(1-4):407–415, 2010.
robotic interception of moving objects. IEEE Transactions [Shin, 2005] Eun-Hwan Shin. Estimation Techniques for
on Systems, Man, and Cybernetics - Part B: Cybernetics, Low-Cost Inertial Navigation. PhD thesis, 2005.
28(2), 1998.
[Vicon, 2012] Vicon. Vicon motion capture system, 2012.
[Dubins, 1957] Lester E. Dubins. On curves of minimal
[Xuan-Nam et al., 1994] Bui Xuan-Nam, J. D. Boissonnat,
length with a constraint on average curvature, and with
prescribed initial and terminal positions and tangents. P. Soueres, and J. P. Laumond. Shortest path synthesis for
American Journal of Mathematics, 79(3):19, 1957. dubins non-holonomic robot, 8-13 May 1994.

[Government, 1976] U.S. Government. U.s. standard atmo-

sphere, 1976.
[Hota and Ghose, 2009] S. Hota and D. Ghose. A modified
dubins method for optimal path planning of a miniature air
vehicle converging to a straight line path, 2009.
[Jia, 2004] Dong Jia. Parallel evolutionary algorithms for
uav path planning, 2004.

View publication stats