From 59863e39b20dc90adcffaedc85b10cb826c593ae Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 16 Dec 2017 10:29:07 -0800 Subject: [PATCH] it can goal!! --- .../dynamic_window_approach.py | 123 +++++------------- 1 file changed, 35 insertions(+), 88 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index d86b2bf4..ee8bf19b 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -6,66 +6,6 @@ author: Atsushi Sakai (@Atsushi_twi) bstacleR=0.5;%衝突判定用の障害物の半径 -global dt; dt=0.1;%刻み時間[s] - - -%Dynamic Window[vmin,vmax,ωmin,ωmax]の作成 -Vr=CalcDynamicWindow(x,model); -%評価関数の計算 -[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam); - -%各評価関数の正規化 -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 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) %障害物との距離評価値を計算する関数 @@ -78,20 +18,6 @@ for io=1:length(ob(:,1)) 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; - """ import math @@ -106,14 +32,16 @@ class Config(): self.max_speed = 1.0 self.min_speed = -0.5 self.max_yawrate = 40.0 * math.pi / 180.0 - self.max_accel = 0.2 + self.max_accel = 0.5 self.max_dyawrate = 40.0 * math.pi / 180.0 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.predict_time = 2.0 # [s] self.to_goal_cost_gain = 1.0 # [s] + self.speed_cost_gain = 1.0 # [s] + self.robot_radius = 1.0 # [s] def plot_arrow(x, y, yaw, length=0.5, width=0.1): @@ -168,35 +96,55 @@ def calc_trajectory(xinit, v, y, config): return traj -def evaluate_path(x, dw, config, goal): +def evaluate_path(x, dw, config, goal, ob): xinit = x[:] - - min_cost = float("Inf") + min_cost = 10000.0 min_u = [0.0, 0.0] - - # print(dw) + best_traj = None 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) + to_goal_cost = calc_to_goal_cost(traj, goal, config) + speed_cost = config.speed_cost_gain * \ + (config.max_speed - traj[-1, 3]) + ob_cost = calc_obstacle_cost(traj, ob, config) + # print(ob_cost) - final_cost = angle_cost + final_cost = to_goal_cost + speed_cost + ob_cost if min_cost >= final_cost: min_cost = final_cost min_u = [v, y] + best_traj = traj + if best_traj is not None: + plt.plot(best_traj[:, 0], best_traj[:, 1]) # print(min_u) # input() return min_u +def calc_obstacle_cost(traj, ob, config): + + for ii in range(len(traj[:, 1])): + for i in range(len(ob[:, 0])): + ox = ob[i, 0] + oy = ob[i, 1] + dx = traj[ii, 0] - ox + dy = traj[ii, 1] - oy + + r = math.sqrt(dx**2 + dy**2) + if r <= config.robot_radius: + return float("Inf") + + return 0.0 + + def calc_to_goal_cost(traj, goal, config): cost = 0 @@ -216,11 +164,11 @@ def calc_to_goal_cost(traj, goal, config): return cost -def dwa_control(x, u, config, dt, goal): +def dwa_control(x, u, config, dt, goal, ob): dw = calc_dynamic_window(x, config) - u = evaluate_path(x, dw, config, goal) + u = evaluate_path(x, dw, config, goal, ob) return u @@ -240,7 +188,6 @@ def main(): [5.0, 5.0], [5.0, 6.0], [5.0, 9.0], - [8.0, 8.0], [8.0, 9.0], [7.0, 9.0], [12.0, 12.0] @@ -252,7 +199,7 @@ def main(): for i in range(1000): plt.cla() - u = dwa_control(x, u, config, dt, goal) + u = dwa_control(x, u, config, dt, goal, ob) x = motion(x, u, dt)