Obstacle Avoidance

Published on January 2017 | Categories: Documents | Downloads: 29 | Comments: 0 | Views: 232
of 12
Download PDF   Embed   Report

Comments

Content

Vaughn College of Aeronautics and Technology
MCE420 - Mechatronics II
Mechatronics Engineering
Spring Semester 2015

Assignment Report #4
Obstacle Avoidance Algorithm

Prepared for:
Dr. Flavio Cabrera
By:
Anjali Dhobale

Place of Experiment: Room W160
Date of Written Report: 05.07.2015

2

TABLE

OF

CONTENTS

1 INTRODUCTION

3

2 PROBLEM FORMULATION

3

3 INTERACTIVE ROBOT CONTROL ALGORITHM

3

3.1 MAIN CODE
3.2 DRIVE MODULE
3.3 OBSTACLE AVOIDANCE MODULE

3
5
6

4 DESIGN RESULTS

6

5 CONCLUSIONS

6

6 APPENDIX – MATLAB SIMULATION SCRIPTS

9

3
1

INTRODUCTION

Obstacle   avoidance   is   the   process   of   satisfying   a   control   objective   without   colliding   into
obstacles. Obstacle avoidance should be written such that in the case of obstacles, the robot
makes appropriate motions around the obstacles while trying to achieve the goal. If there are no
obstacles, the robot should move directly towards the goal location
In this assignment, an algorithm for obstacle avoidance was developed using proximal control.
Previously, control of robot path and position was developed using virtual vehicle approach and
PID controllers. Building upon these basics, the robot has now been given sensor vision to view
and avoid the obstacle before the robot crashes into it. This obstacle avoidance algorithm was
simulated and tested in the MATLAB environment.
1

PROBLEM STATEMENT

In the previous assignment, we designed an algorithm that allows the robot to approach a specific
location chosen by the user. Our task now is to avoid the obstacles in the surroundings and
continue to reach the target goal. The main objectives of the obstacle avoidance algorithm is:
1. Import image of robot environment with obstacles.
2. Detect the obstacle using sensors located towards the heading direction of the robot.
3. Rotate the robot once obstacle is detected
4. Continue towards target once obstacle has been crossed.

4
2

INTERACTIVE ROBOT CONTROL ALGORITHM

Figure 1 - Obstacle Avoidance Flowchart

2.1

MAIN CODE

The main code launches the obstacle avoidance algorithm. It contains all the variables required
to set up the initial conditions and positions.
Firstly, we require an obstacle course to be imported into MATLAB environment. The Image
Acquisition toolbox enables the user to create a matrix, which contains numeric values of the
image. This image creates a visual environment in MATLAB graphics figure for the robot to
move around in. Every pixel of the image is converted into numeric values from 0-255. A value
of 255 indicates open white space and 0 indicates closed black space. This concept creates the
foundation for obstacle detection, discussed further in Drive module.
map=rgb2gray(imread('map2.jpg'));

Secondly, as shown in Figure 2, the matlab ginput command is used to invoke the user place the
robot and indicate target positions. The robot positions represent (xr,yr) and the target goal
positions represent (xg,yg). The robot body and target goal is drawn using the drawbot function.
In this function, a circular body is drawn around the selected points. These position co-ordinates
are then sent into the Drive Module.
[xr,yr] = ginput(1);
[xg,yg] = ginput(1);

5

Figure 2- User is prompted to place the robot and target position in MATLAB figure.

2.2

DRIVE MODULE

In the driving phase, the robot approaches the target using PID Controller. This algorithm is
extensively discussed in Assignment 2. A new modification involves the implementation of the
proximity sensors and obstacle detection.
The proximity sensors is programmed and drawn using the robot’s heading angle and circle
parametric equations. The three different angles are (1) robot’s heading, (2) 45 degrees left of the
robot heading and (3) 45 degrees right of robot heading. These angles are entered with a radius
of 4 units from the robot’s heading direction.
angs = [(psiR(t)-pi/4) psiR(t) (psiR(t)+pi/4)];

for i=1:1:3;
xSens(1,i) = xr(t) + 4*cos(angs(1,i));
ySens(1,i) = yr(t) + 4*sin(angs(1,i));

6

Figure 3- Robot in driving phase approaching the goal

An obstacle is detected using the co-ordinates of the robot and the image matrix. When the coordinates of the proximity sensors within the map environment match a numeric value of 0, it
indicates that a closed black space is present. When this occurs, the robot has successfully
detected the presence of an obstacle.
if(map(round(ySens(1,i)),round(xSens(1,i))) == 0)
disp(['Collision Detected at (' num2str(xSens(1,i))
',' num2str(ySens(1,i)) ') by sensor ' num2str(i)']);

During the driving phase, this robot is responsible for repeatedly checking three proximity
sensors in the front. If any obstacles are detected the robot stops, rotates 45 degrees in the
clockwise direction from the current position, and invokes the obstacle avoidance module.

7

5: Robot turns 45 degrees in the clockwise dir
Figure 4: Sensors detects edge of Figure
the obstacle

2.3

OBSTACLE AVOIDANCE MODULE

The obstacle avoidance module is responsible to guide the robot for moving forward and ignore
the target for the time being. This module is active only when any of the three proximity sensors
detect a closed black space in the map environment.

Figure 6 & 7– After turning 45 degrees, the robot moves forward along the edge of the obstacle. If it encounters an
obstacle a second time, the robot further turns 45 degrees and moves forward ignoring the target.

Implementing the robot to move forward is relatively easy. A new temporary goal is created 5
steps ahead of the robot’s current heading position.
xg1(t)=xr(t-1)+5*cos(psiR(t-1));
yg1(t)=yr(t-1)+5*sin(psiR(t-1));

8
When the robot reaches this new goal, the temporary goal is again moved forward 5 steps. This
process runs repeatedly until all the proximity sensors are clear. Once the sensors are free of
obstacles, this module is terminated and control returns to drive module.
if(map(round(ySens(1,i)),round(xSens(1,i))) ~= 0)
disp(['No collision anymore']);
break,
end

Therefore, the robot follows the edge of the obstacle until it points towards the target goal point
again and no obstacle are in front of the robot.

Figure 8 & 9 – Once all the sensors are clear, the robot returns to the drive module and continues to approach the
target from its current location.

3

CONCLUSION

Obstacle avoidance in mobile robots refers to a robot’s ability to autonomously adjust its path in
response to objects in its environment. It has been found that the algorithm developed in this
assignment, works best when the obstacle size is small. In other words, if the size of the obstacle
is much bigger, this algorithm becomes computationally expensive. This approach is similar to
the bug algorithm wherein the robot has only local knowledge of the environment and a global
target. Developing this obstacle avoidance algorithm has been a fun and challenging project.

9
4

APPENDIX – MATLAB SIMULATION SCRIPTS

%Project 3
%Obstacle Avoidance Behaviour of Robot
%Import Map, create robot and sensor to avoid obstacle
clear all
close all
clc
global v
%Initial Conditions
psiR=pi;
v=1.2; %velocity
%Import Map Environment
map=rgb2gray(imread('map2.jpg'));
imagesc(map), axis image on; colormap gray;
title('Click to specify robots starting & goal position');
%collect 2 input points for robot starting posn and goal position.
[xr,yr] = ginput(1);
[xg,yg] = ginput(1);
hold on
%Display robot and goal position for 0.1 seconds
title('Let the fun begin');
drawRobot(xr,yr,xg,yg, map);
pause (0.5)
flag=1;
while flag
[arrivalFlag,eye,xr,yr,psiR]=drive5(xr,yr,psiR,xg,yg,map);
if arrivalFlag ==1
flag=0;
end
end
flag=0;

function [arrivalFlag,eye,xr,yr,psiR]=drive5(xr,yr,psiR,xg,yg,map)
global omega
%Gains for PID Controller
k=1; kp=1; ki=1;
dt=1;
psiV=0;
t=1;
xSens = [ 0 0 0];

10
ySens = [0 0 0];
eye = [ 0 0 0];
flag1=1;
while flag1
t=t+1;
%PID Controller to approach the input mouseClick event
E(t) = (psiR(t-1) - psiV(t-1));
dE(t) = atan2(sin(E(t)),cos(E(t))); %To convert error domain to [180,180]
P(t) = kp*dE(t); %Proportional to error
I(t) = ki*cumsum(dE(t))*dt;%cumulative integral
pidControl(t)=P(t)*I(t) ;
omega = k*pidControl(t);
%Call ODE45 Function
[T,Y]=ode45(@x_prime,[0 1],[xr(t-1) yr(t-1) psiR(t-1)]);
xr(t)= Y(length(Y),1);
yr(t)= Y(length(Y),2);
psiR(t)=Y(length(Y),3);
psiV(t) = atan2((yg-yr(t)),(xg-xr(t)));
rho(t) = sqrt((xg-xr(t)).^2 + (yg-yr(t)).^2);
if rho(t)<1
disp('Reached Goal! YAYY!')
arrivalFlag=1;
break,
end
%function [eye] = sensor1(xr,yr,psiR,map)
%get a circle representing the robot's body
angs = [(psiR(t)-pi/4) psiR(t) (psiR(t)+pi/4)];
for i=1:1:3;
xSens(1,i) = xr(t) + 4*cos(angs(1,i));
ySens(1,i) = yr(t) + 4*sin(angs(1,i));
if(map(round(ySens(1,i)),round(xSens(1,i))) == 0)
%if so, return 1
eye(1,i) = 1;
disp(['Collision Detected at (' num2str(xSens(1,i))...
',' num2str(ySens(1,i)) ') by sensor '
num2str(i)']);

%
%
%

if ((eye(1,1) | eye(1,2) | eye(1,3))==1)
%if yes, check if eye 2 is blocked
if (eye(1,2)==1)
%if yes, change heading
psiR(t)=psiR(t)+pi/4;
avoidObstacle2( xr(t),yr(t),psiR(t),xg,yg,map)

11
%

else
%if no, eye2 is not blocked
avoidObstacle2( xr(t),yr(t),psiR(t),xg,yg,map)

else
%
%

end
else

return

eye(1,i)=0;
end
plot(xSens(1,i),ySens(1,i),'o');
pause(0.000001)
end

drawRobot(xr(t), yr(t), xg,yg, map);
hold on
pause(0.000001)
end
flag1=0;

function [arrivalFlag]=avoidObstacle2(xr,yr,psiR,xg,yg,map)
global omega
%Gains for PID Controller
k=1; kp=1; ki=1;
dt=1;
psiV=0;
t=1;
flag2=1
while flag2
t=t+1;
xg1(t)=xr(t-1)+5*cos(psiR(t-1));
yg1(t)=yr(t-1)+5*sin(psiR(t-1));
%PID Controller to approach the input mouseClick event
E(t) = (psiR(t-1) - psiV(t-1));
dE(t) = atan2(sin(E(t)),cos(E(t))); %To convert error domain to [180,180]
P(t) = kp*dE(t); %Proportional to error
I(t) = ki*cumsum(dE(t))*dt;%cumulative integral
pidControl(t)=P(t)*I(t) ;

12
omega = k*pidControl(t);
%Call ODE45 Function
[T,Y]=ode45(@x_prime,[0 1],[xr(t-1) yr(t-1) psiR(t-1)]);
xr(t)= Y(length(Y),1);
yr(t)= Y(length(Y),2);
psiR(t)=Y(length(Y),3);
psiV(t) = atan2((yg1(t)-yr(t)),(xg1(t)-xr(t)));
rho(t) = sqrt((xg1(t)-xr(t)).^2 + (yg1(t)-yr(t)).^2);
angs = [(psiR(t)-pi/4) psiR(t) (psiR(t)+pi/4)];
for i=1:1:3;
xSens(1,i) = xr(t) + 4*cos(angs(1,i));
ySens(1,i) = yr(t) + 4*sin(angs(1,i));

end

if(map(round(ySens(1,i)),round(xSens(1,i))) ~= 0)
%if so, return 1
eye(1,i) = 1;
disp(['No collision anymore']);
flag2=0;
break,
end
plot(xSens(1,i),ySens(1,i),'o');
pause(0.000001)

drawRobot(xr(t), yr(t), xg,yg, map);
hold on
pause(0.000001)
end
flag2=0;

function prime = x_prime(T,Y);
global v omega;
prime = zeros(3,1);
prime(1)=v*cos(Y(3));
prime(2)=v*sin(Y(3));
prime(3)=omega;

Sponsor Documents

Or use your account on DocShare.tips

Hide

Forgot your password?

Or register your new account on DocShare.tips

Hide

Lost your password? Please enter your email address. You will receive a link to create a new password.

Back to log-in

Close