动态窗口法_version1.3

  1 """
  2 version1.3
  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     trajectory_space = np.array([x])  # 记录搜索所有采样的轨迹,用来画图
168
169     # evaluate all trajectory with sampled input in dynamic window
170     # v,生成一系列速度,w,生成一系列角速度
171     for v in np.arange(vr[0], vr[1], config.v_reso):
172         for w in np.arange(vr[2], vr[3], config.yawrate_reso):
173
174             trajectory = calc_trajectory(x_init, v, w, config)
175
176             trajectory_space = np.vstack((trajectory_space, trajectory))
177
178             # calc cost
179             to_goal_cost = calc_to_goal_cost(trajectory, goal, config)
180             speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
181             ob_cost = calc_obstacle_cost(trajectory, ob, config)
182             #  print(ob_cost)
183
184             # 评价函数多种多样,看自己选择
185             # 本文构造的是越小越好
186             final_cost = to_goal_cost + speed_cost + ob_cost
187
188             # search minimum trajectory
189             if min_cost >= final_cost:
190                 min_cost = final_cost
191                 min_u = [v, w]
192                 best_trajectory = trajectory
193
194     # print(min_u)
195     #  input()
196
197     return min_u, best_trajectory, trajectory_space
198
199
200 def dwa_control(x, u, config, goal, ob):
201     """
202     调用前面的几个函数,生成最合适的速度空间和轨迹搜索空间
203     :param x:
204     :param u:
205     :param config:
206     :param goal:
207     :param ob:
208     :return:
209     """
210     # Dynamic Window control
211
212     vr = calc_dynamic_window(x, config)
213
214     u, trajectory, trajectory_space = calc_final_input(x, u, vr, config, goal, ob)
215
216     return u, trajectory, trajectory_space
217
218
219 def plot_arrow(x, y, yaw, length=0.5, width=0.1):
220     """
221     arrow函数绘制箭头,表示搜索过程中选择的航向角
222     :param x:
223     :param y:
224     :param yaw:航向角
225     :param length:
226     :param width:参数值为浮点数,代表箭头尾部的宽度,默认值为0.001
227     :return:
228     length_includes_head:代表箭头整体长度是否包含箭头头部的长度,默认值为False
229     head_width:代表箭头头部的宽度,默认值为3*width,即尾部宽度的3倍
230     head_length:代表箭头头部的长度度,默认值为1.5*head_width,即头部宽度的1.5倍
231     shape:参数值为‘full‘、‘left‘、‘right‘,表示箭头的形状,默认值为‘full‘
232     overhang:代表箭头头部三角形底边与箭头尾部直接的夹角关系,通过该参数可改变箭头的形状。
233     默认值为0,即头部为三角形,当该值小于0时,头部为菱形,当值大于0时,头部为鱼尾状
234     """
235     plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
236               head_length=1.5 * width, head_width=width)
237     plt.plot(x, y)
238
239
240 def main():
241     """
242     主函数
243     :return:
244     """
245     # print(__file__ + " start!!")
246     # 初始化位置空间
247     x = np.array([0.0, 0.0, math.pi / 2.0, 0.2, 0.0])
248
249     goal = np.array([10, 10])
250
251     # matrix二维矩阵
252     ob = np.matrix([[-1, -1],
253                     [0, 2],
254                     [4.0, 2.0],
255                     [5.0, 4.0],
256                     [5.0, 5.0],
257                     [5.0, 6.0],
258                     [5.0, 9.0],
259                     [8.0, 9.0],
260                     [7.0, 9.0],
261                     [12.0, 12.0]
262                     ])
263     # ob = np.matrix([[0, 2]])
264     u = np.array([0.2, 0.0])
265     config = Config()
266     trajectory = np.array(x)
267
268     for i in range(1000):
269
270         u, best_trajectory, trajectory_space = dwa_control(x, u, config, goal, ob)
271
272         x = motion(x, u, config.dt)
273         # print(x)
274
275         trajectory = np.vstack((trajectory, x))  # store state history
276
277         if show_animation:
278             draw_dynamic_search(best_trajectory, x, goal, ob, trajectory_space)
279
280         # check goal
281         if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
282             print("Goal!!")
283
284             break
285
286     print("Done")
287
288     # draw_path(trajectory, goal, ob, x)
289
290
291 def draw_dynamic_search(best_trajectory, x, goal, ob, trajectory_space):
292     """
293     画出动态搜索过程图
294     :return:
295     """
296     plt.cla()  # 清除上次绘制图像
297     plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
298
299     plt.plot(trajectory_space[:, 0], trajectory_space[:, 1], ‘-g‘)
300
301     plt.plot(x[0], x[1], "xr")
302     plt.plot(0, 0, "og")
303     plt.plot(goal[0], goal[1], "ro")
304     plt.plot(ob[:, 0], ob[:, 1], "bs")
305     plot_arrow(x[0], x[1], x[2])
306     plt.axis("equal")
307     plt.grid(True)
308     plt.pause(0.0001)
309
310
311 def draw_path(trajectory, goal, ob, x):
312     """
313     画图函数
314     :return:
315     """
316     plt.cla()  # 清除上次绘制图像
317
318     plt.plot(x[0], x[1], "xr")
319     plt.plot(0, 0, "og")
320     plt.plot(goal[0], goal[1], "ro")
321     plt.plot(ob[:, 0], ob[:, 1], "bs")
322     plot_arrow(x[0], x[1], x[2])
323     plt.axis("equal")
324     plt.grid(True)
325     plt.plot(trajectory[:, 0], trajectory[:, 1], ‘r‘)
326     plt.show()
327
328
329 if __name__ == ‘__main__‘:
330     main()

原文地址:https://www.cnblogs.com/yangmingustb/p/8941166.html

时间: 2024-11-06 16:27:43

动态窗口法_version1.3的相关文章

ROS Navigation 局部避障的动态窗口法(DWA)

耳熟能详的局部避障动态窗口法(Dynamic Window Approach) 概述:机器人局比避障规划方法有很多,ROS中主要采用的是动态窗口法(和原来的论文中的dwa方法不一样).动态窗口法主要是在速度(v,w)空间中采样多组速度,并模拟机器人在这些速度下一定时间内的轨迹.在得到多组轨迹后,对这些轨迹进行评价,选取最优得轨迹所对应的速度来驱动机器人运动.该算法的图书店在于动态窗口这个名词,它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围. 1.机器人运动模型 见本博客

ROS动态窗口法(dwa)解析

dwa是很成熟的一套适用于差动轮式机器人的局部规划方法,项目需要,对其源码进行解析,看看有那些改进的点 有空把详细的解析发出来,自己理理思路 1.计算速度 2.获取轨迹 3.评价轨迹 4.评价机制 5.参数说明

ROS(14)----局部避障的动态窗口算法(DWA)

Dynamic Window Approach(DWA)是重要的局部轨迹规划算法,ROS中使用了DWA算法获得了很好的局部路径规划的效果.这里收集了些很不错的资料. 参考资料: [1]. 机器人局部避障的动态窗口法(dynamic window approach)(*****) [2]. Autonomous Robot Navigation [3]. Circular Motion in 2D for graphics and robotics [4]. A Tutorial and Elem

多尺度滑动窗口法,multiple-scale sliding window method

当前的人体检测技术,基本都会采用到多尺度滑动窗口法,该方法需要对图像做不同尺度的缩放,然后用固定大小的滑动窗口以等距步长在整幅图像上滑动,并对每一个滑动窗口做人体检测. 因此,这个方法的最大优点就是漏检率极低,因为它会对整幅图像都滑动,不会漏掉任何一个可能会出现人体的位置.但是这种优势,是用巨大的搜索空间和时间消耗换来的,检测效率自然会受到较大的影响.这个方法的大致原理,可用下面的流程图演示. 我们以大小为480*640的图像为例,缩放尺度为1.1,首先构建图片中最左侧的金字塔(构建金字塔的结束

无重复字符的最长子串——滑动窗口法?

给定一个字符串,请你找出其中不含有重复字符的 最长子串 的长度. 示例 1: 输入: "abcabcbb" 输出: 3 解释: 因为无重复字符的最长子串是 "abc",所以其长度为 3. 示例 2: 输入: "bbbbb" 输出: 1 解释: 因为无重复字符的最长子串是 "b",所以其长度为 1. 示例 3: 输入: "pwwkew" 输出: 3 解释: 因为无重复字符的最长子串是 "wke&qu

【python-leetcode904-滑动窗口法】水果成篮

问题描述: 在一排树中,第 i 棵树产生 tree[i] 型的水果.你可以从你选择的任何树开始,然后重复执行以下步骤:把这棵树上的水果放进你的篮子里.如果你做不到,就停下来.移动到当前树右侧的下一棵树.如果右边没有树,就停下来.请注意,在选择一颗树后,你没有任何选择:你必须执行步骤 1,然后执行步骤 2,然后返回步骤 1,然后执行步骤 2,依此类推,直至停止.你有两个篮子,每个篮子可以携带任何数量的水果,但你希望每个篮子只携带一种类型的水果.用这个程序你能收集的水果总量是多少? 示例 1: 输入

C#编写Unity基础GUI之动态窗口

1.创建窗口 1 //声明窗口位置大小 2 private Rect pos = new Rect(10,60,120,50); 3 4 //回调函数 5 void Wincallback(int id) 6 { 7 if(GUI.Button(new Rect(10,20,100,20),"窗口内按钮")){ 8 print("按钮在窗口内被单击"); 9 } 10 } 11 12 void OnGUI() 13 { 14 pos = GUI.Window(0,

“爱加密” 动态脱壳法

测试环境为: 安卓2.3 IDA6.6 下面看具体操作步骤: 具体怎样用IDA动态调试我就不多说了,网上己经有很多文章了,下面直接进入正题. 1.准备好调式环境后 用IDA附加进程. 2.附加成功后按"G"键 跳到dvmDexFileOpenPartial函数与fopen函数与fgets函数去下好断点. 3.下好断点后我们来说说它的反调试吧,下面我给两张图片大家可能就会明白它的反调试是怎么一回事了. 在调试状态下查看进程 TracerPid 不为0 在非调试状态下查看进程 Tracer

八连通(vector动态数组法)

题目和一般的八连通一样,但行数和列数未定,相乘对于1e6,直接开a[1e6][1e6]的数组肯定会爆内存.用二维的动态vector就能很好的解决这个问题 #include<bits/stdc++.h> using namespace std; int dx[10]={-1,0,1,-1,1,-1,0,1}; int dy[10]={1,1,1,0,0,-1,-1,-1}; int n,m; void dfs(vector< vector < bool > >&a