临时DWA

最新推荐文章于 2025-06-07 15:13:01 发布

原创最新推荐文章于 2025-06-07 15:13:01 发布 · 357 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#python

Python 专栏收录该内容

16 篇文章

订阅专栏

该博客介绍了一个基于动态窗口法（DWA）的改进算法，用于机器人路径规划，考虑了不同类型的机器人形状（圆形或矩形），并处理了目标不可达和局部最优的情况。算法通过预测轨迹、成本计算和速度限制来避免障碍物，同时优化到达目标的速度。此外，还包含了速度和距离的局部静态分析，以适应动态和静态环境的变化。

import math

import time

from queue import Queue

import matplotlib.pyplot as plt

import numpy as np

class ImprovedDWA:

'''

robot_type: 0--circle, 1--rectangle

'''

def __init__(self, robot_type):

self.max_speed = 0.8

self.min_speed = 0.0

self.max_accel = 0.4

self.max_angular = 80.0 * math.pi / 180.0

self.max_angular_accel = 160.0 * math.pi / 180.0

self.dt = 0.1

self.predict_time = 3.2

self.v_resolution = 0.02

self.a_resolution = 4.0 * math.pi / 180.0

self.goal_cost_gain = 3.5

self.speed_cost_gain = 10.0

self.obstacle_cost_gain = 1.0

self.robot_type = robot_type

self.robot_radius = 0.35

self.ob_radius = 0.10

self.robot_width = 0.5

self.robot_length = 1.2

# 动静态分离数组，通过速度和距离判断

self.qqsize = 6

self.dis_speed = Queue(self.qqsize)

self.static_local = False

self.static_local_thr = 0.2 # m/s

def motion(self, x, u):

"""

motion model

X = x + v*dt*cosα

Y = y + v*dt*sinα

θ = α + ω*dt

"""

x[0] += u[0] * math.cos(x[2]) * self.dt

x[1] += u[0] * math.sin(x[2]) * self.dt

x[2] += u[1] * self.dt

x[3] = u[0]

x[4] = u[1]

return x

def speed_angular_limit(self, x):

# speed limit

Vs = [self.min_speed, self.max_speed,

-self.max_angular, self.max_angular]

# accel limit

Vd = [x[3] - self.max_accel * self.dt,

x[3] + self.max_accel * self.dt,

x[4] - self.max_angular_accel * self.dt,

x[4] + self.max_angular_accel * self.dt]

# [v_min, v_max, 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

def predict_trajectory(self, x_init, v, a):

x = np.array(x_init)

trajectory = np.array(x)

time = 0

while time <= self.predict_time:

x = self.motion(x, [v, a])

trajectory = np.vstack((trajectory, x))

time += self.dt

return trajectory

def dwa_trajectory(self, x, dw, goal, ob):

x_init = x[:]

min_cost = float("inf")

best_u = [0.0, 0.0]

best_trajectory = np.array([x])

trajectories = []

dis_speed_arr = []

obsDis = self.calcObsDis(ob, x_init)

goalDis = self.calcGoalDis(goal, x_init)

for v in np.arange(dw[0], dw[1], self.v_resolution):

for a in np.arange(dw[2], dw[3], self.a_resolution):

trajectory = self.predict_trajectory(x_init, v, a)

trajectories.append(trajectory)

# calc cost

goal_cost = self.goal_cost_gain * self.calc_to_goal_cost(trajectory, goal)

ob_cost = self.obstacle_cost_gain * self.calc_obstacle_cost(trajectory, ob)

speed_cost = self.speed_cost_gain * (self.max_speed - trajectory[-1, 3])

flag = 0

# 解决目标不可达

if goalDis < 2.0 and goalDis+self.ob_radius*1.5 < obsDis:

flag = 1

self.goal_cost_gain = 3.5

self.speed_cost_gain = 10.0

self.obstacle_cost_gain = 1.0

final_cost = (obsDis / goalDis)**goalDis * goal_cost + (1.0 / goalDis) * speed_cost + (goalDis / obsDis)**goalDis * ob_cost

# 解决局部最优，这里更改了系数，在其它if else中要改回来 0.3 8.0 0.5

elif self.static_local:

flag = 2

self.goal_cost_gain = 0.3

self.speed_cost_gain = 8.0

self.obstacle_cost_gain = 0.5

final_cost = goal_cost + speed_cost + ob_cost

else:

flag = 3

self.goal_cost_gain = 3.5

self.speed_cost_gain = 10.0

self.obstacle_cost_gain = 1.0

final_cost = goal_cost + speed_cost + ob_cost

print(flag)

#print('{: .2f} {: .2f} {: .3f} {: .3f} {: .3f} {: .3f}'.format(v, a, goal_cost, speed_cost, ob_cost, final_cost))

if final_cost < min_cost:

min_cost = final_cost

best_u = [v, a]

best_trajectory = trajectory

# 局部最优的速度和距离计算

self.static_local = self.dealSpeedDis([obsDis, best_u[0]])

print('======================', round(best_u[0],2), round(best_u[1],3), '=======================\n')

return best_u, best_trajectory, trajectories

def dealSpeedDis(self, dis_speed_arr):

# 数据不足，添加后直接返回

if(self.dis_speed.qsize() < self.qqsize):

self.dis_speed.put(dis_speed_arr)

return False

else:

# 从queue取出数据

tmp_dis = []

tmp_speed = []

for i in range(self.qqsize):

tmp = self.dis_speed.get()

tmp_dis.append(tmp[0])

tmp_speed.append(tmp[1])

# 丢掉最远的数据，再将最近的数据放回队列

for i in range(1, self.qqsize):

self.dis_speed.put([tmp_dis[i], tmp_speed[i]])

self.dis_speed.put(dis_speed_arr)

# 对数据进行分析

speedMax = max(tmp_speed)

disMin, disMax = min(tmp_dis), max(tmp_dis)

print(speedMax)

if speedMax < self.static_local_thr and (disMax-disMin) < speedMax*self.dt*self.qqsize:

return True

return False

def calcObsDis(self, ob, x_init):

ox = ob[:, 0]

oy = ob[:, 1]

dx = x_init[0] - ox[:, None]

dy = x_init[1] - oy[:, None]

dis = np.hypot(dx, dy)

return np.min(dis)

def calcGoalDis(self, goal, x_init):

return np.sqrt((goal[0] - x_init[0])**2 + (goal[1] - x_init[1])**2)

def calc_obstacle_cost(self, trajectory, ob):

ox = ob[:, 0]

oy = ob[:, 1]

dx = trajectory[:, 0] - ox[:, None]

dy = trajectory[:, 1] - oy[:, None]

r = np.hypot(dx, dy)

if self.robot_type == 0:

if np.array(r <= self.robot_radius+self.ob_radius).any():

return float("Inf")

elif self.robot_type == 1:

yaw = trajectory[:, 2]

rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]])

rot = np.transpose(rot, [2, 0, 1])

local_ob = ob[:, None] - trajectory[:, 0:2]

local_ob = local_ob.reshape(-1, local_ob.shape[-1])

local_ob = np.array([local_ob @ x for x in rot])

local_ob = local_ob.reshape(-1, local_ob.shape[-1])

upper_check = local_ob[:, 0] <= self.robot_length / 2

right_check = local_ob[:, 1] <= self.robot_width / 2

bottom_check = local_ob[:, 0] >= -self.robot_length / 2

left_check = local_ob[:, 1] >= -self.robot_width / 2

if (np.logical_and(np.logical_and(upper_check, right_check), np.logical_and(bottom_check, left_check))).any():

return float("Inf")

min_r = np.min(r)

return 1.0 / min_r

def calc_to_goal_cost(self, trajectory, goal):

dx = goal[0] - trajectory[-1, 0]

dy = goal[1] - trajectory[-1, 1]

error_angle = math.atan2(dy, dx)

cost_angle = error_angle - trajectory[-1, 2]

cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))

return cost

def plot_arrow(self, x, y, yaw, length=0.5, width=0.1):

plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),

head_length=width, head_width=width)

plt.plot(x, y)

def plot_robot(self, x, y, yaw):

if self.robot_type == 1:

outline = np.array([[-self.robot_length / 2, self.robot_length / 2,

(self.robot_length / 2), -self.robot_length / 2,

-self.robot_length / 2],

[self.robot_width / 2, self.robot_width / 2,

- self.robot_width / 2, -self.robot_width / 2,

self.robot_width / 2]])

Rot1 = np.array([[math.cos(yaw), math.sin(yaw)],

[-math.sin(yaw), math.cos(yaw)]])

outline = (outline.T.dot(Rot1)).T

outline[0, :] += x

outline[1, :] += y

plt.plot(np.array(outline[0, :]).flatten(),

np.array(outline[1, :]).flatten(), "-k")

elif self.robot_type == 0:

circle = plt.Circle((x, y), self.robot_radius, color="b")

plt.gcf().gca().add_artist(circle)

out_x, out_y = (np.array([x, y]) +

np.array([np.cos(yaw), np.sin(yaw)]) * self.robot_radius)

plt.plot([x, out_x], [y, out_y], "-k")

def plot_obs_circle(self, ob):

for i in range(len(ob)):

circle = plt.Circle((ob[i][0], ob[i][1]), self.ob_radius, fill=False, color="k")

plt.gcf().gca().add_artist(circle)

def obUpdate(self, ob, obVel, bound):

for i in range(len(ob)):

ob[i][0] += obVel[i][0] * math.cos(ob[i][2] * math.pi / 180.0) * self.dt

ob[i][1] += obVel[i][0] * math.sin(ob[i][2] * math.pi / 180.0) * self.dt

ob[i][2] += obVel[i][1] * self.dt

if ob[i][0] < bound[0] or ob[i][0] > bound[1]:

ob[i][2] = 180.0 - (ob[i][2]) % 360

elif ob[i][1] < bound[2] or ob[i][1] > bound[3]:

ob[i][2] = - (ob[i][2]) % 360

return ob

def show_graph(self, predicted_trajectory, trajectories, goal, ob, x, show_animation, bound):

if show_animation:

plt.cla()

# for stopping simulation with the esc key.

plt.gcf().canvas.mpl_connect(

'key_release_event',

lambda event: [exit(0) if event.key == 'escape' else None])

plt.xlim([bound[0], bound[1]])

plt.ylim([bound[2], bound[3]])

plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-r", linewidth = 2.0)

for i in range(len(trajectories)):

plt.plot(trajectories[i][:len(trajectories[i])*63//64, 0], trajectories[i][:len(trajectories[i])*63//64, 1], "-.b", linewidth = 0.5)

plt.plot(goal[0], goal[1], "xr")

plt.plot(ob[:, 0], ob[:, 1], "ok", markersize=2)

self.plot_robot(x[0], x[1], x[2])

self.plot_arrow(x[0], x[1], x[2])

self.plot_obs_circle(ob)

#plt.axis("equal")

#plt.grid(True)

plt.pause(0.08)

def pathPlan(self, start, goal, ob, obVel, show_animation, bound):

print('Planning start...')

# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]

x = np.array([start[0], start[1], start[2], 0.0, 0.0])

goal = np.array(goal)

trajectory = np.array(x)

while True:

dw = self.speed_angular_limit(x)

u, predicted_trajectory, trajectories = self.dwa_trajectory(x, dw, goal, ob)

x = self.motion(x, u)

trajectory = np.vstack((trajectory, x))

self.show_graph(predicted_trajectory, trajectories, goal, ob, x, show_animation, bound)

dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])

if dist_to_goal <= self.robot_radius:

print("Achieve goal !!")

break

ob = self.obUpdate(ob, obVel, bound)

if show_animation:

plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")

plt.pause(0.01)

plt.show()

return x

if __name__ == '__main__':

robot_type = 0

show_animation = True

dwa = ImprovedDWA(robot_type)

# 局部极小，障碍在目标前面的情况

# start, goal, bound = [1.0, 1.5, 0.0], [4.0, 1.5], [0.0, 6.0, 0.0, 3.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[3.0, 1.5, 0.0]])

# obVel = np.array([[0.0, 0.0]])

# 目标不可达，障碍在目标后面的情况

# start, goal, bound = [1.0, 1.5, 0.0], [3.7, 1.5], [0.0, 6.0, 0.0, 3.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[4.0, 1.5, 0.0]])

# obVel = np.array([[0.0, 0.0]])

# 目标不可达，障碍在目标侧边的情况

# start, goal, bound = [1.0, 5.0, 0.0], [2.0, 2.5], [0.0, 6.0, 0.0, 6.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[1.8, 2.3, 0.0]])

# obVel = np.array([[0.0, 0.0]])

############# 目标不可达，障碍在目标前面的情况 ##################

# 这种情况还没

# start, goal, bound = [1.0, 1.5, 0.0], [4.3, 1.5], [0.0, 6.0, 0.0, 3.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[4.0, 1.5, 0.0]])

# obVel = np.array([[0.0, 0.0]])

# 障碍稠密区

# start, goal, bound = [1.0, 2.3, 0.0], [5.5, 1.5], [0.0, 6.0, 0.0, 3.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[2.5, 2.3, 0.0], [2.5, 1.3, 0.0], [4.2, 1.8, 0.0], [3.5, 0.8, 0.0], [4.8, 0.8, 0.0]])

# obVel = np.array([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0]])

# Gazebo环境

# start, goal, bound = [1.0, 5.0, 1.57], [3.7, 5.8], [0.0, 15.0, 0.0, 10.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[3.9, 5.6, 0.0], [4.1, 5.6, 0.0], [3.0, 4.3, 90]])

# obVel = np.array([[0.0, 0.0], [0.0, 0.0], [0.5, 0.0]])

# start, goal, bound = [3.4, 6.1, -0.1], [4.3, 5.8], [0.0, 15.0, 0.0, 10.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[3.9, 5.6, 0.0], [4.1, 5.6, 0.0], [3.0, 4.3, 90]])

# obVel = np.array([[0.0, 0.0], [0.0, 0.0], [0.5, 0.0]])

# start, goal, bound = [4.1, 6.1, 0.1], [8.0, 3.0], [0.0, 15.0, 0.0, 10.0] # bound[xmin, xmax, ymin, ymax]

# ob = np.array([[3.9, 5.6, 0.0], [4.1, 5.6, 0.0], [3.0, 4.3, 90], [8.3, 3.3, 0.0], [5.5, 4.8, 90]])

# obVel = np.array([[0.0, 0.0], [0.0, 0.0], [0.5, 0.0], [0.0, 0.0], [0.5, 0.0]])

start, goal, bound = [7.7, 3.2, -0.1], [12.0, 2.0], [0.0, 15.0, 0.0, 10.0] # bound[xmin, xmax, ymin, ymax]

ob = np.array([[3.9, 5.6, 0.0], [4.1, 5.6, 0.0], [3.0, 4.3, 90], [8.3, 3.3, 0.0], [5.5, 4.8, 90], [9.3, 3.3, 0.0], [8.8, 2.0, 0.0]])

obVel = np.array([[0.0, 0.0], [0.0, 0.0], [0.5, 0.0], [0.0, 0.0], [0.5, 0.0], [0.0, 0.0], [0.0, 0.0]])

dwa.pathPlan(start, goal, ob, obVel, show_animation, bound)