Read A Real-Time Obstacle Detection and Reactive Path Planning System for Autonomous Small-Scale Helicopters text version

AIAA Guidance, Navigation and Control Conference and Exhibit 20 - 23 August 2007, Hilton Head, South Carolina

AIAA 2007-6413

A Real-time Obstacle Detection and Reactive Path Planning System for Autonomous Small-Scale Helicopters

Joshua Redding Jayesh N. Aminand Jovan D. Bokovi´ § , s c Scientific Systems Company, Inc. Yeonsik Kang¶and Karl Hedrick University of California, Berkeley Jason Howlett U.S. Army Research, Development, and Engineering Command Scott Poll NASA Ames Research Center

This paper presents the design of a comprehensive Multi-layer Architecture for Trajectory Replanning and Intelligent eXecution (MATRIX) system and its evaluation through high-fidelity simulations of the R-50 helicopter in the Real-time Interactive Prototype Technology Integration/Development Environment (RIPTIDE). The main feature of the MATRIX system is that it assures effective real-time obstacle detection & avoidance in a cluttered environment. Specifically, this paper discusses the (i) Design of the MATRIX System; (ii) Design, implementation & testing of individual modules; (iii) Integration & implementation under RIPTIDE; and (iv) Study of the features and limitations of the proposed architecture and algorithms. The MATRIX system consists of several subsystems including: Path Planner based on Rapidly-exploring Random Trees (RRT), shortest path algorithm for pruning of redundant waypoints, a safety corridor for preventing collisions with obstacles, 4D trajectory generator, nonlinear adaptive outer-loop controller, representation of obstacles using Quadtrees, and obstacle detection using a scanning laser and Interacting Multiple Model Kalman Filters (IMM-KF). These subsystems were developed and integrated within RIPTIDE, and extensive testing was carried out using R-50 helicopter dynamics. It was found that the MATRIX system rapidly generates new paths and trajectories to avoid unknown obstacles, is highly robust, and achieves excellent overall performance.

I. Introduction


HE last decade has seen a growing interest in the design and implementation of Unmanned Aerial Vehicles (UAVs) for both military and commercial use. Many of the issues associated with this rising interest have been studied in detail and their potential to replace manned aircraft in a variety of missions is now well understood. A major technology related to the wide use of UAVs and UCAVs (Unmanned Combat Aerial Vehicles) is their autonomy the capability to complete a mission without human intervention, even in the presence of faults, failures, upsets, and rapidly varying environments. Many elements of UAV autonomy have been studied in detail within suitable architectures. 1­6 Several of the key elements contributing to this autonomous capability include: accurate situational awareness sensors, decision making, motion planning, health monitoring and prognostics, and failure and damage accommodation. This paper discusses the development of UAV autonomy in the context of obstacle detection, representation and reactive path planning for real-time obstacle avoidance. research was supported by NASA Ames Research Center under contract No. NNA04AA63C to Scientific Systems Company. Engineer, Member AIAA, [email protected] Senior Research Engineer, [email protected] § Intelligent & Autonomous Control Systems Group Leader, Senior Member AIAA, [email protected] ¶ Graduate Student, Vehicle Dynamics Lab, [email protected] Director, Vehicle Dynamics Lab, [email protected] Research Engineer, Aeroflightdynamics Directorate, [email protected] Aerospace Engineer, Intelligent Systems Division [email protected]

Research This

1 of 22 American Institute of Aeronautics and and Astronautics, Inc., with permission. Copyright © 2007 by Scientific Systems Company Inc. Published by the American Institute of Aeronautics Astronautics

Obstacle Detection: In the area of obstacle detection there are several types of sensors that have been tested with varying success (IR, vision, scanning laser), and some of them have been demonstrated as capable of acquiring fairly accurate obstacle data in flight.3, 7 In addition to obstacle sensing, a fundamental issue in reactive motion planning is a suitable obstacle representation and its relationship with the existing databases (e.g. terrain elevation databases). 3 Obstacle Representation: A few of the desirable features of obstacle representation include: (i) Efficient storage; (ii) Easy addition and deletion of data (e.g. obstacle edges); and (iii) Easy intersection/collision checks. 3 Many of the current representations used in the context of obstacle avoidance for unmanned vehicles are not well suited for realtime or near real-time implementation.8­11 However, in the video gaming industry the issue of obstacle representation has been studied in detail since fast and accurate collision detection is fundamental. Obstacle representation formats can be loosely classified into the following categories:12 (i) Raw Data (e.g. point clouds, vertices-edges sets), (ii) Bounding Volumes, both simple and hierarchical (e.g. Axis-Aligned Bounding Boxes (AABB), Oriented Bounding Boxes (OBB), OBB Trees, k-dop trees), and (iii) Spatial Partitioning (e.g. Uniform or hierarchical (Hgrid) grids, Binary Space Partitioning (BSP), K-d trees (KDT), Quadtrees and Octrees). These representations vary in the amount of required storage space, complexity of data access and intersection/collision query operations, and complexity involved in addition/deletion of data. The choice of method is usually application domain dependent. In the context of autonomous vehicles, obstacle representation builds a foundation for successful motion planning since collision-free paths require continuous access to up-to-date obstacle information. Autonomous Motion Planning: One important element of UAV autonomy is autonomous reactive motion planning where the vehicle's control system detects previously unknown obstacles, and designs a new path and trajectory to avoid the obstacle in real time, and continue the mission. This issue is particularly pronounced in the case when the vehicle is flying at high speeds in a cluttered environment and under unfavorable weather. While motion planning has been extensively studied in the context of mobile ground robots, most of the existing techniques have not reached an implementation stage for real UAVs. In the area of motion planning for ground robots, several approaches have been developed. Motion planning techniques based on potential field functions have been extensively studied. However, these techniques are characterized by some inherent limitations, such as local minima and the difficulty in selecting appropriate potential functions. 13 Probabilistic Road Maps (PRM)14 are based on a heuristic method to randomly generate a roadmap through the configuration space and then search that map for the lowest cost route. It has been shown that as the calculations based on the PRM algorithm progress, the probability of finding the optimal path tends to one. 15 An extension of PRM to the case of dynamic constraints of the vehicle has also been studied. 15 However, since the algorithms explore all possible paths within the configuration space, it may not be well suited for real-time implementation. 16 An extension of the PRM approach, called Rapidly-exploring Random Trees (RRT), has been demonstrated as an effective and computationally efficient tool for complex online motion planning. 15, 17­23 When applied to the route planning problem, RRT's main advantage is its ability to quickly find a feasible solution between complex obstacle configurations. Its main disadvantage is that the solution may be far from optimal. Several modifications discussed in this paper, resulted in an RRT algorithm design that generates solutions that are close to optimal. The basic RRT algorithm and these modifications are described in the following chapters. Several optimization-based approaches using Mixed Integer Linear Programming have also been proposed. 24 However, due to the complexity and a very high computational demands, these algorithms have not yet been demonstrated as practically feasible. While MILP-based planners have a potential to generate optimal solutions for obstacle avoidance, the MILP algorithm, even in the case of simple problems involving only a few obstacles, results in hundreds of constraints. The associated computation burden prevents it from being applicable. Another set of techniques for motion planning use graph-based methods. One of the widely used approaches is the D* algorithm. Stentz uses this algorithm to generate paths in an environment represented by a grid. 25 However, grid representations do not scale well as the size of the environment increases. D* planning was also applied to pre-mission route planning in the Army Rotorcraft Pilot Associates program.5 McLain and Beard use the Voronoi graph to plan the safest route through a field of point threats, where it is desired to minimize exposure to the threats. 26 From the Voronoi graph idea, the US Army Aeroflightdynamics Directorate (AFDD) has developed its own obstacle-field route planner (OFRP).3 OFRP provides very fast and efficient planning in complex obstacle fields and has been flight tested and is currently in use on a small unmanned rotorcraft operated by AFDD. The Voronoi-based planner, however, has some limitations in the case of a changing obstacle field. Since it is not clear how to localize the computation of the Voronoi graph to a finite region around an obstacle, the graph for the entire region must be recomputed as the obstacle information changes. This repeated evaluation may not be practical for real-time operation in a dynamically changing environment.

2 of 22 American Institute of Aeronautics and Astronautics

In this paper, we describe an integrated approach to motion planning, where several subsystems are developed and integrated within a comprehensive architecture shown in Figure 1 and referred to as the Multi-layer Architecture for Trajectory Replanning and Intelligent eXecution (MATRIX).


RRT-based Path Planning Spatial Constraints Obstacle Representation

Trajectory Generation

Dynamic Constraints

IMM-based Obstacle Detection

Outer-loop Controller

Vehicle & Inner-loop Controller

Scanning Laser Sensor

Figure 1. MATRIX System and subsystems The MATRIX system consists of the following subsystems: (i) Obstacle detection subsystem based on filtering the scanning laser information using Interacting Multiple Model Kalman Filters (IMM-KF); (ii) Obstacle representation subsystem based on Quadtrees; (iii) Path planning subsystem based on a modified RRT and route enhancement based on the Dijkstra algorithm; (iv) Trajectory generation subsystem; and (v) Outer-loop controller. These subsystems are described in detail in the following sections. This paper presents a comprehensive Multi-layer Architecture for Trajectory Replanning and Intelligent eXecution (MATRIX) system through high-fidelity simulations of the R-50 helicopter in RIPTIDE. The main feature of the MATRIX system is that it assures effective real-time obstacle detection & avoidance in a cluttered environment. Specifically, this paper discusses the (i) Design of the MATRIX System; (ii) Design, implementation & testing of individual modules; (iii) Integration & implementation under RIPTIDE; and (iv) Study of the features and limitations of the proposed architecture and algorithms.

II. Obstacle Detection based on Interacting Multiple Models (IMM)

In this project, the Kalman filter-based Interacting Multiple Model (IMM) technique is used to employ a measurement model and to recursively build the probabilistic map of the sensor field. The localization of the unmanned vehicle is not addressed in this project since our research goal is to provide a fast and robust mapping algorithm for an unmanned vehicle equipped with a precise position sensor suite of GPS and INS. Therefore, it is assumed that the position of the vehicle can be measured or estimated. The IMM method was developed by the aerospace industry to track aircraft trajectories using radar measurements.27 The method has also been used for many different applications such as fault detection and identification, 28 maneuver detection of a nearby vehicle29 and automatic track formulation.30 In air traffic control system, this method is used to estimate the future trajectory of the airplane and avoid collision, 31 while in automotive industry, the method is employed as a driver assistance system to give the driver a warning of possible collisions. 29, 32 In this project, the algorithm developed for the automatic track formulation is extended to produce a probabilistic occupancy grid map using Bayes rule. By this expansion, the outputs from the track formulation and target-tracking algorithm can be reused to generate a map of stationary obstacles. Therefore, both the target tracking and mapping can be obtained simultaneously. Especially in a noisy outdoor environment, this method can significantly reduce the noise that may degrade the quality of the map. The derived recursive algorithm can be calculated fast enough to run in real-time applications such as UAV navigation. With this algorithm, the true target probability (TTP) of each target can be calculated and the target with a TTP less than a certain lower threshold will be removed. After a TTP reaches a certain upper threshold, the algorithm will declare that an obstacle is detected. This TTP with the position estimates and estimation covariances is used to build a probabilistic occupancy map. In the IMM-based obstacle detection algorithm, we run two different Probabilistic Data Association Filters (PDAFs) in parallel, each based on a different hypothesis. One model assumes that there is an observable obstacle, and therefore

3 of 22 American Institute of Aeronautics and Astronautics

the measurement originates from a real target. The other model assumes that there is no obstacle or that the obstacle is unobservable, and therefore the measurement must originate from a false measurement (a.k.a. clutter). The two models run the same kinematic model describing the motion of an obstacle, but have different detection probability PD , which represents the probability that the true measurement is detected by the sensor. For the PDAF assuming there is an observable obstacle, the PD is high while the other PDAF assuming there is no obstacle, the PD is zero. An obstacle is declared present when the probability of the model that assumes measurements originate from a real obstacle reaches a certain user-specified level. A combined state estimate from these two models is provided using the model probabilities as weights on each model-conditioned estimate. 30, 33

III. Quadtree Obstacle Representation

In order to use the obstacle information extracted from sensor measurements for path planning or obstacle avoidance algorithms, we use the metric probabilistic occupancy grid map to represent the occupancy of the environment. This approach fits very well with the developed IMM-based obstacle detection as well as popular path planning algorithms for unmanned vehicle. In probabilistic occupancy grid mapping, the area of interests are divided into grid cells and the value between 0 to 1 is assigned to each grid cell which represents the probability that the cell is occupied. In quadtree-based obstacle representation, these continuous values are discretized into binary values of either 0 or 1 to represent occupancy depending on the probability threshold. The outputs from the IMM obstacle detection algorithm are used to update the probability of the grid cell that has some nearby tracks. The necessary inputs for the developed probabilistic map updating algorithm are the state estimates, covariances and the model probabilities of the tracks. The Quadtree data structure reduces two-dimensional regional information into quadrants for faster retrieval and efficient storage. The hierarchical data structure known as a quadtree has advantages for geographic data storage and is used extensively for Geographic Information System (GIS) applications. Quadtrees recursively decompose a two-dimensional spatial region into quadrants, as shown in Figure 2


Figure 2. Quadtree representation Several variants of quadtrees exist that vary on the method used to select the quadrant decomposition at each step. Point quadtrees use the objects inside the given region as the dividing attribute and hence are data dependent. Region quadtrees, which we use, divide the region to provide geometric/spatial symmetry and are not data dependent. Data dependency usually leads to inefficiency of storage and query operations in a dynamic environment. Each quadrant becomes a node or cell of the quadtree which may be further decomposed based upon the resolution requirements and/or presence of any objects contained within that quadrant. We implemented a version of a Spatial Location Code (SLC) based quadtree method34, 35 for fast and efficient data manipulation (e.g. addition/deletion or intersection/collision checks). We focus our discussions on a quadtree based data structure since, based on the preliminary literature survey and our past experience, we anticipate quadtrees to be data structure of choice for GIS and obstacle field navigation applications. Region quadtrees are generally better suited for dynamic environments where most of the operating world information is generated on-the-fly and may have multiple moving objects. Most of the other formats that depend on good apriori information about the world and contained objects, become inefficient due to required recomputations

4 of 22 American Institute of Aeronautics and Astronautics

involved in inserting new information or changing existing information. We propose to compare and evaluate three data structures that can provide data independent formats while also being efficient in information retrieval. These methods are quadtrees, uniform grids and BSPs with various possible choices of partitioning planes. Subsequent material is focused on quadtree based database (QTDB) representation since we anticipate it to be a better choice, however much of it can be easily ported to the other candidate structures.

IV. Path Planning using a modified Rapidly-exploring Random Tree (RRT) Algorithm

While the RRT generates a feasible path through a cluttered environment defined by non-convex obstacles, the resulting path may be far from optimal. To alleviate this problem, several modifications of the RRT were made, including the basic Dijkstra algorithm and its adaptive version, addition of a suitable corridor, and the use of dual RRT. The basic RRT algorithm and these modifications are described in detail in the following sections. IV.A. RRT Description The Rapidly-exploring Random Tree (RRT) structure and algorithm is designed to efficiently and quickly explore high-dimensional spaces that are not necessarily convex.17­20 The main advantage of the RRT, aside from its computational efficiency, is its ability to quickly find a feasible solution between complex obstacle configurations. Its main disadvantage is that the solution may be far from optimal. Despite this, however, the RRT algorithm is well suited for use in motion planning since it inherently accounts for the differential constraints arising from vehicle dynamics (i.e. the constraint of the form x = f (x, u), where u Su = {u : uimin ui uimax , i = 1, 2, ..., m}), as well as the spatial constraints due to the presence of obstacles in the environment (i.e. the constraints of the form h(x) 0). RRT is well suited for path planning in a cluttered environment with non-convex obstacles. It is more computationally efficient than other approaches such as Nonlinear Dynamic Programming, Mixed-Integer Linear Programming (MILP) or Probabilistic Road Maps (PRM) methods and algorithms. RRT is well suited for non-convex obstacles without requiring any increase in the computational complexity, unlike MILP. Unlike Voronoi-based methods, RRT easily incorporates obstacles that are described by geometric shapes, as well as dynamic obstacles. Another attractive feature of RRT is that it can easily be tailored to meet specific computational restrictions. For example, RRT can be modified to use adaptive segment lengths when creating a path so that shorter segments can be used for more densely populated areas and vice versa. The primary factors affecting RRT performance are the number of random points generated, segment length used for tree extension, probability distribution used for random point generation, and the number of trees used. RRT is typically initialized with one node q near (starting point) and no edges. Figure 3 shows a simple RRT, and the q new basic steps involved in its development. q random The main concept behind RRT is to bias the growth of the tree towards the largest unexplored regions of space. By uniformly picking a random point and finding the nearq init est point on the RRT to extend, the search is biased towards splitting the largest Voronoi region for all the existing vertices on the tree. Note that the probability that a vertex is selected for extension (and hence its Voronoi region split) is proportional to the area of its Voronoi region. Figure 4 shows this process as an RRT grows in a 2D space. Figure 3. RRT expansion The steps of the RRT algorithm are summarized below: 1. At each iteration, a new random point in the search space is chosen. The choice may either be generated by a pseudo-random number generator, or by a quasi-random selection process, such as the Halton sequence. The latter yields a more even distribution of random points throughout the search space, and this provides some speed advantages when used with RRT.21 2. The random point is examined against the current tree of reachable points, and the RRT vertex nearest the random point is selected. "Nearest" is measured using a metric that will provide a point with the optimal

5 of 22 American Institute of Aeronautics and Astronautics

Figure 4. Voronoi bias in the RRT18, 19 obstacle-free trajectory towards the random point. The choice of optimality may be based on path distance, fuel consumption, total time, risk minimization, or other desired criteria. 3. If no trajectory can be found, the random point is discarded. Otherwise, an edge of length is added along the feasible trajectory from the closest point to the random point. 4. Then, a check is performed to see if the goal is reachable from the new node added to the tree. If it is, a feasible path has been found. If not, steps 1 through 4 are repeated until it is, or until a maximum number of steps have been taken. 5. The RRT search may continue to run even after a solution has been found in order to find a potentially shorter or lower-cost path. Many variations to these basic steps have been tested by researchers in many applications with varying success. The goal of these variations is generally to achieve some increase in performance, and aside from paths that may be far from optimal the RRT algorithm has a few other areas where this could happen. Since, in practice, the number of iterations (i.e. random points generated) cannot be very large, a solution may not be found within the window of iterations performed. Also, nearest neighbor searches are quite computationally expensive. One method suggested to work around the iterations limitation is to simply repeat the procedure, or perhaps backtrack and use a different starting point. As for nearest neighbor searches, these could depend on the density/complexity of obstacles saving exhaustive searches for densely populated areas. There are numerous ideas for tuning and enhancing the basic RRT algorithm. However, the biggest performance increase has been achieved by using dual RRTs, that is one tree expanding from the starting point and another from the end point. IV.B. Dual RRT The dual RRT is a natural extension of the single RRT. By allowing the end point to grow simultaneously with the start point, feasible paths can be achieved in less time. Each RRT is treated as a single tree, and the steps outlined in section IV.A can be followed exactly. In step 4 however, when a check is made to discover the reachability of the end

6 of 22 American Institute of Aeronautics and Astronautics

point, the dual RRT will check the reachability of any point on the opposing RRT. When the two trees meet, a feasible path is found. As will be described in a moment, the waypoints generated by RRT can be pruned using one or more iterations of Dijkstra's algorithm, resulting in shorter paths that approach the optimal. We implemented a few modifications to improve the speed of RRT algorithm. The costliest operation in an RRT algorithm is that of finding the nearest point to connect to when a new random point is generated. An approximate distance function based on octagonal approximation of the circular Euclidean distance function was used to eliminate the computationally expensive square-root operation which is replaced by a much simpler absolute value function. This approximate distance may deviate from the true Euclidean distance by up to 6%. However, this accuracy tradeoff is certainly worth the improvements observed in the algorithm execution speed in large operating fields where several thousands of distance evaluations are needed with the non-critical artifact of picking the non-nearest point in the RRT to extend in a very small number of cases. IV.C. Adding a Corridor to the RRT We also implemented another modification to account for uncertainty in vehicle dynamics and disturbances in the form of safety corridor checking when extending a line segment from RRT. This checked corridor can be either fixed width around the line segment to be extended or could be made adaptive on either ends of the segment based on the turn angle needed. This implicitly accounts for the vehicle turn radius constraints in presence of uncertainties. IV.D. Dijkstra Algorithm RRT generated paths are sub-optimal and generally contain redundant and meandering path segments. An effective modification to a typical RRT involves the Dijkstra algorithm to remove these redundant points and arrive at paths that are closer to optimal. The Dijkstra algorithm step is implemented by computing a reachability graph using the waypoints (nodes) provided by the RRT algorithm, and then finding the shortest path. The resulting path is again resampled at some pre-specified smaller segment length to provide intermediate nodes. Another pass of Dijkstra's algorithm prunes redundant nodes from this set and calculates the new shortest path, as shown from left to right in Figure 5. This process can be repeated as frequently as computational resources, path optimality and resolution requirements allow.

Figure 5. Dijkstra algorithm post-processing the RRT path The addition of the Dijkstra algorithm has many advantages over "pure" RRT, including finding the shortest path among the points generated by RRT. Also, since implementation is computationally very efficient, it does not inhibit extensions of RRT to be used, such as dual RRT trees. Implementation should be done with care as the RRT path may need several passes of the Dijkstra algorithm with re-segmentation in between before the "best" path is found. Also, when re-segmenting, too short of segments lead to large computational effort, and too long of segments may lead to sub-optimal paths. However, when these considerations are met, Dijkstra proves a very effective tool in assisting the RRT path planner.

7 of 22 American Institute of Aeronautics and Astronautics

V. Autonomous Trajectory Generator & Tracker (ATGT) for R-50 Autonomous Helicopter

In this section we describe the design of the on-line Autonomous Trajectory Generator & Tracker (ATGT) system that has been developed under the project. The ATGT consists of an Autonomous Trajectory Planner (ATP) and outerloop controller, and was designed for the R-50 autonomous small-scale helicopter. The ATGT was first implemented in Matlab and Simulink, and then under RIPTIDE. V.A. Autonomous Trajectory Generator & Tracker (ATGT) System Architecture The role of the ATP is to generate a feasible trajectory for the helicopter to follow. The trajectories generated by the ATP are consistent with vehicle dynamics since maximum accelerations and turn rates are explicitly taken into account. The outer-loop controller compares current states with the desired ones generated by the ATP and generates control action aimed at decreasing the tracking error. Since, besides the heading, the inputs into the controller are desired and actual inertial positions and velocities, the latter variables are transformed into the body frame velocities and input into the inner-loop controller. The inner-loop controller came with the R-50 model in RIPTIDE, and is not a part of our design.

Figure 6. Simulation diagram of the ATGT for R-50 autonomous helicopter

V.B. R-50 Model The R-50 Model block contains a quaternion attitude model and a model of helicopter dynamics with parameters taken in their entirety from Reference.36 Through extensive flight testing, a very accurate model of the R-50 helicopter was identified for both hover and cruise flight regimes. The model from 36 is implemented in a simulink S-function. The 4 control inputs going into the model, P ed, Lat, Long, Cyc are all passed through first order filters with transfer 15 functions W (s) = s+15 , and saturation blocks to normalize the signals from -1 to 1. V.C. Inner-Loop Controller The inner-loop controller generates control inputs that assure the helicopter outputs follow the desired reference signal. In this application the following inputs were chosen: u, v, w, and , where u, v, and w are velocities in the body frame of the helicopter, and is the heading angle. The inner-loop controller contains several PI(D) loops that assure closedloop stability and the desired performance. The response of the closed-loop system to the various inputs is close to 6 a system described by a second order transfer function. The inputs into the PIDs are all filtered by a s+6 first order transfer function, to ensure smooth inputs into the controller and to avoid rate saturations. V.D. Outer-Loop Controller In this section we describe the design of the outer-loop controller. The role of the outer loop controller is to generate commands to follow the desired trajectory provided from the trajectory planner. This commands are inputs into the inner-loop controller. Under this research it was assumed that the dynamics of the closed-loop flight control system, consisting of the R-50 model and the inner-loop controller, can be sufficiently accurately approximated by a 6th order nonlinear model of the form: x y z = V · cos() · cos() = V · sin() · cos() = V · sin()

8 of 22 American Institute of Aeronautics and Astronautics

(1) (2) (3)


= -V · (V - Vc ) = - · ( - c ) = - · ( - c )

(4) (5) (6)

where is the heading angle, is the flight-path angle, V is the total velocity of the helicopter, x, y, and z are the positions of the helicopter in the inertial frame, V , , and are the positive constants, and c , Vc , and c are the control inputs. Let p = [x y z]T . After taking the second derivative of p, one obtains: x ¨ Vc (7) ¨ y = p = f (V, , ) + g(V, , ) · c , ¨ c z ¨ where -V · V · cos() · cos() + · · V · sin() · cos() + · · V · cos() · sin() f (V, , ) = -V · V · sin() · cos() - · · V · cos() · cos() + · · V · sin() · sin() -V · V · sin() - · · V · cos() and V · cos() · cos() - · V · sin() · cos() - · V · cos() · sin() g(V, , ) = V · sin() · cos() · V · cos() · cos() - · V · sin() · sin() . 0 · V · cos() V · sin() Vc V + Vcc It is seen that by choosing c = + cc , the expression (7) simplifies to: c + cc Vcc p = g(V, , ) · cc . ¨ cc



Let p (t) be a twice differentiable function describing the desired trajectory. One possible control law that assures tracking of the desired trajectory p (t) is that based on nonlinear inverse dynamics and is of the form: Vcc -1 ¨ cc = g (V, , ) -k1 (p - p ) - k2 (p - p ) + p , cc where k1 , k2 > 0. The resulting closed-loop control system consists of three decoupled second-order error equations of the form: e + k2 e + k1 e = 0, ¨ where e = p - p , so that limt e(t) = limt e(t) = 0. However, the problem with this control law comes from the fact that g(V, , ), as defined in Equation (8), becomes singular for V = 0, which corresponds to the hover condition. For this reason the control law is modified as: Vcc ¨ cc = [g T (V, , )g(V, , ) + I]-1 g T (V, , ) -k1 (p - p ) - k2 (p - p ) + p . cc

This control law is referred to as the One Step Ahead Optimal (OSAC) controller and, for V >> , results in a small tracking error. It is seen that the control signals, or outputs of the controller, are Vc (t), c (t), and c (t). Since the inner-loop controller accepts inputs defined in the body frame of the helicopter, the outer-loop controller outputs need to be converted to the body frame velocities u, v, w. This conversion is a simple rotation that depends on the current states , , and . To accomplish this rotation, going into the inner-loop controller is taken directly from the trajectory planner.

9 of 22 American Institute of Aeronautics and Astronautics

VI. System Integration and Implemenation

The development of the MATRIX system was primarily carried out in the Matlab environment. After successful unit testing, the subsystems were integrated and transitioned to RIPTIDE where full-scale integration and testing was performed. This chapter describes implementation issues and the MATRIX system test results obtained. Software development for obstacle detection and mapping is described first, followed by the description of the integration and testing of the overall MATRIX system in RIPTIDE. VI.A. Obstacle Representation Tests To ensure that detected obstacles were properly filtered and populated into the quadtree structure, we developed a set of unit tests, as shown in Figures 7 and 8. Figure 7 gives a visualization of the quadtree structure before and after a flight through the simulated Moffet Field. Figure 7(b) shows the helicopter's flight path as the thick black line and all detected obstacles (all of which were known apriori, in this case) as red squares. As seen, the obstacles detected through the sensor and IMM filter match very well against the previously populated obstacle data. This indicates that the sensor, IMM filter and quadtree data structure are properly configured and successfully integrated.

1000 900 800 700 600 500 400 300 200 100 0 1000 900 800 700 600 500 400 300 200 100 0























(a) Moffet Field data pre-populating the quadtree

(b) Test flight (thick black line) and corresponding obstacle detections (red squares)

Figure 7. Unit tests for obstacle representation in the quadtree data structure Figure 8(a) is a screengrab from a simulation in RIPTIDE. A scanning laser is mounted on the helicopter and is detecting the side of a building using a ray intersection method. It is this detection that feeds into the IMM filter and leads to the population of the quadtree. In Figure 8(b) we are reading and plotting the ouput of the IMM filter to ensure only actual obstacles are populating the quadtree. As seen, there is a very nice match between Figures 8(a) and 8(b). VI.B. Path Planning Tests Initial implementation as well as unit testing of most system elements was done in Matlab. Details on the path planning approach and results of Matlab testing and implementation were given previously. 23 Figure 9 shows the random points generated by the RRT algorithm (connected with the thin green lines) and the extent of their connectivity. Also shown is the original feasible path (thick green line) and the results of the first (thick black line) and second (thick red line) Dijkstra passes on the original path. It is important to note that all paths avoid all known obstacles and the Dijkstra algorithm presents improvements on the original path. In more complex simulations, these improvements will likely be much more pronounced.

10 of 22 American Institute of Aeronautics and Astronautics

(a) Obstacle detection in RIPTIDE with ray intersection

(b) Matlab confirmation of the detected obstacle in the quadtree

Figure 8. Unit tests for obstacle detection in RIPTIDE and representation in the quadtree data structure VI.C. Integration with RIPTIDE Simulation RIPTIDE is a real-time simulator developed by the US Army AAFDD to rapidly evaluate the expected responses of the Unmanned Aerial Vehicles (UAVs) using a complex control system. In this project, the performance of the developed algorithm is evaluated in RIPTIDE. One advantage of using RIPTIDE is its compatibility with the Matlab SIMULINK. RIPTIDE provides interfaces which can be used in Matlab SIMULINK to design and evaluate control algorithms. It also provides the capability of using a binary executable converted from the SIMULINK environment through Matlab Real-time Workshop. These features allow rapid modification and evaluation of the control laws developed by control engineers and helps to shorten the design cycle. After the controller is designed in SIMULINK model and converted into executable file by Real-time Workshop, the designed controller can be selected under the "Control Laws" tab in the configuration window of RIPTIDE, shown in Figure 10(a). In Figure 10(b), the embedded GUI interface is also shown with three displaying windows, each from a different view point. The view points can be reconfigured individually by using embedded GUI or keyboard shortcuts. Overall, RIPTIDE provides an effective and highly customizable simulation tool for UAV control development. VI.D. RIPTIDE Simulation Results The simulation accomplishes obstacle avoidance through reactive path planning using scanning laser range detectors, IMM Kalman filters, a quadtree data structure for obstacle representation and RRT-based path planning in RIPTIDE. Figure 11 shows the graphical details of RIPTIDE with the R-50 and its scanning laser. To thoroughly test the overall system, we developed three main scenarios, each designed to exploit possible weaknesses while showing off system strengths. Before describing each scenario, it is important to note that in each of the Figures 12 through 24, there are several lines and colors that represent different aspects of the MATRIX system. The green line shows the output of the RRT algorithm that quickly yields a feasible, sub-optimal path between desired waypoints. The blue waypoints connected with dashed blue lines indicate the output of the Dijkstra algorithm with resegmentation while the solid blue line represents the actual UAV trajectory. Solid blue squares denote the obstacles known apriori while yellow squares are those detected through use of the scanning laser, filtered through the IMM and populated into the quadtree structure for obstacle representation. In addition, the UAV's status is displayed in the text next the vehicle while system messages show up in the scrolling text box at the lower left of each figure. These scenarios are outlined as follows: Scenario 1: Pre-population of all known obstacles into the quadtree Under this scenario, the quadtree data structure was pre-populated with accurate Moffet Field data before any path planing occurs. This represents the ideal situation where all possible threats are known and paths are planned accordingly. In this case, pop-up threats do not exist and a reactive path planning capability is not necessary. Naturally,

11 of 22 American Institute of Aeronautics and Astronautics

Feasible Paths Known Hidden RRT RRT+DA RRT+2DA End


Figure 9. RRT branches, original path, Dijkstra path, 2nd Dijkstra path

Figure 10. (a) RIPTIDE main window (b) RIPTIDE embedded GUI interface this represents the benchmark situation for UAV safety since all threats are known apriori. Although desirable, this scenario is virtually non-existent in the environments foreseen for unmanned helicopters such as the R-50. Figures 12 through 15 depict a complex simulation navigating from one corner of Moffett Field to another, planning a path through the maze of known buildings. Although the quadtree was pre-populated with all the buildings surrounding the region of interest, Figure 13 shows the UAV replanning due to a pop-up obstacle. Although not planned, this represents the real possibility that the data used to populate the quadtree does not match perfectly with actual buildings or structures. In this case, the path planner generated a path that was a "safe" distance from all known building/obstacle locations according to the quadtree map. However, as the UAV began to fly this path it came slightly closer to an obstacle than the "safe" distance due to the resolution of the obstacle map and a replan was forced. The replan was done in real-time and the planner actually found a shorter route to its desired destination. Figures 14 and 15 show the UAV safely following its desired path and reaching its desired destination. Note the detection of obstacles along the flight path (the yellow squares) matches well, however not perfectly against the known obstacle map (blue squares). This mismatch is expected due to the current resolution of the quadtree. Increasing map resolution would lessen this problem at the expense of more cells to check, or query, therefore increasing planning times. Scenario 2: Pre-population of most known obstacles into the quadtree, omitting several key buildings In this scenario, a key building was omitted from the pre-populated obstacle data rather than pre-loading all obstacles into the data structure. This represents a more feasible situation where intelligence about the region is out of date

12 of 22 American Institute of Aeronautics and Astronautics

Figure 11. RIPTIDE simulations show detailed environment, UAV and scanning laser and new buildings have been erected since the data was collected. For the test, waypoints were selected such that the UAV would likely pass directly through the new, or unknown, building. Figure 16 shows the original desired path. In Figure 17, the new building is first detected and the RRT algorithm suggests that the UAV travel around the large set of buildings to its left. With the addition of Dijkstra's algorithm and the other modifications discussed in Section IV, the UAV alters the initial RRT path and finds a more direct route while avoiding the pop-up threat. In Figure 18, more of the unknown building is detected and another replan is triggered. Again, the intial RRT output suggests that the UAV travel safely around the large set of buildings. And again, Dijkstra and the other techniques provide a much shorter alternative, although not optimal. Figure 19 shows the UAV safely reached its final waypoint. From the yellow squares in the figure, the outline of the previously unknown building is seen. Scenario 3: No pre-population of known obstacles, i.e. begin with an empty quadtree Under this scenario, a more complex situation is presented in that the UAV begins with no knowledge of its environment. The quadtree data map is filled as surrounding obstacles are detected through the on-board sensor and filtered through the IMM. This is shown in Figures 20 through 24. Figure 20 shows that the UAV starting point is in a narrow corridor and its surroundings are discovered as it plans a path to the end waypoint. In Figures 21, 22 and 23 it is seen that the UAV is forced to replan again and again as more if its surroundings are detected, thus building a more detailed map. As seen in Figure 24, the final destination is reached safely and a fairly complete map of the immediate region was developed. Overall, each scenario provided distinct functional testing of the overall integration of the MATRIX system for real-time obstacle detection and reactive path planning. Through simulation of each scenario, the MATRIX system provides an implementable, real-time solution for path planning in both known and unknown cluttered environments.

13 of 22 American Institute of Aeronautics and Astronautics

Figure 12. Scenario 1: All buildings known. Intial path is planned and initiated.

VII. Conclusions

In this paper, a comprehensive Multi-layer Architecture for Trajectory Replanning and Intelligent eXecution (MATRIX) system was developed and implemented through high-fidelity simulations of the R-50 helicopter in RIPTIDE. The MATRIX system consists of several subsystems including: Path Planner based on Rapidly-exploring Random Trees (RRT), shortest path algorithm for pruning of redundant waypoints, a safety corridor for preventing collisions with obstacles, 4D trajectory generator, nonlinear adaptive outer-loop controller, representation of obstacles using Quadtrees, and obstacle detection using a scanning laser and Interacting Multiple Model Kalman Filters (IMM-KF). The main feature of the MATRIX system is that it assures effective real-time obstacle detection & avoidance in a cluttered environment and this was shown through extensive simulations. Under the MATRIX system, the problem of real-time obstacle detection and reactive path planning is conveniently decomposed into: 1. Shortest path generation that takes into account dynamics & constraints through a convenient use of the safety corridor in conjunction with RRT+Dijkstra; 2. Efficient checking for obstacles within the flying corridor using the QT representation; 3. Effective trajectory generation where constraints are taken into account in the outer-loop as the trajectory generator generates accelerations and turn rates that are within bounds, and inner-loop constraint violation is prevented. Also maximum transient overshoots are guaranteed to be within the corridor for a range of forward velocities; Overall, the system is fast and efficient, and most importantly, implementable on-line in the presence of known as well as hidden/pop-up/unknown obstacles. Some of the limitations of the MATRIX system include: 1. Current implementation works for low to moderate forward velocities; this issue can be addressed by redesigning the outer-loop controller to allow for aggressive maneuviering by including a higher-fidelity model of the closedloop R50 dynamics. 2. Needs a customized architecture (parallel processing of RRT path planner, trajectory generator, and sensory information);

14 of 22 American Institute of Aeronautics and Astronautics

Figure 13. Scenario 1: All buildings known. Continue on planned path. Here a known obstacle is detected to be larger than original data showed and is protruding into the safety corridor, thus triggering a replan. 3. Extensions to 3D need to be explored; while initial analysis has revealed that the extension to 3D should be relatively straightforward, the main issue here is the cost associated with turning around a building or flying above it. This problem needs a detailed analysis. 4. On-the-fly obstacle avoidance needs further work; this problem is a difficult one since the safe stopping distance needs to be taken into account. The framework proposed here appears to be well suited for studying this problem further. In addition, a new method for building probabilistic occupancy maps was developed and its performance was evaluated with realistic sensor models in real-time simulator where it proved to be both fast and efficient. The algorithm filtered clutter and measurement noise using the existing machinery of an IMM algorithm for target tracking and formation. The output of the filters produced a high quality probabilistic occupancy grid map of obstacles that show a nice match with the physical layout of the obstacles in the field


Our special thanks go to Dr. Mark Tischler for his strong support throughout the project, Mr. Hossein Mansur for his excellent technical support with RIPTIDE and to our consultants Profs. Emilio Frazzoli and Steven LaValle for their help in gaining deeper insight into the properties of the RRT algorithm.


1 E. P. Anderson, R. W. Beard and T. W. McLain, "Real Time Dynamic Trajectory Smoothing for Uninhabited Aerial Vehicles", IEEE Transactions on Control Systems Technology, Vol. 13, No. 3, pp. 471-477, May 2005. 2 J. D. Bo kovi´ , R. Prasanth and R. K. Mehra, "A Multi-Layer Autonomous Intelligent Control Architecture for Unmanned Aerial Vehicles", s c AIAA Journal of Aerospace Computing, Information, and Communication (JACIC), Special Issue on Intelligent Systems, Vol. 1, pp. 605-628, December 2004. 3 J. Howlett, G. Schulein and M. H. Mansour, "A Practical Approach to Obstacle Field Route Planning for Unmanned Rotorcraft", Presented

15 of 22 American Institute of Aeronautics and Astronautics

Figure 14. Scenario 1: All buildings known. Continue on re-planned path.

at the American Helicopter Society 60th Annual Forum, Baltimore, MD, June 7-10, 2004. 4 M. Whalley, M. Freed, R. Harris, M. Takahashi, G. Schulein and J. Howlett, "Design, Integration, and Flight Test Results for an Autonomous Surveillance Helicopter", Presented at The AHS International Specialists Meeting on Unmanned Rotorcraft, Mesa, AZ, Jan. 2005. 5 M. Whalley, M. Freed, M. Takahashi, D. Christian, A. Patterson-Hine, G. Schulein and R. Harris, "The NASA/Army Autonomous Rotorcraft Project", Presented at the American Helicopter Society 59th Annual Forum, Phoenix, Arizona, May 6-8, 2003. 6 L. Wills, S. Sander, S. Kannan, A. Kahn, J.V.R. Prasad and D. Schrage, "An Open Control Platform for Reconfigurable, Distributed, Hierarchical Control Systems," in Proceedings of the Digital Avionics Systems Conference, Philadelphia, PA, October 2000. 7 J. D. Bo kovi´ , R. Prasanth, J. Byrne, D. Garagi´ , Jayesh Amin, S. Bergstrom, M. Cosgrove, and R. K. Mehra, "Development of Intelligent s c c Model Predictive Control Algorithms and a Software Design Toolbox for Autonomous Systems", Final Report for DARPA Phase II SBIR, Contract No. DAAH01-00-C-R187, March 2005. 8 J. Guivant and E. Nebot, "Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementation" IEEE Transactions of Robotics and Automation, May 2001. 9 J. R. Miller, "A 3D Color Terrain Modeling System for Small Autonomous Helicopters," Technical Report No. CMU-RI-TR-02-07, Robotics Institute, Carnegie Mellon University, February 2002. 10 J. R. Miller and O. Amidi, "3-D Site Mapping with the CMU Autonomous Helicopter," in Proceedings of the 5th International Conference on Intelligent Autonomous Systems, June 1998. 11 S. Thrun, M. Diel and D. Hahnel, "Scan Alignment and 3-D Surface Modeling with a Helicopter Platform", The 4th International Conference on Field and Service Robotics, July 14-16, 2003. 12 C. Ericson, Real-Time Collision Detection, Morgan Kaufmann Publishers, San Francisco, CA, 2005 ( 13 Y. Koren, and J. Borenstein, "Potential field methods and their inherent limitations for mobile robot navigation", Proceedings of the IEEE Conference on Robotics and Automation, Sacramento, CA, April 1991. 14 L. Kavraki, P. Svestka, J. Latombe, and M. Overmars, "Probabilistic roadmaps for path planning in high-dimensional configuration spaces", IEEE Transactions on Robotics and Automation, Vol. 12, (4), August 1996. 15 E. Frazzoli, M. A. Daleh and E. Feron, "Real-Time Motion Planning for Agile Autonomous Vehicles", in Proceedings of the AIAA Guidance, Navigation and Control Conference, Paper No. AIAA-2000-4056, Denver, CO, August 2000. 16 L. Kavraki, M. Kolountzakis, and J. Latombe, "Analysis of probabilistic roadmaps for path planning", IEEE Transactions on Robotics and Automation, Vol. 14, (1), February 1998. 17 S. M. LaValle. From dynamic programming to RRTs: Algorithmic design of feasible trajectories. In A. Bicchi, H. I. Christensen, and D. Prattichizzo, editors, Control Problems in Robotics, pages 19­37. Springer-Verlag, Berlin, 2002. 18 S. M. LaValle and J. J. Kuffner. Randomized kinodynamic planning. International Journal of Robotics Research, 20(5):378­400, May 2001. 19 S. M. LaValle and J. J. Kuffner. Rapidly-exploring random trees: Progress and prospects. In B. R. Donald, K. M. Lynch, and D. Rus, editors, Algorithmic and Computational Robotics: New Directions, pages 293­308. A K Peters, Wellesley, MA, 2001.

16 of 22 American Institute of Aeronautics and Astronautics

Figure 15. Scenario 1: All buildings known. Helicopter arrives safely at destination

20 S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. TR 98-11, Computer Science Dept., Iowa State University, Oct. 1998. 21 E. Frazzoli, "Quasi-Random Algorithms for Real-Time Spacecraft Motion Planning and Coordination", International Astronautical Congress, Houston, TX, 2002. 22 J. D. Bo kovi´ , J. Amin, R. Mehra, Y. Kang, D. Cavaney and K. Hedrick, "Multi-layer Architecture for Trajectory Replanning and Intelligent s c plan eXecution (MATRIX)", Annual Report for NASA Ames Research Center, Contract No. NNA04AA63C, May 2005. 23 J. Amin, J. D. Bo kovi´ and R. Mehra, "A Fast and Efficient Approach to Path Planning for Unmanned Vehicles" in Proceedings of AIAA s c Guidance, Navigation and Control Conference, Aug 2006. 24 T. Schouwenaars, M. Valenti, E. Feron, and J. P. How, "Implementation and Flight Test Results of MILP-based UAV Guidance," Proceedings of the IEEE Aerospace Conference, Feb. 2005. 25 A. Stentz, "The focused D* algorithm for real-time replanning", Proceedings of the International Joint Conference on Artificial Intelligence, Montreal, Canada, August 1995. 26 T. McLain and R. Beard, "Trajectory planning for coordinated rendezvous of unmanned air vehicles", Proceedings of the AIAA Guidance Navigation and Control Conference, Denver, CO, August 2000. 27 A. Houles and Y. Bar-Shalom. Multisensor tracking of a maneuvering target in clutter. IEEE Transactions on aerospace and electronic systems, AES-25(2), March 1989. 28 Y. Zhang and X. Rong Li. Detection and diagnosis of sensor and actuator failures using imm estimator. IEEE Transactions on aerospace and electronic systems, 34, 1998. 29 D. S. Caveney. Multiple target tracking in the adaptive cruise control environment using multiple models and probabilistic data association. M. S. Thesis of U. C. Berkeley, 1999. 30 Y. Bar-Shalom and X. R. Li. Multitarget-multisensor tracking: Principles and techniques. YBS Publishing, 1995. 31 I. Hwang, H. Balakrishnan, K. Roy, and C. Tomlin. Multiple-target tracking and identity management in clutter, with application to aircraft tracking. Proceedings of American Control Conference, 4:3422­3428, 2004. 32 D. S. Caveney and J. K. Hedrick. Multiple target tracking in the adaptive cruise control environment using multiple models and probabilistic data association. Proceedings of ASME IMECE, 2001. 33 Y. Bar-shalom, K. C. Chang, and H. A. P. Blom. Automatic track formation in cluter with a recursive algorithm. IEEE Proceedings on Decision and Control, December 1989. 34 P. Bhattacharya, "Efficient Neighbor Finding Algorithms in Quadtree and Octree", Master's Thesis Department of Computer Science & Engineering, Indian Institute of Technology, Kanpur, 2001. 35 S. Frisken and R. Perry, "Simple and Efficient Traversal Methods for Quadtrees and Octrees", Journal of Graphics Tools, Vol. 7, Issue 3, May 2003 (ACM Press). 36 B. Mettler, M. Tischler and T. Kanade, T, "System identification modeling of a small-scale unmanned rotorcraft for flight control design", Journal of the American Helicopter Society, Vol. 47, No. 1, January 2002.

17 of 22 American Institute of Aeronautics and Astronautics

Figure 16. Scenario 2: Key building unknown. Intial path is planned and initiated.

Figure 17. Scenario 2: Key building unknown. A pop-up obstacle is detected and a replan is triggered. Note that the benefit of the Dijkstra algorithm is strongly evidenced here

18 of 22 American Institute of Aeronautics and Astronautics

Figure 18. Scenario 2: Key building unknown. More of the unkown building is detected and another replan is triggered. Again, Dijkstra's algorithm severely shortens the original RRT path.

Figure 19. Scenario 2: Key building unknown. Helicopter arrives safely at destination.

19 of 22 American Institute of Aeronautics and Astronautics

Figure 20. Scenario 3: All buildings unknown. Intial path is planned to avoid those obstacles already detected, shown in yellow.

Figure 21. Scenario 3: All buildings unknown. Newly detected obstacles trigger replan.

20 of 22 American Institute of Aeronautics and Astronautics

Figure 22. Scenario 3: All buildings unknown. An obstacle map is slowly built as the helicopter attempts to reach the final destination.

Figure 23. Scenario 3: All buildings unknown. Here it is evident that RRT-based paths are not optimal, epsecially in the absence of a global obstacle map.

21 of 22 American Institute of Aeronautics and Astronautics

Figure 24. Scenario 3: All buildings unknown. Helicopter arrives safely at destination with a decent obstacle map.

22 of 22 American Institute of Aeronautics and Astronautics


A Real-Time Obstacle Detection and Reactive Path Planning System for Autonomous Small-Scale Helicopters

22 pages

Find more like this

Report File (DMCA)

Our content is added by our users. We aim to remove reported files within 1 working day. Please use this link to notify us:

Report this file as copyright or inappropriate


You might also be interested in

A method for robot navigation toward a moving goal with unknown maneuvers
Microsoft PowerPoint - S 1 - Introduction.ppt
A Real-Time Obstacle Detection and Reactive Path Planning System for Autonomous Small-Scale Helicopters