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 Non­functional 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   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

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