Obstacle Avoidance Project: Final Report

ERTS: Embedded & Real Time System Version: 0.0.1 Date: December 19, 2008 Purpose: A report on P545 project: Obstacle Avoidance. This document serves as report for P545 class project on obstacle avoidance Status: Complete Author: Yasir Ibrahim Obstacle Avoidance Project: Final Report Contents 1 Requirements...3 1.1 Introduction...3 1.2 Functional requirements...3 1.3 Non-functional requirements (constraints)...4 2 Design...5 2.1 The problem...5 2.2 Obstacle avoidance...6 2.2.1 Algorithm 1: Obstacle avoidance...6 2.2.2 Algorithm 2: Path follower...8 2.2.3 Algorithm 3: Steering...9 2.3 Path planning...9 2.3.1 Algorithm 4: path planning...10 2.4 Other designed features...12 2.4.1 Speed control...12 2.4.2 Collision Bubble concept...12 2.4.3 Obstacles averaging...14 3 Implementation...16 1

3.1 Python...16 3.2 SyncFS...16 3.3 Spheical coordinates...17 3.4 Distance to projections...17 3.5 Global variables...18 3.6 Implementation units...18 4 Code...20 5 Tests...21 5.1 Steering...21 5.2 Path following...23 5.3 Obstacle avoidance...24 5.4 Obstacle avoidance...24 References...26 Appendix...27 A Developer Instructions...27 B User Instructions...27 2

1 Requirements 1.1 Introduction The final implementation of this project is a component that is part of a larger system. The component satisfies some requirements collected during all phases of the development. In this section, requirements for the obstacle avoidance component are introduced. These requirements are critical parts for both of the design and implementation processes of the component. To achieve better solution, the implementation and the design should meet these requirements. The requirements listed in this section were collected from the requirements of P545 class project at the Computer Science department Indiana University at Bloomington, replacing customer requirements, and domain experts worked on the project; namely: the professor of the class and two assistants and design engineers. Also, some of the requirements were collected as a result of negotiations among students and the domain experts. We call all the participants in producing these requirements as stakeholders. 1.2 Functional requirements 1.2.1 Inputs 1.2.1.1 Communicating with other components The component should be able to communicate with the following peripherals and components: (1) System clock (2) Compass (3) GPS device (4) Range laser detector (5) Amplifier 1.2.1.2 Waypoints The component should be able to receive a list of GPS points considered as waypoints. These waypoints form a path for the vehicle to follow. 1.2.2 Outputs 1.2.2.1 Heading control The component should correct the heading of the vehicle towards any given GPS point. It should do that linearly. 1.2.2.2 Path following 3

The component should follow a predefined and best path connecting any number of GPS points, or waypoints. 1.2.2.3 Obstacle avoidance The component should detect, avoid, and remember obstacles in its path. This requirement is critical. 1.2.2.4 Speed control The component should control the vehicle s speed. 1.3 Non functional requirements (constraints) 1.3.1 Real time execution All requirements should be carried out in real time. That is, it should reference system clock and process events accordingly. 1.3.2 Crash/interrupt recovery The component should continue functioning after a recovery from system crash. The recovery is not part of the component, but the component should function on the current position of the vehicle and the current system s components configurations if the system recovers from crash or interrupt. 1.3.3 Safety requirements 1.3.3.1 Maximum speed The component should not exceed track s maximum speed at any segment of it. 1.3.3.2 Faulty inputs The component should detect faulty inputs from other components in the system and replace them with default values. 1.3.3.3 Staying in track boundary The component should detect vehicle s position inside any track given by any two consecutive GPS waypoints. 1.3.4 Platform The component should run on a platform that provides a synchronous file system. 4

2 Design 2.1 The problem The main aim of the project is obstacle avoidance. To design an efficient algorithm for that we should look at the problem as follows: the algorithm must address all the requirements mentioned in section 1 by figuring out the best path for the track and best path around obstacles appearing in front of the vehicle. That is, the problem is divided into two sub problems: (1) Obstacle avoidance (2) Path planning. Although both of these two sub problems can be viewed as the same thing, with the current configuration it is better to differentiate between them. To see why we should design our solution as two independent algorithms consider the following: The main mechanism of avoiding obstacles is, obviously, steering the vehicle away from them. To achieve this, obstacles must be detected early enough and beyond vehicle s stopping distance. This is the worst case for the algorithm; stopping vehicle if the algorithm will break any of the requirements such as not keeping the vehicle inside the track or hitting a previous obstacle. Failure to meet this constraint is definitely failure to avoid obstacles. Also, the algorithm should be aware of a predetermined side of the obstacle to turn away from that side. As we will see subsequently, this knowledge of a predetermined side of obstacles is required to address requirement 1.2.2.2 in section 1.2. That is, which side of the obstacle to dodge from is contributing to the concept of best path. The problem now is that current configurations do not allow the algorithm to detect obstacles beyond vehicle s stopping distance. Obviously, stopping distance is a lower boundary to the vehicle s turn radius trajectory. As both of vehicle s stopping distance and turn radius are proportional to its speed, they are also proportional to the steering angle of the wheels, vehicle s mass, wheels friction etc. At all speeds, stopping distance takes less time and distance for the algorithm to make an avoidance decision, thus it ensures that the vehicle will avoid the obstacle if it were detected beyond vehicles stopping distance. For that, the next sections address each of these two sub problems separately. 5

2.2 Obstacle avoidance 2.2.1 Algorithm 1: Obstacle avoidance Define W as a set of partially ordered elements w 1 < w 2 < w 3 < < w s ; each element is a point in some coordinate. Assume there is some algorithm, algorithm 2, drives vehicle V from w 1 to w 2 to w 3 to w s to w 1 and continues to go over W again. V is a point in the same coordinate and has a direction property D v. If O is an obstacle being detected between w i and w j, where 1 j < i s, and O is in the path of V, we can compute the angle D o between V and O. Taking the difference between D v and Do as follows: D n = D v D o, where D n is the difference in degrees, gives us an awareness of which side of V O is. If D n is a negative value then O is on the right of V. If Dn is positive value then O is on the left of V. Knowing D n we can compute a new point w o that is 90 degrees with distance e, some positive value, to the right of O, or left according to D n, and pass it to algorithm 2 that will drive V towards the new w o. Only one condition should be applied on w o : it must be between w i and w j. Since W has partially ordered elements, w o must be also partially ordered and satisfy w i <w o <w j. Once algorithm 2 successfully has driven V towards w o, we can consider that vehicle V avoided obstacle O. In addition we can insert w o into W to become a permanent element of W. Thus, the next time V comes between w i and w j, it finds w o and automatically avoids O. 6

Figure (1): Algorithm 1 during calculating the avoidance point Figure (2): Algorithm 1 after has calculated the avoidance point 7

This is the basic obstacle avoidance algorithm. It uses another algorithm, algorithm 2, to take care of driving the vehicle from one waypoint to another (e.g algorithm 2 allows the vehicle to follow the path). It takes advantage of the presence of algorithm 2 to: avoid obstacles and remember the avoidance way point w o in the future. Next is a description of algorithm 2. 2.2.2 Algorithm 2: Path follower Define W as a set of partially ordered elements w 1 < w 2 < w 3 < < w s ; each element is a point in some coordinate. Define L ij as a straight line connects points w i and w j, where 1 j < i s. L ij has the distance property D ij that gives us the distance between any two consecutive points, where 1 j < i s. Define vehicle V as point in the same coordinate. If V is moving from w j to w i, and some algorithm, algorithm 3, corrects V s direction D v towards w i, we can compute the distance d wi as the distance from V to w i, and d wj as the distance from V to w j. Now, we can know whether V has come close enough near w i or not by computing the following: define e as a small value by which we can tell that V has reached w i. If d wi < e or d wj D ij, we can tell algorithm 3 to use w i+1 instead of w i to correct V s direction towards it. By doing that, we are making V to move from w j to w i where j = i and i = i + 1. Figure (3): Algorithm 2 concepts 8

This is a basic path follower algorithm. It uses algorithm 3 to steer the vehicle towards its destination. Once the vehicle has reached its destination, we update algorithm 3 to use a new destination, which typically the next waypoint in our list. Next is a description of algorithm 3. 2.2.3 Algorithm 3: Steering Define w i as point in some coordinate. Define V as point in the same coordinate and has the property D v as its direction towards w i. Also, define D w as the direction from the origin of the coordinate to w i. Now, we can compute the difference De between D v and D w. Assume V has a property S which is proportional to D e, and by setting S to some value then we change D v in a way that guarantees De becomes smaller. To correct V s direction towards w i (steer it), we need to make De smaller than some value e that guarantees V s correct direction towards w i. We do that by setting V s S property to a value computed as follows: S v = (De*P), where P is the p gain of a PID control. According to the algorithm, it takes advantage of the p gain of the PID control. The algorithm has to work linearly to effectively steer the vehicle towards any given waypoint. By doing so, we we certain to some degree that the vehicle would steer to: follow the path according algorithm 2, and avoids obstacles according algorithm 1. Next we go to design a solution to the second sub problem of obstacle avoidance, which is path planning. 2.3 Path planning Path planning deals with planning a better path around a course that a vehicle is traveling in. Looking at algorithm 2 and algorithm 3 we can see that they are sufficient to navigate a vehicle in a predetermined path. That path might and might not be the best path a vehicle should follow. According to the specifications of the project s path, given requirement 1.2.1.2 in section 1 (which is receiving a list of waypoints to follow), the 9

path given to follow by this list is initially straight lines that connect waypoints. Each line can be thought of as a side of the course and has width. Half of this width is defined as the current side s LBO (Lateral Boundary Offset). Figure (5) gives a graphical visualization for this path. To plan a better path given this initial one, we have many options of algorithms to apply. One of these algorithms is just reducing the corner distances. That is, smoothening every corner made by three consecutive waypoints. We can do that by replacing each initial waypoint with two waypoints, one before and one after that waypoint. Here is a description of path planning algorithm 2.3.1 Algorithm 4: path planning Define W as a set of partially ordered elements w 1 < w 2 < w 3 < < w s ; each element is a point in some coordinate. Define two straight lines: L ji between two consecutive points w j and w i, where 1 j < i s and j = i 1, and L ik between two consecutive points w i and w k, where i < k s and k = i +1. Define two angles: D ji as the angles formed between line L ji and the y axis of the coordinate, and D ik and the angle formed between line L ik and the y axis of the coordinate. Now we can compute two points w p1 and w p2 around w i, where w p1 is at distance e 1 form w i in the complement direction to D ji and w p2 is at e 2 distance from w i in D ik direction, where e 1 and e 2 are positive values that guarantee w p1 and w p2 respectively form a smoother corner around w i. As W s elements are defined to be partially ordered, the new points w p1 and w p2 have to be also partially ordered in W and satisfy w j < w p1 < w i and w i < w p2 < w k. By repeating the algorithm for all W s elements, we are creating another set of points W that is partially ordered. Then we can use W on all of the algorithms described in this section that use W. 10

Figure (4): Algorithm 4 receives W as initial points set Figure (5): Algorithm 4 finished calculating W set of points. This would be the new set for all other algorithm. The is a better path around the course. It is a very generic algorithm 11

As mentioned before, this algorithm smoothens corners around the initial, or original, waypoints. There are other algorithms can plan a better paths, only as obstacle avoidance is the concern for this project, this algorithm suffices. 2.4 Other designed features Beside these algorithms, there are more two features, rather than algorithms, were designed to provide more reliable solution. These features are: (1) Speed control (2) staying inside corridor 2.4.1 Speed control The speed control is just to make sure that speed requirements are met. Also, it serves as a methodology to soften the other feature, staying inside corridor, consequences. If the vehicle is going outside the corridor, then it is either should slow down, turn hard to correct the direction, or stop. The stop control, or the deceleration, is a non linear function with speed. Thus, implementing speed control was easier to control the acceleration of the vehicle. If algorithm 1 detects obstacle or staying inside the corridor returns is negative (using the collision bubble concept), then the speed control lowers vehicle s acceleration to allow the steering control to have more time to bring the cart in stable state; e.g. avoided the obstacle or vehicle returned back inside the corridor. 2.4.2 Collision Bubble concept The collision bubble concept is quite useful in many aspects: (1) checking vehicle s position inside corridor. (2) Checking if obstacles are in the path of the vehicle. The concept is basically easy: We read vehicle s current speed, then we compute a point ahead of the vehicle. That point is a center to a circle with a radius that is half of the vehicle width. If any other point, obstacle or corridor edge is in the circle, then there is a possible collision. Algorithm 1 uses this concept to detect if obstacles are in the path of the vehicle. Also, speed control uses this concept to slow down the vehicle if the cart is in front of an obstacle or if the vehicle is going out. 12

The following figure shows how the collision bubble concept works. Figure (6): collision bubble computed ahead of vehicle s front. 13

Figure (7): collision bubble computed ahead of vehicle s front. The vehicle is going out, then slow down 2.4.3 Obstacles averaging If there are more than one obstacle in the path of the vehicle, then the following equation computes their average and reports on big obstacle instead of several one spanned over multiple clock cycles. The averaged obstacle AO is computed according to all obstacles in the path. 14

Figure (8): averaging multiple obstacles in 1 system clock 15

3 Implementation The implementation of the design introduced in the previous section is straight forward. All the aspects of the algorithms were implemented to very last detail. Although the code of the implementation is provided and is readable, some of the details are worth noting to make the code easier to understand. The next a few sections will detail how some concepts in the algorithms were implemented in computer environment; that is, programmed. As the reader may notice that the algorithms have some aspects and calculations carried out in similar ways. For example, the algorithms define sets of points in some coordinate and carry some calculation based on this definition. For real implementation, and looking at the nature of the problem, all of these coordinates are spherical coordinates. Therefore, all calculations are implemented on this basis. Before going into these details a note on the programming language that was used and the infrastructure of the component. 3.1 Python As part of the project specifications, the Python programming language was one of the options available to use. Due to Python s amazing OOP features and its agile prototyping ability, it was the best choice to use as the implementation language for our design. Also, Python language has a very clean and readable code structure that would illuminate the need for too much explanations and commenting. 3.2 SyncFS SyncFS is an amazing synchronous file system designed by the owners of the project. It uses P9 protocol architecture and does provide a synchronous communication across multiple components of the system. In our implementation, SyncFS was the way to communicate with the other components and sensors of the underlying system. Such components are: compass, laser detector, system clock, GPS device and others. SyncFS is built on the idea of mounting file systems on shared memory and organizes the access to the memory. Also, SyncFS is deployed on an x86 machine with Linux OS. For that, the whole implementation was under Linux OS. But that was not a problem at all since SyncFS has provided us a very obvious abstraction from the low level details of the hardware and system calls. 16

3.3 Spheical coordinates The coordinate on which the code was implemented is the spherical coordinate. That is because of the nature of the problem; vehicle motion on earth. The earth is a sphere, thus using a GPS system would make all distances between points and angles calculations correct. Calculations are guaranteed to be correct only to some degree. That is because several factors such as floating point arithmetic. For that and applying the OOP features of Python language, some modules were imported from third parties that do all spherical coordinates calculations we need during the implementation process. One such module is Geopy. This module provides a class called distance that has two functions our code replied on the most. These two functions are: distance.distance() function and distance.destination() function. Function distance.distance() has two properties: (1) kilometers: which computes the distance between two GPS points and returns kilometers. Of course that was changed to meters for our implementation. (2) forward_azimuth: calculates the forward angle formed between two GPS points and returns angles in degrees. Function distance.destination() calculates a GPS point given some GPS point, angle to for the desired GPS point, and the distance to it and it returns that calculated GPS point in tuple. 3.4 Distance to projections Some functions in our implementation relied on calculating the distance between some GPS point and its projection on the distance between other two GPS points. The following figure illustrates that. 17

Figure (9): Distance to projection concept 3.5 Global variables Several global variables were used in the code. Global variables are useful in two ways: (1) they provide a faster methodology for prototyping (2) they can be used as communication flags across multiple functions inside a class. For both reasons, global variables were used in to implementation of obstacle avoidance, but more to represent flags that can be set to True of False. 3.6 Implementation units The component was encapsulated in a class. The class has the following structure: init (): initializes global variables and provides the inputs to the class; e.g. waypoints lis. do_turn(): implements the steering functionality described in algorithm 3 follow_path(): implements the path follower algorithm; algorithm 2 18

which_side_of_obstacle(): determines at which side of the obstacle the vehicle is. synthesize_waypoint(): implements algorithm 1; obstacle avoidance. Also, it has a feature of detecting gates formed between obstacles. read_laser() : reads the range detector laser. It uses a flag to indicate obstacle detection. Other functions use this flag to function accordingly. is_obstacle_in_my_path(): check whether the detected obstacle by the read_laser() function is actually in the path of the vehicle. adjust_speed(): it implements the speed control. is_cart_going_out(): implements the bubble collision concept, and check if the vehicle is going out of the boundaries of the corridor. plan_path(): implements algorithm 5; path planning and smoothening corridor corners. process() : an infinite loop that runs continuously and ties all the parts of the different algorithms. run(): acts an intermediate function between the main function and process(). It calls process() in an infinite loop main (): runs the component 19

4 Code The code file is called jdriver.py. 20

5 Tests As part of the class, labs were carried out steadily. During the labs, heavy code tests were conducted. The tests served as tools to: (1) verify the requirements (2) validate the algorithms (3) analyze the output and redesign parts of the solutions. Here is some of the result of 4 different tests. 5.1 Steering Data collected to measure the vehicle response to steering command. Steering command is called turn radius inverse. The steering command s values are measured according algorithms 3. The following two figures show: the error in the direction mentioned in the algorithm, which the other shows the command s to steer. It is obvious that the command s values are the complements to the error to make the vehicle move towards the correct direction; to give zero error. Figure (10): error in the direction 21

Figure (11): steering command (turn radius inverse) values in response to the above errors The next figure shows how the steering controls the vehicle. The test was to make the vehicle to move in squares. Some latency in the response was due to the fact that the vehicle was sometimes moving from higher to lower grounds, thus affected the speed of the vehicle and then the turning behavior. 22

5.2 Path following Figure (12): steering vehicle s trajectory after moving in squares The following figures show how algorithm 2 worked in the implementation. The test was about making the vehicle follow a predefined path of GPS points. Figure (13): algorithm 3 in action (path following) 23

5.3 Obstacle avoidance The following figure shows how algorithm 1 avoided one static obstacle. The code used a slightly different algorithm as is was designed to read synthesized obstacles; not real obstacle by reading the range laser detector. That was for early development purposes. Figure (14): algorithm 1 in action. Avoiding one obstacle, steering, and path following. 5.4 Obstacle avoidance The following figure shows how algorithm 1 avoided multiple synthesized obstacles. This test was conducted to test all the algorithms at once: steering, path following, and obstacle avoidance. 24

Figure (15): algorithm 1 in action. Avoiding multiple obstacles, steering, and path following. 25

References [1] Geopy module, http://exogen.case.edu/projects/geopy/ 26

Appendix A Developer Instructions See inline comments in jdriver.py file. B User Instructions See README file accompanied jdriver.py file. 27