ERTS: Embedded & Real Time System
Version: 0.0.1Date: December 19, 2008Purpose: A report on P545 project: Obstacle Avoidance. This document serves as report
for P545 class project on obstacle avoidanceStatus: CompleteAuthor: Yasir Ibrahim
Obstacle Avoidance Project: Final Report
Contents1 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 Inputs1.2.1.1 Communicating with other componentsThe 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 WaypointsThe 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 Outputs1.2.2.1 Heading controlThe 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 avoidanceThe component should detect, avoid, and remember obstacles in its path. This requirement is critical.
1.2.2.4 Speed controlThe component should control the vehicle’s speed.
1.3 Nonfunctional requirements (constraints)
1.3.1 Real time executionAll requirements should be carried out in real time. That is, it should reference system clock and process events accordingly.
1.3.2 Crash/interrupt recoveryThe 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 requirements1.3.3.1 Maximum speedThe component should not exceed track’s maximum speed at any segment
of it.
1.3.3.2 Faulty inputsThe component should detect faulty inputs from other components in the system and replace them with default values.
1.3.3.3 Staying in track boundaryThe component should detect vehicle’s position inside any track given by any two consecutive GPS waypoints.
1.3.4 PlatformThe 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 w1< w2< w3<… < ws; each element is a point in some coordinate. Assume there is some algorithm, algorithm 2, drives vehicle V from w1 to w2 to w3 … to ws to w1 and continues to go over W again. V is a point in the same coordinate and has a direction property Dv. If O is an obstacle being detected between wi and wj, where 1 ≤ j < i ≤ s, and O is in the path of V, we can compute the angle Do between V and O. Taking the difference between Dv and Do as follows:
Dn = Dv – Do, where Dn is the difference in degrees,
gives us an awareness of which side of V O is. If Dn 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 Dn we can compute a new point wo that is 90 degrees with distance e, some positive value, to the right of O, or left according to Dn, and pass it to algorithm 2 that will drive V towards the new wo. Only one condition should be applied on wo: it must be between wi and wj. Since W has partially ordered elements, wo must be also partially ordered and satisfy wi<wo<wj.
Once algorithm 2 successfully has driven V towards wo, we can consider that vehicle V avoided obstacle O. In addition we can insert wo into W to become a permanent element of W. Thus, the next time V comes between wi and wj, it finds wo and automatically avoids O.
6
Figure (1): Algorithm 1 during calculating the avoidance point
Figure (2): Algorithm 1 after has calculated the avoidance point7
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 wo 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 w1< w2< w3<… < ws; each element is a point in some coordinate. Define Lij as a straight line connects points wi and wj, where 1 ≤ j < i ≤ s. Lij has the distance property Dij 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 wj to wi, and some algorithm, algorithm 3, corrects V’s direction Dv
towards wi, we can compute the distance dwi as the distance from V to wi, and dwj as the distance from V to wj. Now, we can know whether V has come close enough near wi or not by computing the following: define e as a small value by which we can tell that V has reached wi. If dwi < e or dwj ≥ Dij, we can tell algorithm 3 to use wi+1 instead of wi to correct V’s direction towards it. By doing that, we are making V to move from wj to wi
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 wi as point in some coordinate. Define V as point in the same coordinate and has the property Dv as its direction towards wi. Also, define Dw as the direction from the origin of the coordinate to wi. Now, we can compute the difference De between Dv and Dw. Assume V has a property S which is proportional to De, and by setting S to some value then we change Dv in a way that guarantees De becomes smaller. To correct V’s direction towards wi (steer it), we need to make De smaller than some value e that guarantees V’s correct direction towards wi. We do that by setting V’s S property to a value computed as follows:
Sv = (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 w1< w2< w3<… < ws; each element is a point in some coordinate. Define two straight lines: Lji between two consecutive points wj
and wi, where 1 ≤ j < i ≤ s and j = i 1, and Lik between two consecutive points wi and wk, where i < k ≤ s and k = i +1. Define two angles: Dji as the angles formed between line Lji
and the y axis of the coordinate, and Dik and the angle formed between line Lik and the y axis of the coordinate. Now we can compute two points wp1 and wp2 around wi, where wp1
is at distance e1 form wi in the complement direction to Dji and wp2 is at e2 distance from wi in Dik direction, where e1 and e2 are positive values that guarantee wp1 and wp2
respectively form a smoother corner around wi. As W’s elements are defined to be partially ordered, the new points wp1 and wp2 have to be also partially ordered in W and satisfy wj < wp1 < wi and wi < wp2 < wk. 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 nonlinear 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
Figure (12): steering vehicle’s trajectory after moving in squares
5.2 Path following
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