1 簡介

移動機(jī)器人路徑規(guī)劃一直是一個(gè)比較熱門的話題,A星算法以及其擴(kuò)展性算法被廣范地應(yīng)用于求解移動機(jī)器人的最優(yōu)路徑.該文在研究機(jī)器人路徑規(guī)劃算法中,詳細(xì)闡述了傳統(tǒng)A星算法的基本原理,并通過柵格法分割了機(jī)器人路徑規(guī)劃區(qū)域,利用MATLAB仿真平臺生成了機(jī)器人二維路徑仿真地圖對其進(jìn)行仿真實(shí)驗(yàn),并對結(jié)果進(jìn)行分析和研究,為今后進(jìn)一步的研究提供經(jīng)驗(yàn).

2 部分代碼

function PathCell = MRPP_CBSHWY(MapMat,Highway,StartStates,GoalStates,ctime)robotNum=size(StartStates,1);PathCell=cell(robotNum,1);OPEN_COUNT=1;MAX_OPEN_SIZE=1000;MAX_TIME_STEP=300;OPEN=cell(MAX_OPEN_SIZE,1);OPEN_CHECK=zeros(MAX_OPEN_SIZE,1);%% initializeRoot.ID=1;Root.parent=0;Root.constraints=[];solution=cell(robotNum,1);AllPath=cell(robotNum,1);costVec=zeros(robotNum,1);for i=1:robotNum ? ?tempMat=MapMat; ? ?tempMat(GoalStates(i,1),GoalStates(i,2))=0; ? ?path=AStarSTDiffHWY(tempMat,Highway,StartStates(i,:),GoalStates(i,:),[]); ? ?path(:,4)=path(:,4)+ctime; ? ?%make sure robots at goal positions will stay forever ? ?pathSize=size(path,1); ? ?newPath=zeros(MAX_TIME_STEP,4); ? ?newPath(1:pathSize,:)=path; ? ?newPath(pathSize+1:MAX_TIME_STEP,1:3)=repmat(path(end,1:3),[MAX_TIME_STEP-pathSize,1]); ? ?newPath(pathSize+1:MAX_TIME_STEP,4)=(path(pathSize,4)+1:MAX_TIME_STEP-1)'; ? ?AllPath{i,1}=path; ? ?solution{i,1}=newPath; ? ?costVec=size(path,1)-1;endRoot.AllPath=AllPath;Root.solution = solution;Root.costVec=costVec;Root.cost=sum(costVec);OPEN{1,1}=Root;OPEN_CHECK(1,1)=0;while ~isempty(find(OPEN_CHECK==0, 1)) ? ?bestCost=0; ? ?bestNodeID=0; ? ?for i=1:OPEN_COUNT ? ? ? ?if ?OPEN_CHECK(i,1)==0 ? ? ? ? ? ?node=OPEN{i,1}; ? ? ? ? ? ?if node.cost>bestCost ? ? ? ? ? ? ? ?bestNodeID=node.ID; ? ? ? ? ? ? ? ?bestCost=node.cost; ? ? ? ? ? ?end ? ? ? ?end ? ?end ? ?P=OPEN{bestNodeID,1}; ? ?%% conflict detection ? ?solution=P.solution; ? ?conflictFoundFlag=0; ? ?constraints=zeros(2,4); ? ?for i=1:robotNum ? ? ? ?iPath=solution{i,1}; ? ? ? ?iPath(:,3)=[]; ? ? ? ?for j=1:robotNum ? ? ? ? ? ?if j<=i ? ? ? ? ? ? ? ?continue; ? ? ? ? ? ?end ? ? ? ? ? ?jPath=solution{j,1}; ? ? ? ? ? ?jPath(:,3)=[]; ? ? ? ? ? ?iLen=size(iPath,1); ? ? ? ? ? ?jLen=size(jPath,1); ? ? ? ? ? ?if iLen>1 && jLen>1 ? ? ? ? ? ? ? ?iMat=zeros(iLen-1,6); ? ? ? ? ? ? ? ?iMat(:,1:3)=iPath(1:iLen-1,:); ? ? ? ? ? ? ? ?iMat(:,4:6)=iPath(2:iLen,:); ? ? ? ? ? ? ? ?jMat=zeros(jLen-1,6); ? ? ? ? ? ? ? ?jMat(:,4:6)=jPath(1:jLen-1,:); ? ? ? ? ? ? ? ?jMat(:,1:3)=jPath(2:jLen,:); ? ? ? ? ? ?end ? ? ? ? ? ?temp=jMat(:,6); ? ? ? ? ? ?jMat(:,6)=jMat(:,3); ? ? ? ? ? ?jMat(:,3)=temp; ? ? ? ? ? ?%vertex and edge conflict detection ? ? ? ? ? ?[vertexKind,~]=ismember(iPath,jPath,'rows'); ? ? ? ? ? ?[edgeKind,~]=ismember(iMat,jMat,'rows'); ? ? ? ? ? ?vertex=find(vertexKind==1, 1); ? ? ? ? ? ?edge=find(edgeKind==1, 1); ? ? ? ? ? ?if ~isempty(vertex) && ~isempty(edge) %both conflicts exist ? ? ? ? ? ? ? ?vertexConflict=iPath(vertex(1),:); ? ? ? ? ? ? ? ?edgeConflict=iMat(edge(1),:); ? ? ? ? ? ? ? ?if vertexConflict(1,3)<=edgeConflict(1,6) ? ? ? ? ? ? ? ? ? ?constraints(1,:)=[i vertexConflict]; ? ? ? ? ? ? ? ? ? ?constraints(2,:)=[j vertexConflict]; ? ? ? ? ? ? ? ?else ? ? ? ? ? ? ? ? ? ?constraints(1,:)=[i edgeConflict(1,4:6)]; ? ? ? ? ? ? ? ? ? ?constraints(2,:)=[j edgeConflict(1,1:2) edgeConflict(1,6)]; ? ? ? ? ? ? ? ?end ? ? ? ? ? ? ? ?conflictFoundFlag=1; ? ? ? ? ? ? ? ?break; ? ? ? ? ? ?elseif ~isempty(vertex) && isempty(edge) %only vertex conflict ? ? ? ? ? ? ? ?conflictFoundFlag=1; ? ? ? ? ? ? ? ?vertexConflict=iPath(vertex(1),:); ? ? ? ? ? ? ? ?constraints(1,:)=[i vertexConflict]; ? ? ? ? ? ? ? ?constraints(2,:)=[j vertexConflict]; ? ? ? ? ? ? ? ?break; ? ? ? ? ? ?elseif ?isempty(vertex) && ~isempty(edge) %only edge conflict ? ? ? ? ? ? ? ?conflictFoundFlag=1; ? ? ? ? ? ? ? ?edgeConflict=iMat(edge(1),:); ? ? ? ? ? ? ? ?constraints(1,:)=[i edgeConflict(1,4:6)]; ? ? ? ? ? ? ? ?constraints(2,:)=[j edgeConflict(1,1:2) edgeConflict(1,6)]; ? ? ? ? ? ? ? ?break; ? ? ? ? ? ?else ? ? ? ? ? ? ? ?continue; ? ? ? ? ? ?end ? ? ? ?end ? ? ? ?if conflictFoundFlag == 1 ? ? ? ? ? ?break; ? ? ? ?end ? ?end ? ?if conflictFoundFlag == 0 ? ? ? ?PathCell=P.AllPath; ? ? ? ?break; ? ?end ? ?%% branching and expanding the tree ? ?OPEN_CHECK(bestNodeID,1)=1; ? ?for i=1:2 ? ? ? ?agentID=constraints(i,1); ? ? ? ?A.ID=OPEN_COUNT+1; ? ? ? ?A.parent=P.ID; ? ? ? ?A.constraints=[P.constraints;constraints(i,:)]; ? ? ? ?pSolution=P.solution; ? ? ? ?startRCA=StartStates(agentID,:); ? ? ? ?goalRCA=GoalStates(agentID,:); ? ? ? ?AConstraints=A.constraints; ? ? ? ?AConstraints=AConstraints(AConstraints(:,1)==agentID,2:end); ? ? ? ?tempMat=MapMat; ? ? ? ?tempMat(goalRCA(1,1),goalRCA(1,2))=0; ? ? ? ?path=AStarSTDiffHWY(tempMat,Highway,startRCA,goalRCA,AConstraints); ? ? ? ?%make sure robots at goal positions will stay forever ? ? ? ?pathSize=size(path,1); ? ? ? ?newPath=zeros(MAX_TIME_STEP,4); ? ? ? ?newPath(1:pathSize,:)=path; ? ? ? ?newPath(pathSize+1:MAX_TIME_STEP,1:3)=repmat(path(end,1:3),[MAX_TIME_STEP-pathSize,1]); ? ? ? ?newPath(pathSize+1:MAX_TIME_STEP,4)=(path(pathSize,4)+1:MAX_TIME_STEP-1)'; ? ? ? ?pSolution{agentID,1}=newPath; ? ? ? ?A.solution=pSolution; ? ? ? ?pSolution{agentID,1}=path; ? ? ? ?A.AllPath=pSolution; ? ? ? ?costVec=P.cost; ? ? ? ?costVec(agentID,1)=size(path,1)-1; ? ? ? ?A.costVec=costVec; ? ? ? ?A.cost=sum(A.costVec); ? ? ? ?OPEN_COUNT=OPEN_COUNT+1; ? ? ? ?OPEN{OPEN_COUNT,1}=A; ? ? ? ?OPEN_CHECK(OPEN_COUNT,1)=0; ? ?end end % while loopend

3 仿真結(jié)果

4 參考文獻(xiàn)

[1]周宇杭, 王文明, 李澤彬,等. 基于A星算法的移動機(jī)器人路徑規(guī)劃應(yīng)用研究[J]. 電腦知識與技術(shù)：學(xué)術(shù)版, 2020, 16(13):4.

博主簡介：擅長智能優(yōu)化算法、神經(jīng)網(wǎng)絡(luò)預(yù)測、信號處理、元胞自動機(jī)、圖像處理、路徑規(guī)劃、無人機(jī)等多種領(lǐng)域的Matlab仿真，相關(guān)matlab代碼問題可私信交流。

部分理論引用網(wǎng)絡(luò)文獻(xiàn)，若有侵權(quán)聯(lián)系博主刪除。