动态窗口法

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

 

转载于:https://www.cnblogs.com/yangmingustb/p/9575141.html

  •                     <li class="tool-item tool-active is-like "><a href="javascript:;"><svg class="icon" aria-hidden="true">
                            <use xlink:href="#csdnc-thumbsup"></use>
                        </svg><span class="name">点赞</span>
                        <span class="count"></span>
                        </a></li>
                        <li class="tool-item tool-active is-collection "><a href="javascript:;" data-report-click="{&quot;mod&quot;:&quot;popu_824&quot;}"><svg class="icon" aria-hidden="true">
                            <use xlink:href="#icon-csdnc-Collection-G"></use>
                        </svg><span class="name">收藏</span></a></li>
                        <li class="tool-item tool-active is-share"><a href="javascript:;"><svg class="icon" aria-hidden="true">
                            <use xlink:href="#icon-csdnc-fenxiang"></use>
                        </svg>分享</a></li>
                        <!--打赏开始-->
                                                <!--打赏结束-->
                                                <li class="tool-item tool-more">
                            <a>
                            <svg t="1575545411852" class="icon" viewBox="0 0 1024 1024" version="1.1" xmlns="http://www.w3.org/2000/svg" p-id="5717" xmlns:xlink="http://www.w3.org/1999/xlink" width="200" height="200"><defs><style type="text/css"></style></defs><path d="M179.176 499.222m-113.245 0a113.245 113.245 0 1 0 226.49 0 113.245 113.245 0 1 0-226.49 0Z" p-id="5718"></path><path d="M509.684 499.222m-113.245 0a113.245 113.245 0 1 0 226.49 0 113.245 113.245 0 1 0-226.49 0Z" p-id="5719"></path><path d="M846.175 499.222m-113.245 0a113.245 113.245 0 1 0 226.49 0 113.245 113.245 0 1 0-226.49 0Z" p-id="5720"></path></svg>
                            </a>
                            <ul class="more-box">
                                <li class="item"><a class="article-report">文章举报</a></li>
                            </ul>
                        </li>
                                            </ul>
                </div>
                            </div>
            <div class="person-messagebox">
                <div class="left-message"><a href="https://blog.csdn.net/aihuo7077">
                    <img src="https://profile.csdnimg.cn/3/9/0/3_aihuo7077" class="avatar_pic" username="aihuo7077">
                                            <img src="https://g.csdnimg.cn/static/user-reg-year/2x/4.png" class="user-years">
                                    </a></div>
                <div class="middle-message">
                                        <div class="title"><span class="tit"><a href="https://blog.csdn.net/aihuo7077" data-report-click="{&quot;mod&quot;:&quot;popu_379&quot;}" target="_blank">aihuo7077</a></span>
                                            </div>
                    <div class="text"><span>发布了0 篇原创文章</span> · <span>获赞 0</span> · <span>访问量 495</span></div>
                </div>
                                <div class="right-message">
                                            <a href="https://im.csdn.net/im/main.html?userName=aihuo7077" target="_blank" class="btn btn-sm btn-red-hollow bt-button personal-letter">私信
                        </a>
                                                            <a class="btn btn-sm  bt-button personal-watch" data-report-click="{&quot;mod&quot;:&quot;popu_379&quot;}">关注</a>
                                    </div>
                            </div>
        </div>
    
已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页