動態(tài)窗口法概述

DWA是一種基于速度的局部規(guī)劃器，可計算達到目標所需的機器人的最佳無碰撞速度。

程序實現(xiàn)

DWA算法主要分三步：

計算動態(tài)窗口

計算最優(yōu)[ v , ω ]

更新機器人狀態(tài)

流程圖如下：

以下代碼參考：

https://github.com/AtsushiSakai/PythonRobotics

初始化機器人狀態(tài)、目標位置、障礙物位置

# 初始化機器人狀態(tài) [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
# 目標位置 [x(m), y(m)]
goal = np.array([gx, gy])
# 障礙物位置 [x(m), y(m)]
ob = np.array([[-1, -1], ...... , [13.0, 13.0]])

獲取動態(tài)窗口

這個動態(tài)窗口就是機器人在當前狀態(tài)下能達到的速度v 和轉速ω范圍，受到自身機械特性以及當前狀態(tài)的影響。

def calc_dynamic_window(x, config):
 ? ?"""
 ? ?calculation dynamic window based on current state x
 ? ?"""


 ? ?# Dynamic window from robot specification
 ? ?Vs = [config.min_speed, config.max_speed,
 ? ? ? ? ?-config.max_yawrate, config.max_yawrate]


 ? ?# Dynamic window from motion model
 ? ?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]


 ? ?# ?[vmin, vmax, yaw_rate min, yaw_rate max]
 ? ?dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
 ? ? ? ? ?max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]


 ? ?return dw

計算最優(yōu)[ v , ω ]?

def calc_control_and_trajectory(x, dw, config, goal, ob):
 ? ?"""
 ? ?calculation final input with dynamic window
 ? ?"""


 ? ?x_init = x[:]
 ? ?min_cost = float("inf")
 ? ?best_u = [0.0, 0.0]
 ? ?best_trajectory = np.array([x])


 ? ?# 計算動態(tài)窗口內所有的采樣樣本的代價函數(shù)
 ? ?for v in np.arange(dw[0], dw[1], config.v_reso):
 ? ? ? ?for y in np.arange(dw[2], dw[3], config.yawrate_reso):


 ? ? ? ? ? ?trajectory = predict_trajectory(x_init, v, y, config)


 ? ? ? ? ? ?# 計算代價函數(shù)
 ? ? ? ? ? ?to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
 ? ? ? ? ? ?speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
 ? ? ? ? ? ?ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)


 ? ? ? ? ? ?final_cost = to_goal_cost + speed_cost + ob_cost


 ? ? ? ? ? ?# 尋找具有最小代價的樣本以及它的軌跡
 ? ? ? ? ? ?if min_cost >= final_cost:
 ? ? ? ? ? ? ? ?min_cost = final_cost
 ? ? ? ? ? ? ? ?best_u = [v, y]
 ? ? ? ? ? ? ? ?best_trajectory = trajectory


 ? ?return best_u, best_trajectory

更新狀態(tài)

根據(jù)最優(yōu)u = [ v , ω ] 更新機器人狀態(tài)

x = motion(x, u, config.dt)??

編輯：黃飛

?