• dwa 设置多个目标点,倒车设计


    参考链接: https://blog.csdn.net/peakzuo/article/details/86487923   原理分析

                       https://blog.csdn.net/aihuo7077/article/details/101361499   python代码参考

    简单修改后的代码如下:

    """
    version1.1
    Mobile robot motion planning sample with Dynamic Window Approach
    """

    import math
    import numpy as np
    import matplotlib.pyplot as plt

    show_animation = True # real time drawing
    start = 0 # go through point

    class Config(object):
    """
    Parameters used for simulation
    """

    def __init__(self):
    # robot parameter
    self.max_speed = 2.0 # [m/s] # max v
    # self.min_speed = -0.5 # [m/s] # min v Set to reverse
    self.min_speed = -2.0 # [m/s] # min v Set to can not reverse
    self.max_yawrate = 40.0 * math.pi / 180.0 # [rad/s] # max angle speed
    self.max_accel = 0.2 # [m/ss] # max a
    self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss] # max yaw a
    self.v_reso = 0.01 # [m/s] Speed resolution
    self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s] yaw Speed resolution
    self.dt = 0.1 # [s] # sampling time T
    self.predict_time = 3.0 # [s] # Three seconds predict
    self.to_goal_cost_gain = 1.0 # Target cost gain
    self.speed_cost_gain = 1.0 # Target cost reduction
    self.robot_radius = 1.0 # [m] # robor radius


    def motion(x, u, dt):
    """
    :param x: positon
    :param u: (w, v)
    :param dt: time
    :return:
    """
    # The velocity updating formula is simple, and the vehicle displacement changes greatly in a very short time
    #
    x[0] += u[0] * math.cos(x[2]) * dt # x
    x[1] += u[0] * math.sin(x[2]) * dt # y
    x[2] += u[1] * dt # heading
    x[3] = u[0] # v
    x[4] = u[1] # w
    # print(x)

    return x


    def calc_dynamic_window(x, config, start):
    """

    """
    # if start == 1:
    # config.max_speed == -1.4

    # max_v, min_v
    vs = [config.min_speed, config.max_speed,
    -config.max_yawrate, config.max_yawrate]

    # max_v, min_v
    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]
    # print(Vs, Vd)

    #
    vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
    max(vs[2], vd[2]), min(vs[3], vd[3])]
    # if start == 1:
    # print(vr)
    return vr


    def calc_trajectory(x_init, v, w, config):
    """
    Predict trajectory in 3 seconds
    :param x_init:postion space
    :param v:v
    :param w:w
    :param config:
    :return: trajectory point
    """
    x = np.array(x_init)
    trajectory = np.array(x)
    time = 0
    while time <= config.predict_time:
    x = motion(x, [v, w], config.dt)
    trajectory = np.vstack((trajectory, x)) # vertical
    time += config.dt

    # print(trajectory)
    return trajectory


    def calc_to_goal_cost(trajectory, goal, config):
    """
    Calculate the cost from trajectory to target point
    :param trajectory:
    :param goal:
    :param config:
    :return: Euclidean distance from track to target point
    """
    # calc to goal cost. It is 2D norm.

    dx = goal[0] - trajectory[-1, 0]
    dy = goal[1] - trajectory[-1, 1]
    goal_dis = math.sqrt(dx ** 2 + dy ** 2)
    cost = config.to_goal_cost_gain * goal_dis

    return cost


    def calc_obstacle_cost(traj, ob, config):
    """
    min Euclidean distance from track to target point dist(v,w)
    :param traj:
    :param ob:
    :param config:
    :return:
    """
    # calc obstacle cost inf: collision 0:free

    min_r = float("inf") #

    for ii in range(0, len(traj[:, 1])):
    for i in range(len(ob[:, 0])):
    ox = ob[i, 0]
    oy = ob[i, 1]
    dx = traj[ii, 0] - ox
    dy = traj[ii, 1] - oy

    r = math.sqrt(dx ** 2 + dy ** 2)
    if r <= config.robot_radius:
    return float("Inf") # collision

    if min_r >= r:
    min_r = r

    return 1.0 / min_r #


    def calc_final_input(x, u, vr, config, goal, ob):
    """

    :param x:
    :param u:
    :param vr:
    :param config:
    :param goal:
    :param ob:
    :return:
    """
    x_init = x[:]
    min_cost = 10000.0
    min_u = u

    best_trajectory = np.array([x])

    # evaluate all trajectory with sampled input in dynamic window
    # v,w
    for v in np.arange(vr[0], vr[1], config.v_reso):
    for w in np.arange(vr[2], vr[3], config.yawrate_reso):

    trajectory = calc_trajectory(x_init, v, w, config)

    # calc cost
    to_goal_cost = calc_to_goal_cost(trajectory, goal, config)
    speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
    ob_cost = calc_obstacle_cost(trajectory, ob, config)
    # print(ob_cost)

    #
    #
    final_cost = to_goal_cost + speed_cost + ob_cost

    # search minimum trajectory
    if min_cost >= final_cost:
    min_cost = final_cost
    min_u = [v, w]
    best_trajectory = trajectory

    # print(min_u)
    # input()

    return min_u, best_trajectory


    def dwa_control(x, u, config, goal, ob):
    """

    :param x:
    :param u:
    :param config:
    :param goal:
    :param ob:
    :return:
    """
    # Dynamic Window control

    vr = calc_dynamic_window(x, config, start)

    u, trajectory = calc_final_input(x, u, vr, config, goal, ob)

    return u, trajectory


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

    :param x:
    :param y:
    :param yaw:
    :param length:
    :param .001
    :return:
    length_includes_head
    """
    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
    head_length=1.5 * width, head_width=width)
    plt.plot(x, y)


    def main():
    """

    :return:
    """
    global start
    # print(__file__ + " start!!")
    #
    x = np.array([0.0, 0.0, math.pi / 2.0, 0.2, 0.0])

    goal = np.array([10, 3])  # 第一个目标点
    goal2 = np.array([7.5, 10])  # 第二个目标点

    #
    # ob = np.matrix([[-1, -1],
    # [0, 2],
    # [4.0, 2.0],
    # [5.0, 4.0],
    # [5.0, 5.0],
    # [5.0, 6.0],
    # [5.0, 9.0],
    # [8.0, 9.0],
    # [7.0, 9.0],
    # [12.0, 12.0]
    # ])
    ob = np.matrix([[0, 5], [2, 8]])
    u = np.array([0.2, 0.0])
    config = Config()
    trajectory = np.array(x)

    for i in range(1000):
    # if start == 0:
    u, best_trajectory = dwa_control(x, u, config, goal, ob)
    # if start == 1:
    # u, best_trajectory = dwa_control(x, u, config, goal2, ob)
    print(u[0])
    x = motion(x, u, config.dt)
    # print(x)

    trajectory = np.vstack((trajectory, x)) # store state history

    # if show_animation and start == 0:
    draw_dynamic_search(best_trajectory, x, goal, ob)
    # if start == 1 and show_animation:
    # draw_dynamic_search(best_trajectory, x, goal2, ob)

    # check goal
    if math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2) <= config.robot_radius:
    print("Goal1!")
    start = 1
    # #
    # if math.sqrt((x[0] - goal2[0]) ** 2 + (x[1] - goal2[1]) ** 2) <= config.robot_radius:
    # print("Goal2!")
    break

    x = np.array([x[0], x[1], x[2], -0.2, x[4]])  # 设置初始速度为负,可进行倒车
    for i in range(1000):
    # if start == 0:
    u, best_trajectory = dwa_control(x, u, config, goal2, ob)
    # if start == 1:
    # u, best_trajectory = dwa_control(x, u, config, goal2, ob)
    print(u[0])
    x = motion(x, u, config.dt)
    # print(x)

    trajectory = np.vstack((trajectory, x)) # store state history

    # if show_animation and start == 0:
    draw_dynamic_search(best_trajectory, x, goal2, ob)
    # if start == 1 and show_animation:
    # draw_dynamic_search(best_trajectory, x, goal2, ob)

    # check goal
    if math.sqrt((x[0] - goal2[0]) ** 2 + (x[1] - goal2[1]) ** 2) <= config.robot_radius:
    print("Goal1!")
    start = 1
    # #
    # if math.sqrt((x[0] - goal2[0]) ** 2 + (x[1] - goal2[1]) ** 2) <= config.robot_radius:
    # print("Goal2!")
    break

    print("Done")

    # draw_path(trajectory, goal, ob, x)
    draw_path(trajectory, goal2, ob, x)


    def draw_dynamic_search(best_trajectory, x, goal, ob):
    """

    :return:
    """
    plt.cla() #
    plt.plot(best_trajectory[:, 0], best_trajectory[:, 1], "-g")
    plt.plot(x[0], x[1], "xr")
    plt.plot(0, 0, "og")
    plt.plot(goal[0], goal[1], "ro")
    plt.plot(ob[:, 0], ob[:, 1], "bs")
    plot_arrow(x[0], x[1], x[2])
    plt.axis("equal")
    plt.grid(True)
    plt.pause(0.0001)


    def draw_path(trajectory, goal, ob, x):
    """

    :return:
    """
    plt.cla() #

    plt.plot(x[0], x[1], "xr")
    plt.plot(0, 0, "og")
    plt.plot(goal[0], goal[1], "ro")
    plt.plot(ob[:, 0], ob[:, 1], "bs")
    plot_arrow(x[0], x[1], x[2])
    plt.axis("equal")
    plt.grid(True)
    plt.plot(trajectory[:, 0], trajectory[:, 1], 'r')
    plt.show()


    if __name__ == '__main__':
    main()

  • 相关阅读:
    JavaWeb——库存管理系统(1)jsp部分---18.12.13
    Java——多线程---18.11.22
    Java——异常处理---18.11.14
    Java——final代码块是否一定被执行---18.11.08
    暑期的周总结们
    Javaweb——四则运算---18.11.01
    Java——equals方法---18.10.18
    微信小程序记账本进度六
    微信小程序记账本进度七
    微信小程序记账本进度一
  • 原文地址:https://www.cnblogs.com/yang220/p/13095449.html
Copyright © 2020-2023  润新知