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:
- 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.
- 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!
