From 3bb922abc9f76c4bc0565815f2f6c54da6de2692 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 16 Dec 2017 09:59:10 -0800 Subject: [PATCH] add to goal cost --- .../dynamic_window_approach.py | 195 ++++++------------ 1 file changed, 60 insertions(+), 135 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index a497a6b7..d86b2bf4 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -8,73 +8,12 @@ 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); @@ -119,18 +58,6 @@ 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; @@ -165,45 +92,6 @@ 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 @@ -220,11 +108,12 @@ class Config(): self.max_yawrate = 40.0 * math.pi / 180.0 self.max_accel = 0.2 self.max_dyawrate = 40.0 * math.pi / 180.0 - self.v_reso = 0.1 + self.v_reso = 0.01 self.yawrate_reso = 0.1 * math.pi / 180.0 self.dt = 0.1 # [s] self.predict_time = 5.0 # [s] + self.to_goal_cost_gain = 1.0 # [s] def plot_arrow(x, y, yaw, length=0.5, width=0.1): @@ -244,23 +133,23 @@ def motion(x, u, dt): return x -def calc_dynamic_window(x, rspec, dt): +def calc_dynamic_window(x, config): # Dynamic window from robot specification - Vs = [rspec.min_speed, rspec.max_speed, - -rspec.max_yawrate, rspec.max_yawrate] + Vs = [config.min_speed, config.max_speed, + -config.max_yawrate, config.max_yawrate] # Dynamic window from motion model - Vd = [x[3] - rspec.max_accel * dt, - x[3] + rspec.max_accel * dt, - x[4] - rspec.max_dyawrate * dt, - x[4] + rspec.max_dyawrate * dt] - print(Vs, Vd) + Vd = [x[3] - config.max_accel * config.dt, + x[3] + config.max_accel * config.dt, + x[4] - config.max_dyawrate * config.dt, + x[4] + config.max_dyawrate * config.dt] + # print(Vs, Vd) # [vmin,vmax, yawrate min, yawrate max] dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]), max(Vs[2], Vd[2]), min(Vs[3], Vd[3])] - print(dw) + # print(dw) return dw @@ -279,23 +168,59 @@ def calc_trajectory(xinit, v, y, config): return traj -def calc_cost(x, dw, rspec, dt): +def evaluate_path(x, dw, config, goal): xinit = x[:] - for v in np.arange(dw[0], dw[1], rspec.v_reso): - for y in np.arange(dw[2], dw[3], rspec.yawrate_reso): - traj = calc_trajectory(xinit, v, y, rspec) - plt.plot(traj[:, 0], traj[:, 1]) + min_cost = float("Inf") + min_u = [0.0, 0.0] + + # print(dw) + + for v in np.arange(dw[0], dw[1], config.v_reso): + for y in np.arange(dw[2], dw[3], config.yawrate_reso): + traj = calc_trajectory(xinit, v, y, config) + # plt.plot(traj[:, 0], traj[:, 1]) + # print(v, ",", y) + + angle_cost = calc_to_goal_cost(traj, goal, config) + + final_cost = angle_cost + + if min_cost >= final_cost: + min_cost = final_cost + min_u = [v, y] + + # print(min_u) + # input() + + return min_u -def dwa_control(x, u, rspec, dt): +def calc_to_goal_cost(traj, goal, config): + cost = 0 - dw = calc_dynamic_window(x, rspec, dt) + # traj_end_yaw = traj[-1, 2] + dy = goal[0] - traj[-1, 0] + dx = goal[1] - traj[-1, 1] - calc_cost(x, dw, rspec, dt) + # goal_yaw = math.atan2(dy, dx) - u = np.array([0.5, 0.0]) + goal_dis = math.sqrt(dx**2 + dy**2) + # print(goal_dis) + + # cost = config.angle_cost_gain * abs(goal_yaw - traj_end_yaw) + goal_dis + cost = config.to_goal_cost_gain * goal_dis + # print(cost) + + return cost + + +def dwa_control(x, u, config, dt, goal): + + dw = calc_dynamic_window(x, config) + + u = evaluate_path(x, dw, config, goal) return u @@ -304,7 +229,7 @@ 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, 0.0, 0.0]) + x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0]) # goal position [x(m), y(m)] goal = np.array([10, 10]) # obstacles [x(m) y(m), ....] @@ -323,11 +248,11 @@ def main(): dt = 0.1 u = np.array([0.0, 0.0]) - rspec = Config() + config = Config() - for i in range(100): + for i in range(1000): plt.cla() - u = dwa_control(x, u, rspec, dt) + u = dwa_control(x, u, config, dt, goal) x = motion(x, u, dt)