【路径规划】基于粒子群算法求解机器人动态路径规划Matlab代码
1 简介
基于粒子群算法的移动机器人路径规划,通过建立目标函数,变换坐标等对环境建模,再引入粒子群优化算法,得到全局最优路径.MATLAB仿真结果显示,此方法可有效地解决空间作业机器人路径规划及避障问题.与传统遗传算法比,该法建模容易,计算快捷,可以在不同的障碍物环境下得到不同的优化轨迹
2 部分代码
%-----路径规划函数--------------------------------------
function result = pathplanning(void)
%---声明全局变量-------
global segR;
global V;
global curpoint;
global pathused;
global curstep;
global pamoveta;
global goalta;
global pathta;
global numta;
global g;
global movelen
global searchsued; %是否搜索成功
global robotv;
global ploR;
global hadsteps;
global goalsteps;
%------
result = false;
%判断是否找到目标点了
%%如果找到则searchsued为true
%从维数中依此搜索路径
%一个for循环
for i=1:numta
%g(1)=50+2*curstep;
if g(1)<100
g(1)=g(1)+0.6;
g(2)=100;
else
g(1)=g(1);
g(2)=100;
end
goalsteps(curstep,1)=g(1);
goalsteps(curstep,2)=g(2);
%求出在第几维中
if distance(curpoint(1),curpoint(2),g(1),g(2))<=robotv
searchsued = true;
%判断当前点到目标点的距离是否小于传感器范围
%%如果小于的话就重新设置机器人的移动距离
else if distance(curpoint(1),curpoint(2),g(1),g(2))<ploR
movelen = distance(curpoint(1),curpoint(2),g(1),g(2));
else
movelen = ploR
end
end
%判断搜索是否成功，成功的话就退出
if searchsued
result = true;
return;
end
%---接下来就是搜索主题了---
%求出当前点与目标点的方程
gaolta = straightLine(curpoint(1),curpoint(2),g(1),g(2));
%求出下一个点在第几维空间中
dim = ceil(gaolta/pathta)
if mod(dim+i-1,numta) == 0
dimtemp = numta;
else
dimtemp = mod(dim+i-1,numta);
end
%算出机器人的速度(矢量)
V(1) = movelen*cos(gaolta+(i-1)*pathta);
V(2) = movelen*sin(gaolta+(i-1)*pathta);
%求出下一个
nextpoint(1) = curpoint(1)+V(1);
nextpoint(2) = curpoint(2)+V(2);
%先判断这一维是否可以走通
if pamoveta(curstep,dimtemp) == 1
%判断在这一维中是否与目标点相连
%%如果相连的话
if Conn(curpoint(1),curpoint(2),nextpoint(1),nextpoint(2))
%将这一维的字赋值维2,然后初始化下一个节点
pamoveta(curstep,dimtemp) = 2;%2表示已经走过了
curstep = curstep+1;
pamoveta(curstep,:)= 1;
%将下一段的维值
tempta = mod(numta/2+dimtemp,numta);
if tempta == 0
tempta = 6;
end
pamoveta(curstep,tempta) = 3;%3表示来的路
%将走的点赋值给当前值，继续搜索
curpoint(1) = nextpoint(1);
curpoint(2) = nextpoint(2);
hadsteps(curstep,1) = curpoint(1);
hadsteps(curstep,2) = curpoint(2);
if pathplanning()
result = true;
return;
end
else
pamoveta(curstep,dimtemp) = 0;%2表示已经走过了
end
%如果不能走通的话，继续循环（什么都不用做）
end
end
if i==numta && pamoveta(curstep,i)~=1
curstep = curstep-1;
end
3 仿真结果
4 参考文献
[1]来羽. "基于粒子群算法的传感网络节点动态目标跟踪." 计算机仿真 3(2016):4.
部分理论引用网络文献，若有侵权联系博主删除。