map=int16(im2bw(imread('map1.bmp'))); % input map read from a bmp file. for new maps write the file name here
source=[50 50]; % source position in Y, X format
goal=[450 450]; % goal position in Y, X format
robotDirection=pi/8; % initial heading direction
robotSize=[10 10]; %length and breadth
robotSpeed=10; % arbitrary units
maxRobotSpeed=10; % arbitrary units
S=10; % safety distance
distanceThreshold=30; % a threshold distace. points within this threshold can be taken as same.
maxAcceleration=10; % maximum speed change per unit time
maxTurn=10*pi/180; % potential outputs to turn are restriect to -60 and 60 degrees.
k=3; % degree of calculating potential
attractivePotentialScaling=300000; % scaling factor for attractive potential
repulsivePotentialScaling=300000; % scaling factor for repulsive potential
minAttractivePotential=0.5; % minimum attractive potential at any point
%%%%% parameters end here %%%%%
currentPosition=source; % position of the centre of the robot
currentDirection=robotDirection; % direction of orientation of the robot
robotHalfDiagonalDistance=((robotSize(1)/2)^2+(robotSize(2)/2)^2)^0.5; % used for distance calculations
pathFound=false; % has goal been reached
pathCost=0;
t=1;
imshow(map==1);
rectangle('position',[1 1 size(map)-1],'edgecolor','k')
pathLength=0;
if ~plotRobot(currentPosition,currentDirection,map,robotHalfDiagonalDistance)
error('source lies on an obstacle or outside map');
end
M(t)=getframe;
t=t+1;
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
tic;
while ~pathFound
% calculate distance from obstacle at front
i=robotSize(1)/2+1;
while true
x=int16(currentPosition+i*[sin(currentDirection) cos(currentDirection)]);
if ~feasiblePoint(x,map), break; end
i=i+1;
end
distanceFront=i-robotSize(1)/2; % robotSize(1)/2 distance included in i was inside the robot body
% calculate distance from obstacle at left
i=robotSize(2)/2+1;
while true
x=int16(currentPosition+i*[sin(currentDirection-pi/2) cos(currentDirection-pi/2)]);
if ~feasiblePoint(x,map), break; end
i=i+1;
end
distanceLeft=i-robotSize(2)/2;
% calculate distance from obstacle at right
i=robotSize(2)/2+1;
while true
x=int16(currentPosition+i*[sin(currentDirection+pi/2) cos(currentDirection+pi/2)]);
if ~feasiblePoint(x,map), break; end
i=i+1;
end
distanceRight=i-robotSize(2)/2;
% calculate distance from obstacle at front-left diagonal
i=robotHalfDiagonalDistance+1;
while true
x=int16(currentPosition+i*[sin(currentDirection-pi/4) cos(currentDirection-pi/4)]);
if ~feasiblePoint(x,map), break; end
i=i+1;
end
distanceFrontLeftDiagonal=i-robotHalfDiagonalDistance;
% calculate distance from obstacle at front-right diagonal
i=robotHalfDiagonalDistance+1;
while true
x=int16(currentPosition+i*[sin(currentDirection+pi/4) cos(currentDirection+pi/4)]);
if ~feasiblePoint(x,map), break; end
i=i+1;
end
distanceFrontRightDiagonal=i-robotHalfDiagonalDistance;
% calculate angle from goal
angleGoal=atan2(goal(1)-currentPosition(1),goal(2)-currentPosition(2));
% calculate diatnce from goal
distanceGoal=( sqrt(sum((currentPosition-goal).^2)) );
if distanceGoal<distanceThreshold, pathFound=true; end
% compute potentials
repulsivePotential=(1.0/distanceFront)^k*[sin(currentDirection) cos(currentDirection)] + ...
(1.0/distanceLeft)^k*[sin(currentDirection-pi/2) cos(currentDirection-pi/2)] + ...
(1.0/distanceRight)^k*[sin(currentDirection+pi/2) cos(currentDirection+pi/2)] + ...
(1.0/distanceFrontLeftDiagonal)^k*[sin(currentDirection-pi/4) cos(currentDirection-pi/4)] + ...
(1.0/distanceFrontRightDiagonal)^k*[sin(currentDirection+pi/4) cos(currentDirection+pi/4)];
attractivePotential=max([(1.0/distanceGoal)^k*attractivePotentialScaling minAttractivePotential])*[sin(angleGoal) cos(angleGoal)];
totalPotential=attractivePotential-repulsivePotentialScaling*repulsivePotential;
% perform steer
preferredSteer=atan2(robotSpeed*sin(currentDirection)+totalPotential(1),robotSpeed*cos(currentDirection)+totalPotential(2))-currentDirection;
while preferredSteer>pi, preferredSteer=preferredSteer-2*pi; end % check to get the angle between -pi and pi
while preferredSteer<-pi, preferredSteer=preferredSteer+2*pi; end % check to get the angle between -pi and pi
preferredSteer=min([maxTurn preferredSteer]);
preferredSteer=max([-maxTurn preferredSteer]);
currentDirection=currentDirection+preferredSteer;
% setting the speed based on vehicle acceleration and speed limits. the vehicle cannot move backwards.
preferredSpeed=sqrt(sum((robotSpeed*[sin(currentDirection) cos(currentDirection)] + totalPotential).^2));
preferredSpeed=min([robotSpeed+maxAcceleration preferredSpeed]);
robotSpeed=max([robotSpeed-maxAcceleration preferredSpeed]);
robotSpeed=min([robotSpeed maxRobotSpeed]);
robotSpeed=max([robotSpeed 0]);
if robotSpeed==0, error('robot had to stop to avoid collission'); end
% calculating new position based on steer and speed
newPosition=currentPosition+robotSpeed*[sin(currentDirection) cos(currentDirection)];
pathCost=pathCost+distanceCost(newPosition,currentPosition);
currentPosition=newPosition;
if ~feasiblePoint(int16(currentPosition),map), error('collission recorded'); end
% plotting robot
if ~plotRobot(currentPosition,currentDirection,map,robotHalfDiagonalDistance)
error('collission recorded');
end
M(t)=getframe;t=t+1;
end
fprintf('processing time=%d \nPath Length=%d \n\n', toc,pathCost);
没有合适的资源?快使用搜索试试~ 我知道了~
基于Matlab实现potential算法路径规划仿真(源码+图片).rar
共10个文件
bmp:5个
m:4个
db:1个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
5星 · 超过95%的资源 1 下载量 41 浏览量
2023-05-08
11:12:13
上传
评论
收藏 59KB RAR 举报
温馨提示
1、资源内容:基于Matlab实现potential算法路径规划仿真(源码+图片).rar 2、适用人群:计算机,电子信息工程、数学等专业的学习者,作为“参考资料”参考学习使用。 3、解压说明:本资源需要电脑端使用WinRAR、7zip等解压工具进行解压,没有解压工具的自行百度下载即可。 4、免责声明:本资源作为“参考资料”而不是“定制需求”,代码只能作为参考,不能完全复制照搬。不一定能够满足所有人的需求,需要有一定的基础能够看懂代码,能够自行调试代码并解决报错,能够自行添加功能修改代码。由于作者大厂工作较忙,不提供答疑服务,如不存在资源缺失问题概不负责,谢谢理解。
资源推荐
资源详情
资源评论
收起资源包目录
基于Matlab实现potential算法路径规划仿真(源码+图片).rar (10个子文件)
基于Matlab实现potential算法路径规划仿真(源码+图片)
Thumbs.db 96KB
map4.bmp 245KB
map2.bmp 245KB
map5.bmp 245KB
distanceCost.m 52B
map3.bmp 245KB
plotRobot.m 874B
map1.bmp 245KB
feasiblePoint.m 246B
astart.m 6KB
共 10 条
- 1
资源评论
- Hqinqin2024-01-15果断支持这个资源,资源解决了当前遇到的问题,给了新的灵感,感谢分享~
Matlab仿真实验室
- 粉丝: 2w+
- 资源: 2180
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功