IntroductionIn the article we will look at implementation of bug 2 algorithm for motion planning
Bug AlgorithmsThe aim of path planning algorithm is to complete a collision free path from initial to goal position. Bug algorithms are simplest type of path planning algorithms.
In Bug algorithms no global model of the world is assumed ,the location and shapes of the obstacles are unknown.Only information acquired through sensing is known.The Bug algorithms assume local knowledge of environment and a global goal.
The environment is a Two-dimensional scene filled with unknown obstacles. The perimeter of any obstacle is finite, and that the number of obstacles is finite and can be of arbitrary shape.
We consider a point robot that moves among arbitrary obstacles in a planar environment. We assume that the workspace is bounded.
The robot is employed with ideal localization module.The robot can always keep track of position of robot. Thus we can always measure the distance between the source and goal.
The robot is employed with tactile touch or a range sensor sensor.The touch sensor can detect contact with the object .
Sample workspace configurations are shown in .The red dot is robot center and green dot is the goal.
Bug algorithmThe robot is equipped with a range sensor.Range sensor with a small radius can be considered to be equivalent to a tactile sensor.
At any given point of time robot is assumed to be in one of following low level states Motion,Boundary Detection,Navigate Boundary,Done
MotionIn this mode the robot moves along a specified heading direction which is specified in terms of angle wrt global co-ordinate system.
At initialization we have information only about the source and the goal position. The shortest distance between two points is a straight line. The heading direction is along this line from source towards the goal.
The robot begins to move along the straight line towards the goal.
To specify displacement to robot to move along the x and y directions, the unit vectors along the heading direction are computed.Let denote the unit vectors
Then robot take a small displacement in the heading direction by taking a small steps proportional to components of unit vector along the x and y directions. Each time position is incremented the displacement is recorded Let us call this displacement .
Boundary DetectionThe robot is equipped with a range sensor.The sensor range is has a finite radius R.If a obstacle is encountered the range sensor gives the direction and distance to the object. Let is assume that direction resolution of sensor is 1 degree.
Thus if a range sensor detects some object lying within the radius R.The robot enters the boundary detection state.
Since range is limited.The robot cannot estimate if single or multiple obstacles are encountered.
It now enters the navigate and starts moving along the heading direction.
Navigate BoundaryNow the robot has to navigate along the boundary of the obstacle till some predefined criteria is satisfied.
Thus robot must change it heading direction.The direction along with shortest distance to the boundary recorded by the range sensor is determined.
The robot changes its heading along line perpendicular to direction along shortest distance to the boundary point.
In this stage the robot still can sense obstacle with its range and it determines the heading direction at each instant as direction perpendicular to direction along shortest distance to obstacle. It continues to move along the heading direction.
While moving along the heading direction a point is reached where no obstacles are encountered. This can happen if robot has reached a sharp edge or it has moved away from the boundary while navigating the boundary due to inaccuracies in determining the shortest distance along the boundary.
The new position of robot is determine on the same lines as that described in motion towards the goal section. At each instant the current heading direction and previous displacement information is stored.
We take a step back using and go back to the last position.And begin the move in a direction perpendicular to the current heading direction. This is done by remaining in the same state and changing the heading direction.
This will cause the robot either to move back to boundary or move along the edge.
In this state the robot will circumnavigate the boundary of the obstacle.
To move towards the goal,at some point based on some predefined criteria robot will leave the navigation state and enter the motion towards the goal state.
Different bug algorithms differ in the way they define transition from boundary following to motion towards goal.
Simulation EnvironmentA basic simulation environment is developed to test the algorithm. The input to the simulation environment is a image file containing the obstacles.Thus obstacles of any complexity can be included for testing.
The sensor also needs to be simulated.Thus given a location if a point lying along the circle with center at robot present location lies withing the obstacle needs to be determined.Also how far inside the obstacle the point lies determines the distance of the robot from the obstacle.
Simulation environment plots the obstacles,robot and goal positions, trajectory,heading direction,direction along which obstacles are detected. Debug information is also displayed along with plots.
OpenCV libraries are used for plotting .To determine if a point lies inside a polygon the function provided by opencv libraries are used.
There are two types of obstacles defined boundary and normal obstacles. The boundary obstacles just is boundary of simulation environment.
Thus for boundary obstacles the simulation env determine if the point on the circle lies outside the boundary while of other obstacles it checks if point lies inside the boundary.
SensorThe sensor simulation if performed by finding point that lie on the circle of sensor range .The test is performed if the point lies inside/outside a polygon obstacles or not.All the direction along circle are evaluated .The current heading direction is indexed 0.
Bug 2 AlgorithmBug Algorithms are greedy algorithms hence not predictible.
At the initialization the MLine is defined as the line joining the starting location and the goal.
Leave point is point at which robot transitions from boundary following to motion towards the goal states.
In the Bug 2 algorithm the leave point is a point on the MLine encountered after encountering hit point during boundary following.
Leave point is a point at we are able to leave the surface of obstace and have heading towards the goal.
The leave point is corresponding point on the bounday at the other side of obstacle that lies along the line joining the hit point and goal is called as leave point.
In some situations it may happen that Bug2 algorithm takes you farther away from the target, or it may be stuck in a loop and robot never reaches the target
to avoid such situations a constraint is placed that the leave point should be closer to the goal than the hit point.
Let the distance between source to goal be denoted by Let the perimeter of the obstacle be denoted by The shortest distance Robot will travel using bug2 algorithm is .
Another variation is to check if there are obstacles along the direction to the goal. This no obstacles are detected then change the state to motion towards the goal. The freeheading flag is set if there are no obstacles in the direction of the goal. However since the range/tactile sensor have limited range ,a free heading does not necessarily indicate a free heading towards the goal.
Additional constraints such that potential leave point must be close to the goal that the previous hit point and on heading angle are imposed to ensure boundary following is exited at proper position. if(h1==cur_heading && p.distance==-1) freeheading=true; pp=p; if(freeheading==true) //cv::waitKey(0); float distance1=FPoint::dist(hl,goal.position); float distance2=FPoint::dist(goal.position,pp.position); if(distance1>distance2 && heading
AnalysisWhen the obstale encounters the bounday it will move in clockwise and anti clockwise direction till it encounters the Mline again.Thus maximum distance it will traverse is
It may happen that robot intersects the same obstacle multiple times .