（详细）—— 机器人 —— DWA(dynamic window approach) —— 路径规划算法 —— Python代码

具体解释：

https://blog.csdn.net/m0_46578404/article/details/147053201

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as patches
from matplotlib import animation
import math
import random
 
 
# 机器人参数
class Config:
    def __init__(self):
        # 机器人参数
        self.max_speed = 1.0  # [m/s] 最大速度
        self.min_speed = -0.5  # [m/s] 最小速度
        self.max_yaw_rate = 40.0 * math.pi / 180.0  # [rad/s] 最大偏航率
        self.max_accel = 0.2  # [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.1 * math.pi / 180.0  # [rad/s] 偏航率分辨率
        self.dt = 0.1  # [s] 时间间隔
        self.predict_time = 3.0  # [s] 预测时间
        self.to_goal_cost_gain = 0.15  # 目标代价增益
        self.speed_cost_gain = 1.0  # 速度代价增益
        self.obstacle_cost_gain = 1.0  # 障碍物代价增益
        self.robot_radius = 0.5  # [m] 机器人半径
        self.goal_tolerance = 0.2  # [m] 目标容忍度
 
 
# 生成随机障碍物地图
def generate_map(width=20, height=20, num_obstacles=25):
    obstacles = []
 
    # 边界障碍物
    for x in range(width):
        obstacles.append([x, 0])
        obstacles.append([x, height - 1])
    for y in range(height):
        obstacles.append([0, y])
        obstacles.append([width - 1, y])
 
    # 随机障碍物
    for _ in range(num_obstacles):
        x = random.randint(1, width - 2)
        y = random.randint(1, height - 2)
        obstacles.append([x, y])
 
    return np.array(obstacles)
 
 
# 运动模型
def motion_model(x, u, dt):
    """
    机器人运动模型
    :param x: 状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    :param u: 控制输入 [v(m/s), omega(rad/s)]
    :param dt: 时间间隔
    :return: 新状态
    """
    x[2] += u[1] * dt  # 更新角度
    x[0] += u[0] * math.cos(x[2]) * dt  # 更新x位置
    x[1] += u[0] * math.sin(x[2]) * dt  # 更新y位置
    x[3] = u[0]  # 更新速度
    x[4] = u[1]  # 更新角速度
    return x
 
 
# 计算动态窗口
def calc_dynamic_window(x, config):
    """
    计算动态窗口
    :param x: 状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    :param config: 配置参数
    :return: 动态窗口 [min_v, max_v, min_yaw_rate, max_yaw_rate]
    """
    # 速度动态窗口
    vs = [config.min_speed, config.max_speed,
          -config.max_yaw_rate, config.max_yaw_rate]
 
    # 基于当前速度和加速度的动态窗口
    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]
 
    # 最终动态窗口
    vr = [max(vs[0], vd[0]), min(vs[1], vd[1]),
          max(vs[2], vd[2]), min(vs[3], vd[3])]
 
    return vr
 
 
# 计算轨迹
def calc_trajectory(x_init, v, yaw_rate, config):
    """
    计算轨迹
    :param x_init: 初始状态
    :param v: 速度
    :param yaw_rate: 偏航率
    :param config: 配置参数
    :return: 轨迹
    """
    x = np.array(x_init)
    traj = np.array(x)
    time = 0
 
    while time <= config.predict_time:
        x = motion_model(x, [v, yaw_rate], config.dt)
        traj = np.vstack((traj, x))
        time += config.dt
 
    return traj
 
 
# 计算障碍物代价
def calc_obstacle_cost(traj, obstacles, config):
    """
    计算障碍物代价
    :param traj: 轨迹
    :param obstacles: 障碍物列表
    :param config: 配置参数
    :return: 障碍物代价
    """
    ox = obstacles[:, 0]
    oy = obstacles[:, 1]
    dx = traj[:, 0] - ox[:, None]
    dy = traj[:, 1] - oy[:, None]
    r = np.hypot(dx, dy)
 
    if np.min(r) <= config.robot_radius:
        return float("inf")
 
    return 1.0 / np.min(r)  # 距离障碍物越近，代价越大
 
 
 
 
# 计算速度代价
def calc_speed_cost(traj, config):
    """
    计算速度代价
    :param traj: 轨迹
    :param config: 配置参数
    :return: 速度代价
    """
    return config.max_speed - traj[-1, 3]
 
 
# 评估轨迹
def evaluate_trajectory(traj, goal, obstacles, config):
    """
    改进版的轨迹评估函数
    :param traj: 轨迹
    :param goal: 目标位置
    :param obstacles: 障碍物列表
    :param config: 配置参数
    :return: 总代价
    """
    # 计算到终点的距离
    final_dist = math.hypot(traj[-1, 0] - goal[0], traj[-1, 1] - goal[1])
 
    # 动态调整目标代价增益：距离越近，权重越大
    dynamic_gain = config.to_goal_cost_gain * (1 + 5.0 / (final_dist + 0.1))
 
    # 目标代价（距离越近代价越小）
    to_goal_cost = dynamic_gain * final_dist
 
    # 终点奖励（当非常接近目标时给予奖励）
    if final_dist < config.robot_radius * 2:
        to_goal_cost *= 0.1  # 大幅降低代价
 
    # 速度代价（鼓励保持适当速度）
    speed_cost = config.speed_cost_gain * (config.max_speed - traj[-1, 3])
 
    # 障碍物代价
    obstacle_cost = config.obstacle_cost_gain * calc_obstacle_cost(traj, obstacles, config)
 
    return to_goal_cost + speed_cost + obstacle_cost
 
 
def calc_to_goal_cost(traj, goal):
    """
    改进版的目标点代价计算
    :param traj: 轨迹
    :param goal: 目标位置 [x(m), y(m)]
    :return: 目标代价（非负值）
    """
    # 计算轨迹终点到目标的距离
    dx = goal[0] - traj[-1, 0]
    dy = goal[1] - traj[-1, 1]
    dist = math.hypot(dx, dy)
 
    # 分级代价函数
    if dist < 0.5:  # 非常接近目标
        return dist * 0.1  # 极低代价
    elif dist < 2.0:  # 接近目标
        return dist * 0.5
    else:  # 远离目标
        return dist
 
 
# DWA算法
def dwa_control(x, goal, obstacles, config):
    """
    DWA算法
    :param x: 状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    :param goal: 目标位置 [x(m), y(m)]
    :param obstacles: 障碍物列表
    :param config: 配置参数
    :return: 最优控制 [v(m/s), omega(rad/s)], 最优轨迹
    """
    # 计算动态窗口
    vr = calc_dynamic_window(x, config)
 
    # 评估所有可能的轨迹
    min_cost = float("inf")
    best_u = [0.0, 0.0]
    best_traj = np.array([x])
 
    # 遍历所有可能的速度和偏航率
    for v in np.arange(vr[0], vr[1], config.v_resolution):
        for yaw_rate in np.arange(vr[2], vr[3], config.yaw_rate_resolution):
            # 计算轨迹
            traj = calc_trajectory(x, v, yaw_rate, config)
 
            # 计算代价
            cost = evaluate_trajectory(traj, goal, obstacles, config)
 
            # 更新最优轨迹
            if cost < min_cost:
                min_cost = cost
                best_u = [v, yaw_rate]
                best_traj = traj
 
    return best_u, best_traj
 
 
 
def main():
    # 初始化配置
    config = Config()
 
    # 初始状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    x = np.array([2.0, 2.0, math.pi / 8.0, 0.0, 0.0])
    goal = np.array([18.0, 18.0])
    obstacles = generate_map(20, 20, 25)
 
    # 启用交互模式
    plt.ion()
    fig, ax = plt.subplots(figsize=(10, 10))
    ax.set_xlim(0, 20)
    ax.set_ylim(0, 20)
    ax.set_aspect('equal')
    ax.grid(True)
 
    # 绘制障碍物和目标
    ax.plot(obstacles[:, 0], obstacles[:, 1], 'sk', markersize=10)
    ax.plot(goal[0], goal[1], 'xr', markersize=15)
 
    # 初始化机器人显示
    robot = patches.Circle((x[0], x[1]), config.robot_radius,
                           fc='g', ec='k', alpha=0.5, label='Robot')
    ax.add_patch(robot)
    trajectory_line, = ax.plot([], [], '-b', linewidth=2, label='Trajectory')
    best_traj_line, = ax.plot([], [], '--g', linewidth=1, alpha=0.5, label='Best Trajectory')
    ax.legend(loc='upper right')
 
    trajectory = [x[:2]]
 
    for _ in range(1000):
        # DWA控制
        u, predicted_traj = dwa_control(x, goal, obstacles, config)
        x = motion_model(x, u, config.dt)
        trajectory.append(x[:2])
 
        # 更新图形
        robot.center = (x[0], x[1])
        trajectory_line.set_data(*zip(*trajectory))
        best_traj_line.set_data(predicted_traj[:, 0], predicted_traj[:, 1])
 
        plt.pause(0.05)  # 控制更新速度
 
        # 检查是否到达目标
        dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])
        if dist_to_goal <= config.goal_tolerance:
            print("Goal reached!")
            break
 
    plt.ioff()
    plt.show()
 
 
if __name__ == '__main__':
    main()