DWA/DWB局部路径规划

飘渺＆无心

640人浏览 · 2026-03-28 09:58:31

飘渺＆无心 · 2026-03-28 09:58:31 发布

先给出一个完整导航链路的总览，把之前写的AMCL（定位）和NavFn（全局规划）串起来：

AMCL（定位）：解决「我在哪」；

NavFn（全局规划）：解决「我要走哪条大路线」；

DWA/DWB（局部规划）：解决「我下一时刻方向盘打多少、油门踩多少」，把全局路线转换成底盘能执行的实时速度指令，同时完成动态避障、路径跟踪。

下面是这篇文章的重点：局部路径规划算法。DWB 是 DWA 的模块化、可扩展、ROS2 原生优化升级版，核心思想完全同源，工程架构与灵活性全面升级。即两者思想完全相同，dwb是工程化的产品，提供了更灵活的框架，可扩展性。

dwa的python可视化过程如下：

Video Project 1

需要代码请留下邮箱。

一、DWA（Dynamic Window Approach，动态窗口法）详解

1. 一句话通俗理解

你开车跟着导航（全局路径）走，路上突然出现行人、障碍物，你需要实时调整方向盘和油门：既要跟着导航路线走，又不能撞东西，还要开得稳、尽量快。DWA 就是干这个的 —— 它是机器人的「实时司机」，输出瞬时的线速度、角速度，让机器人安全、精准地跟踪全局路径。

2. 核心定位

移动机器人领域最经典的基于采样的局部路径规划算法，是 ROS1 move_base 的默认局部规划器，专为差速驱动机器人设计，兼顾实时性、避障能力与路径跟踪精度。

3. 输入与输出

输入（DWA 要什么）

全局参考路径：NavFn 输出的全局路径点序列（导航大路线）；
局部代价地图：带障碍物、膨胀区的实时栅格地图（机器人眼睛看到的周围环境）；
机器人当前状态：AMCL 输出的位姿（x,y,θ）、里程计输出的当前线速度 v_cur、角速度 ω_cur；
目标点：全局路径的终点；
硬件约束参数：最大 / 最小线速度、最大 / 最小角速度、最大线加速度、最大角加速度。

输出（DWA 给什么）

机器人下一控制周期的瞬时速度指令：线速度 v + 角速度 ω，直接发送给机器人底盘执行；同时输出模拟的局部轨迹，用于可视化调试。

4. 核心原理与完整流程

DWA 的核心逻辑是 **「采样→预测→评分→选优」** 的循环，每一个控制周期（通常 100ms）执行一次，核心分为 4 步。

步骤 1：确定动态窗口（速度采样范围）

这是算法名字的来源：动态窗口 = 当前时刻机器人能安全、物理可达的速度范围。不是所有速度都能选，窗口由 3 个硬约束叠加而成，最终取三者的交集。

约束类型	通俗解释	数学公式
硬件速度约束	机器人电机本身的最大 / 最小速度，物理极限	$latex v \in [v_{min}, v_{max}], \quad \omega \in [\omega_{min}, \omega_{max}]$
加速度约束	电机加减速能力有限，一个控制周期内能达到的速度范围（窗口动态变化的核心）	$latex v \in [v_{cur} - a_{max} \cdot \Delta t, v_{cur} + a_{max} \cdot \Delta t] \\ \omega \in [\omega_{cur} - \alpha_{max} \cdot \Delta t, \omega_{cur} + \alpha_{max} \cdot \Delta t]$
安全制动约束	必须保证能在碰到障碍物前刹住车，否则直接排除该速度	$latex v \leq \sqrt{2 \cdot d_{obs} \cdot a_{max}} \\ \omega \leq \sqrt{2 \cdot \theta_{obs} \cdot \alpha_{max}}$

步骤 2：速度采样 + 轨迹预测

在动态窗口内，离散采样多组 (v, ω) 速度对（比如采样 20 组线速度、40 组角速度，组合成 800 个候选）；对每一组速度，用机器人运动模型，模拟未来的运动轨迹。

# sample_velocities() + generate_trajectory()
def sample_velocities(self):
    v_range, w_range = self.compute_dynamic_window()
    velocities = []
    
    # 均匀采样
    v_samples = np.linspace(v_range[0], v_range[1], self.config.vx_samples)
    w_samples = np.linspace(w_range[0], w_range[1], self.config.vtheta_samples)
    
    # 笛卡尔积组合
    for v in v_samples:
        for w in w_samples:
            velocities.append(Velocity(v, w))
    return velocities  # 例如 20×40 = 800 个候选

def generate_trajectory(self, start_pose, velocity):
    """差速模型轨迹预测"""
    poses = []
    pose = Pose(start_pose.x, start_pose.y, start_pose.theta)
    num_steps = int(self.config.sim_time / self.config.dt)
    
    for _ in range(num_steps):
        # 差速运动模型
        pose.x += velocity.linear_x * np.cos(pose.theta) * self.config.dt
        pose.y += velocity.linear_x * np.sin(pose.theta) * self.config.dt
        pose.theta += velocity.angular_z * self.config.dt
        poses.append(Pose(pose.x, pose.y, pose.theta))
    
    return Trajectory(poses=poses, velocity=velocity)

差速机器人运动模型（核心公式）

$\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}$

大白话：按当前速度，一步步算出未来每个时刻机器人的位置和朝向，连成一条轨迹。

步骤 3：评价函数给轨迹打分

对每一条模拟的轨迹，用评价函数打分，分数越高，轨迹越优。经典 DWA 的评价函数由 3 个核心项加权求和而成：

$G(v,\omega) = \alpha \cdot heading(v,\omega) + \beta \cdot dist(v,\omega) + \gamma \cdot vel(v,\omega)$

每个项的通俗解释

评价项	公式含义	通俗作用
`heading(v,ω)`	轨迹终点朝向与全局路径 / 目标点的偏差，偏差越小，分数越高	保证机器人朝着目标走，不跑偏
`dist(v,ω)`	轨迹上离最近障碍物的距离，距离越远，分数越高；碰到障碍物直接打 0 分	核心避障项，保证机器人不撞墙
`vel(v,ω)`	轨迹的线速度大小，速度越快，分数越高	保证机器人尽量快地到达目标
`α/β/γ`	权重系数	调参核心：想避障优先就加大 β，想跟线优先就加大 α，想跑得快就加大 γ

# evaluate_trajectory()
def evaluate_trajectory(self, trajectory, environment):
    # 1. 障碍物评分（碰到障碍物直接淘汰）
    trajectory.obstacle_score = self._obstacle_score(trajectory, environment)
    if trajectory.obstacle_score >= 254.0:
        trajectory.score = float('inf')  # 碰撞轨迹
        return trajectory

    # 2-5. 其他评分项
    path_align_score = self._path_align_score(trajectory, environment)
    path_dist_score = self._path_dist_score(trajectory, environment)
    goal_align_score = self._goal_align_score(trajectory, environment)
    goal_dist_score = self._goal_dist_score(trajectory, environment)
    velocity_score = self._velocity_score(trajectory)

    # 加权求和
    trajectory.score = (
        weights.obstacle_weight * trajectory.obstacle_score +
        weights.path_align_weight * path_align_score +
        weights.path_dist_weight * path_dist_score +
        weights.goal_align_weight * goal_align_score +
        weights.goal_dist_weight * goal_dist_score +
        50.0 * velocity_score
    )
    return trajectory

步骤 4：选最优轨迹，输出速度指令

淘汰所有会碰撞的轨迹（dist=0）；
剩下的轨迹里，选评分最高的那一条；
把这条轨迹对应的 (v, ω) 输出给机器人底盘执行。

然后进入下一个控制周期，重复以上 4 步，直到到达目标点。


def find_best_trajectory(self, trajectories, environment):
    best_trajectory = None
    best_score = float('inf')
    
    for trajectory in trajectories:
        evaluated = self.evaluate_trajectory(trajectory, environment)
        if evaluated.score < best_score:
            best_score = evaluated.score
            best_trajectory = evaluated
    
    return best_trajectory

# update() 主循环
def update(frame):
    # 1. 速度采样
    velocities = dwa.sample_velocities()
    
    # 2. 轨迹预测
    trajectories = [dwa.generate_trajectory(env.robot_pose, vel) for vel in velocities]
    
    # 3. 轨迹评分
    evaluator.evaluate_all(trajectories, env)
    best_trajectory = evaluator.find_best_trajectory(trajectories, env)
    
    # 4. 执行最优轨迹
    if best_trajectory and len(best_trajectory.poses) > 1:
        env.robot_pose = best_trajectory.poses[1]  # 移动一步
        dwa.set_current_velocity(
            best_trajectory.velocity.linear_x,
            best_trajectory.velocity.angular_z
        )

5. DWA 的优缺点

优点	缺点
考虑机器人动力学约束，输出的速度完全可执行，不会超出硬件能力	评价项固定，扩展性差，想加自定义规则需要改源码
实时性强，计算量可控，适合嵌入式低算力平台	对差速机器人适配好，全向、阿克曼机器人支持弱
动态避障效果好，能实时响应环境变化	调参复杂，权重系数需要反复适配场景
成熟稳定，ROS1 生态完善，文档丰富	缺少原生的震荡抑制、机器人轮廓避障等高级功能

二、DWB（Dynamic Window Basic）详解

1. 一句话通俗理解

DWB 是 DWA 的工业化改装版：把 DWA 原来固定死的功能，全部拆成了可插拔、可替换、可自定义的插件。你想加避障规则、换轨迹生成逻辑、适配全向机器人，不用改源码，直接加个插件就行。它是 ROS2 Nav2 导航框架的默认局部控制器，是工业机器人开发的主流选择。

2. 核心定位

DWB 全称 Dynamic Window Basic（也叫 Dynamic Window Based），是 DWA 算法的现代化重构版本，由 Locus Robotics 开发，专为 ROS2 设计，完全继承了 DWA 的核心思想，重点解决了 DWA 扩展性差、难定制、适配性弱的痛点。

3. 核心改进（和 DWA 的核心差异）

DWB 没有颠覆 DWA 的「采样→预测→评分→选优」核心逻辑，核心改进集中在工程架构、扩展性、功能完整性三个维度：

1. 全插件化架构，极致灵活

这是 DWB 最核心的升级：把算法拆成了 3 个独立的插件模块，每个模块都可以自定义替换，不用改核心源码：

轨迹生成器插件：负责速度采样、轨迹预测，原生支持差速、全向机器人，可扩展阿克曼模型；
Critic 评分插件：把原来 DWA 固定的评价项，拆成了一个个独立的评分插件。原来 DWA 只有 3 个固定评分项，DWB 原生提供了十几项，还可以自己写插件加规则；
运动学约束插件：自定义速度、加速度约束，适配不同机器人底盘。

2. 原生丰富的 Critic 评分插件（核心功能）

DWB 把评分规则完全解耦，每个评分项都是一个独立的 Critic 插件，可自由开启、关闭、调整权重，甚至自己开发。原生常用插件包括：

Critic 插件	作用	对应 DWA 的功能
`PathAlignCritic`	轨迹和全局路径的朝向对齐	原 `heading` 项
`PathDistCritic`	轨迹和全局路径的距离偏差	路径跟踪核心项
`BaseObstacleCritic`	轨迹与障碍物的距离，避障核心	原 `dist` 项
`ObstacleFootprintCritic`	按机器人实际轮廓（footprint）判断碰撞，比 DWA 的圆形机器人更精准	DWA 无此功能
`GoalAlignCritic` / `GoalDistCritic`	终点朝向 / 距离对齐，精准到达目标点	DWA 弱支持
`OscillationCritic`	抑制机器人来回晃动、原地打转	DWA 无原生支持
`PreferForwardCritic`	优先向前走，禁止频繁倒车	可自定义
`RotateToGoalCritic`	到达目标点后，纯旋转调整朝向	DWA 硬编码实现，DWB 可配置

3. 其他关键优化

原生支持全向机器人、差速机器人，可扩展阿克曼转向模型；
完善的调试工具，可输出每条轨迹的详细评分，调参更简单；
ROS2 原生适配，支持动态参数重配置，不用重启节点就能改权重；
碰撞检测更精准，支持非圆形机器人轮廓，避免边角碰撞；
采样策略更灵活，可自定义采样密度、模拟时间。

4. DWB 完整工作流程

和 DWA 核心逻辑一致，仅在评分环节做了插件化解耦，流程如下：

接收全局路径、局部代价地图、机器人当前位姿 / 速度；
由轨迹生成器插件，根据运动学约束确定动态窗口，采样速度对、模拟轨迹；
所有开启的Critic 插件，分别给每条轨迹打分，按权重求和得到总分；
淘汰碰撞轨迹，选总分最高的轨迹；
输出轨迹对应的 (v, ω) 速度指令给底盘。

AtomGit开源社区

AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。

更多推荐

从词嵌入到RNN（其一）

AtomGit开源社区

2025年AI编程工具Cost分析 — 每个开发者都该看的省钱攻略

不要为不需要的功能付费— 如果你不用终端 Agent，就不需要 Claude Code善用免费组合— Copilot Free + Cursor Free + DeepClaude 覆盖90%场景Token 是隐形成本— 关注 API 调用量，选择合适的模型和上下文窗口按团队规模选工具— 个人用轻量级，团队用企业级持续关注开源替代— DeepClaude 这类项目发展很快，每月都有新功能声明：本文