From 4e38d8a35c54a6a7d4488e109136ac1c4e4b35e6 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 14 Dec 2017 15:44:11 -0800 Subject: [PATCH] keep implementing --- .../dynamic_window_approach.py | 258 +++++++++++++++++- 1 file changed, 257 insertions(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index a4fb30b5..7ab8b47e 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -1,12 +1,268 @@ """ -author: Atsushi Sakai +Mobile robot motion planning sample with Dynamic Window Approach + +author: Atsushi Sakai (@Atsushi_twi) + + +bstacleR=0.5;%衝突判定用の障害物の半径 +global dt; dt=0.1;%刻み時間[s] + +%ロボットの力学モデル +%[最高速度[m/s],最高回頭速度[rad/s],最高加減速度[m/ss],最高加減回頭速度[rad/ss], +% 速度解像度[m/s],回頭速度解像度[rad/s]] +Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)]; + +%評価関数のパラメータ [heading,dist,velocity,predictDT] +evalParam=[0.1,0.2,0.1,3.0]; +area=[-1 11 -1 11];%シミュレーションエリアの広さ [xmin xmax ymin ymax] + +%シミュレーション結果 +result.x=[]; +tic; +%movcount=0; +% Main loop +for i=1:5000 + %DWAによる入力値の計算 + [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR); + x=f(x,u);%運動モデルによる移動 + + %シミュレーション結果の保存 + result.x=[result.x; x']; + + %ゴール判定 + if norm(x(1:2)-goal')<0.5 + disp('Arrive Goal!!');break; + end + + %====Animation==== + hold off; + ArrowLength=0.5;%矢印の長さ + %ロボット + 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 +figure(2) +plot(result.x(:,4)); +toc +%movie2avi(mov,'movie.avi'); + + +function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R) +%DWAによる入力値の計算をする関数 + +%Dynamic Window[vmin,vmax,ωmin,ωmax]の作成 +Vr=CalcDynamicWindow(x,model); +%評価関数の計算 +[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam); + +if isempty(evalDB) + disp('no path to goal!!'); + u=[0;0];return; +end + +%各評価関数の正規化 +evalDB=NormalizeEval(evalDB); + +%最終評価値の計算 +feval=[]; +for id=1:length(evalDB(:,1)) + feval=[feval;evalParam(1:3)*evalDB(id,3:5)']; +end +evalDB=[evalDB feval]; + +[maxv,ind]=max(feval);%最も評価値が大きい入力値のインデックスを計算 +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) + %軌跡の推定 + [xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model); + %各評価関数の計算 + heading=CalcHeadingEval(xt,goal); + dist=CalcDistEval(xt,ob,R); + vel=abs(vt); + + evalDB=[evalDB;[vt ot heading dist vel]]; + trajDB=[trajDB;traj]; + end +end + +function EvalDB=NormalizeEval(EvalDB) +%評価値を正規化する関数 +if sum(EvalDB(:,3))~=0 + EvalDB(:,3)=EvalDB(:,3)/sum(EvalDB(:,3)); +end +if sum(EvalDB(:,4))~=0 + EvalDB(:,4)=EvalDB(:,4)/sum(EvalDB(:,4)); +end +if sum(EvalDB(:,5))~=0 + EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5)); +end + +function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model) +%軌跡データを作成する関数 +global dt; +time=0; +u=[vt;ot];%入力値 +traj=x;%軌跡データ +while time<=evaldt + time=time+dt;%シミュレーション時間の更新 + x=f(x,u);%運動モデルによる推移 + traj=[traj x]; +end + +function stopDist=CalcBreakingDist(vel,model) +%現在の速度から力学モデルに従って制動距離を計算する関数 +global dt; +stopDist=0; +while vel>0 + stopDist=stopDist+vel*dt;%制動距離の計算 + vel=vel-model(3)*dt;%最高原則 +end + +function dist=CalcDistEval(x,ob,R) +%障害物との距離評価値を計算する関数 + +dist=2; +for io=1:length(ob(:,1)) + disttmp=norm(ob(io,:)-x(1:2)')-R;%パスの位置と障害物とのノルム誤差を計算 + if dist>disttmp%最小値を見つける + dist=disttmp; + end +end + +function heading=CalcHeadingEval(x,goal) +%headingの評価関数を計算する関数 + +theta=toDegree(x(3));%ロボットの方位 +goalTheta=toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));%ゴールの方位 + +if goalTheta>theta + targetTheta=goalTheta-theta;%ゴールまでの方位差分[deg] +else + targetTheta=theta-goalTheta;%ゴールまでの方位差分[deg] +end + +heading=180-targetTheta; + +function Vr=CalcDynamicWindow(x,model) +%モデルと現在の状態からDyamicWindowを計算 +global dt; +%車両モデルによるWindow +Vs=[0 model(1) -model(2) model(2)]; + +%運動モデルによるWindow +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))]; +%[vmin,vmax,ωmin,ωmax] + +function x = f(x, u) +% Motion Model +global dt; + +F = [1 0 0 0 0 + 0 1 0 0 0 + 0 0 1 0 0 + 0 0 0 0 0 + 0 0 0 0 0]; + +B = [dt*cos(x(3)) 0 + dt*sin(x(3)) 0 + 0 dt + 1 0 + 0 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; """ +import math +import numpy as np +import matplotlib.pyplot as plt + + +def plot_arrow(x, y, yaw, length=1.0, width=0.5): + plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), + head_length=width, head_width=width) + plt.plot(x, y) + + +def motion(x, u, dt): + + x[0] += u[0] * math.cos(x[2]) * dt + x[1] += u[0] * math.sin(x[2]) * dt + x[2] += u[1] * dt + + return x + def main(): print(__file__ + " start!!") + # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] + x = np.array([0.0, 0.0, math.pi / 4.0]) + # goal position [x(m), y(m)] + goal = np.array([10, 10]) + # obstacles [x(m) y(m), ....] + ob = np.matrix([[0, 2], + [4.0, 2.0], + [5.0, 4.0], + [5.0, 5.0], + [5.0, 6.0], + [5.0, 9.0], + [8.0, 8.0], + [8.0, 9.0], + [7.0, 9.0] + ]) # 4 4 + + dt = 0.1 + u = np.array([1.0, 0.1]) + + for i in range(100): + + x = motion(x, u, dt) + + plt.cla() + plt.plot(x[0], x[1], "xr") + plt.plot(goal[0], goal[1], "xb") + plt.plot(ob[:, 0], ob[:, 1], "ok") + plot_arrow(x[0], x[1], x[2]) + plt.axis("equal") + plt.grid(True) + plt.pause(0.0001) + + plt.show() + if __name__ == '__main__': main()