Fast Local Planner for Autonomous Helicopter Alexander Washburn talexan@seas.upenn.edu Faculty advisor: Maxim Likhachev April 22, 2008 Abstract: One challenge of autonomous flight is creating a system for planning routes and reacting to environments that is fast enough to take full advantage of the high speed of aircraft. This problem of obstacle avoidance is especially relevant when creating a program that lands robotic helicopters in urban environments. Since these areas are highly concentrated with static obstacles like buildings as well as moving ones like cars and people, the helicopter needs to be able to navigate many obstacles while reacting to unforeseen changes quickly and efficiently. Autonomous vehicle company Dragonfly Pictures Incorporated is building a helicopter that is designed to fly in these urban settings. It will be around 300 pounds, have a dual rotor design, and feature LIDAR, camera, and GPS sensors. The University of Pennsylvania has been chosen to design a system for identifying and navigating to safe landing zones. Our system will consist of a module that looks at the whole environment and evaluates the different possible landing zones, a module that plans the best route to
the chosen landing area, and a collision avoidance system. My project focuses on designing the third component. It will need to react quickly to the immediate surroundings of the helicopter. Using information from a global planner and data from sensors, the obstacle avoider will generate commands that steer the helicopter away from nearby hazards while still continuing toward its ultimate goal. Related Work: A few classic obstacle avoidance methods are described in Obstacle Avoidance Procedure for Mobile Robots by Becker, Dantas, and Macedo. First is edge-detection, where the robot simply searches for the edges of objects and simply steers to the proper side. A better method uses certainty grids which model the environment as blocks that have a certainty value. This value represents how confident the robot is that an obstacle is within the corresponding block. By moving throughout the world and updating the certainty values as it goes, the robot can create a more accurate picture of its surroundings. The potential field method improves on these methods by calculating imaginary forces that repel from obstacles and pull toward the goal. Manipulating the strengths of these forces helps streamline behavior. These methods can be combined as in the Koren and Borenstein s Virtual Force Field. Their model uses a certainty grid with the potential field function applied to it. This was improved upon in their next method, the Vector Field Histogram which uses a
polar certainty grid based upon the robot s position. It measures the certainty of an object in any given direction and then uses potential field function to decide on directions. Separately, Fox, Burgard, and Thrun developed Dynamic Window Approach. It calculates the actual area to which the robot can possibly move in its next step by taking into account its physical limitations. Then obstacle avoidance calculations take into account only this area when planning the next move. Common throughout these methods is the concept of obstacles and goals exerting force vectors on the robot. One project that implements these ideas is described in Flying Fast and Low among Obstacles by Scherer et al. Their helicopter s obstacle avoidance system uses a three dimensional certainty grid combined with goal-attraction and obstacle-repulsion functions to avoid unknown obstacles. The helicopter can successfully navigate paths at up to 10 m/s and in up to 24 knot winds. The system can navigate areas sparsely populated with obstacles but more dense areas would slow the helicopter down or cause excessive reactions to large objects such as buildings. Our goal is to be able to handle these urban settings better so our helicopter can navigate and land in a city-like environment. Technical Approach: The obstacle avoidance system I am creating will be given goals from a global planner and positions of obstacles from sensors then use an attraction and repulsion algorithm to produce commands for controlling the helicopter.
The global path planner is currently being written by Alex Kushleyevin and will produce an output in the form of a series of poses. These poses represent the positions and orientations for the helicopter to reach. These points are used as goals for the obstacle avoidance system. They are input into a linked list that is cleared and replaced every time new poses are received. The sensors being used are a Fibertek forward sensing LIDAR laser range detector and a Navsys downward facing camera. The sensor units are also being produced by the creators of the helicopter and their raw data will be converted and output as a three dimensional map of the helicopter s environment. From this my system takes three dimensional coordinates as an input and they get placed into a three dimensional array that encompasses only the approximately twenty meter, immediate surroundings to search for obstacles. Collision avoidance is obtained by calculating an appropriate rotational acceleration to use in calculating the next pose using a formula based on Fajen and Warren s control law. This law is stated as follows: #obstacles φ = bφ k g φ ψ g e c 1d g + c 2 + k o φ ψ oi e c 3 φ ψ oi e c 4d oi i=1
φ is the helicopter s rotational acceleration, φ is its rotational velocity, and b is a constant. By making b between 1 and zero, the equation starts with a negative fraction of the rotational velocity. This prevents the helicopter from gaining too much rotational velocity until it is just spinning in one spot. The next part of the equation, k g φ ψ g, represents the attraction to the goal with φ ψ g being the angle from heading to goal, and k g being a constant. This means that the greater the angle between the helicopter s heading and the heading it should be taking to get to the goal, the greater the acceleration in that direction. Next the e c 1d g + c 2 where d g is the distance to goal and c 1 and c 2 are constant parameters. This portion of the law means that as the helicopter comes closer to the goal, the heading difference has more pull. This prevents the helicopter from flying by the goal or entering orbit around the goal because it is not turning fast enough. The final part is the sum of the repulsive forces of all the individual obstacles. Here φ ψ o is the angle from the heading to an obstacle d o is the distance to the obstacle, and k o, c 3, and c 4 are constant parameters. This portion turns the helicopter away from obstacles. The first power of e increases the push away from the obstacles and the second power of e means that the closest obstacles have the most push. This allows the helicopter to react to the closer objects more than the farther away objects. When this rotational acceleration is calculated it is added to the current rotational velocity which is in turn added to the current heading. A new pose is created using this heading along with the current speed and output to the helicopter controls. This equation is constantly being calculated as long as it has goals to follow. When new goal poses are
added, the loop is interrupted and the poses are added to the goal linked list. From the head of this list comes the goal used in calculating the rotation acceleration and once the helicopter reaches it, the goal is removed from the list. When adding obstacles the loop is interrupted as before and the obstacles are added to the map array. When calculating the acceleration each entry in the map array is iterated through when summing the pushes from obstacles. The biggest challenge is finding the correct values for the constants within Fajen and Warren s control law, #obstacles φ = bφ k g φ ψ g e c 1d g + c 2 + k o φ ψ oi e c 3 φ ψ oi e c 4d oi Making b too large causes a large portion of the rotational velocity to always be cancelled out and causes the helicopter to have a large turning radius. If it is too small the helicopter will tend to swerve and turn more than necessary. The values k g and k o need to be balanced correctly so the pull of the goal does not cause the helicopter to fly through i=1
obstacles or the obstacles do not send the helicopter flying away from the goal. The values of c 1, c 2 and c 4 are directly related to how close the helicopter needs to be before the effects of the pushing and pulling are strongly felt. The most difficult part of these constants is that they must all be balanced against each other and changing on even a little means they all need to be changed. To make the algorithm safer for the helicopter in real life, my system also employs a test at every step as to whether the current heading and velocity is such that the minimum turning radius will crash the helicopter. If that is the case this portion of the code takes over and brings the helicopter to a halt. Conclusion: Over the course of this project I created an obstacle avoidance system that to a degree successfully navigates its way around an environment. The figure below (Fig. 1) shows three different paths between two objects to three different goals. It shows that the helicopter is able to navigate to goals while giving obstacles a sufficient berth and that it cancels out obstacles on either side very well. Fig. 1-3 paths to 3 goals.
Here is a second example (Fig. 2) that shows how the helicopter goes from goal to goal. Once a goal is reach it continues on to the next one while continuing to avoid obstacles. Fig. 2 - A path through 3 goals. Another example (Fig. 3) which shows a single path through a few objects exhibits an interesting issue. While a straight path from the start to the goal without hitting an obstacle is possible the helicopter still takes a curved path. This shows that while successful the paths chosen are not necessarily the most efficient. This may be made better through tweaking the constants or the lower efficiency may be traded for a safer path.
Fig. 3 - A path through 3 objects. Another issue is dealing with thin obstacles. While usually successful with thin obstacles, it does sometimes have trouble because the lower weight of these obstacles causes less push. This is exhibited in the example below (Fig. 4) where the helicopter cuts a close path to the obstacle on the right. Fig. 4 - A path with thin obstacles.
The biggest problem by far I had to overcome however was finding correct values for the constants. Changing one a little can throw off the path by a lot and it took me a good amount of time to find the constants that I am using. One of my first attempts sent the helicopter simply flying off into the opposite direction of everything. I solved this by increasing the pull of the goal. Another attempt saw the helicopter spiraling everywhere. I found this to be caused by the rotational velocity building over the course of the flight. I finally found a combination that worked but I am sure that this can be improved upon. In Fajen and Warren s experiments they record the movements of live subjects moving around real world obstacles. They then used that data to calculate constants. Unfortunately I do not have the resources are the ability to do this so I had to use a trial and error approach which means that while my constants work, they most likely can be improved upon with more trials. Inserting my obstacle avoider into the entire helicopter system is simple. Poses sent from the planner via inter-process communication are taken and placed into the linked list. If new poses are sent out the linked list is replaced with the new goals. When the sensors detect new obstacles they are put directly into the array of obstacles. If a current obstacle is found to be gone it is simply deleted. Then as the poses are computed they are output into a controller that converts them into the machine language used by the helicopter. Over the course of this project I have developed a system that allows autonomous helicopters to react to obstacles in a fast way. I created a system of cataloguing obstacles and recognizing goals. I implemented an algorithm that uses pushes and pulls from obstacles and goals to navigate and I found constants for this formula that sufficiently
avoid obstacles. It can still be improved and tweaked, but I have accomplished my main goal of creating a fast local planner for autonomous helicopters.
References: S. Scherer, S. Singh, L.J. Chamberlain, and S. Saripalli, Flying Fast and Low Among Obstacles, Proceedings International Conference on Robotics and Automation, April, 2007. This paper was published in 2007 at an IEEE robotics conference. It is about their implementation of an autonomous helicopter. It is the most similar work to our implementation. They use a similar method of using a global planner along with a system for obstacle avoidance. There implementation was very successful in an outdoor environment with sparsely placed obstacles. Our project needs to work in a more urban setting. R. Zapata and P. Lepinay, Flying among obstacles, in Workshop on Advanced Mobile Robots (Eurobot), Zurich, Switzerland, September 6-8 1999, pp. 81 88. This article was published in 1999 at a workshop in Zurich for robotics. It details problems involved with navigating robotic planes and submarines in threedimensional space. The most interesting thing from this paper is their use of vectors to represent all interactions between the robot and obstacles. Y. Kitamura, T. Tanaka, F. Kishino, and M.Yachida, Real-time path planning in a dynamic 3-d environment, in Proc. IEEE/RSJ International Conference on Intelligent Robots & Systems, 1996.
This paper was is older having been published in 1996 but it has some interesting ideas and deals with navigating environments that contain other moving objects. This is something our helicopter might have to deal with in an urban environment. D. Fox, W. Burgard, and S. Thrun, The Dynamic Window Approach to Collision Avoidance, IEEE Robotics & Automation Magazine, March 1997, pp. 23-33. This paper from 1997 was published by IEEE s own robotics magazine. It deals with the dynamic window approach of narrowing down the robots next movement to only possible locations where it can physically move too. This has the potential to reduce the amount of work down by the local planner. B. Fajen and W. Warren, Behavioral Dynamics of Steering, Obstacle Avoidance, and Route Selection, Journal of Experimental Psychology: Human Perception and Performance, vol. 29, no. 2, 2003. This paper from 2003 was actually published in a psychology journal and deals mostly with how beings move around objects. It also includes the Fajen and Warren Control Law as the mathematical formula for how beings move around obstacles. This provided a very strong base from which I built my system. M. Becker,.C Dantas, and W. Macedo, Obstacle Avoidance Procedure for Mobile Robots, ABCM Symposium in Mechatronics, vol. 2, 2006, pp. 250-257.
This 2006 paper was introduced at symposium and is an example of robots moving in a two dimensional space around moving and un-moving objects. Some of their key ideas are the use of Reachable Avoidance Velocities which combine the vectors from R. Zapata and P. Lepinay s paper with the Dynamic Window Approach.