局部规划：DWA算法详解

DWA（Dynamic Window Approach，动态窗口法）是一种经典的，专为移动机器人动态避障设计。其核心思想是通过在当前速度空间内动态生成速度窗口，评估可行轨迹的安全性、朝向目标性和速度效率，最终选择最优运动指令。简单来说，给定机器人起点和目标点，机器人会在当下判断“最合适”的线速度与角速度(v,ω)去移动，躲避障碍物并最终到达目标点。注意与全局规划不同的是，DWA算法是局部规划器，只

m0_46578404

2708人浏览 · 2025-04-19 11:15:36

m0_46578404 · 2025-04-19 11:15:36 发布

一、什么是DWA算法？

DWA（Dynamic Window Approach，动态窗口法）是一种经典的局部路径规划算法，专为移动机器人动态避障设计。其核心思想是通过在当前速度空间内动态生成速度窗口，评估可行轨迹的安全性、朝向目标性和速度效率，最终选择最优运动指令。

简单来说，给定机器人起点和目标点，机器人会在当下判断“最合适”的线速度与角速度(v,ω)去移动，躲避障碍物并最终到达目标点。注意与全局规划不同的是，DWA算法是局部规划器，只能规划出一定时间内的路径。

二、DWA算法的详细过程

2.1 动态空间窗口

DWA算法输出的是线速度与角速度(v,ω)，但是由于物理性质和安全约束，不能随意去输出速度大小，因此动态时间窗口指的是小车能够实现的速度范围。

假设小车 $T$ 时刻的线速度与角速度 $(v_t,\omega_t)$ ，我们可以根据约束去生成动态时间窗口：

1、机器人动力约束：机器人的最大最小速度与最大加减速能力。即算法输出的机器人速度需要受到机器人本身速度限制（ $v_{\mathrm{robot~max}}$ , $v_{\mathrm{robot~min}}$ ）和加速度( $\dot{v}_{\max}$ , $\dot{v}_{\min}$ )限制。

$\begin{cases} v_{\min}=\max(v_{\mathrm{robot~min}},v_t-\dot{v}_{\max}\cdot\Delta t) \\ v_{\max}=\min(v_{\mathrm{robot~max}},v_t+\dot{v}_{\max}\cdot\Delta t) \\ \omega_{\min}=\max(-\omega_{\mathrm{robot~max}},\omega_t-\dot{\omega}_{\max}\cdot\Delta t) \\ \omega_{\max}=\min(\omega_{\mathrm{robot~max}},\omega_t+\dot{\omega}_{\max}\cdot\Delta t) & \end{cases}$

2、安全制动约束：为确保机器人能在障碍物前安全停止，需满足：

$v_\mathrm{safe}\leq\sqrt{2\cdot d_\mathrm{min}\cdot\dot{v}_\mathrm{max}}$

其中:

$d_\mathrm{min}$ 是轨迹点到最近障碍物的最小距离。 $\dot{v}_\mathrm{max}$ 是最大减速度（负加速度）。

最终动态窗口为：

$\begin{cases} v_\mathrm{dw}=[\max(v_\mathrm{min},v_\mathrm{safe~min}),\min(v_\mathrm{max},v_\mathrm{safe~max})] \\ \omega_\mathrm{dw}=[\omega_\mathrm{min},\omega_\mathrm{max}] & \end{cases}$

（安全制动约束未考虑角速度，如需要自行添加）

2.2 速度空间采样

当生成动态窗口之后，就需要对速度进行离散采用处理，假设线速度和角速度动态窗口为 $[v_{dw min},v_{dw max}]$ 和 $[\omega_{dw min},\omega_{dw max}]$ ，采样频率分别为 $v_{step}$ 和 $\omega_{step}$ ，那么生成的速度对为：

（ $v_{dw min}$ ， $\omega_{dw min}$ ）	（ $v_{dw min}$ ， $\omega_{dw min}+\omega_{step}$ ）	...	（ $v_{dw min}$ ， $\omega_{dw max}$ ）
（ $v_{dw min} + v_{step}$ ， $\omega_{dw min}$ ）	（ $v_{dw min} + v_{step}$ ， $\omega_{dw min}+\omega_{step}$ ）		（ $v_{dw min} + v_{step}$ ， $\omega_{dw max}$ ）
...	...
（ $v_{dw max}$ ， $\omega_{dw min}$ ）	（ $v_{dw max}$ ， $\omega_{dw min}+\omega_{step}$ ）		（ $v_{dw max}$ ， $\omega_{dw max}$ ）

2.3 轨迹模拟

假设机器人采用差分驱动模型（如两轮差速机器人），其运动学方程为：

$\begin{cases} \dot{x}=v\cdot\cos(\theta) \\ \dot{y}=v\cdot\sin(\theta) \\ \dot{\theta}=\omega & \end{cases}$

离散化后：

$\begin{cases} x_{t+1}=x_t+v\cdot\cos(\theta_t)\cdot\Delta t \\ y_{t+1}=y_t+v\cdot\sin(\theta_t)\cdot\Delta t \\ \theta_{t+1}=\theta_t+\omega\cdot\Delta t & \end{cases}$

其中：

$(x,y)$ ：机器人坐标

$\theta$ ：机器人朝向

$\Delta t$ ：时间步长

将2.2中每个采样的速度 $(v,\omega)$ 都需要进行轨迹模拟，通过机器人的运动学模型得到机器人的位姿，注意每次时间步长 $\Delta t$ 都会进行一次模拟，更新机器人的状态并进行记录，而在总模拟时间 $T_{sim}$ 内会进行多次模拟得到最终位姿。例如：若 $\Delta t=0.1\mathrm{s},T_{\mathrm{sim}}=2.0\mathrm{s}$ ，则需要模拟 $2.0/0.1=20$ 步。每次模拟则会得到新的 $[x_t,y_t,\theta_t]$ 作为下一个 $\Delta t$ 的输入，而采用的速度 $(v,\omega)$ 则在这个 $T_{sim}$ 内保持不变。

2.4 评价函数设计

当生成多种轨迹之后，就需要对这些轨迹进行评估，以下是四种评价偏好：

1、目标接近度评价：

$\mathrm{GoalScore}=-w_g\cdot\sqrt{(x_g-x_e)^2+(y_g-y_e)^2}$

$(x_g,y_g)$ ：目标点坐标

$(x_e,y_e)$ ：轨迹终点坐标

$w_g$ ：目标权重

2、障碍物距离评价：

$\text{ObstacleScore}=\left\{\begin{matrix} -\infty & \mathrm{if }d_{\min}<r_{\mathrm{robot}}\\ w_o\cdot d_{\min} & \mathrm{otherwise} & \end{matrix}\right.$

$d_{\min}= \mathop{\min}_{ \begin{array} {c}(x,y)\in\mathrm{traj} \\ (x_o,y_o)\in\mathrm{obs} \end{array}}\sqrt{(x-x_o)^2+(y-y_o)^2}$

$r_{robot}$ ：机器人安全半径

$w_o$ ：障碍物权重

3、速度偏好评价：

$\text{SpeedScore}=w_v\cdot v$

$v$ ：当前线速度

$w_v$ ：速度权重

4、方位角评价:

$\text{AngleScore}=-w_a\cdot\Delta\theta$

$\Delta\theta=\min\left(|\theta_e-\theta_{tg}|,2\pi-|\theta_e-\theta_{tg}|\right)$

$\theta_{tg}=\arctan\left(\frac{y_g-y_e}{x_g-x_e}\right)$

$\theta_e$ ：轨迹终点朝向角

$\theta_{tg}$ ：目标方向角

$w_a$ ：方位角权重

综合评分公式:

$\text{TotalScore}=\mathrm{GoalScore}+\text{ObstacleScore}+\text{SpeedScore}+\text{AngleScore}$

在DWA算法的轨迹评价函数中，各评价项的物理意义和作用方向如下：目标接近度评价通过计算轨迹终点与目标点之间的欧氏距离，并乘以负权重系数（ $w_g$ ），使得距离越小得分越高，从而引导机器人向目标位置移动；障碍物距离评价首先检查轨迹点与所有障碍物的最小距离（ $d_{\min}$ ），若该距离小于机器人安全半径（ $r_{robot}$ ）则直接否决该轨迹，否则乘以正权重系数（wowo）使得远离障碍物的轨迹获得更高评分；速度偏好评价直接采用当前线速度（ $v$ ）乘以正权重系数（ $w_v$ ），鼓励机器人保持合理运动速度以避免停滞；新增的方位角评价通过计算轨迹终点朝向角（ $\theta_e$ ）与目标方向角（ $\theta_{tg}$ ）的最小差值（ $\Delta\theta$ ），并乘以负权重系数（ $w_a$ ），使得机器人朝向与目标方向越一致得分越高，这对需要精确对准的场景（如泊车、抓取等）尤为重要。所有评价项的加权和构成最终轨迹评分，其中目标接近度和方位角评价采用负权重实现"越小越好"的优化方向，而障碍物距离和速度偏好采用正权重实现"越大越好"的优化方向。

注意在轨迹评价函数中，为了确保各评价指标具有可比性并避免某些指标因量纲不同而主导评价结果，需要对各指标进行归一化处理。即一次 $T_{sim}$ 中，每次时间步长 $\Delta t$ 中的每一项得分需要除以所有 $\Delta t$ 中同类型得分的总和，例如：

$\text{NormalObstacleScore}_i = \text{ObstacleScore}_i /\sum_{i=1}^n \text{ObstacleScore}_i$

这样，机器人获得”最合适“的速度去运行，并不断迭代至目标点处。

三、DWA python代码实现

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()

DWA算法演示动画

2048 AI社区

有“AI”的1024 = 2048，欢迎大家加入2048 AI社区

更多推荐

UFW防火墙安全指南

UFW（Uncomplicated Firewall）是Ubuntu/Debian系统中简化防火墙管理的工具，通过直观命令帮助用户有效控制网络流量，提升系统安全性。文章详细介绍了UFW的基本命令，包括启停防火墙、添加规则、限制连接速率和日志配置等操作，并提供了安全最佳实践，如默认拒绝策略、IP地址限制和服务级规则管理。同时，还涵盖高级配置技巧，例如多网络接口设置、规则优先级调整、IPv6支持及与f