?

%------main主函數(shù)--------------------------------------------------

%------初始格式化--------------------------------------------------

clear all;
clc;
format bank;

%------定義全局變量----------------------------------------------

%-粒子群的-
global c1; ? ? ? ? ? ? %學(xué)習(xí)因子1
global c2; ? ? ? ? ? ? %學(xué)習(xí)因子2
global w; ? ? ? ? ? ? ?%慣性權(quán)重
global MaxDT; ? ? ? ? ? ?%最大迭代次數(shù)
global m; ? ? ? ? ? ? ? ? ?%搜索空間維數(shù)（未知數(shù)個數(shù)）
global N; ? ? ? ? ? ? ? ? ?%初始化群體個體數(shù)目
global eps; ? ? ? ? ? %設(shè)置精度(在已知最小值時候用)
global Kmax; ? ? ? ? ?%初始化x時用的最大迭代次數(shù)
global Qmax; ? ? ? ? ?%初始化x時粒子全部重新初始化用的最大迭代次數(shù)
global fitw1; ? ? ? ? %適應(yīng)值函數(shù)中的兩個權(quán)重
global fitw2;
global pathta ; ? ? ? %移動的角度為60度
global psosued; ? ? ? ? %粒子群成功
global pathsued; ? ? ? ?%路徑可行

%-路徑規(guī)劃的-
global robotv; ? ? ? ? ?%機(jī)器人半徑
global s; ? ? ? ? ? ? ? %起始點(diǎn)
global g; ? ? ? ? ? ? ? %目標(biāo)點(diǎn)
global obstaclesx; ? ? ? %障礙物點(diǎn)
global obstaclesy;
global Nsteps; ? ? ? ? ?%機(jī)器人最多能移動的步數(shù)
global ploR; ? ? ? ? ? ?%極坐標(biāo)半徑
global segR; ? ? ? ? ? ?%極坐標(biāo)的分段半徑
global mapmin; ? ? ? ? % 地圖大小
global mapmax;
global curpoint; ? ? ? ?%當(dāng)前點(diǎn)的位置
global hadsteps; ? ? ? ?%只記錄curpoint的集:鏈表一樣
global curstep; ? ? ? ? %當(dāng)前的步數(shù)
global rangOb; ? ? ? ? ?%在粒子范圍內(nèi)的障礙物點(diǎn)
global obIndex; ? ? ? ? %記錄在粒子范圍內(nèi)的障礙物點(diǎn)的索引
global goalLine; ? ? ? ?%記錄當(dāng)前點(diǎn)和目標(biāo)點(diǎn)之間連線的方程
global movelen; ? ? ? ? %機(jī)器人移動長度
global V; ? ? ? ? ? ? ? %機(jī)器人的速度(矢量)

global pointIndex; ? ? ?%記錄點(diǎn)的索引
global pathpoint; ? ? ? %記錄所有的點(diǎn)
global goalta ? ? ? ? ? ? ? %當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的角度

global numta; ? ? ? ? ? %把圈分為幾個角
global pamoveta; ? ? ? ?%粒子的可行角度，維數(shù)由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

總結(jié)

以上是生活随笔為你收集整理的障碍物规避算法的MATLAB仿真的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。