文档章节

动态窗口法(DWA)python

o
 osc_x4h57ch8
发布于 2018/04/24 12:41
字数 1807
阅读 11
收藏 0

精选30+云产品,助力企业轻松上云!>>>

  1 """
  2 version1.1
  3 Mobile robot motion planning sample with Dynamic Window Approach
  4 结合https://blog.csdn.net/heyijia0327/article/details/44983551来看,里面含中文注释
  5 符号参考《煤矿救援机器人地图构建与路径规划研究》矿大硕士论文
  6 """
  7 
  8 import math
  9 import numpy as np
 10 import matplotlib.pyplot as plt
 11 
 12 show_animation = True  # 动画
 13 
 14 
 15 class Config(object):
 16     """
 17     用来仿真的参数,
 18     """
 19 
 20     def __init__(self):
 21         # robot parameter
 22         self.max_speed = 1.4  # [m/s]  # 最大速度
 23         # self.min_speed = -0.5  # [m/s]  # 最小速度,设置为可以倒车
 24         self.min_speed = 0  # [m/s]  # 最小速度,设置为不倒车
 25         self.max_yawrate = 40.0 * math.pi / 180.0  # [rad/s]  # 最大角速度
 26         self.max_accel = 0.2  # [m/ss]  # 最大加速度
 27         self.max_dyawrate = 40.0 * math.pi / 180.0  # [rad/ss]  # 最大角加速度
 28         self.v_reso = 0.01  # [m/s],速度分辨率
 29         self.yawrate_reso = 0.1 * math.pi / 180.0  # [rad/s],角速度分辨率
 30         self.dt = 0.1  # [s]  # 采样周期
 31         self.predict_time = 3.0  # [s]  # 向前预估三秒
 32         self.to_goal_cost_gain = 1.0  # 目标代价增益
 33         self.speed_cost_gain = 1.0  # 速度代价增益
 34         self.robot_radius = 1.0  # [m]  # 机器人半径
 35 
 36 
 37 def motion(x, u, dt):
 38     """
 39     :param x: 位置参数,在此叫做位置空间
 40     :param u: 速度和加速度,在此叫做速度空间
 41     :param dt: 采样时间
 42     :return:
 43     """
 44     # 速度更新公式比较简单,在极短时间内,车辆位移也变化较大
 45     # 采用圆弧求解如何?
 46     x[0] += u[0] * math.cos(x[2]) * dt  # x方向位移
 47     x[1] += u[0] * math.sin(x[2]) * dt  # y
 48     x[2] += u[1] * dt  # 航向角
 49     x[3] = u[0]  # 速度v
 50     x[4] = u[1]  # 角速度w
 51     # print(x)
 52 
 53     return x
 54 
 55 
 56 def calc_dynamic_window(x, config):
 57     """
 58     位置空间集合
 59     :param x:当前位置空间,符号参考硕士论文
 60     :param config:
 61     :return:目前是两个速度的交集,还差一个
 62     """
 63 
 64     # 车辆能够达到的最大最小速度
 65     vs = [config.min_speed, config.max_speed,
 66           -config.max_yawrate, config.max_yawrate]
 67 
 68     # 一个采样周期能够变化的最大最小速度
 69     vd = [x[3] - config.max_accel * config.dt,
 70           x[3] + config.max_accel * config.dt,
 71           x[4] - config.max_dyawrate * config.dt,
 72           x[4] + config.max_dyawrate * config.dt]
 73     #  print(Vs, Vd)
 74 
 75     # 求出两个速度集合的交集
 76     vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
 77           max(vs[2], vd[2]), min(vs[3], vd[3])]
 78 
 79     return vr
 80 
 81 
 82 def calc_trajectory(x_init, v, w, config):
 83     """
 84     预测3秒内的轨迹
 85     :param x_init:位置空间
 86     :param v:速度
 87     :param w:角速度
 88     :param config:
 89     :return: 每一次采样更新的轨迹,位置空间垂直堆叠
 90     """
 91     x = np.array(x_init)
 92     trajectory = np.array(x)
 93     time = 0
 94     while time <= config.predict_time:
 95         x = motion(x, [v, w], config.dt)
 96         trajectory = np.vstack((trajectory, x))  # 垂直堆叠,vertical
 97         time += config.dt
 98 
 99         # print(trajectory)
100     return trajectory
101 
102 
103 def calc_to_goal_cost(trajectory, goal, config):
104     """
105     计算轨迹到目标点的代价
106     :param trajectory:轨迹搜索空间
107     :param goal:
108     :param config:
109     :return: 轨迹到目标点欧式距离
110     """
111     # calc to goal cost. It is 2D norm.
112 
113     dx = goal[0] - trajectory[-1, 0]
114     dy = goal[1] - trajectory[-1, 1]
115     goal_dis = math.sqrt(dx ** 2 + dy ** 2)
116     cost = config.to_goal_cost_gain * goal_dis
117 
118     return cost
119 
120 
121 def calc_obstacle_cost(traj, ob, config):
122     """
123     计算预测轨迹和障碍物的最小距离,dist(v,w)
124     :param traj:
125     :param ob:
126     :param config:
127     :return:
128     """
129     # calc obstacle cost inf: collision, 0:free
130 
131     min_r = float("inf")  # 距离初始化为无穷大
132 
133     for ii in range(0, len(traj[:, 1])):
134         for i in range(len(ob[:, 0])):
135             ox = ob[i, 0]
136             oy = ob[i, 1]
137             dx = traj[ii, 0] - ox
138             dy = traj[ii, 1] - oy
139 
140             r = math.sqrt(dx ** 2 + dy ** 2)
141             if r <= config.robot_radius:
142                 return float("Inf")  # collision
143 
144             if min_r >= r:
145                 min_r = r
146 
147     return 1.0 / min_r  # 越小越好
148 
149 
150 def calc_final_input(x, u, vr, config, goal, ob):
151     """
152     计算采样空间的评价函数,选择最合适的那一个作为最终输入
153     :param x:位置空间
154     :param u:速度空间
155     :param vr:速度空间交集
156     :param config:
157     :param goal:目标位置
158     :param ob:障碍物
159     :return:
160     """
161     x_init = x[:]
162     min_cost = 10000.0
163     min_u = u
164 
165     best_trajectory = np.array([x])
166 
167     # evaluate all trajectory with sampled input in dynamic window
168     # v,生成一系列速度,w,生成一系列角速度
169     for v in np.arange(vr[0], vr[1], config.v_reso):
170         for w in np.arange(vr[2], vr[3], config.yawrate_reso):
171 
172             trajectory = calc_trajectory(x_init, v, w, config)
173 
174             # calc cost
175             to_goal_cost = calc_to_goal_cost(trajectory, goal, config)
176             speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
177             ob_cost = calc_obstacle_cost(trajectory, ob, config)
178             #  print(ob_cost)
179 
180             # 评价函数多种多样,看自己选择
181             # 本文构造的是越小越好
182             final_cost = to_goal_cost + speed_cost + ob_cost
183 
184             # search minimum trajectory
185             if min_cost >= final_cost:
186                 min_cost = final_cost
187                 min_u = [v, w]
188                 best_trajectory = trajectory
189 
190     # print(min_u)
191     #  input()
192 
193     return min_u, best_trajectory
194 
195 
196 def dwa_control(x, u, config, goal, ob):
197     """
198     调用前面的几个函数,生成最合适的速度空间和轨迹搜索空间
199     :param x:
200     :param u:
201     :param config:
202     :param goal:
203     :param ob:
204     :return:
205     """
206     # Dynamic Window control
207 
208     vr = calc_dynamic_window(x, config)
209 
210     u, trajectory = calc_final_input(x, u, vr, config, goal, ob)
211 
212     return u, trajectory
213 
214 
215 def plot_arrow(x, y, yaw, length=0.5, width=0.1):
216     """
217     arrow函数绘制箭头
218     :param x:
219     :param y:
220     :param yaw:航向角
221     :param length:
222     :param width:参数值为浮点数,代表箭头尾部的宽度,默认值为0.001
223     :return:
224     length_includes_head:代表箭头整体长度是否包含箭头头部的长度,默认值为False
225     head_width:代表箭头头部的宽度,默认值为3*width,即尾部宽度的3倍
226     head_length:代表箭头头部的长度度,默认值为1.5*head_width,即头部宽度的1.5倍
227     shape:参数值为'full''left''right',表示箭头的形状,默认值为'full'
228     overhang:代表箭头头部三角形底边与箭头尾部直接的夹角关系,通过该参数可改变箭头的形状。
229     默认值为0,即头部为三角形,当该值小于0时,头部为菱形,当值大于0时,头部为鱼尾状
230     """
231     plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
232               head_length=1.5 * width, head_width=width)
233     plt.plot(x, y)
234 
235 
236 def main():
237     """
238     主函数
239     :return:
240     """
241     # print(__file__ + " start!!")
242     # 初始化位置空间
243     x = np.array([0.0, 0.0, math.pi / 2.0, 0.2, 0.0])
244 
245     goal = np.array([10, 10])
246 
247     # matrix二维矩阵
248     # ob = np.matrix([[-1, -1],
249     #                 [0, 2],
250     #                 [4.0, 2.0],
251     #                 [5.0, 4.0],
252     #                 [5.0, 5.0],
253     #                 [5.0, 6.0],
254     #                 [5.0, 9.0],
255     #                 [8.0, 9.0],
256     #                 [7.0, 9.0],
257     #                 [12.0, 12.0]
258     #                 ])
259     ob = np.matrix([[0, 2]])
260     u = np.array([0.2, 0.0])
261     config = Config()
262     trajectory = np.array(x)
263 
264     for i in range(1000):
265 
266         u, best_trajectory = dwa_control(x, u, config, goal, ob)
267 
268         x = motion(x, u, config.dt)
269         print(x)
270 
271         trajectory = np.vstack((trajectory, x))  # store state history
272 
273         if show_animation:
274             draw_dynamic_search(best_trajectory, x, goal, ob)
275 
276         # check goal
277         if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
278             print("Goal!!")
279 
280             break
281 
282     print("Done")
283 
284     draw_path(trajectory, goal, ob, x)
285 
286 
287 def draw_dynamic_search(best_trajectory, x, goal, ob):
288     """
289     画出动态搜索过程图
290     :return:
291     """
292     plt.cla()  # 清除上次绘制图像
293     plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
294     plt.plot(x[0], x[1], "xr")
295     plt.plot(0, 0, "og")
296     plt.plot(goal[0], goal[1], "ro")
297     plt.plot(ob[:, 0], ob[:, 1], "bs")
298     plot_arrow(x[0], x[1], x[2])
299     plt.axis("equal")
300     plt.grid(True)
301     plt.pause(0.0001)
302 
303 
304 def draw_path(trajectory, goal, ob, x):
305     """
306     画图函数
307     :return:
308     """
309     plt.cla()  # 清除上次绘制图像
310 
311     plt.plot(x[0], x[1], "xr")
312     plt.plot(0, 0, "og")
313     plt.plot(goal[0], goal[1], "ro")
314     plt.plot(ob[:, 0], ob[:, 1], "bs")
315     plot_arrow(x[0], x[1], x[2])
316     plt.axis("equal")
317     plt.grid(True)
318     plt.plot(trajectory[:, 0], trajectory[:, 1], 'r')
319     plt.show()
320 
321 
322 if __name__ == '__main__':
323     main()

 

o
粉丝 0
博文 500
码字总数 0
作品 0
私信 提问
加载中
请先登录后再评论。

暂无文章

敖丙20 张图揭开内存管理的迷雾

前言 之前有不少读者跟我反馈,能不能写图解操作系统? 既然那么多读者想看,我最近就在疯狂的复习操作系统的知识。 操作系统确实是比较难啃的一门课,至少我认为比计算机网络难太多了,但它...

敖丙
07/02
11
0
拉勾网拉你上勾

预览 需求简介 拉勾网是一个互联网行业的一个招聘网站,上面有许多职位,于是乎,小编想提取指定职位的基本信息(职位名,薪水,工作经验,工作地点,教育背景),然后插入 MongoDB 数据库,...

木下瞳
2019/04/17
20
0
我是一个线程(第一人称)

来源 | 转自 码农翻身 作者 | 刘欣 全文总共 | 4600 字 预计阅读时间 | 12 分钟 第一回 初生牛犊 我是一个线程,我一出生就被编了个号:0x3704,然后被领到一个昏暗的屋子里,在这里我发现了...

geniusian
2019/11/04
16
0
SkyWalking 权限认证

版本:7.0.0 描述 为了数据传输安全,确保网络连接是安全的。采用 Token 认证确保采集的应用数据是被信任的。 当前版本,仅支持简单的字符串 Token 配置 代理端配置文件agent.config设置 # ...

zm123321
43分钟前
17
0
是否允许实体正文进行HTTP DELETE请求? - Is an entity body allowed for an HTTP DELETE request?

问题: When issuing an HTTP DELETE request, the request URI should completely identify the resource to delete. 发出HTTP DELETE请求时,请求URI应该完全标识要删除的资源。 However,......

javail
昨天
27
0

没有更多内容

加载失败,请刷新页面

加载更多

返回顶部
顶部