【路径规划】基于粒子群算法求解机器人动态路径规划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 movelenglobal searchsued; %是否搜索成功global robotv;global ploR;global hadsteps;global goalsteps;%------result false;%判断是否找到目标点了%%如果找到则searchsued为true%从维数中依此搜索路径%一个for循环for i1:numta%g(1)502*curstep;if g(1)100g(1)g(1)0.6;g(2)100;elseg(1)g(1);g(2)100;endgoalsteps(curstep,1)g(1);goalsteps(curstep,2)g(2);%求出在第几维中if distance(curpoint(1),curpoint(2),g(1),g(2))robotvsearchsued true;%判断当前点到目标点的距离是否小于传感器范围%%如果小于的话就重新设置机器人的移动距离else if distance(curpoint(1),curpoint(2),g(1),g(2))ploRmovelen distance(curpoint(1),curpoint(2),g(1),g(2));elsemovelen ploRendend%判断搜索是否成功成功的话就退出if searchsuedresult true;return;end%---接下来就是搜索主题了---%求出当前点与目标点的方程gaolta straightLine(curpoint(1),curpoint(2),g(1),g(2));%求出下一个点在第几维空间中dim ceil(gaolta/pathta)if mod(dimi-1,numta) 0dimtemp numta;elsedimtemp mod(dimi-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 curstep1;pamoveta(curstep,:) 1;%将下一段的维值tempta mod(numta/2dimtemp,numta);if tempta 0tempta 6;endpamoveta(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;endelsepamoveta(curstep,dimtemp) 0;%2表示已经走过了end%如果不能走通的话继续循环什么都不用做endendif inumta pamoveta(curstep,i)~1curstep curstep-1;end3 仿真结果4 参考文献[1]来羽. 基于粒子群算法的传感网络节点动态目标跟踪. 计算机仿真 3(2016):4.部分理论引用网络文献若有侵权联系博主删除。