一.DWA的定义

        DWA动态窗口法（Dynamic Windows Approach）算法在每一个控制周期内，根据机器人的当前状态，全局路径以及局部环境感知信息（包括障碍物等），在允许的速度范围内找到一组最优的速度控制量，选择出一条朝向，离目标最近以及速度最好的一个轨迹。

        简单来说，DWA 的核心思想是：

        在每个控制周期内尝试多组“线速度 v+角速度 ω”，预测机器人短时间内的运动轨迹，再从中选择一条安全且综合表现最好的轨迹。

        经典 DWA 通常直接在 (v,ω) 速度空间中进行搜索，比较适合差速式或类似运动模型的移动机器人。

二.DWA的输入输出

输入：机器人的当前状态（位姿 (x, y, θ) 及当前速度 (v, ω)）

           全局路径

           局部感知信息

输出：线速度

           角速度

          对应的最优轨迹

三.DWA的步骤

         1.获取机器人当前的状态（位姿和朝向角）以及当前线速度和角速度。当前的全局路径和局部环境信息（障碍物信息等）

        2.动态速度窗口

        这个速度窗口并不是说随便计算出来的速度都可以，是要根据机器人的当前状态，以及最大线加速度和最大角加速度的约束得到的。

        速度采样空间并非固定不变，而是由当前速度及动力学约束共同决定的动态窗口。在单位时间 Δt 内，可达的速度范围由加速度限制给出：

        角速度  [ωc​−aω​Δt, ωc​+aω​Δt]

        线速度  [vc​−av​Δt, vc​+av​Δt]

        此外，实际窗口还需与机器人绝对速度极限、安全制动距离约束取交集。

        3.均匀采样

       在构建的动态窗口内，按设定的分辨率对线速度和角速度进行离散化采样，得到若干组候选速度对 (v_i, ω_j)。

         在动态窗口内，按照一定的速度分辨率分别采样线速度和角速度：

                                vi=vmin⁡+iΔv

                                ωj=ωmin⁡+jΔω

                然后通过笛卡尔积组合形成候选速度对：

                                           (vi,ωj)

        例如，线速度和角速度分别采样3个值，则会得到：

                                3×3=9

        组候选速度。

        这些速度不是随机配对，而是按照设定的分辨率遍历组合。

        4.预测每组速度对应的轨迹             

        对于每一组候选速度 (vi,ωj)，假设机器人在较短的预测时间内保持该组速度不变，并利用差速运动模型预测轨迹：

                                        xk+1=xk+vicos⁡θkΔt

                                         yk+1=yk+visin⁡θkΔt

                                                θk+1=θk+ωjΔt

        对上述模型迭代计算，可以得到未来 Tp​ 时间内的候选轨迹。

        5.评价候选轨迹

        （1）朝向评价

                判断轨迹末端是否朝向目标：衡量轨迹终点朝向与全局目标方向之间的夹角，偏差越小得分越高。

                              Jheading​=π−∣wrap(θgoal​−θend​)∣

                        方向越接近目标，得分越高。

           （2）障碍物安全距离，衡量轨迹上各点与最近障碍物的最小距离 d_min，距离越远越安全，得分越高。

                                        Jclearance​=dmin

                              轨迹与障碍物距离越远，安全性越大

            （3）速度评价

                                        Jvelocity​=v

                                速度越大，通行效率越高。

                 计算加权和：J=αJheading​+βJclearance​+γJvelocity​

         α越大，机器人越执着于目标；β越大，机器人约远离障碍物；γ越大，机器人速度约快。

注意：如果某条轨迹上的点与障碍物的点距离小于安全距离，那么该轨迹直接被删除，确保安全性。

        从所有安全轨迹中选择得分最高的速度组合：

                                (v∗,ω∗)=argmaxJ(v,ω)

        6.选择最优轨迹并输出        

        在所有的有效轨迹中，选择评分最高的轨迹对应的速度对作为最终控制指令：
(v*, ω*) = argmax J(v, ω)          

        7.循环     

     将选出的速度指令下发至机器人底层控制器，随后等待下一个控制周期，并跳转至步骤1，实现持续的滚动规划与控制。

四. DWB（Dynamic Window Based controller）

        DWB在 ROS 2 Nav2 中常作为局部轨迹规划与控制器使用，它对速度的处理与DWA基本相同，将速度向量拆解为 v_x（纵向前进）、v_y（横向平移） 和 ω（旋转） 三个维度（全向运动模型）。

        全插件化架构‌：将轨迹生成器、评分规则（Critic）和运动学约束完全解耦为独立插件。

        DWB有丰富的评分插件，如;

• PathAlign（路径对齐）：不只看终点，而是看整条模拟轨迹与全局规划路径的贴合程度，惩罚偏离路径的轨迹。

• GoalDist（距离评价）：评价候选轨迹末端与目标位置之间的距离。

  

DWA vs DWB 核心差异

对比维度	经典DWA	DWB
架构	单体函数，紧密耦合	插件化，模块解耦
速度模型	2维 (v, ω)	3维 (v_x, v_y, ω)
评价函数	3项固定加权	多评论家可插拔组合
路径跟踪	只看目标点方位角	PathAlign 贴合全局路径