【路径规划】基于matlab蚁群算法栅格地图路径规划及避障【含Matlab源码 2088期】
一、蟻群算法及柵格地圖簡(jiǎn)介
隨著機(jī)器人技術(shù)在諸多領(lǐng)域的應(yīng)用, 如機(jī)器人協(xié)作焊接、災(zāi)后搜救、軍事、太空探索、深?？碧?、家用和服務(wù)行業(yè)等, 機(jī)器人的發(fā)展正向智能化方向延伸, 要求其具有自組織、自學(xué)習(xí)、自適應(yīng)等能力.機(jī)器人路徑規(guī)劃是指機(jī)器人從初始位置按某種法則避開障礙物、無(wú)碰撞地到達(dá)目標(biāo)位置.目前國(guó)內(nèi)外對(duì)路徑規(guī)劃的研究應(yīng)用較多的方法主要有:人工勢(shì)場(chǎng)法、概率路徑圖法[3]、可視圖法、柵格法、神經(jīng)網(wǎng)絡(luò)算法、遺傳算法、粒子群算法、蟻群算法.
柵格法是將機(jī)器人的搜索空間分解成若干個(gè)規(guī)格相等單元, 將復(fù)雜的環(huán)境問(wèn)題分解成簡(jiǎn)單問(wèn)題, 適用于靜態(tài)環(huán)境的路徑規(guī)劃, 且算法計(jì)算量小, 便于實(shí)現(xiàn), 但在復(fù)雜的環(huán)境中, 易使規(guī)劃時(shí)間延長(zhǎng), 系統(tǒng)的實(shí)時(shí)性不夠.蟻群算法是一種新型的仿生算法, 以螞蟻覓食為模型, 通過(guò)前代螞蟻在走過(guò)的路徑上遺留信息素的強(qiáng)弱選擇路徑.該算法有較好的正反饋性、并行性及魯棒性;但當(dāng)遇到復(fù)雜問(wèn)題時(shí), 會(huì)導(dǎo)致搜索時(shí)間長(zhǎng)、陷入局部最優(yōu)、停滯和死鎖等情況.因此, 結(jié)合柵格法和蟻群算法的優(yōu)缺點(diǎn), 將柵格法與蟻群算法相結(jié)合進(jìn)行路徑規(guī)劃, 先建立柵格地圖, 再利用蟻群算法進(jìn)行全局搜索, 可提高算法性能.
1 柵格模型建立
1.1 柵格法應(yīng)用于路徑規(guī)劃的簡(jiǎn)介
柵格法是由W.E.Howden于1968年提出, 主要是根據(jù)環(huán)境建立一個(gè)路徑柵格地圖 (map) .基本原理是將機(jī)器人工作環(huán)境分割成無(wú)數(shù)細(xì)小的具有二值信息的網(wǎng)格單元, 每個(gè)網(wǎng)格的規(guī)格由機(jī)器人的步長(zhǎng)決定, 即一個(gè)步長(zhǎng)代表一個(gè)網(wǎng)格大小.在進(jìn)行網(wǎng)格劃分時(shí), 無(wú)論是障礙物柵格還是非障礙物柵格不滿一個(gè)時(shí), 將其填滿, 按一個(gè)柵格計(jì)算.
環(huán)境信息由黑白網(wǎng)格表示.黑色網(wǎng)格代表障礙物 (barrier) , 表示不可行區(qū)域;白色網(wǎng)格代表可通行區(qū)域, 又稱自由區(qū)域.柵格法將不可行區(qū)域和自由區(qū)域用一個(gè)二進(jìn)制矩陣表示, 矩陣中1代表障礙物, 0代表自由柵格, 由此在環(huán)境中建立一個(gè)可描述環(huán)境的路徑規(guī)劃地圖.
1.2 柵格地圖的建立
假設(shè)SP為機(jī)器人在二維空間中的一個(gè)規(guī)則的凸多邊形運(yùn)動(dòng)場(chǎng)地, 將場(chǎng)地分解成M×N個(gè)柵格, 由自由柵格和障礙物柵格組成, 其運(yùn)動(dòng)方式主要為八叉樹型形式.自由柵格的集合P={P1, P2, …, Pm}, 障礙物柵格的集合B={B1, B2, …, Bn}, 設(shè)A為機(jī)器人工作場(chǎng)地的柵格集合, 其表達(dá)式為A=P∪B.
本文根據(jù)實(shí)驗(yàn)場(chǎng)地建立一個(gè)10×10柵格地圖, 如圖1所示.圖中柵格的序號(hào)集合C={1, 2, 3, …, 100}.假設(shè)1號(hào)位為初始位置Gstart, 100號(hào)位為目標(biāo)位置Ggoal, 機(jī)器人從初始位置通過(guò)n次迭代搜索找到最優(yōu)路徑, 其中初始位置Gstart∈A且, 目標(biāo)位置Ggoal∈A且, 規(guī)定初始位置與目標(biāo)位置不重合, 在進(jìn)行路徑搜索時(shí)主要以八叉樹型形式搜索.
2 蟻群算法的路徑規(guī)劃問(wèn)題描述
2.1 基本蟻群算法的描述
蟻群算法是一種模仿蟻群覓食的仿生學(xué)算法, 其基本原理是將每臺(tái)機(jī)器人看作螞蟻群體中的一只螞蟻.螞蟻在進(jìn)行路徑搜索時(shí)通過(guò)蟻群在路徑上遺留的信息素強(qiáng)度, 向其它螞蟻傳遞信息, 實(shí)現(xiàn)機(jī)器人之間的信息交換.通常情況下路徑是未知的, 螞蟻在選擇路徑時(shí)一般根據(jù)概率Pkij (t) 隨機(jī)選擇.
其中:α為啟發(fā)式因子;β是期望啟發(fā)式因子;τij為機(jī)器人k從位置i到位置j這段路徑上所留信息素強(qiáng)度;allowedk表示機(jī)器人未訪問(wèn)過(guò)的柵格的集合;ηij (t) 為啟發(fā)式函數(shù), 啟發(fā)式函數(shù)的大小與i、j之間的距離有關(guān), dij值越小, i, j之間的關(guān)系越親密, 反之則疏遠(yuǎn), 其表達(dá)式為
式中 (xi, yi) 為點(diǎn)的位置坐標(biāo), (xj, yj) 為點(diǎn)的位置坐標(biāo).
2.2 螞蟻信息素更新
機(jī)器人在路徑搜索的過(guò)程中會(huì)在路徑上留下新的信息素, 隨著時(shí)間推移留在路徑上的信息素強(qiáng)度不斷增加, 為避免在搜索路徑上殘留的信息素過(guò)多而淹沒(méi)啟發(fā)式信息, 因此, 當(dāng)螞蟻完成一次搜索時(shí), 對(duì)所有路徑上的信息素進(jìn)行一次更新, 其表達(dá)式為
其中:1-ρ表示信息素殘留因子, ρ表示信息素?fù)]發(fā)系數(shù);Δτij (t) 表示螞蟻在本次循環(huán)中路徑 (i, j) 上的信息素增量, Δτkij (t) 表示第k只螞蟻經(jīng)過(guò)路徑 (i, j) 時(shí)在本次循環(huán)中的信息素增加量.根據(jù)信息素規(guī)則, 選擇蟻周 (ant-cycle) 模型作為螞蟻信息素更新模型.
式中, Q表示信息素強(qiáng)度, Lk表示第k只螞蟻在本次循環(huán)中所有路徑的總長(zhǎng)度.
二、部分源代碼
clc
clear all
close all
% tic
G=[0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 1 1 1 0 0;
0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 1 1 0 0;
0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 1 1 0 0;
0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0;
0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;
0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;
1 1 1 1 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;
1 1 1 1 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 1 1 0;
0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 1 1 0;
0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;];
MM=size(G,1); % G 地形圖為01矩陣，如果為1表示障礙物
Tau=ones(MMMM,MMMM); % Tau 初始信息素矩陣
Tau=8.Tau;
K=100; %迭代次數(shù)（指螞蟻出動(dòng)多少波）
M=50; %螞蟻個(gè)數(shù)
S=1 ; %最短路徑的起始點(diǎn)
E=1MM; %最短路徑的目的點(diǎn)
Alpha=1.0158; % Alpha 表征信息素重要程度的參數(shù)
Beta=5.4066; % Beta 表征啟發(fā)式因子重要程度的參數(shù)
Rho=0.3 ; % Rho 信息素蒸發(fā)系數(shù)
Q=1; % Q 信息素增加強(qiáng)度系數(shù)
minkl=inf;
mink=0;
minl=0;
D=G2D(G);
N=size(D,1); %N表示問(wèn)題的規(guī)模（象素個(gè)數(shù)）
a=1; %小方格象素的邊長(zhǎng)
Ex=a*(mod(E,MM)-0.5); %終止點(diǎn)橫坐標(biāo)
if Ex==-0.5
Ex=MM-0.5;
end
Ey=a*(MM-ceil(E/MM)); %終止點(diǎn)縱坐標(biāo)
Eta=zeros(N); %啟發(fā)式信息，取為至目標(biāo)點(diǎn)的直線距離的倒數(shù)
%以下啟發(fā)式信息矩陣
for i=1:N
ix=a*(mod(i,MM)-0.5);
if ix==-0.5
ix=MM-0.5;
end
iy=a*(MM+0.5-ceil(i/MM));
if i~=E
Eta(i)=1/((ix-Ex)2+(iy-Ey)2)^0.5;
else
Eta(i)=100;
end
end
ROUTES=cell(K,M); %用細(xì)胞結(jié)構(gòu)存儲(chǔ)每一代的每一只螞蟻的爬行路線
PL=zeros(K,M); %用矩陣存儲(chǔ)每一代的每一只螞蟻的爬行路線長(zhǎng)度
%啟動(dòng)K輪螞蟻覓食活動(dòng)，每輪派出M只螞蟻
for k=1:K
for m=1:M
%狀態(tài)初始化
W=S; %當(dāng)前節(jié)點(diǎn)初始化為起始點(diǎn)
Path=S; %爬行路線初始化
PLkm=0; %爬行路線長(zhǎng)度初始化
TABUkm=ones(N); %禁忌表初始化
TABUkm(S)=0; %已經(jīng)在初始點(diǎn)了，因此要排除
DD=D; %鄰接矩陣初始化
%下一步可以前往的節(jié)點(diǎn)
DW=DD(W,:);
DW1=find(DW);
for j=1:length(DW1)
if TABUkm(DW1(j))==0
DW(DW1(j))=0;
end
end
三、運(yùn)行結(jié)果
四、matlab版本及參考文獻(xiàn)
1 matlab版本
2014a
2 參考文獻(xiàn)
[1]周東健,張興國(guó),馬海波,李成浩,郭旭.基于柵格地圖-蟻群算法的機(jī)器人最優(yōu)路徑規(guī)劃[J].南通大學(xué)學(xué)報(bào)(自然科學(xué)版). 2013,12(04)
3 備注
簡(jiǎn)介此部分摘自互聯(lián)網(wǎng)，僅供參考，若侵權(quán)，聯(lián)系刪除
總結(jié)
以上是生活随笔為你收集整理的【路径规划】基于matlab蚁群算法栅格地图路径规划及避障【含Matlab源码 2088期】的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
