©2020-2024 ioDraw All rights reserved
%------main主函数--------------------------------------------------
%------初始格式化--------------------------------------------------
clear all;
clc;
format bank;
%------定义全局变量----------------------------------------------
%-粒子群的-
global c1; %学习因子1
global c2; %学习因子2
global w; %惯性权重
global MaxDT; %最大迭代次数
global m; %搜索空间维数（未知数个数）
global N; %初始化群体个体数目
global eps; %设置精度(在已知最小值时候用)
global Kmax; %初始化x时用的最大迭代次数
global Qmax; %初始化x时粒子全部重新初始化用的最大迭代次数
global fitw1; %适应值函数中的两个权重
global fitw2;
global pathta ; %移动的角度为60度
global psosued; %粒子群成功
global pathsued; %路径可行
%-路径规划的-
global robotv; %机器人半径
global s; %起始点
global g; %目标点
global obstaclesx; %障碍物点
global obstaclesy;
global Nsteps; %机器人最多能移动的步数
global ploR; %极坐标半径
global segR; %极坐标的分段半径
global mapmin; % 地图大小
global mapmax;
global curpoint; %当前点的位置
global hadsteps; %只记录curpoint的集:链表一样
global curstep; %当前的步数
global rangOb; %在粒子范围内的障碍物点
global obIndex; %记录在粒子范围内的障碍物点的索引
global goalLine; %记录当前点和目标点之间连线的方程
global movelen; %机器人移动长度
global V; %机器人的速度(矢量)
global pointIndex; %记录点的索引
global pathpoint; %记录所有的点
global goalta %当前点到目标点的角度
global numta; %把圈分为几个角
global pamoveta; %粒子的可行角度，维数由numta定
global searchsued; %是否搜索成功
%-------初始化矩阵----------------------------------------------
initial;
%pause;
%-------开始寻找路径--------------------------------------------
result = pathplanning();
if result == 1
disp('找到路径');
hadsteps(curstep+1,1) = g(1);
hadsteps(curstep+1,2) = g(2);
else
disp('路径找不到');
end
%-最后处理-----------------------------
pointIndex = pointIndex+1;
pathpoint(pointIndex,1) = g(1);
pathpoint(pointIndex,2) = g(2);
pathpoint
%-------画---------------------------------------
figure(1)
hold on
%plot(pathpoint(:,1),pathpoint(:,2),'r-')
plot(hadsteps(:,1),hadsteps(:,2),'b-')
hold off
%plot(pathpoint);
D-122