Autonomous Robots Bug 2 algorithm with the e-puck mobile ...

Autonomous RobotsBug 2 algorithm with the e-puck mobile robot

Al Arafat,Abdullah Abdulaziz,Micheale HaderaApril 2015

MSc in VIBOT 2014/15

1. INTRODUCTION

Obstacle following is one of the most basic prob-lems in robot controlling and it constitutes an impor-tant step in higher level autonomous motion planning.It consists in obtaining information from the sensorsand, based on that acquired knowledge, move the robotin the environment in such a way that it can avoid ob-stacle collision. This problem can be solved using thebehavioral approach, which generates a specific motorresponse from a given perceptual stimulus.[1]

Bug 2 constitutes one of the Bug algorithms forpath planning, and despite its simplicity, but effective-ness, it can guarantee that the goal position will alwaysbe found. In this algorithm, the robot heads towards thegoal along the line that goes from the initial position tothe goal, called m-line. When it senses an obstacle onits way, it follows the obstacle until it meets the m-lineagain.

The objective of this lab work is to implement theBug 2 algorithm to go to the goal without crashing withobstacles. The algorithm exhibits two behaviors: headtoward goal behavior and obstacle following. In thislab work, we combined the obstacle following behav-ior of the robot with its head toward goal behavior.Andthe report is organized in following manners. In the nextsection, the Bug algorithm is described briefly. Depend-ing on the algorithm all the robot behavior is divided into some states. Those states are over viewed and ex-plained in details in the algorithm and implementationsection.Some conclusive remarks on the laboratory as-signment, e-puck robot and bug2 algorithm are mentionin the Conclusion.

2. Bug Algorithm

The Bug algorithms are a path finding algorithms,which simplifies a bug movement into some simpler al-gorithms, to be adaptable for sensor based robots.

• The first algorithm is Bug 0 algorithm. Here therobot starts moving towards the goal until obstacleis detected, once obstacle is found, it follow the ob-stacle wall until it can head toward the goal again(Figure 1 a). The draw back of Bug 0 algorithm isthat it is not robust for complex environment(map).

• In Bug 1 algorithm, where the robot do not stopfollowing the obstacle once direct path to goal isfound, rather it keeps following the obstacle, un-til it complete the circumnavigation of the wholeobject. Then it goes back to the point from wherethe distance to goal is the lowest and start headingtowards the goal again (Figure 1 b). Comparing toBug 0 algorithm, Bug 1 algorithm is advantageousbecause no matter whether we start left or right wecan reach the goal even for complex environment .

• The Bug 2 algorithm is another type of algorithmwhere it do not circumnavigate the whole objectrather first it computes a straight line between theinitial robot position and goal position called m-line. Then it starts moving towards the goal follow-ing that line, once obstacle is detected, it followsthe wall of the obstacle until the m-line is foundagain. Once found, it leaves the obstacle and fol-low the m-line until the goal is reached (Figure 2c).

3. Algorithm and Implementation

3.1. Heading towards the goal

The aim of this behavior is to approach the m-line and follow it until reaching the goal. However, ifan obstacle is encountered on the path, the robot willfollow the obstacle which is another behavior until itfinds again the m-line. While following the head to-wards goal behavior, the following 3 geometric condi-tions should have to be satisfied.

1

(a) (b) (c)

Figure 1: Illustration of Bug algorithms (a) Bug 0 (b)Bug 1 (c) Bug 2

• Distance to the goal: It is the Euclidian distancebetween the center of the robot (rx, ry) and the goalposition (gx, gy).

dgoal =√(gx− rx)2 +(gy− rx)2 (1)

• Distance to the m-line: It is the shortest Euclidiandistance between a point (center of robot (rx, ry))and the line connecting between the starting po-sition to the goal position.To do so we define them-line as line equation as follows:

ax+by+ c = 0a = sy−gy

b = gx− sx

c = sx ∗gy−gx ∗ sy

(2)

where (sx,sy) is the starting coordinate of therobot.

dmline =|arx +bry + c|√

a2 +b2(3)

• Delta: It is the angle between the x-axis of therobot and the goal position, see figure (2).

delta = tan−1(gy− ry

gx− rx)−θ (4)

Where θ is the angle between the horizontal andthe x-axis of the robot.The implementation for thethree geometry is described below

1 % E u c l i d e a n d i s t a n c e between t h e g o a l p o s i t i o nand t h e r o b o t p o s i t i o n

2 E u c l i d e a n d i s t = s q r t ( ( g o a l x − X) ˆ2 + ( g o a l y− Y) ˆ 2 ) ;

3 % Angula r d i s t a n c e between t h e X−a x i s o f t h er o b o t and t h e r o b o t−g o a l v e c t o r

4 D e l t a = wrapToPi ( a t a n 2 ( g o a l y − Y, g o a l x − X)− The ta ) ;

5 D e l t a d e g r e e s = D e l t a *180 / p i ;6

(a)

Figure 2: Geometric representation of the robot.

To control this behavior, the previous equations forδ and the distance to the goal, will be used. If deltais higher than a threshold angle (i.e. 20◦), we applythe maximum angular speed (60) and 0 linear speed inthe proper direction. Otherwise, we apply the angularspeed from maximum to 0 proportionally and constantlinear speed (300).

When the robot is sufficiently close to the goalpoint, the distance to the goal will be relatively small (2cm) and the robot stops considering that it has alreadyreached the point.

1 f u n c t i o n [ l i n e a r , a n g u l a r ] = Head2Goal ( g o a l x ,g o a l y )

2 g l o b a l go3 g l o b a l X4 g l o b a l Y5 g l o b a l The ta67 % E u c l i d e a n d i s t a n c e between t h e g o a l p o s i t i o n

and t h e r o b o t p o s i t i o n2

8 E u c l i d e a n d i s t = s q r t ( ( g o a l x − X) ˆ2 + ( g o a l y− Y) ˆ 2 ) ;

9 % Angula r d i s t a n c e between t h e X−a x i s o f t h er o b o t and t h e r o b o t−g o a l v e c t o r

10 D e l t a = wrapToPi ( a t a n 2 ( g o a l y − Y, g o a l x − X)− The ta ) ;

11 D e l t a d e g r e e s = D e l t a *180 / p i ;1213 i f D e l t a d e g r e e s > 2014 a n g u l a r = 6 0 ;15 l i n e a r = 0 ;16 e l s e i f D e l t a d e g r e e s < −2017 a n g u l a r = −60;18 l i n e a r = 0 ;19 e l s e20 a n g u l a r = 3 * D e l t a d e g r e e s ;21 l i n e a r = 300 ;22 end2324 i f E u c l i d e a n d i s t < 0 . 0 125 go = 0 ;26 end2728 end29

Finally the implemented code is tested with differentpositions and their trajectory results is shown in figure3.

3.2. Obstacle following

The robot can conduct an obstacle following be-havior using the high-level controller, that is, when itmeets an obstacle, it circumnavigates the object. Themain purpose is to keep the robot moving around the ob-stacle keeping almost a constant distance of 1 cm. Theobstacle following behavior is divided into two subse-quent steps :

• Perpendicular rotation: In this state, if the robotencounters an obstacle on its right side, then rotateleft until it becomes perpendicular to the obstacleand vice-versa. By default, if the robot encountersan obstacle in front of it then it would tend to goleft.

• circumnavigation: In this state, the robot wouldlikely circumnavigate the obstacle till it finds a m-line which is nearer to the goal but not the previoushit point.

3.2.1. Perpendicular rotation. Whenever the robotencounters an obstacle in front of it, the rotational be-havior mode gets activated. The goal of such rotationis to align the robot parallel to the obstacle, which thenswitch the robot to another state to circumnavigate theobstacle.For this operation, the direction of the obstacle appear-ance has been checked. Two front right side sensors

(a)

(b)

(c)

Figure 3: Trajectory results of head towards goal (a)goal (40,0) (b) goal (0,40) (c) goal(40,20)

3

(IR0 & IR1) have been used to measure the distance tocheck whether there is an obstacle on the right side ofthe robot. If it finds some obstacle it rotates till the (IR2)senses 1cm distance form the obstacle.And two of the front right side sensors (IR7 & IR6) havebeen used to check whether there is an obstacle on theright side of the robot. It will check the (IR5) sensormeasurement from the obstacle and continues rotatingtill the distance remains 1cm.The following figures show the strategy for perpendic-ular rotation.

Figure 4: Perpendicular rotation to right

Figure 5: Perpendicular rotation to left

Figure 6: Perpendicular rotation to straight

The MATLAB implementation is attached with thereport.

3.2.2. Circumnavigation. Once the robot is alignedto the obstacle, the robot shifts to circumnavigationstate. First, the front sensor (IR1) is used to keepconstant distance between the obstacle and the robot.If the sensor gives a reading less than a predefinedthreshold, it indicates that the robot is too close to theobstacle which will set the angular speed to 60◦ torotate the robot right. And if the robot is too far fromthe obstacle then the angular speed to -60◦ to rotate therobot left. Otherwise go straight with a linear speed120 and angular speed 0◦. This cases are also used forrotating at the corner of the obstacle.While the robot continues to circumnavigating theobstacle, the distance of the robot to the m-line hasalways been checked. If the distance is less than 2cm, the robot shifts away from this obstacle followingbehavior. Here, to avoid following the m-line from thepoint of start, or extension of m-line another conditionhas been used.The hit-points have been used to check the distanceto goal when it re-encounters the m-line, and if itis smaller than the distance to goal from the startpoint of obstacle following (with some threshold), theobstacle following phase terminates and the robot startspropagating through m-line.

4

The way of navigating the robot at the corner isshown in figure 7:

Figure 7: Circumnavigating at the corner

When there is no m-line the robot circumnavigates thewhole obstacle. The trajectory is given in figure 8:

Figure 8: Circumnavigating the obstacle with out m-line

3.3. The complete Bug 2

After implementing the Obstacle following andHead towards goal behaviors, we have combined themto complete the bug 2 algorithm. Always the robot startswith the behavior Head towards goal and when it en-counters an obstacle, the head towards goal behavior isdeactivated and the obstacle following behavior startsand the robot follows the obstacle. The condition thatterminates the obstacle following and trigger the headtowards behavior is the distance to m-line. while therobot is following the obstacle, if its distance betweenthe robot and the m-line is less than the threshold given,the obstacle following behavior stops and the head to-wards goal restarted. The results are shown in Figure 9

and 10 for different environments.

(a)

(b)

Figure 9: a)Environment with one obstacle b)trajectoryfollowing one obstacles,

5

(a)

(b)

Figure 10: a)Environment with two obstacleb)trajectory following two obstacles

4. Conclusions

Bug 2 algorithm is an efficient path planning al-gorithm that lets the robot find the goal position in thepresence of obstacles in a systematic way. This algo-rithm is better than the Bug 0 and Bug 1 algorithm. Ithas lesser chance to get trapped and better chance toreach the goal without losing much time by circumnav-igating whole object.

In this lab work we implement Bug 2 algorithm.The results show that the algorithm performs well underthe tested conditions. One drawback of the algorithm isdepending on the map it can perform less efficient, insuch scenario Bug 1 performs better than Bug 2.

References

[1] J. A. Oroko, and G.A Nyakoe, Obstacle Avoidance AndPath Planning Schemes for Autonomous Navigation of aMobile robots

[2] Buniyamin N., Wan Ngah W.A.J., Sariff N., MohamadZ. A Simple Local Path Planning Algorithm for Au-tonomous Mobile Robots

6