Obstacle Avoidance
Control of Mobile Robots: Programming & Simulation Week 4
Jean-Pierre de la Croix
ECE Ph.D. Candidate
Georgia Inst. of Technology
Overview
The purpose of this weeks programming assignment is to
avoid any obstacles near the robot.
1. Transform the IR distance to a point in the robots coordinate frame.
2. Transform this point from the robots coordinate frame to the world
coordinate frame.
3. Compute a vector that points away from any obstacles.
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Coordinate Frames
Robots coordinates are in
the world frame.
( x, y, )
World Frame
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Coordinate Frames
Robot Frame
( xs , y s 4 , s 4 )
4
Sensors coordinates are in
the robots frame (centered
at robot with robots
orientation).
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Coordinate Frames
d 4
0
IR distance is defined in a
sensors frame (centered at
sensor, with sensors
orientation)
Sensor is Frame
World Frame
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Rotation and Translation in 2D
cos( ' ) sin( ' )
R( x ' , y ' , ' ) sin( ' ) cos( ' )
0
0
x'
y'
1
xdi
d i
y R ( x, y , ) R ( x , y , ) 0
si
si
si
di
1
1
5
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Obstacle Avoidance
We will implement the obstacle avoidance in a new
controller.
+simiam/+controller/AvoidObstacles.m
classdef AvoidObstacles < simiam.controller.Controller
%% AVOIDOBSTACLES steers the robot away from
% any nearby obstacles (i.e., towards free space)
s
6
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Transforming the IR distances
function ir_distances_wf = apply_sensor_geometry(obj, ir_distances,
state_estimate)
% 1. Apply the transformation to robot frame.
ir_distances_rf = (3,numel(ir_distances));
for i=1:numel(ir_distances)
x_s = obj.sensor_placement(1,i);
y_s = obj.sensor_placement(2,i);
theta_s = obj.sensor_placement(3,i);
%% START CODE BLOCK %%
R = obj.get_transformation_matrix(0,0,0);
ir_distances_rf(:,i) = zeros(3,1);
%% END CODE BLOCK %%
end
7
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Transforming the IR distances
% 2. Apply the transformation to world frame.
[x,y,theta] = state_estimate.unpack();
%% START CODE BLOCK %%
R = obj.get_transformation_matrix(0,0,0);
ir_distances_wf = zeros(3,9);
%% END CODE BLOCK %%
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Summing up Vectors
Sum up the vectors from
robot to the transformed IR
distances (u_i).
PID controller steers robot
to the orientation
(theta_ao) of this vector,
u_ao.
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Computing u_ao and theta_ao
%% START CODE BLOCK %%
% 3. Compute the heading vector
sensor_gains = [1 1 1 1 1];
u_i = zeros(2,5);
u_ao = sum(u_i,2);
% Compute the heading and error for the PID controller
theta_ao = 0;
e_k = 0;
%% END CODE BLOCK %%
10
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Testing
Robot should wander aimlessly around the world without
colliding with any obstacle or the walls.
+simiam/+controller/+quickbot/QBSupervisor.m
function obj = QBSupervisor()
%% SUPERVISOR Constructor
obj = obj@simiam.controller.Supervisor();
[ ]
obj.v = 0.1;
obj.goal = [-1,1];
11
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
Tips
12
Refer to the section for Week 4 in the manual for more
details!
Keep in mind that this hard work will pay off next week!
Jean-Pierre de la Croix - Control of Mobile Robots: Programming & Simulation Week 4
