function dobsdot = findDerDobs(robo)
dobsdot = [0;0];
global world; % world with obstacles
noOfObs = size(world,1);
% find der. to bounding sphere
% position of the bounding sphere
xBnd = world(1,1);
yBnd = world(1,2);
for i=1:noOfObs
xObs = world(i,1);
yObs = world(i,2);
dx = 1;
dy = 1;
if (i == 1)
xTop = -2 * ( robo(1) - xObs);
yTop = -2 * ( robo(2) - yObs);
for j=1:noOfObs
xObs = world(j,1);
yObs = world(j,2);
rObs = world(j,3);
if ( j ~= i)
if ( j == 1) % bounding sphere
dx = dx * (-1 * (findSqDistance(robo,[xObs yObs]))^2) + rObs^2;
dy = dy * (-1 * (findSqDistance(robo,[xObs yObs]))^2) + rObs^2;
else % other obstacles
dx = dx * ((findSqDistance(robo, [xObs yObs]))^2 - rObs^2);
dy = dy * ((findSqDistance(robo, [xObs yObs]))^2 - rObs^2);
end
end
end
else
xTop = 2 * ( robo(1) - xObs);
yTop = 2 * ( robo(2) - yObs);
for j=1:noOfObs
xObs = world(j,1);
yObs = world(j,2);
rObs = world(j,3);
if ( j ~= i)
if ( j == 1) % bounding sphere
dx = dx * (-1 * (findSqDistance(robo,[xObs yObs]))^2) + rObs^2;
dy = dy * (-1 * (findSqDistance(robo,[xObs yObs]))^2) + rObs^2;
else % other obstacles
dx = dx * ((findSqDistance(robo, [xObs yObs]))^2 - rObs^2);
dy = dy * ((findSqDistance(robo, [xObs yObs]))^2 - rObs^2);
end
end
end
end
xTop = xTop * dx;
yTop = yTop * dy;
dobsdot(1) = dobsdot(1) + xTop;
dobsdot(2) = dobsdot(2) + yTop;
end
end
potentials.zip_pageik1_巡航_群机器人
版权申诉
131 浏览量
2022-07-14
05:07:52
上传
评论
收藏 7KB ZIP 举报
potentials.zip （15个子文件） 
findObsDistance.m 527B findSqDistance.m 107B potEventFunc.m 377B navPotField.m 362B plotNavigation.m 463B navigationFunc.m 1KB navGrad.m 1KB navEventFunc.m 323B potPotField.m 1KB findDerDobs.m 2KB checkObstacle.m 358B checkBoundary.m 260B plotPotential.m 462B potGrad.m 2KB potentialFunc.m 1KB
评论0