quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold?on;??

plot(result.x(:,1),result.x(:,2),'-b');hold?on;??

plot(goal(1),goal(2),'*r');hold?on;??

plot(obstacle(:,1),obstacle(:,2),'*k');hold?on;??

%?探索軌跡??

if?~isempty(traj)??

for?it=1:length(traj(:,1))/5??

ind=1+(it-1)*5;??

plot(traj(ind,:),traj(ind+1,:),'-g');hold?on;??

end??

end??

axis(area);??

grid?on;??

drawnow;??

%movcount=movcount+1;??

%mov(movcount)?=?getframe(gcf);%???

end??

toc??

%movie2avi(mov,'movie.avi');??

?

?

function?[u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)??

?

%?Dynamic?Window?[vmin,vmax,wmin,wmax]??

Vr=CalcDynamicWindow(x,model);??

?

%?評價(jià)函數(shù)的計(jì)算??

[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);??

?

if?isempty(evalDB)??

disp('no?path?to?goal!!');??

u=[0;0];return;??

end??

?

%?各評價(jià)函數(shù)正則化??

evalDB=NormalizeEval(evalDB);??

?

%?最終評價(jià)函數(shù)的計(jì)算??

feval=[];??

for?id=1:length(evalDB(:,1))??

feval=[feval;evalParam(1:3)*evalDB(id,3:5)'];??

end??

evalDB=[evalDB?feval];??

?

[maxv,ind]=max(feval);%?最優(yōu)評價(jià)函數(shù)??

u=evalDB(ind,1:2)';%???

?

function?[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam)??

%???

evalDB=[];??

trajDB=[];??

for?vt=Vr(1):model(5):Vr(2)??

for?ot=Vr(3):model(6):Vr(4)??