Python 2D 路径规划算法 - 动态窗口方法，机器人在达到目标之前失速

问：

我一直在尝试通过从在线一些来源定制一个类似的示例来为差速驱动机器人实现 DWA pp 算法。问题是机器人似乎与目标目的地一致移动，然后似乎停滞不前，再次查看数学/代码并更改权重。我觉得我忽略了一些非常简单的事情。

AtsushiSakai Github Repo Andrew Davison 的实现

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

show_animation = True


def dwa_control(x, config, goal, ob):
    """
    Dynamic Window Approach control
    """
    dw = calc_dynamic_window(x, config)

    u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)

    return u, trajectory


class RobotType(Enum):
    circle = 0


class Config:
    """
    simulation parameter class
    """

    def __init__(self, objects=[]):
         # robot parameter
         self.max_speed = 1  # [m/s]
         self.min_speed = 0.1  # [m/s]
         self.max_yaw_rate = 40.0 * math.pi / 180.0  # [rad/s]
         self.max_accel = 0.5  # [m/ss]
         self.max_delta_yaw_rate = 40.0 * math.pi / 180.0  # [rad/ss]
         self.v_resolution = 0.01  # [m/s]
         self.yaw_rate_resolution = 0.001 * math.pi / 180.0  # [rad/s]
         self.dt = 0.1  # [s] Time tick for motion prediction
         self.predict_time = 3.0  # [s]
         
         self.to_goal_cost_gain = 1
         self.speed_cost_gain = 1
         self.obstacle_cost_gain = 0.2
         
         self.robot_stuck_flag_cons = 0.1  # constant to prevent robot being stuck
         self.robot_radius = 0.08  # [m] for collision check

         ########################################
         #### Create the boarder ################
         ########################################
         # Set the interval and the half-length of the square
         interval = 0.1  # Set the interval
         half_length = 1.5  # Half of the square's side length

         # Define the coordinates for the four corners of the square
         corners = np.array([[-half_length, -half_length],
                           [-half_length, half_length],
                           [half_length, half_length],
                           [half_length, -half_length],
                           [-half_length, -half_length]])  # Close the square

         # Initialize lists to hold the x and y coordinates of the perimeter points
         x_coords = []
         y_coords = []

         # Loop over each edge of the square
         for i in range(4):
            # Get the x and y coordinates of the start and end points of the edge
            x_start, y_start = corners[i]
            x_end, y_end = corners[i+1]
            
            # Compute the number of points on the edge (excluding the end point)
            num_points = int(np.linalg.norm([x_end - x_start, y_end - y_start]) / interval)
            
            # Generate the x and y coordinates of the points along the edge
            x_edge = np.linspace(x_start, x_end, num_points+1, endpoint=False)
            y_edge = np.linspace(y_start, y_end, num_points+1, endpoint=False)
            
            # Add the generated coordinates to the lists
            x_coords.extend(x_edge)
            y_coords.extend(y_edge)

         # Convert the lists of x and y coordinates to numpy arrays
         x_coords = np.array(x_coords)
         y_coords = np.array(y_coords)

         # Stack the x and y coordinates horizontally to get the perimeter coordinates
         perimeter_coords = np.column_stack((x_coords, y_coords))
         ########################################
         #### Create the boarder ################
         ########################################
         
         original_list =[[0.8969064108491706, -0.5651451250864348],
                        [1.0854540479797645, -0.004174765982885018],
                        [1.3828062660399116, 0.451466868665515],
                        [1.1216589663314864, 1.238792062624063],
                        [0.2084636335510397, 0.7959588658531208],
                        [-1.1115975896775006, 0.883937535573489],
                        [-1.0916288401200633, -0.031936362295673044],
                        [-0.6641606999682717, -0.47796167568644166],
                        [-0.06879411068633204, -0.30915717155985245],
                        [0.1899610618971512, -0.9471530659845089],
                        [0.760028409037262, 0.6834467529016156],
                        [1.3510913382496061, 1.0674145068585537],
                        [1.3759976980309085, -0.4530586281364102],
                        [0.7631617249072551, -0.9427117705816841],
                        [0.28885775051621065, -0.3788668917799926],
                        [-0.9452180429272703, -1.0168972472130489],
                        [-0.9078708465646994, 0.10253493795037877],
                        [-1.1256202508375832, 1.265576336316949],
                        [-0.2858945061253501, 0.19947932814292693],
                        [-0.2236097796312978, 1.0420796482650274]]


         # Concatenate the perimeter coordinates and the scaled obstacles
         self.ob = np.concatenate((original_list, perimeter_coords), axis=0)


config = Config()


def motion(x, u, dt, operate={}):
      """
      motion model: Differential Drive Robot
      """
      # u: control input [v(m/s), w(rad/m)]
      # x: state    [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
      # dt: change in time

      # dt = drive_meas.dt
      #   if angular_velocity == 0:
      #       self.state[0] += np.cos(self.state[2]) * linear_velocity * dt
      #       self.state[1] += np.sin(self.state[2]) * linear_velocity * dt
      #   else:
      #       th = self.state[2]
      #       self.state[0] += linear_velocity / angular_velocity * (np.sin(th+dt*angular_velocity) - np.sin(th))
      #       self.state[1] += -linear_velocity / angular_velocity * (np.cos(th+dt*angular_velocity) - np.cos(th))
      #       self.state[2] += dt*angular_velocity
      
   
      # Check for zero angular velocity to avoid division by zero
      if u[1] == 0:
          x[0] += u[0] * math.cos(x[2]) * dt  # Update x-position
          x[1] += u[0] * math.sin(x[2]) * dt  # Update y-position
      else:
          th = x[2]
          x[0] += u[0] / u[1] * (math.sin(th + dt * u[1]) - math.sin(th))  # Update x-position
          x[1] += -u[0] / u[1] * (math.cos(th + dt * u[1]) - math.cos(th))  # Update y-position
          x[2] += dt * u[1]  # Update yaw

      x[3] = u[0]  # Update linear velocity
      x[4] = u[1]  # Update angular velocity

      return x


def calc_dynamic_window(x, config):
    """
    calculation dynamic window based on current state x 
    """
    # Dynamic window from robot specification
    Vs = [config.min_speed, config.max_speed, -config.max_yaw_rate, config.max_yaw_rate]
    # Dynamic window from motion model
    Vd = [x[3] - config.max_accel * config.dt, x[3] + config.max_accel * config.dt, x[4] - config.max_delta_yaw_rate * config.dt, x[4] + config.max_delta_yaw_rate * config.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(x_init, v, y, config):
      """
      predict trajectory with an input
      """
      # x_init is the initial state 
      # v is the linear velocity control input.
      # y is the angular velocity control input.
      # config is the conf object

      x = np.array(x_init)
      trajectory = np.array(x)
      time = 0
      while time <= config.predict_time:
         x = motion(x, [v, y], config.dt)
         trajectory = np.vstack((trajectory, x))
         time += config.dt

      return trajectory



def calc_control_and_trajectory(x, dw, config, goal, ob):
    """
    calculation final input with dynamic window
    """

    x_init = x[:]
    min_cost = float("inf")
    best_u = [0.0, 0.0]
    best_trajectory = np.array([x])

    # evaluate all trajectory with sampled input in dynamic window
    for v in np.arange(dw[0], dw[1], config.v_resolution):
        for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):

            trajectory = predict_trajectory(x_init, v, y, config)
            # calc cost
            to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
            speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
            ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)

            final_cost = to_goal_cost + speed_cost + ob_cost

            # search minimum trajectory
            if min_cost >= final_cost:
                min_cost = final_cost
                best_u = [v, y]
                best_trajectory = trajectory
                if abs(best_u[0]) < config.robot_stuck_flag_cons \
                        and abs(x[3]) < config.robot_stuck_flag_cons:
                    # to ensure the robot do not get stuck in
                    # best v=0 m/s (in front of an obstacle) and
                    # best omega=0 rad/s (heading to the goal with
                    # angle difference of 0)
                    best_u[1] = -config.max_delta_yaw_rate
    return best_u, best_trajectory





def calc_obstacle_cost(trajectory, ob, config):
   """
   calc obstacle cost inf: collision
   """
   ox = ob[:, 0]
   oy = ob[:, 1]
   dx = trajectory[:, 0] - ox[:, None]
   dy = trajectory[:, 1] - oy[:, None]
   r = np.hypot(dx, dy)

   if np.array(r <= config.robot_radius).any():
      return float("Inf")

   min_r = np.min(r)
   return 1.0 / min_r  # OK






def calc_to_goal_cost(trajectory, goal):
    """
        calc to goal cost with angle difference
    """

    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





####################################
######### Plot Functions ###########
####################################

def plot_arrow(x, y, yaw, length=0.1, width=0.01):  # pragma: no cover
    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(x, y, yaw, config):  # pragma: no cover
   circle = plt.Circle((x, y), config.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)]) * config.robot_radius)
   plt.plot([x, out_x], [y, out_y], "-k")

####################################
######### Plot Functions ###########
####################################






def main(gx=1.0, gy=1.0):
    print(__file__ + " start!!")
    # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
    # goal position [x(m), y(m)]
    goal = np.array([gx, gy])

    # input [forward speed, yaw_rate]

    trajectory = np.array(x)
    ob = config.ob
    while True:
        u, predicted_trajectory = dwa_control(x, config, goal, ob)
        x = motion(x, u, config.dt)  # simulate robot
        trajectory = np.vstack((trajectory, x))  # store state history

        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.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
            plt.plot(x[0], x[1], "xr")
            plt.plot(goal[0], goal[1], "xb")
            plt.plot(ob[:, 0], ob[:, 1], "ok")
            plot_robot(x[0], x[1], x[2], config)
            plot_arrow(x[0], x[1], x[2])
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.0001)

        # check reaching goal
        dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])
        if dist_to_goal <= config.robot_radius:
            print("Goal!!")
            break

    print("Done")
    if show_animation:
        plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")
        plt.pause(0.0001)
        plt.show()

我试图更改参数，即......

self.max_speed = 1  # [m/s]
         self.min_speed = 0.1  # [m/s]
         self.max_yaw_rate = 40.0 * math.pi / 180.0  # [rad/s]
         self.max_accel = 0.5  # [m/ss]
         self.max_delta_yaw_rate = 40.0 * math.pi / 180.0  # [rad/ss]
         self.v_resolution = 0.01  # [m/s]
         self.yaw_rate_resolution = 0.001 * math.pi / 180.0  # [rad/s]
         self.dt = 0.1  # [s] Time tick for motion prediction
         self.predict_time = 3.0  # [s]
         
         self.to_goal_cost_gain = 1
         self.speed_cost_gain = 1
         self.obstacle_cost_gain = 0.2
         
         self.robot_stuck_flag_cons = 0.1  # constant to prevent robot being stuck
         self.robot_radius = 0.08  # [m] for collision check

然而，所有组合都没有效果。在我看来，当机器人θ向零前进时，建议的轨迹也有零轨迹或类似的东西。