J Intell Robot SystDOI 10.1007/s10846-015-0292-1

Autonomous Navigation of UAV in Foliage Environment

Jin Q. Cui · Shupeng Lai ·Xiangxu Dong ·Ben M. Chen

Received: 16 December 2014 / Accepted: 8 October 2015© Springer Science+Business Media Dordrecht 2015

Abstract This paper presents a navigation systemthat enables small-scale unmanned aerial vehicles tonavigate autonomously using a 2D laser range finderin foliage environment without GPS. The navigationframework consists of real-time dual layer control,navigation state estimation and online path planning.In particular, the inner loop of a quadrotor is stabilizedusing a commercial autopilot while the outer loopcontrol is implemented using robust perfect tracking.The navigation state estimation consists of real-timeonboard motion estimation and trajectory smoothingusing the GraphSLAM technique. The onboard real-time motion estimation is achieved by a Kalman filter,fusing the planar velocity measurement from match-ing the consecutive scans of a laser range finder and

J. Q. Cui (�) · X. DongTemasek Laboratories, National University of Singapore,Singapore, Singaporee-mail: jinqiang@u.nus.edu

X. Donge-mail: tsldngx@nus.edu.sg

S. LaiNUS Graduate School for Integrative Sciences,Engineering, National University of Singapore,Singapore, Singaporee-mail: shupenglai@nus.edu.sg

B. M. ChenDepartment of Electrical, Computer Engineering,National University of Singapore, Singapore, Singaporee-mail: bmchen@nus.edu.sg

the acceleration measurement of an inertial measure-ment unit. The trajectory histories from the real-timeautonomous navigation together with the observedfeatures are fed into a sliding-window based pose-graph optimization framework. The online path plan-ning module finds an obstacle-free trajectory basedthe local measurement of the laser range finder. Theperformance of the proposed navigation system isdemonstrated successfully on the autonomous naviga-tion of a small-scale UAV in foliage environment.

Keywords Unmanned aerial vehicle · Simultaneouslocalization and mapping · Path planning · GPS-lessnavigation

1 Introduction

Navigation of mobile robotics platforms in GPS-denied environments is being intensively studied in theresearch community, such as indoor offices [22, 25],underwater [19] and urban canyons [9]. This paperpresents the autonomous navigation of a small-scaleunmanned aerial vehicle (UAV) in foliage environ-ment. Navigation of ground vehicles in foliage envi-ronment has been addressed in [10, 11] where a carequipped with a laser range finder drove through Vic-toria park in Sydney, Australia. The steep terrain, thickunderstorey vegetation and abundant debris character-istic of many forests prohibit the deployment of anautonomous ground vehicle in such scenario. A UAV

J Intell Robot Syst

with autonomous navigation capability would be ofparamount importance in forest survey, explorationand reconnaissance [3, 5].

The idea of autonomous flight of UAV in foresthas been attempted using a low-cost inertial measure-ment unit (IMU) and a monocular camera [14], inwhich an unscented Kalman filter (UKF) was usedto estimate the locations of obstacles and the stateof UAV. Experiment verification was carried out witha remote control car running in synthetic outdoorenvironment. More recently, Ross et al. [21] realizedautonomous flight through forest by mimicking thebehavior of human pilots using a novel imitation learn-ing technique. The application of learning technique isinnovative but the system suffers from relatively highfailure rate which the UAV can not afford.

To the best of our knowledge, this paper presentsthe first successful autonomous navigation of a small-scale UAV in unknown foliage environment. Thenavigation framework consists of real-time dual layercontrol, onboard motion estimation and pose-graphoptimization based on GraphSLAM [23] and onlinepath planning [12]. The whole navigation system isimplemented on a quadrotor UAV equipped with alaser range finder and an IMU. The modular designof the framework makes it readily applicable to othermobile robotic navigation system in obstacle-strewnenvironment without GPS.

The remaining part of the manuscript is orga-nized as follows: Section 2 presents the systemstructure, including the hardware and software struc-ture respectively. Section 3 introduces the dynamicsmodel structure of the UAV and presents the designof a robust perfect tracking control law. Section 4presents the state estimation framework consisting ofreal-time motion estimation for autonomous controland the pose-graph optimization based on Graph-SLAM. Section 5 presents the online path planning

with only the local measurement of the laser rangefinder. Section 6 presents the experimental resultsregarding the autonomous flight and pose-graph opti-mization in outdoor environment. Finally, Section 7concludes the paper and gives direction for futureresearch.

2 Unmanned System Structure

The system structure of the UAV includes the hard-ware platform configuration, the software structureand the navigation system structure. Each of thethree parts is indispensable for realizing a success-ful autonomous flight in GPS-denied environment.We will present these structures in this section andhighlight the motivation for using such configurations.

2.1 Navigation System Structure

The navigation system of UAV is depicted in Fig. 1,including functional modules such as the UAV dynam-ics, autonomous control, state estimation and pathplanning. We first report the techniques used in eachmodule in order to realize the autonomous flight ofUAV in forest. More details of each part will becovered in latter sections.

To maneuver in forests consisting of a large num-ber of obstacles, the platform has to be compact in sizeand is capable of hovering in the air. Platforms likequadrotors, helicopters and coaxial rotors all fulfillsuch requirements. In this paper, we use a quadrotordue to its unique advantages, such as the symmet-ric structure, the easy assembly of payload and thevast availability of commercial off-the-shelf (COTS)autopilot. The dynamics of a quadrotor consists of theinner loop dynamics and the outer loop dynamics. Theinner loop dynamics relates the control input to the

Fig. 1 System diagram ofUAV navigation system

J Intell Robot Syst

angular motion while the outer loop dynamics relatesthe angular motion to the linear velocity and position.We utilize a COTS autopilot to stabilize the inner loopdynamics and identify the outer loop dynamics in thefrequency domain.

After the inner loop dynamics is stabilized, wedesign a Robust and Perfect Tracking (RPT) controllaw [16] to control the UAV outer loop to track atrajectory reference generated from the path planningmodule. The RPT problem is to design a controllersuch that the resulting closed-loop system is asymptot-ically stable and the controlled output almost perfectlytracks a given reference signal in the presence ofany initial conditions and external disturbances. Thealmost perfect tracking means the ability of a con-troller to track a given reference signal with arbitrarilyfast settling time despite of external disturbances andinitial conditions.

The foliage environment renders the GPS signalunreliable, making the state estimation in such envi-ronment a challenging problem. We design the stateestimation to include two parts: motion estimation andGraphSLAM [23]. The motion estimation is essen-tially a laser odometry technique, which matches theconsecutive laser scans to generate the 2D velocityestimation. Combined with the acceleration measure-ment of IMU, a Kalman filter is designed to estimatethe position and velocity of the UAV, which are useddirectly for the closed-loop control. On the other hand,the position estimate from the motion estimation isprone to drift, thus GraphSLAM is used as a post opti-mization process to achieve the optimal trajectory andconsistent mapping.

Fig. 2 The virtual design of the quadrotor UAV

In unknown obstacle-strewn environment, the pathplanning part plays a critical role in providing a trajec-tory reference which is both obstacle-free and meetsthe dynamics constraints of the UAV. Without a globalmap, the path planning needs to rely on only the localmeasurement of each scan to extract such a feasiblepath. The measurement of the laser scanner is inher-ently realized in polar coordinate, thus a polar gridmap is first built based on the current laser scan mea-surement. In the polar grid map, by setting a startingpoint and a goal point, we use A* searching algorithmto find a series of line segments. The first sharp turn-ing point in the line segments is selected as the localtarget. With the current starting point and the localtarget, the trajectory generation problem is treatedas a two point boundary value problem for a tripleintegrator with constrained states and input. Details ofthe path planning will be covered in Section 5.

2.2 Hardware System Structure

The quadrotor is a fully customized platform(Figs. 2–3) designed by the NUS UAV Team. Theplatform is configured to be applicable in both indoorand outdoor environments, such as modern officesand forests. The platform is composed of carbon fiberplates and rods with a durable acrylonitrile butadi-ene styrene (ABS) landing gear to reduce the bareplatform weight. The overall dimensions are 35 cm in

Fig. 3 NUS quadrotor platform with two onboard laser rangefinders

J Intell Robot Syst

height and 86 cm from tip-to-tip. Different configura-tions of the rotor blade and the motor are comparedbefore an optimal design is achieved. The motors usedfor the platform are 740KV T-Motors with TurnigyPlush - 25A Bulletproof electronic speed controllers.The propellers are APC 12×3.8 clockwise and anti-clockwise fixed pitch propellers. Each motor and pro-peller setup could generate 15 kN static thrust. Thefinal bare platform’s main body weighs 1 kg. Its maxi-mum total take-off weight reaches 3.3 kg with a 4 cell4300mAh lithium polymer battery. We have testedthat the platform was able to fly at 8m/s for a periodof 10 to 15 minutes depending on the total weight andthe battery volume.

The platform is also fully customizable in termsof sensor arrangement and is scalable such that addi-tional computational boards could be mounted with astack-based design. As shown in Fig. 3, the platformis equipped with two laser range finders. The aboveone is Hokuyo UTM-30LX, used to detect the envi-ronment in the horizontal plane. The bottom one isHokuyo URG-04LX, used to scan the vertical planeto measure the height of the UAV in complex terrainconditions. One noteworthy thing is that the wholeavionics system is mounted on the platform throughfour mechanical isolators (CR1-100 from ENIDINE).Experiment results show that the noise of acceleration

measurements in x, y, z axis of the IMU decreases by5 times compared with that without any vibration iso-lation. The vibration isolation is also beneficial for thelaser range finder which can only withstand 20 g shockimpact for 10 times.

2.3 Software System Structure

Considering the comprehensive functions and logicsimplemented on UAV onboard system, it is furtherstructured into two main modules given the hierar-chical property of navigation and control. As shownin Fig. 4, two onboard processors are adopted exclu-sively for each modules: Mission plan processor andFlight control processor. As mission plan tasks nor-mally involve computationally intensive algorithmssuch as path planning, obstacle avoidance and SLAM,a high-end powerful Intel Core i7 based proces-sor called Mastermind (from Ascending Technolo-gies Germany) with Ubuntu 12.04 is deployed asthe mission plan processor. The Ubuntu operatingsystem has mature development environment withrich libraries for robotics applications, which canfacilitate the overall development. For the criticalflight control, a lightweight yet powerful OMAP3530based Computer-On-Module (COM) called GumstixOvero Fire is adopted. The flight control system

Fig. 4 Software structure of the UAV navigation system. Robust perfect tracking control is implemented in CTL, and scan matchingin Laser, Kalman filter in IMU, GraphSLAM in SLAM and obstacle avoidance in Path plan

J Intell Robot Syst

is implemented based on QNX Neutrino real-timeoperating system (RTOS). QNX RTOS is developedwith a true microkernel architecture which integratesonly the fundamental services including CPU schedul-ing, interprocess communication, interrupt and timers.Drivers and user applications are all executed as userprocesses. This architecture can provide a quite smallyet fully customizable and manageable user applica-tion suits with necessary drivers and libraries.

Based on the specifications from the system struc-ture, tasks to realize the flight missions are examined.The tasks are assigned, from the high level naviga-tion to the low level flight control, into Mission planprocessor and Flight control processor respectively.Since the Mastermind processor possesses powerfulprocessing capabilities, high level tasks such as SLAMand Path planning are scheduled. For the flight controlsubsystem, its subtasks are scheduled into the follow-ing order to achieve the closed-loop control system.Navigation sensors are retrieved first with Laser andIMU. With the laser data, the scan matching is per-formed on the two consecutive scan data, generatingincremental rotation and translation estimates. Afterbeing fused with the acceleration measurement of theIMU in a Kalman filter, the incremental translationand rotation produce the navigation state estimateswhich are further used for the control task CTL. Withthe generated automatic control signal, motor driv-ing signals are sent to the UAV from the SVO task torealize 6 degree of freedom (DOF) movement. Otherauxiliary tasks are also implemented: the communi-cation task CMM is used to send status data backto Ground Control System (GCS) for user monitor-ing and receiving user commands, the data loggingtask DLG is used to record flight status data for postflight analysis. Finally, to pass high level navigationdata to Flight control processor and share UAV sta-tus with Mission plan processor, the inter-processor

communication task ICMM is implemented on bothprocessors.

All the tasks are scheduled in a periodic fashion,whose executions follow the order in Fig. 4. On Flightcontrol processor, all the tasks are scheduled in 50Hz. As high level data is only for navigation pur-pose, a relative low scheduling frequency of 10 Hz isimplemented on Mission plan processor. CMM andDLG are executed every one second to fully utilize theefficiency of the processor.

3 Modeling and Control

3.1 Model Structure

Following the routines of traditional aircraft model [2,20], the model structure of the quadrotor platform isseparated into the inner loop model and the outer loopmodel, as illustrated in Fig. 5. The 6-DOF dynamics ofquadrotor consists of the 3-DOF translation dynamicsand the 3-DOF angular dynamics. Since the angulardynamics of quadrotor is much faster than its trans-lation dynamics, it is practical to extract the angulardynamic model and the translational dynamic model.The separation of the model structure facilitates thedesign of separate controllers for the inner-loop modeland the outer-loop model.

In the current system hardware configuration, theinner-loop attitude dynamics is stabilized using acommercial autopilot ‘NAZA-M’, which is an all-in-one stability controller for multi-rotor platforms. Thecontrol inputs (δlat, δlon, δcol, δped) from the remotetransmitter are fed into the onboard autopilot. Then‘NAZA-M’ controller generates pulse width modula-tion (PWM) signals to drive the four rotors to generatethe thrust forces. The four combined thrusts lift theplatform and maintain the attitude stability at the same

Fig. 5 Model structure of the quadrotor UAV

J Intell Robot Syst

time. From the perspective of ‘NAZA-M’, the fourinputs from the remote transmitter correspond to thecontrol references for the roll angle (φ), the pitch angle(θ ), the yaw angular rate (r), and the average motorspeed (�) respectively.

In the outer loop aspect, the states regarding linearvelocity (u, v, w) and heading angle ψ are of con-cern. In the lateral and longitudinal dynamics, thereis a dynamics model which maps the roll and pitchangle to the lateral and longitudinal velocity respec-tively, which is governed by the rigid body kinematicswith some damping effect from the air resistance. Theheave velocity w and heading angle ψ are governeddirectly by the inner loop controller.

Without knowledge of the internal structure offirmware in ‘NAZA-M’, we treat the inner loop as ablack box and identify the inner loop model. This isessential since we need to know the bandwidth of theinner loop model to facilitate the design of the outerloop controller. The inner model can be decoupled intofour input/output pairs. Due to the symmetric structureof the platform, we note that the dynamics model inthe roll and pitch directions share the same set of equa-tions. Referring to Fig. 5, the inner loop model can beseparated into three groups: the roll/pitch dynamics,the yaw dynamics and the heave dynamics.

The inner loop model identification is performedin the frequency domain. First, the platform with theinner loop controller is perturbed in all directions dur-ing which the corresponding input/output are logged.Then the logged data are fed into a software calledCIFER [1] to derive a transfer function which matchesthe best with the logged data [25]. The identified subsystem models are listed as follows:

– Roll/Pitch dynamics: input δlat/δlon, output φ/θTransfer function:

H1(s)= 9688

s4 + 27.68 s3 + 485.9 s2 + 5691 s + 15750.

(1)

– Yaw dynamics: input δped, output ψTransfer function:

H2(s) = 3.372

s. (2)

– Heave dynamics: input δcol, output wTransfer function:

H3(s) = −13.35

s + 2.32. (3)

3.2 Outer Loop Control

In obstacle-strewn environments, fast and accuratemaneuvering of the UAV is required to avoid any pos-sible collision. This poses challenges for the designof outer loop controller. We need to control the UAVto follow external references including the linear posi-tion and the heading angle. In order to perform fastand precise tracking of the given references, the RPTcontroller is adopted from [4, 16]. The procedures ofdesigning an RPT controller for the state feedbackcase has been addressed in [16] and a case study isexemplified in [24].

For precision control, it’s desirable to include anintegrator to ensure zero steady state error in case ofstep input. We propose the RPT controller which con-siders the integration of position tracking error as anaugmented state. The system with state-augmentationis formulated as:

�xyAUG :

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

˙xxy=

⎡

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

0 −1 0 0 1 0

0 0 1 0 0 0

0 0 0 1 0 0

0 0 0 0 0 0

0 0 0 0 0 1

0 0 0 0 0 0

⎤

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

xxy+

⎡

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣

0

0

0

0

0

1

⎤

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦

uxy

yxy = xxy

hxy =[

1 0 0 0 0 0]xxy

(4)

where xxy = [∫ e rp rv ra p v]T ; rp, rv , raare the position, velocity and acceleration references;p, v are the actual position and velocity; e = p − rpis the tracking error of position. Since there is errorintegration

∫e in the augmented states, the feedback

control law would contain a term of Ki

∫e. Following

the steps in [16], a linear state feedback control law ofthe form (5) is acquired,

uxy = Fxy(ε) xxy , (5)

where

Fxy(ε) =[−kiω

2n

ε3

ω2n + 2ζωnki

ε2

2ζωn + ki

ε

1 − ω2n + 2ζωnki

ε2− 2ζωn + ki

ε

]

,

(6)

J Intell Robot Syst

where ε is a design parameter to adjust the settlingtime, ωn, ζ, ki are the parameters that determine thedesired pole locations of the infinite zero structure of�

xyAUG through:

p(s) = (s + ki)(s2 + 2ζ ωns + ω2

n) . (7)

In principle, when the design parameter ε is smallenough, the RPT controller gives arbitrarily fastresponse. However, in practice, due to the constraintsof physical system and inner loop dynamics, we wouldlike to limit the bandwidth of the outer loop to be atleast one third of the inner loop system bandwidth.The roll/pitch dynamics H1 has a bandwidth of 3.82rad/s. For roll/pitch outer loop controller, we select theparameters in (8) to have a bandwidth of 0.83 rad/s:

ωn = 0.4, ζ = 1.2, ε = 1, ki = 0.8 . (8)

For the outer loop controller in heave dynamics andyaw dynamics, the inner loop controller already con-trols the heave velocity w and yaw angular velocityr . We only need to design a controller to control theheight z and yaw angle ψ . Similarly, the integral oftracking error is augmented to the original system andforms another augmented system �

hyAUG:

�hyAUG :

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

˙xhy =

⎡

⎢⎢⎢⎢⎣

0 −1 0 1

0 0 1 0

0 0 0 0

0 0 0 0

⎤

⎥⎥⎥⎥⎦xhy +

⎡

⎢⎢⎢⎢⎣

0

0

0

1

⎤

⎥⎥⎥⎥⎦uhy

yhy = xhy

hhy =[

1 0 0 0]xhy

(9)

where xhy = [∫ e rp rv p]T ; rp, rv are the posi-tion, velocity references; p is the actual height or yawangle; e = p − rp is the tracking error of height oryaw angle.

A linear state feedback control law of the form (10)is acquired,

uhy = Fhy(ε) xhy , (10)

where

Fhy(ε) =[

−ω2n

ε

2ζωn

ε21 − 2ζωn

ε2

]

. (11)

For height controller:

ωn = 0.5, ζ = 1.1, ε = 1 . (12)

For yaw angle controller:

ωn = 1, ζ = 1, ε = 1 . (13)

4 Navigation State Estimation

4.1 Estimation Framework

The state of UAV includes the position, the orientationand the velocity. The orientation could be estimatedby the mechanization of angular measurement fromIMU. In the ideal case, the translation velocity couldbe derived by integrating the acceleration measure-ment once and the position for another integration.Since the accelerometer output is subject to bias, thedouble integration of acceleration will result in pro-hibitively large position drift, rendering it inapplicablefor long time navigation of UAV. To facilitate success-ful autonomous control of UAV, we proposed a stateestimation framework as shown in Fig. 6, consistingof two sub-systems: front-end and back-end.

The front-end provides the essential estimate forthe onboard autonomous control and the back-endoptimizes the UAV trajectory for pose correction andconsistent mapping of the environment. However, theGraphSLAM is in essence an off-line optimizationtechnique which can not be ran in parallel with theonboard control loop. In practice, a sliding time win-dow is applied to the trajectory history and only thoseposes inside the sliding window are optimized. After

Fig. 6 System schematics illustrating front-end and back-end

J Intell Robot Syst

the whole trajectory is obtained, a global optimiza-tion in the back-end is performed to obtain the optimaltrajectory and the global consistent map.

In Fig. 6, we have put the loop closure detec-tion block to the back-end. This makes sense becausewe utilize the position and velocity estimates fromKalman filter as the input into the the real-timeautonomous control. By putting the loop detection intothe back-end, the back-end module operates in a self-contained manner which will not influence the realtime performance of the UAV control.

4.2 Front-End Estimation

The front-end block provides high frequency stateestimate for onboard autonomous control and the ini-tial pose estimate for the back-end block. The blockdiagram of the front-end state estimation is shownFig. 7, which consists of three main blocks: featureextraction, scan matching and Kalman Filter.

Feature extraction is the first step toward accuratemotion estimation. Considering the operation envi-ronment to be forests, we choose the center of treesat flight height to be the features. A laser rangefinder scans the environment continuously to providerange information in 30 meter range of 270 degreefield of view. To extract the validated trees, the laserscan range are processed in three steps: preprocess-ing, segmentation and extraction. Preprocessing thescan range is necessary since the raw laser data isnoisy and corrupted with outliers. Segmentation isperformed to generate clusters of range points cor-responding to candidate tree stems. The extractionprocess validates these clusters through a series of

Fig. 7 The front-end state estimation diagram

geometric descriptors, producing the estimated centersof clusters as the landmarks [7].

Scan matching of 2D range scans has been is amature technique to estimate robot pose in unknownenvironment. But it suffers from local minima andlarge rotation errors when using only raw measure-ments. To overcome these problem, we use featurebased scan matching and incorporate the headingangle measurement from IMU in the scan matchingprocess. The extracted features in each scan main-tain a certain spatial distance to each other, making itless possible for ambiguous data association. With theheading measurement from IMU, the latter scan is firstprojected to the previous scan using the heading angledifference. This further reduces the wrong data asso-ciation induced by possible large rotation movement.Once the correct data association is established, theincremental translation and rotation could be solved inclosed-form [17].

A Kalman filter is designed to fuse the sensor infor-mation: the translation measurement (u′

g, v′g) from

scan matching, the height measurement z from barom-eter or mirrored laser beam and the three axis acceler-ation from IMU.

The process model is described by,[Pnvn

]

=[0 I0 0

] [Pn

vn

]

+[0I

]

Rn/b(ab +wa) , (14)

where Rn/b is the rotation matrix from the body frameto the local north-east-down (NED) frame, ab is theacceleration measurement without noise, wa is theacceleration measurement noise vector with normaldistribution, ab + wa is the IMU acceleration mea-surement in body-fixed frame, Pn is the local NEDposition vector, vn is the local NED velocity vector.I and 0 are identity matrix and zero matrix of properdimension.

The measurement model is

y = [03×2 I3×3 03×1

][Pnvn

]

+ R , (15)

where R is the measurement covariance matrix withnormal distribution.

Implementing the KF requires discretizing the con-tinuous process model (14) and the measurementmodel (15) using zero-order-hold method. The follow-ing procedure is a standard KF process with inter-leaved time update and measurement update. One

J Intell Robot Syst

noteworthy point is that the laser motion estimationupdate rate is 10 Hz while the acceleration measure-ment refreshes at 50 Hz. When the motion estimationmeasurement is not available, the state is updatedusing only the process model (14).

4.3 Back-End Estimation

The position estimate from the Kalman filter is onlysuitable for short time navigation. This is due to thefact that the planar position is not observable in themeasurement equation and thus suffers from long termdrift. The back-end estimation use the GraphSLAMtechnique to bound the position drift to facilitate longrange UAV navigation.

GraphSLAM requires the availability of all mea-surements along the trajectory before they are usedto build up a graph and optimize it afterwards.This makes GraphSLAM an off-line method, render-ing it inapplicable for real-time navigation. We havereported our results about using GraphSLAM as anoff-line optimization technique to generate consistentmaps in our previous work [6]. Here we present an on-line GraphSLAM technique using a sliding-windowtechnique.

The main idea of the on-line GraphSLAM is toset a sliding window along the trajectory, limiting thesearch range only to those poses lying in the time win-dow prior to the current pose. As illustrated in Fig. 8,the first pose and it’s measurement is denoted as x0. Italso serves as a reference origin to which all the futureposes will be referred. As the UAV collects more data,a series of new poses and measurements are added,including {x2 · · · xt }. The sliding window is initialized

Fig. 8 Sliding window diagram with poses being pushed in andpopped out

with a capacity of n as the first pose x0 is pushed intothe window.

The online GraphSLAM based on the sliding win-dow significantly decreases the drift of the positionestimate compared with that of the Kalman filter.However, the introduction of sliding window is indeeda sacrifice of the optimization performance by limit-ing the searching range only in the 5 seconds slidingwindow. For long time navigation, the UAV position isstill prone to drift without global optimization. There-fore, a two-layer back-end framework is presented asshown in Fig. 9. Poses and features from the front-end are pushed into the sliding window at each timestep. After the local optimization, the optimized poseestimate is transfered back to the front-end for real-time control. At the same time, the locally optimizedpose is pushed into a larger container to store all theposes and measurements, forming a global graph tobe optimized after the mission. This two-layer graphsetup makes sure the UAV achieves slow drift in thepose estimation during flight and eventually obtainsa globally consistent trajectory and map afterwards.This configuration is justified by the fact that the UAVdoes not need perfect pose estimate during flight andthe slow drift caused by the local sliding window opti-mization is acceptable for UAV operations lasting upto 20 minutes.

Due to hardware constraints, the front-end andback-end algorithms run in two onboard computers.The front-end algorithms, including the scan match-ing and the Kalman filter, run on the Gumstix OveroFire. While the back-end algorithms, including thesliding window online GrpahSLAM and the globalpose graph construction, run on the Mastermind. Thetwo computers are connected through a serial port.Figure 9 shows the message interaction between thetwo computers. Initially, the state P0 is directly fed tothe autonomous control. To optimize the initial stateP0, it is sent to Mastermind with its measurement.The time delay caused by the local optimization, theglobal pose construction and the serial communicationmake it impossible to use P0 directly for flight con-trol. In particular, experiments show that the delay �t

between the initial P0 and the optimized pose Pn0 is

300ms. Recalling that the main loop in the front-end is50Hz, the delay of 15 loops is not negligible for real-time operation. To deal with the delay, we propose todesign a state update scheme to take into account the

J Intell Robot Syst

Fig. 9 A timing graph showing the interaction between the front-end, the sliding window and the global back-end

delay which works as follows: At time t0, we have aninitial pose P0 from the Kalman filter. After time �t

we received pose correction �P0 of P0, then we havea new pose for time t0,

Pn0 = P0�P0. (16)

At any time t > t0 + �t , the initial pose is,

Pt = P0�Pt0, (17)

where �Pt0 is the initial pose difference between Pt

and P0. The update pose Pnt of time t is,

Pnt = Pn

0�Pt0 = P0�P0�Pt

0. (18)

At time t1 a new initial pose is sent to the back-endfor optimization and after time t the update pose �P2

is returned. The update state is again updated usingEq. 18, except that the time index is changed from t0to t1.

5 Path Planning in Unknown ClusteredEnvironment

For the vehicle to navigate safely in the foliage envi-ronment, it is necessary to have real-time path plan-ning and obstacle avoidance capability. Due to the lowcomputational power of the on-board computer andthe general unknown environment, the path planningand obstacle algorithm needs to be fast enough to runin a mobile computer. Conventionally, a path planningalgorithm is treated as an optimization problem. But itis generally difficult to solve due to the high dimen-sion of optimization vector, which includes the state of

the vehicle and the non-linear constraints introducedby the obstacles. Even using state-of-art numericalalgorithms like the CPLEX, the convergence of thesolution is not guaranteed and the real-time capabilityis hard to achieve. Therefore, in practical engineeringapplications, a suboptimal solution is usually consid-ered by separating the optimization problem into twoor more steps.

In our solution, we consider the planning in the con-figuration space and the state space of the vehicle sep-arately. This separation reduces the dimension of theoptimization problem and makes it possible to achievea real-time path planning algorithm in obstacle-strewnenvironment. We propose a path planning system withglobal path planner using A* searching [15] and alocal smoother using efficient boundary value prob-lem solver [13]. The A* searching is performed in adiscrete grid map built from the laser scan measure-ment. The vehicle in the grid map is considered as apoint-mass particle which could move from one gridto all its connected grids. This does not violate theactual vehicle model since the quadrotor could be sim-ply considered as a double or triple integrator when itoperates at low speed [18].

The A* searching provides candidate target pointsfor the vehicle to reach. Due to the dynamic of theUAV, we would like to gain a trajectory that is ableto be limited on jerk, acceleration and velocity. Weuse an bang-zero-bang based time optimal solutionto generate references meeting the vehicle’s dynamicconstraints [13]. Collision checking is also performedto make sure that the smoothed trajectory does not col-lide with any obstacles. This two-step path planning

J Intell Robot Syst

algorithm can provide dynamically feasible trajecto-ries to lead the vehicle from any initial position to anyreachable final position. The path planning structure isgiven in Algorithm 1.

The algorithm consists of several main blocks: theglobal configuration space search using A* (step 1)and the boundary value problem (step 6) [8]. Theboundary value problem seeks to generate a refer-ence trajectory given two sets of conditions on theboundaries. Reflexxes Motion Libraries [13] providesa general solution to this problem. The global configu-ration space search is to give a rough plan that ignoresthe complex dynamics of the vehicle but considersas much topological information as possible. Sincethere is no prior map of the environment, a local mapbased on the current laser range finder measurementis built up in polar coordinate. The A* path planningalgorithm is actually a graph search algorithm. To runthe A* searching algorithm, a polar coordinate mapis first built from the input of a 30m laser scanner.For each obstacle point returned from the 30m laserscanner’s measurement, a Gaussian-based cost filed isadded around it. A preprocessed polar coordinate mapis shown in Fig. 10.

During the A* searching, the algorithm would gen-erate a path with the lowest cost from the currentposition of the vehicle to the target point. A typicalpath is shown in Fig. 10 as the green line. The resultedpath normally consists of a series of waypoints locatedin each grid in the polar coordinate. In order to find thebest direction the vehicle should aim for, a split andmerge algorithm is used to transfer these waypoints

Fig. 10 A Gaussian cost map in polar coordinate. The colorranging from red to blue indicates the closeness of the grid tothe detected obstacle

into a series of line segments. The split and merge pro-cess finds the first turning point Rg, which is used todetermine the best target point to go. The direction andthe distance of the first turning point are then passedto the local path planner to search for a collision freepath from the current vehicle position to the turningpoint.

For the local planner, a similar idea close to thevector field histogram (VFH) method is adopted. InVFH, the vehicle always turns to the direction thatis both obstacle free and also towards the target Rg

from the global planner. The behavior is realized byforming an optimal function that consists of differentobjectives, such as distance to the global target andthe angle difference compared to the last direction. Wesample multiple local target points around the vehi-cle and calculate the resulted trajectory based on thecurrent vehicle states and the local target points. Thetrajectory that is both collision free and closest to Rg

is chosen. Each trajectory starts at the current vehi-cle states and ends at the local target points with zerovelocity and zero acceleration. Therefore, the vehicleis always at a safe state so that it could stop safelywhen following the current trajectory.

6 Experimental Results

All the function blocks, including the RPT control law,the onboard motion estimation and the path planning,

J Intell Robot Syst

are implemented in the UAV onboard processors toform a comprehensive navigation system. The devel-oped navigation system enables our customized UAVto perform autonomous flight through a small area offorest. The front-end state estimation is verified by theflight test, and the back-end state estimation is vali-dated using the onboard data in an off-line processingmanner.

6.1 Autonomous Flight in Forest

The test field is set in a small forest with sparsetrees as shown in Fig. 11. The distances among treesare sparse enough to provide an obstacle-free trajec-tory for the UAV to fly. At the flight height, sometrees have thick branches instead of a single treetrunk. This poses challenges for the onboard featureextraction and data association. The obstacle-free tra-jectory is predefined by assuming knowledge of thetree positions in the environment. This simulates thecase where the onboard obstacle avoidance is properlyfunctional. It should be noted that the map informationis not used during the process of motion estimationand autonomous control.

Figure 11 shows the test platform flying in the testfield. The UAV platform is being autonomously con-trolled while following a predefined trajectory. Thepredefined trajectory is loaded into the system duringthe system power up. The front-end state estimationprovides the position and velocity estimates which arefed back to the RPT control law to control the UAV.

Figure 12 shows the position tracking performancetogether with the tracking error. In the top two sub-figures, the red dashed lines are the position referencesand the blue solid lines are the state estimates from theUAV. As can be seen from the top two sub-figures, theRPT controller controls the UAV to track the positionreference in both x and y direction. The tracking errorin x direction is 0.2 meter and 0.5 meter in y direction.The maximum tracking errors occur at the begin-ning of the trajectory when there is step accelerationreference.

The flight test demonstrates the accuracy of theUAV dynamics model and the efficiency of onboardmotion estimation in a computation-limited process-ing unit. It also shows that the front-end state estimatecan provide reliable state estimate for the RPT con-troller.

6.2 Back-End Optimization

The back-end state estimation seeks to optimize theinitial trajectory to produce a consistent map of theenvironment. To better verify the effect of trajec-tory optimization, we test the GraphSLAM algorithmusing data collected in two environments: the indoorforest environment (Fig. 13) and the outdoor realforest (Fig. 11).

The indoor forest scenario consists synthetic treeswith perfect cylindrical shapes. Besides the poles,square concrete pillars and the interior of the wallwill also be measured by the laser range finder. We

Fig. 11 The outdoortesting scenario with theflying quadrotor

J Intell Robot Syst

Fig. 12 Position andvelocity tracking in X-Ydirection

0 2000 4000 6000 8000−15

−10

−5

0

5

Time (s)

Pos

ition

( m

)

x

XX

ref

0 2000 4000 6000 8000−0.6

−0.4

−0.2

0

0.2

0.4

Time (s)

Pos

ition

( m

)

x error

0 2000 4000 6000 8000−5

0

5

10

15

20

25

Time (s)

Pos

ition

( m

)

y

YY

ref

0 2000 4000 6000 8000−1.5

−1

−0.5

0

0.5

1

Time (s)

Pos

ition

( m

)

y error

manipulate the distance among the trees to make surethere are enough number of features in each scan.An autonomous flight is performed to collect all theonboard data including the trajectory history and thelaser range data.

Figure 14 compares the map projected on the initialtrajectory and the optimized trajectory respectively.Since there is no ground truth available in the indoor

forest dataset, we can not quantitatively analyze theeffect of trajectory optimization. We consider the con-sistency of the projected map as the criteria to evaluatethe back-end state estimation. The initial map and tra-jectory are the results of motion estimation based onscan matching, marked by green dots plot. It can beseen that when all the measurements are projected onthe initial trajectory, the overall map is not consistent.

Fig. 13 Indoor test scenariofor SLAM verification

J Intell Robot Syst

−10 −5 0 5 10 15 20 25 30−25

−20

−15

−10

−5

0

5

10

15

20

X Position (m)

Y P

ositi

on (

m )

Indoor forest map using GraphSLAM

Fig. 14 Map comparison between the initial trajectory and theoptimized trajectory

The interior walls and position of the square pillarsdrift away. The red-dot plot is the optimized map andtrajectory. By visual checking of the map we could seethe optimized map is more consistent than the initialmap. Figure 15 shows close-view of one part of themap, indicating that the red pillar retains its rectangu-lar shape while the green pillars deform to an incorrectway. Figure 15 also clearly manifests the perfect cir-cular contour of the landmarks while the initial greencontours scatter around.

0 1 2 3 4 5 60

1

2

3

4

5

6

X Position ( m )

Y P

ositi

on (

m )

Indoor forest map using GraphSLAM: detail

Fig. 15 Optimized map and trajectory detail

After the evaluation of the GraphSLAM algorithmin indoor environment, we apply the algorithm to thedataset from in a real small forest. Figure 16 showsthe comparison between the initial map and the opti-mized map for the forest. The green plot is the initialmap from the Kalman filter while the red one is theoptimized trajectory and map. It can be seen thatthere are two neighboring clusters of green plots whileonly one cluster of red points. Due to the complexityof the environment, there is no hollow tree contoursextracted. There are still large clusters of objectswhich do not correspond to trees in the environment.

The effect of trajectory optimization is less optimalthan the indoor forest dataset. This is expected sincethe real environment exhibits several challenges: first,the uneven terrain produces undesired ground strikesof the laser range finder, making the feature extractiona challenging task. Second, the real trees in the envi-ronment are relatively smaller than the ones in indoorforest and have thick branches at the flight height.Wrong data association may happen at a certain timestep. Third, the distance among trees are very large,leaving each segment in the clustered range scan withlimited number of points. Estimating the tree centerswith less points produces large position error. Last,

−5 0 5 10 15 20 25

−10

−5

0

5

10

15

20

X Position (m)

Y P

ositi

on (

m)

Update trajectory and map for forest data

Fig. 16 Optimized map and trajectory in the small forest test

J Intell Robot Syst

the cross section of tree trunk at the scanned planemay not follow a circular shape. The estimated posi-tion of tree centers may be different when the treesare scanned at different view angle. All these factorsmake the optimized map less consistent than the onein indoor forest. But with proper feature extraction anddata association, the back-end optimization demon-strates its positive effect in correcting the trajectoryand generating more consistent map.

6.3 Autonomous Flight with Online GraphSLAMand Online Path Planning

We have validated the performance of the UAV nav-igation system using online consistent state estima-tion and robust perfect tracking. The previous twotests have used predefined trajectory references dur-ing flights. In practice, the flight area is unknown priorto the take-off, requiring the UAV to be able to avoidany obstacles in its vicinity during the flight period.

Online path planning is demanded to provide real-time trajectory references which avoid the obstaclesand maintain the original trajectory route as much aspossible.

We designed a flight test to verify the UAV navi-gation system including the online GraphSLAM andonline path planning. The UAV is required to travelto five waypoints (w1-w5) which fall on a rectangleshape, which are labeled as black circle in Fig. 17.Once reaching a waypoint, a hover of 10 seconds isperformed. At each corner of the rectangle, the UAV’sheading is shifted 90 degrees clock wise after the hov-ering. Without obstacles, the designed trajectory is arectangle shape. However, as shown in Fig. 17, thereare two obstacles (T1, T2) lying on the connectedline of the waypoints. To reach the waypoints, theUAVmust find other feasible path instead of the directconnection between the waypoints.

The whole mission was fully autonomous, includ-ing take-off, waypoint navigation, obstacle avoidance,

Fig. 17 Consistent mapwith obstacle avoidancetrajectory. The UAV isrequired to follow arectangle shape trajectorywith five waypoints (blackcircle w1-w5) along theway. At each corner of therectangle, the UAV’sheading is shifted 90degrees clock wise. The reddot trajectory is the realflight path which avoidedobstacles (T1, T2) on therectangle

−5 0 5 10−5

0

5

10Trajectory and map for indoor forest data

Y Position ( m )

X P

ositi

on (

m )

w1

WaypointHeadingMapTrajectory

w2 w3

w4w5

T1

T2

J Intell Robot Syst

online GraphSLAM and landing. The flight datawere recoded onboard and plotted in Fig. 17. The reddot trajectory is the real flight path which avoidedobstacles nearby the rectangle. The blue dots plot isthe accumulative plot of the environment, includingthe trees, rectangle pillars and walls. The blue goodshape of walls and pillars in the map demonstrates

the consistency of the trajectory estimate of theentire mission. Figure 18a–d show the good trackingperformance in x, y, z and heading ψ directions,validating all the algorithm developed in this paper:the consistent state estimation algorithm, the obsta-cle avoidance and the robust and perfect controltechnique.

0 50 100 150−1

0

1

2

3

4

5

6

7X−axis position tracking

time (s)

x−ax

is p

ositi

n (m

)

xx−ref

(a) x position tracking performance

0 50 100 150−2

−1

0

1

2

3

4Y−axis position tracking

time (s)

y−ax

is p

ositi

n (m

)

yy−ref

(b) y position tracking performance

0 50 100 150−1

−0.5

0

0.5

1

1.5

2Z−axis height tracking

time (s)

z−ax

is p

ositi

n (m

)

zz−ref

(c) z height tracking performance

0 50 100 150 200−200

−150

−100

−50

0

50

100

150

200Heading angle tracking

time (s)

Yaw

ang

le (

deg)

ψψ−ref

(d) Yaw angle tracking performance

Fig. 18 Onboard trajectory tracking performance with obstacleavoidance. The whole mission is fully autonomous, includ-ing take-off, waypoint navigation with online GraphSLAM andobstacle avoidance, and landing. Figure 18a–d show the good

tracking performance in x, y, z, and yaw directions, validatingall the algorithms developed in this study: the consistent stateestimation algorithm, the obstacle avoidance and the robust andperfect control

J Intell Robot Syst

7 Conclusion

We have presented the navigation system for UAV infoliage environment and verified the system in realflight tests. Various essential parts of the navigationsystem have been presented, including model and con-trol, state estimation and path planning. The model ofa quadrotor UAV is first structured as inner loop andouter loop models. The inner loop model is stabilizedwith a commercial autopilot. To track any external ref-erence with disturbance rejection capability, we havedesigned a robust perfect tracking outer loop controllaw. The outer loop control law demonstrates perfecttracking of given position reference in the autonomousflight of UAV in forest.

The state estimation framework of UAVs in foliageenvironments is the focus of this manuscript. Theframework consists of a front-end and a back-end. Thefront-end is a real-time motion estimation by match-ing scans from the onboard laser range finder. Thevelocity measurement from the scan matching is fusedwith the acceleration of the IMU in a Kalman filter toprovide the high update rate state estimation for theouter loop control. The back-end optimization is a cus-tomized GraphSLAM algorithm. To make optimiza-tion constant in time, a sliding window technique isintroduced to optimize those poses falling in the timewindow. All initial poses from the Kalman filter inthe time window form a local pose graph. By optimiz-ing the local graph, the initial pose from the Kalmanfilter is updated and fed back to the controller to cor-rect the position drift of the front-end. If necessary, thelocally optimized poses could also form a global posegraph which is optimized at the end of the mission toproduce a globally consistent trajectory and map.

We also developed the real-time path planningusing the measurement of the horizontal laser rangefinder. An A* searching is performed in the local gridmap to produce a candidate target point. Consideringthe dynamics constraints of the UAV, the trajectorygeneration is formulated as a boundary value prob-lem and solved in analytic form, achieving real-timeperformance in obstacle-strewn environment, espe-cially forests.

To verify all the developed algorithms, we per-formed autonomous flight tests in indoor simulatingforests and outdoor practical forests. Given a missionplan with five waypoints, the UAV can navigate pre-cisely using the proposed motion estimation technique

and avoid obstacles along the trajectory. Replaying therecorded laser scans on the estimated poses generatesconsistent map of the environment, demonstrating theaccuracy of the navigation system.

We are now working on the improve the navigationsystem in the following aspects. First, the laser-basedmotion estimation will be fused with other measure-ment, such as visual odometry from optical flowsensors. This is due to the fact that laser range findershave limited measurement range and may return lit-tle measurement points in sparse environments. Visualodometry can serve as a complimentary odometrymeasurement in such scenarios. Second, we are work-ing to increase the flight speed of the UAV form 0.5m/s to 5 m/s. This is essential to increase the oper-ation range of the UAV in the constraint of limitedflight duration. Fast flight of UAVs in obstacle-strewnenvironment poses great challenges for motion esti-mation and path planning. Last but not least, weare trying to develop real-time 3D motion estima-tion and path planning. The current navigation systemrelies on a 2D laser range finder, which is based onan assumption that the environment does not changedrastically in the vertical direction. We are investigat-ing the possibility of using stereo cameras or RGB-D cameras for 3D navigation of UAVs in complexenvironments.

References

1. Cifer. http://uarc.ucsc.edu/flight-control/cifer/2. Alvarenga, J., Vitzilaios, N.I., Valavanis, K.P., Rutherford,

M.J.: Survey of unmanned helicopter model-based naviga-tion and control techniques. J. Intell. Robot. Syst., 1–52(2014)

3. Cai, G., Dias, J., Seneviratne, L.: A survey of small-scale unmanned aerial vehicles: Recent advances and futuredevelopment trends. Unmanned Syst. 02(02), 175–199(2014)

4. Chen, B.M.: Robust and H∞Control. Communications andControl Engineering Series. Springer (2000)

5. Chisholm, R.A., Cui, J.Q., Lum, S.K.Y., Chen, B.M.: UAVLiDAR for below-canopy forest surveys. J. Unmanned Veh.Syst. 01(01), 67–68 (2013)

6. Cui, J.Q., Lai, S., Dong, X., Liu, P., Chen, B.M., Lee,T.H.: Autonomous navigation of UAV in forest. In: 2014International Conference on Unmanned Aircraft Systems,pp. 726–733 (2014)

7. Cui, J.Q., Wang, F., Dong, X., Ang Zong Yao, K., Chen,B.M., Lee, T.H.: Landmark extraction and state estima-tion for UAV operation in forest. In: 32nd Chinese ControlConference, pp. 5210–5215. Xi’an (2013)

J Intell Robot Syst

8. Dong, W., Gu, G.Y., Zhu, X., Ding, H.: Solving the bound-ary value problem of an under-actuated quadrotor withsubspace stabilization approach. J. Intell. Robot. Syst., 1–13 (2014)

9. Georgiev, A., Allen, P.: Localization methods for a mobilerobot in urban environments. IEEE Trans. Robot. 20(5),851–864 (2004)

10. Guivant, J., Masson, F., Nebot, E.: Simultaneous localiza-tion and map building using natural features and absoluteinformation. Robot. Auton. Syst. 40(2–3), 79–90 (2002)

11. Guivant, J., Nebot, E., Baiker, S.: Localization and mapbuilding using laser range sensors in outdoor applications.J. Robot. Syst. 17(10), 565–583 (2000)

12. Kang, K., Prasad, J.V.R.: Development and flight test eval-uations of an autonomous obstacle avoidance system for arotary-wing UAV. Unmanned Syst. 01(01), 3–19 (2013)

13. Kroger, T.: On-Line Trajectory Generation in RoboticSystems. Springer Tracts in Advanced Robotics, vol. 58.Springer, Berlin (2010)

14. Langelaan, J.W.: State estimation for autonomous flight incluttered environments. Ph.D. thesis, Stanford University(2006)

15. LaValle, S.M.: Planning Algorithms. Cambridge UniversityPress (2006)

16. Liu, K., Chen, B.M., Lin, Z.: On the problem of robustand perfect tracking for linear systems with external distur-bances. Int. J. Control 74(2), 158–174 (2001)

17. Lu, F., Milios, E.: Robot pose estimation in unknown envi-ronments by matching 2D range scans. J. Int. Robot. Syst.18, 249–275 (1997)

18. Mellinger, D., Kumar, V.: Minimum snap trajectory gen-eration and control for quadrotors. In: IEEE InternationalConference on Robotics and Automation, pp. 2520–2525(2011)

19. Paull, L., Saeedi, S., Seto, M., Li, H.: AUV navigation andlocalization: A review. IEEE J. Ocean. Eng. 39(1), 131–149(2014)

20. Phang, S.K., Li, K., Yu, K.H., Chen, B.M., Lee, T.H.: Sys-tematic design and implementation of a micro unmannedquadrotor system. Unmanned Syst. 02(02), 121–141 (2014)

21. Ross, S., Melik-Barkhudarov, N., Shankar, K., Wendel,A., Dey, D., Bagnell, J., Hebert, M.: Learning monocularreactive UAV control in cluttered natural environments. In:IEEE International Conference on Robotics and Automa-tion, pp. 1765–1772 (2013)

22. Shen, S., Michael, N., Kumar, V.: Obtaining liftoff indoors:Autonomous navigation in confined indoor environments.Robot. Autom. Mag. IEEE 20(4), 40–48 (2013)

23. Thrun, S., Montemerlo, M.: The GraphSLAM algorithmwith applications to large-scale mapping of urban struc-tures. Int. J. Robot. Res. 25(5–6), 403–429 (2006)

24. Wang, B., Chen, B.M., Lee, T.H.: An RPT approach totime-critical path following of an unmanned helicopter. In:8th Asian Control Conference, pp. 211–216 (2011)

25. Wang, F., Cui, J.Q., Phang, S.K., Chen, B.M., Lee, T.H.: Amono-camera and scanning laser range finder based UAVindoor navigation system. In: International Conference onUnmanned Aircraft Systems, pp. 694–701 (2013)

Jin Q. Cui received the B.S. and M.S degree in Mecha-tronic Engineering from Northwestern Polytechnical Univer-sity, Xi’an, China, in 2005 and 2008 respectively. He hasfinished his Ph.D. in Electrical and Computer Engineering inNational University of Singapore (NUS) in 2015. In the Ph.D.study, his research direction is navigation of unmanned aerialvehicles (UAV) in GPS-denied environments, especially for-est. He is now a research scientist in the Control ScienceGroup at Temasek Laboratories in NUS. His research inter-ests are GPS-less navigation using LIDAR and vision sensingtechnologies.

Shupeng Lai received the B.S. degree from Nanyang Tech-nological University, Singapore, in 2012. He now pursuinghis Ph.D. in National University of Singapore, Singapore. Hisresearch interests include path planning for small-scale UAVsand multi-agent systems.

Xiangxu Dong received the B.S. degree from Xiamen Univer-sity, Xiamen, China, in 2006. In 2012, he received his Ph.D.in National University of Singapore, Singapore. He is now aResearch Scientist with the Temasek Laboratories, NationalUniversity of Singapore. His research interests include real-timesoftware systems, formation flight control, and unmanned aerialvehicles.

Ben M. Chen (S’89–M’92–SM’00–F’07) received the B.S.degree in mathematics fromXiamen University, Xiamen, China,in 1983, the M.S. degree in electrical engineering from GonzagaUniversity, Spokane, WA, USA, in 1988, and the Ph.D. degreein electrical and computer engineering from Washington StateUniversity, Pullman, WA, 1991. He is currently a Professor andthe Director of the Control, Intelligent Systems and RoboticsArea of the Department of Electrical and Computer Engineer-ing with the National University of Singapore, Singapore, wherehe is also the Head of the Control Science Group, TemasekLaboratories. He has authored/coauthored ten research mono-graphs, including H2 Optimal Control (Prentice-Hall, 1995),Robust and H∞ Control (Springer, 2000), Hard Disk DriveServo Systems (Springer, 1st Edition, 2002; 2nd Edition, 2006),Linear Systems Theory (Birkhauser, 2004), Unmanned Rotor-craft Systems (Springer, 2011), and Stock Market Modeling andForecasting (Springer, 2013). His current research interests arein systems and control, unmanned aerial systems, and financialmarket modeling. Dr. Chen currently serves as Editor-in-Chiefof Unmanned Systems and Deputy Editor-in-Chief of ControlTheory and Technology. He had served on the editorial boardsof a number of journals, including the IEEE TRANSACTIONSON AUTOMATIC CONTROL, Systems and Control Letters,and Automatica.