源：機(jī)器人局部避障的動(dòng)態(tài)窗口法(dynamic window approach)??

首先在V_m∩V_d的范圍內(nèi)采樣速度： allowable_v = generateWindow(robotV, robotModel) allowable_w = generateWindow(robotW, robotModel) 然后根據(jù)能否及時(shí)剎車剔除不安全的速度：for each v in allowable_vfor each w in allowable_wdist = find_dist(v,w,laserscan,robotModel)breakDist = calculateBreakingDistance(v)//剎車距離if (dist > breakDist) //如果能夠及時(shí)剎車，該對(duì)速度可接收如果這組速度可接受，接下來利用評(píng)價(jià)函數(shù)對(duì)其評(píng)價(jià)，找到最優(yōu)的速度組

?

來源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html BEGIN DWA(robotPose,robotGoal,robotModel)laserscan = readScanner()allowable_v = generateWindow(robotV, robotModel)allowable_w = generateWindow(robotW, robotModel)for each v in allowable_vfor each w in allowable_wdist = find_dist(v,w,laserscan,robotModel)breakDist = calculateBreakingDistance(v)if (dist > breakDist) //can stop in timeheading = hDiff(robotPose,goalPose, v,w) //clearance與原論文稍不一樣clearance = (dist-breakDist)/(dmax - breakDist) cost = costFunction(heading,clearance, abs(desired_v - v))if (cost > optimal)best_v = vbest_w = woptimal = costset robot trajectory to best_v, best_w END

?

（轉(zhuǎn)載請(qǐng)注明作者和出處：http://blog.csdn.net/heyijia0327?未經(jīng)允許請(qǐng)勿用于商業(yè)用途）

?

參考：

dwa：

1.Fox.《The Dynamic Window Approach To CollisionAvoidance》

2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》

3.http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html

?

運(yùn)動(dòng)模型：

4. http://adrianboeing.blogspot.com.au/2010/09/circular-motion-in-2d-for-graphics-and.html

5.https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf

6.http://rossum.sourceforge.net/papers/DiffSteer/

?

最后貼出matlab仿真代碼：

% ------------------------------------------------------------------------- % % File : DynamicWindowApproachSample.m % % Discription : Mobile Robot Motion Planning with Dynamic Window Approach % % Environment : Matlab % % Author : Atsushi Sakai % % Copyright (c): 2014 Atsushi Sakai % % License : Modified BSD Software License Agreement % -------------------------------------------------------------------------function [] = DynamicWindowApproachSample()close all; clear all;disp('Dynamic Window Approach sample program start!!')x=[0 0 pi/2 0 0]';% 機(jī)器人的初期狀態(tài)[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] goal=[10,10];% 目標(biāo)點(diǎn)位置 [x(m),y(m)] % 障礙物位置列表 [x(m) y(m)] % obstacle=[0 2; % 4 2; % 4 4; % 5 4; % 5 5; % 5 6; % 5 9 % 8 8 % 8 9 % 7 9]; obstacle=[0 2;4 2;4 4;5 4;5 5;5 6;5 98 88 97 96 56 36 86 77 49 89 119 6];obstacleR=0.5;% 沖突判定用的障礙物半徑 global dt; dt=0.1;% 時(shí)間[s]% 機(jī)器人運(yùn)動(dòng)學(xué)模型 % 最高速度m/s],最高旋轉(zhuǎn)速度[rad/s],加速度[m/ss],旋轉(zhuǎn)加速度[rad/ss], % 速度分辨率[m/s],轉(zhuǎn)速分辨率[rad/s]] Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];% 評(píng)價(jià)函數(shù)參數(shù) [heading,dist,velocity,predictDT] evalParam=[0.05,0.2,0.1,3.0]; area=[-1 11 -1 11];% 模擬區(qū)域范圍 [xmin xmax ymin ymax]% 模擬實(shí)驗(yàn)的結(jié)果 result.x=[]; tic; % movcount=0; % Main loop for i=1:5000% DWA參數(shù)輸入[u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);x=f(x,u);% 機(jī)器人移動(dòng)到下一個(gè)時(shí)刻% 模擬結(jié)果的保存result.x=[result.x; x'];% 是否到達(dá)目的地if norm(x(1:2)-goal')<0.5disp('Arrive Goal!!');break;end%====Animation====hold off;ArrowLength=0.5;% % 機(jī)器人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))/5ind=1+(it-1)*5;plot(traj(ind,:),traj(ind+1,:),'-g');hold on;endendaxis(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);% 評(píng)價(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% 各評(píng)價(jià)函數(shù)正則化 evalDB=NormalizeEval(evalDB);% 最終評(píng)價(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)評(píng)價(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)% 軌跡推測(cè); 得到 xt: 機(jī)器人向前運(yùn)動(dòng)后的預(yù)測(cè)位姿; traj: 當(dāng)前時(shí)刻 到 預(yù)測(cè)時(shí)刻之間的軌跡[xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model); %evalParam(4),前向模擬時(shí)間;% 各評(píng)價(jià)函數(shù)的計(jì)算heading=CalcHeadingEval(xt,goal);dist=CalcDistEval(xt,ob,R);vel=abs(vt);% 制動(dòng)距離的計(jì)算stopDist=CalcBreakingDist(vel,model);if dist>stopDist % evalDB=[evalDB;[vt ot heading dist vel]];trajDB=[trajDB;traj];endend endfunction EvalDB=NormalizeEval(EvalDB) % 評(píng)價(jià)函數(shù)正則化 if sum(EvalDB(:,3))~=0EvalDB(:,3)=EvalDB(:,3)/sum(EvalDB(:,3)); end if sum(EvalDB(:,4))~=0EvalDB(:,4)=EvalDB(:,4)/sum(EvalDB(:,4)); end if sum(EvalDB(:,5))~=0EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5)); endfunction [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model) % 軌跡生成函數(shù) % evaldt：前向模擬時(shí)間; vt、ot當(dāng)前速度和角速度; global dt; time=0; u=[vt;ot];% 輸入值 traj=x;% 機(jī)器人軌跡 while time<=evaldttime=time+dt;% 時(shí)間更新x=f(x,u);% 運(yùn)動(dòng)更新traj=[traj x]; endfunction stopDist=CalcBreakingDist(vel,model) % 根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算制動(dòng)距離,這個(gè)制動(dòng)距離并沒有考慮旋轉(zhuǎn)速度，不精確吧！！！ global dt; stopDist=0; while vel>0stopDist=stopDist+vel*dt;% 制動(dòng)距離的計(jì)算vel=vel-model(3)*dt;% endfunction dist=CalcDistEval(x,ob,R) % 障礙物距離評(píng)價(jià)函數(shù)dist=100; for io=1:length(ob(:,1))disttmp=norm(ob(io,:)-x(1:2)')-R;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼if dist>disttmp% 離障礙物最小的距離dist=disttmp;end end% 障礙物距離評(píng)價(jià)限定一個(gè)最大值，如果不設(shè)定，一旦一條軌跡沒有障礙物，將太占比重 if dist>=2*Rdist=2*R; endfunction heading=CalcHeadingEval(x,goal) % heading的評(píng)價(jià)函數(shù)計(jì)算theta=toDegree(x(3));% 機(jī)器人朝向 goalTheta=toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目標(biāo)點(diǎn)的方位if goalTheta>thetatargetTheta=goalTheta-theta;% [deg] elsetargetTheta=theta-goalTheta;% [deg] endheading=180-targetTheta;function Vr=CalcDynamicWindow(x,model) % global dt; % 車子速度的最大最小范圍 Vs=[0 model(1) -model(2) model(2)];% 根據(jù)當(dāng)前速度以及加速度限制計(jì)算的動(dòng)態(tài)窗口 Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];% 最終的Dynamic Window Vtmp=[Vs;Vd]; Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];function x = f(x, u) % Motion Model % u = [vt; wt];當(dāng)前時(shí)刻的速度、角速度 global dt;F = [1 0 0 0 00 1 0 0 00 0 1 0 00 0 0 0 00 0 0 0 0];B = [dt*cos(x(3)) 0dt*sin(x(3)) 00 dt1 00 1];x= F*x+B*u;function radian = toRadian(degree) % degree to radian radian = degree/180*pi;function degree = toDegree(radian) % radian to degree degree = radian/pi*180;

?

轉(zhuǎn)載于:https://www.cnblogs.com/LittleTiger/p/4602191.html

總結(jié)

以上是生活随笔為你收集整理的机器人局部避障的动态窗口法(dynamic window approach) （转）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

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