Obstacle avoidance using Occupancy grid

Hi,

I was trying to write a simple obsatcle avoidance code using an occupancy grid. My robot (a point) was suppose to do a random walk and take a 90-degree turn when it came close to a an obstacle. I do manage to create a random walk and an occupancy grid, although the obstacle avoidance loop doesn't work.

map = robotics.BinaryOccupancyGrid(20,20);
xy = [5 5; 3 2; 8 6; 10 15; 15 20; 9 8; 15 15];
setOccupancy(map,xy,1);
show(map);
hold on
grid on
maxSteps=20;
x = zeros(maxSteps);
y = zeros(maxSteps);
for i=1:20
    for j=1:20
        occval(i,j)= getOccupancy(map,[i j]);
        for stepNumber = 2: maxSteps
            if occval(i,j)==1
                x(stepNumber) = x(stepNumber-1);
                y(stepNumber) = y(stepNumber-1);
            else
                angle = 360 * rand;
                radius = 10*rand;
                x(stepNumber) = x(stepNumber-1)+radius * cosd(angle);
                y(stepNumber) = y(stepNumber-1)+radius * cosd(angle);
            end
        end
    end
end
plot(x,y)

Answer 1

[ag]

Hi Tanya,

I understand that you're trying to simulate a random walk with obstacle avoidance using an occupancy grid in MATLAB. However, your current approach has a few issues that need to be addressed to make the obstacle avoidance work correctly:

1. The nested loops over the grid cells (i and j) are not necessary for the random walk. The focus should be on the robot's current position and check for obstacles around it.
2. You need to check the occupancy of the grid at the robot's current position, and not at all positions in the grid.

Below is the modified code snippet:

map = robotics.BinaryOccupancyGrid(20, 20);
xy = [5 5; 3 2; 8 6; 10 15; 15 20; 9 8; 15 15];
setOccupancy(map, xy, 1);
show(map);
hold on;
grid on;

maxSteps = 200;
x = zeros(1, maxSteps);
y = zeros(1, maxSteps);

% Initial position of the robot
x(1) = 1;
y(1) = 1;

for stepNumber = 2:maxSteps
    % Move in the chosen direction
    radius = 1; % Step size
    angle = 360 * rand; % Randomly choose any direction
    x(stepNumber) = x(stepNumber-1) + radius * cosd(angle);
    y(stepNumber) = y(stepNumber-1) + radius * sind(angle);

    currDir = 0;
    % Check occupancy at the current position
    while getOccupancy(map, [x(stepNumber), y(stepNumber)]) == 1 && currDir < 5
        % If occupied, take a turn
        angle = 90 * currDir;
        x(stepNumber) = x(stepNumber-1) + radius * cosd(angle);
        y(stepNumber) = y(stepNumber-1) + radius * sind(angle);
        currDir = currDir + 1;
    end
    % If can't find an obstacle free next step, exit
    if currDir == 5
        x(stepNumber) = 0;
        y(stepNumber) = 0;
        break;
    end

    % Ensure the robot stays within the grid boundaries
    x(stepNumber) = max(1, min(x(stepNumber), 20));
    y(stepNumber) = max(1, min(y(stepNumber), 20));
end

plot(x, y, '-o');

Hope this helps!