In a layered AMR navigation stack, a local planner operates in a receding-horizon loop: given the current robot state, a local obstacle representation (often derived from costmaps), and a short-term objective (track the global path while avoiding collisions), it outputs a velocity command \( (v, \omega) \): to be executed for a short control period \( \Delta t \):.

The Dynamic Window Approach (DWA) performs this selection by enumerating candidate commands inside a dynamic window defined by both kinematic bounds and acceleration feasibility, then scoring each command via rollout simulation under a unicycle-like model (appropriate for many ground bases).

flowchart TD
  S["State: (x,y,theta,v,w)"] --> W["Dynamic window from limits + \naccel: (v,w) bounds"]
  G["Goal / path \ntracking objective"] --> E["Evaluate candidates"]
  M["Local obstacles \n(inflated map / scan)"] --> E
  W --> Samp["Sample candidate (v,w)"]
  Samp --> Roll["Forward simulate for horizon T"]
  Roll --> Feas["Reject if not stop-safe"]
  Feas --> E
  E --> Best["Pick best score (v*,w*)"]
  Best --> Cmd["Send cmd for dt"]
  Cmd --> S
        

DWA is local: it does not guarantee global optimality, and can be trapped by local minima. However, it is fast, reactive, and integrates naturally with costmap-based obstacle representations introduced earlier in Chapter 14.

For many wheeled mobile bases, the short-horizon local prediction can be approximated by the planar unicycle model with control inputs \( v \): (linear velocity) and \( \omega \): (yaw rate):

\[ \dot x = v\cos\theta,\qquad \dot y = v\sin\theta,\qquad \dot\theta = \omega. \]

DWA uses forward simulation (rollout) with time step \( \Delta t \): over a horizon \( T = N\Delta t \):. With forward Euler discretization:

\[ \begin{aligned} x_{k+1} &= x_k + v\cos\theta_k\,\Delta t,\\ y_{k+1} &= y_k + v\sin\theta_k\,\Delta t,\\ \theta_{k+1} &= \theta_k + \omega\,\Delta t. \end{aligned} \]

For each candidate \( (v,\omega) \): assumed constant over the horizon, the rollout produces a discrete trajectory \( \tau(v,\omega) = \{ {\mathbf{x} }_0,{\mathbf{x} }_1,\dots,{\mathbf{x} }_N\} \): where \( {\mathbf{x} }_k = (x_k,y_k,\theta_k) \):.

Let the robot's current commanded velocities be \( v_t \): and \( \omega_t \):. The dynamic window is the intersection of:

• Kinematic limits: \( v \in [v_{\min}, v_{\max}] \):, \( \omega \in [\omega_{\min}, \omega_{\max}] \):.
• Acceleration feasibility over one control period \( \Delta t \): given bounds \( a_{\max} \): (accel), \( a_{\text{brake} } \): (decel magnitude), and \( \alpha_{\max} \): (yaw accel):

\[ \begin{aligned} v &\in \left[v_t - a_{\text{brake} }\Delta t,\; v_t + a_{\max}\Delta t\right],\\ \omega &\in \left[\omega_t - \alpha_{\max}\Delta t,\; \omega_t + \alpha_{\max}\Delta t\right]. \end{aligned} \]

Therefore the dynamic window bounds are:

\[ \begin{aligned} v_{\text{low} } &= \max\!\left(v_{\min},\, v_t - a_{\text{brake} }\Delta t\right),\qquad v_{\text{high} } = \min\!\left(v_{\max},\, v_t + a_{\max}\Delta t\right),\\ \omega_{\text{low} } &= \max\!\left(\omega_{\min},\, \omega_t - \alpha_{\max}\Delta t\right),\qquad \omega_{\text{high} } = \min\!\left(\omega_{\max},\, \omega_t + \alpha_{\max}\Delta t\right). \end{aligned} \]

Candidate controls are sampled on a grid: \( v = v_{\text{low} } + i\Delta v \):, \( \omega = \omega_{\text{low} } + j\Delta\omega \): with resolutions \( \Delta v \): and \( \Delta\omega \):.

Sampling note: finer grids improve solution quality but increase computation. If \( n_v \): and \( n_\omega \): are samples per dimension and \( N \): rollout steps, the naive complexity per cycle is \( \mathcal{O}(n_v n_\omega N) \): plus the obstacle distance query cost.

A core idea in DWA is to reject velocity commands that cannot stop before reaching an obstacle. Let \( d_{\min}(\tau) \): be the minimum clearance from the robot body (radius \( r_{\text{robot} } \):) along the rollout \( \tau \): to the nearest obstacle (in the local map). For static obstacles and bounded braking deceleration \( a_{\text{brake} } > 0 \):, a conservative stopping distance for forward motion is:

\[ d_{\text{stop} }(v) = \frac{\max(0,v)^2}{2a_{\text{brake} } }. \]

Admissibility condition (stop-safe): keep only candidates satisfying \( d_{\min}(\tau) > d_{\text{stop} }(v) + r_{\text{robot} } \):.

\[ \text{Admissible}(v,\omega)\ \Longleftrightarrow\ d_{\min}(\tau(v,\omega)) > d_{\text{stop} }(v) + r_{\text{robot} }. \]

Lemma (Stop-safety under static obstacles): Assume (i) obstacles are static over the horizon, (ii) the clearance estimate \( d_{\min}(\tau) \): is conservative (never overestimates), and (iii) the robot can always brake with deceleration magnitude at least \( a_{\text{brake} } \):. If at each control cycle the commanded \( (v,\omega) \): is admissible and is held for \( \Delta t \):, then a collision can be avoided by braking at or before the point where the admissibility inequality becomes tight.

Proof (sketch): For an admissible command, the rollout clearance satisfies \( d_{\min}(\tau) - r_{\text{robot} } > d_{\text{stop} }(v) \):. The right-hand side is exactly the distance required to reduce forward speed from \( v \): to zero under deceleration \( a_{\text{brake} } \):: from \( v^2 = 2a_{\text{brake} }d \): we obtain \( d = v^2/(2a_{\text{brake} }) \):. Therefore, even if the robot begins braking immediately, it can come to rest strictly before its body intersects the nearest obstacle along the rollout. Receding-horizon replanning maintains this margin each cycle. □

This lemma clarifies the role of conservative distance estimates and braking bounds. In real systems, moving obstacles, sensing latency, and map errors motivate additional margins and shorter horizons.

Among admissible candidates, DWA selects the command that maximizes a weighted score. A classical decomposition is:

flowchart TD
  T["Trajectory tau(v,w)"] --> H["Heading score: \nalign to goal"]
  T --> C["Clearance score: \nmin obstacle distance"]
  T --> V["Speed score: \nprefer larger v"]
  H --> N["Normalize across candidates"]
  C --> N
  V --> N
  N --> J["Total score J = wh*H + wc*C + wv*V"]
        

A concrete choice (one of many) is:

\[ J(v,\omega) = w_h\,\widehat{H}(v,\omega) + w_c\,\widehat{C}(v,\omega) + w_v\,\widehat{V}(v,\omega), \qquad w_h,w_c,w_v \ge 0,\ \ w_h+w_c+w_v = 1. \]

Here \( H \): measures goal alignment at the end of the rollout (e.g., cosine of heading error), \( C \): is minimum clearance along the trajectory, and \( V \): encourages progress (e.g., normalized speed). Hats denote normalization within the current candidate set, for example:

\[ \widehat{z}_i = \frac{z_i - \min_j z_j}{\max_j z_j - \min_j z_j + \varepsilon}, \qquad \varepsilon > 0. \]

Tuning guideline: Increasing \( w_c \): yields more conservative obstacle clearance; increasing \( w_v \): yields faster motion; increasing \( w_h \): yields stronger goal/path tracking but can cause corner cutting if clearance is underweighted.

In practical AMR stacks, obstacle information is often represented by an inflated costmap (Chapter 14, Lesson 2). For DWA, what matters is a fast query for \( d_{\min}(\tau) \): (clearance) along a rollout. Common approaches include:

• Distance transform: precompute a distance-to-obstacle field over the costmap grid so that clearance queries reduce to table lookups plus bilinear interpolation.
• Point-cloud / scan distance: compute distances from rollout points to a set of obstacle points extracted from LiDAR scans (fast with k-d trees).
• Footprint-aware collision checks: use a polygonal footprint rather than a radius; clearance becomes the minimum distance from the footprint to occupied/inflated cells.

Because DWA is reactive, its stability depends on timing: the control period \( \Delta t \): should be short enough that the local map and robot state are fresh, and the horizon \( T \): should be long enough to see around local obstacles but short enough to keep computation within real-time constraints.

The following reference implementations include a from-scratch DWA core. In production AMR software, you typically wrap the core in middleware (e.g., ROS/ROS2) and replace the obstacle model with real sensor/costmap queries.

7.1 Python Implementation

Code: Chapter15_Lesson3.py

"""
Chapter15_Lesson3.py
Dynamic Window Approach (DWA) — from-scratch reference implementation (2D unicycle).
- No ROS dependencies. Uses obstacle points and a simple receding-horizon rollout.
- You can later wrap `dwa_control()` inside a ROS2 node that publishes geometry_msgs/Twist.
"""

from __future__ import annotations

from dataclasses import dataclass
from typing import List, Tuple
import math
import numpy as np


@dataclass
class State:
    x: float
    y: float
    theta: float   # heading [rad]
    v: float       # linear velocity [m/s]
    w: float       # angular velocity [rad/s]


@dataclass
class Config:
    # Kinematic limits
    v_min: float = -0.2
    v_max: float =  1.0
    w_min: float = -1.5
    w_max: float =  1.5

    # Dynamic limits (accelerations)
    a_max: float = 0.8          # linear accel [m/s^2]
    a_brake: float = 1.0        # linear decel magnitude [m/s^2]
    alpha_max: float = 2.0      # angular accel [rad/s^2]

    # Discretization
    dt: float = 0.1             # control period [s]
    T: float = 2.0              # rollout horizon [s]
    v_res: float = 0.05         # sampling resolution [m/s]
    w_res: float = 0.1          # sampling resolution [rad/s]

    # Robot geometry
    robot_radius: float = 0.3   # [m]

    # Scoring weights
    w_heading: float = 0.4
    w_clear: float = 0.4
    w_speed: float = 0.2


def motion_step(s: State, v: float, w: float, dt: float) -> State:
    """Forward Euler unicycle integration."""
    nx = s.x + v * math.cos(s.theta) * dt
    ny = s.y + v * math.sin(s.theta) * dt
    ntheta = s.theta + w * dt
    return State(nx, ny, ntheta, v, w)


def dynamic_window(s: State, cfg: Config) -> Tuple[float, float, float, float]:
    """
    Dynamic window:
      - kinematic window: [v_min, v_max] x [w_min, w_max]
      - acceleration window: [v - a_brake*dt, v + a_max*dt] x [w - alpha*dt, w + alpha*dt]
      - intersection
    """
    v_low_kin, v_high_kin = cfg.v_min, cfg.v_max
    w_low_kin, w_high_kin = cfg.w_min, cfg.w_max

    v_low_dyn = s.v - cfg.a_brake * cfg.dt
    v_high_dyn = s.v + cfg.a_max * cfg.dt
    w_low_dyn = s.w - cfg.alpha_max * cfg.dt
    w_high_dyn = s.w + cfg.alpha_max * cfg.dt

    v_low = max(v_low_kin, v_low_dyn)
    v_high = min(v_high_kin, v_high_dyn)
    w_low = max(w_low_kin, w_low_dyn)
    w_high = min(w_high_kin, w_high_dyn)
    return v_low, v_high, w_low, w_high


def rollout(s0: State, v: float, w: float, cfg: Config) -> List[State]:
    n = int(cfg.T / cfg.dt)
    traj = [s0]
    s = s0
    for _ in range(n):
        s = motion_step(s, v, w, cfg.dt)
        traj.append(s)
    return traj


def min_clearance(traj: List[State], obstacles: np.ndarray) -> float:
    """
    Obstacles: array shape (M,2) of point obstacles in world coordinates.
    Returns minimum Euclidean distance from any trajectory point to any obstacle.
    """
    pts = np.array([(st.x, st.y) for st in traj], dtype=float)  # (N,2)
    # pairwise distances: (N,M)
    d = np.linalg.norm(pts[:, None, :] - obstacles[None, :, :], axis=2)
    return float(np.min(d))


def heading_score(traj: List[State], goal: Tuple[float, float]) -> float:
    """
    Heading score: cosine alignment between final heading and direction to goal.
    Range roughly [-1, 1], higher is better.
    """
    gx, gy = goal
    last = traj[-1]
    dir_to_goal = math.atan2(gy - last.y, gx - last.x)
    ang_err = wrap_to_pi(dir_to_goal - last.theta)
    return math.cos(ang_err)


def speed_score(v: float, cfg: Config) -> float:
    """Prefer higher forward speed within limits."""
    return (v - cfg.v_min) / max(1e-9, (cfg.v_max - cfg.v_min))


def admissible(v: float, clearance: float, cfg: Config) -> bool:
    """
    Stop-safe admissibility (static obstacles):
      stopping distance d_stop = v^2 / (2*a_brake)
      require clearance > d_stop + robot_radius
    """
    if clearance <= 0.0:
        return False
    d_stop = (max(0.0, v) ** 2) / max(1e-9, 2.0 * cfg.a_brake)
    return clearance > (d_stop + cfg.robot_radius)


def normalize(vals: List[float]) -> List[float]:
    lo, hi = min(vals), max(vals)
    if abs(hi - lo) < 1e-12:
        return [0.0 for _ in vals]
    return [(v - lo) / (hi - lo) for v in vals]


def dwa_control(s: State, goal: Tuple[float, float], obstacles: np.ndarray, cfg: Config) -> Tuple[float, float, List[State]]:
    """
    Compute (v*, w*) maximizing weighted score over sampled (v,w) within dynamic window.
    Returns best velocities and the associated trajectory.
    """
    vL, vU, wL, wU = dynamic_window(s, cfg)

    v_samples = np.arange(vL, vU + 1e-9, cfg.v_res)
    w_samples = np.arange(wL, wU + 1e-9, cfg.w_res)

    cand = []
    for v in v_samples:
        for w in w_samples:
            traj = rollout(s, float(v), float(w), cfg)
            clear = min_clearance(traj, obstacles) - cfg.robot_radius
            if not admissible(float(v), clear, cfg):
                continue
            h = heading_score(traj, goal)
            sp = speed_score(float(v), cfg)
            cand.append((h, clear, sp, float(v), float(w), traj))

    if not cand:
        # fallback: stop
        return 0.0, 0.0, [s]

    hs = normalize([c[0] for c in cand])
    cs = normalize([c[1] for c in cand])
    vs = normalize([c[2] for c in cand])

    best_j = -1e18
    best = cand[0]
    for i, c in enumerate(cand):
        j = cfg.w_heading * hs[i] + cfg.w_clear * cs[i] + cfg.w_speed * vs[i]
        if j > best_j:
            best_j = j
            best = c

    _, _, _, v_best, w_best, traj_best = best
    return v_best, w_best, traj_best


def wrap_to_pi(a: float) -> float:
    """Map angle to (-pi, pi]."""
    return (a + math.pi) % (2.0 * math.pi) - math.pi


def demo():
    """
    Simple closed-loop demo with point obstacles.
    Requires: numpy (and optionally matplotlib for visualization).
    """
    cfg = Config()
    s = State(x=0.0, y=0.0, theta=0.0, v=0.0, w=0.0)
    goal = (8.0, 6.0)

    # Obstacles as points
    obstacles = []
    for x in np.linspace(2.0, 7.0, 15):
        obstacles.append((x, 3.0))
    for y in np.linspace(1.0, 5.0, 12):
        obstacles.append((4.0, y))
    obs = np.array(obstacles, dtype=float)

    path = [(s.x, s.y)]
    for _ in range(600):
        v, w, _ = dwa_control(s, goal, obs, cfg)
        s = motion_step(s, v, w, cfg.dt)
        path.append((s.x, s.y))
        if math.hypot(s.x - goal[0], s.y - goal[1]) < 0.3:
            break

    print("Final state:", s)
    print("Steps:", len(path))

    try:
        import matplotlib.pyplot as plt
        path = np.array(path)
        plt.figure()
        plt.plot(path[:, 0], path[:, 1], label="trajectory")
        plt.scatter(obs[:, 0], obs[:, 1], s=15, label="obstacles")
        plt.scatter([goal[0]], [goal[1]], s=50, marker="*", label="goal")
        plt.axis("equal")
        plt.grid(True)
        plt.legend()
        plt.show()
    except Exception as e:
        print("Matplotlib plot skipped:", e)


if __name__ == "__main__":
    demo()
      

7.2 C++ Implementation

Code: Chapter15_Lesson3.cpp

/*
Chapter15_Lesson3.cpp
Dynamic Window Approach (DWA) — minimal C++17 implementation (no ROS required).
Build (example):
  g++ -std=c++17 -O2 Chapter15_Lesson3.cpp -o dwa_demo
*/

#include <cmath>
#include <iostream>
#include <limits>
#include <tuple>
#include <utility>
#include <vector>

struct State {
    double x{0.0};
    double y{0.0};
    double theta{0.0};
    double v{0.0};
    double w{0.0};
};

struct Config {
    // Kinematic limits
    double v_min{-0.2};
    double v_max{ 1.0};
    double w_min{-1.5};
    double w_max{ 1.5};

    // Dynamics
    double a_max{0.8};
    double a_brake{1.0};
    double alpha_max{2.0};

    // Discretization
    double dt{0.1};
    double T{2.0};
    double v_res{0.05};
    double w_res{0.1};

    // Geometry
    double robot_radius{0.3};

    // Weights
    double w_heading{0.4};
    double w_clear{0.4};
    double w_speed{0.2};
};

static double wrap_to_pi(double a) {
    const double pi = 3.14159265358979323846;
    a = std::fmod(a + pi, 2.0 * pi);
    if (a < 0) a += 2.0 * pi;
    return a - pi;
}

static State motion_step(const State& s, double v, double w, double dt) {
    State ns = s;
    ns.x += v * std::cos(s.theta) * dt;
    ns.y += v * std::sin(s.theta) * dt;
    ns.theta += w * dt;
    ns.v = v;
    ns.w = w;
    return ns;
}

static std::tuple<double,double,double,double> dynamic_window(const State& s, const Config& cfg) {
    double v_low_dyn = s.v - cfg.a_brake * cfg.dt;
    double v_high_dyn = s.v + cfg.a_max * cfg.dt;
    double w_low_dyn = s.w - cfg.alpha_max * cfg.dt;
    double w_high_dyn = s.w + cfg.alpha_max * cfg.dt;

    double vL = std::max(cfg.v_min, v_low_dyn);
    double vU = std::min(cfg.v_max, v_high_dyn);
    double wL = std::max(cfg.w_min, w_low_dyn);
    double wU = std::min(cfg.w_max, w_high_dyn);
    return {vL, vU, wL, wU};
}

static std::vector<State> rollout(const State& s0, double v, double w, const Config& cfg) {
    int n = static_cast<int>(cfg.T / cfg.dt);
    std::vector<State> traj;
    traj.reserve(n + 1);
    traj.push_back(s0);
    State s = s0;
    for (int i = 0; i < n; ++i) {
        s = motion_step(s, v, w, cfg.dt);
        traj.push_back(s);
    }
    return traj;
}

static double min_clearance(const std::vector<State>& traj, const std::vector<std::pair<double,double>>& obs) {
    double best = std::numeric_limits<double>::infinity();
    for (const auto& st : traj) {
        for (const auto& o : obs) {
            double dx = st.x - o.first;
            double dy = st.y - o.second;
            double d = std::sqrt(dx*dx + dy*dy);
            if (d < best) best = d;
        }
    }
    return best;
}

static double heading_score(const std::vector<State>& traj, const std::pair<double,double>& goal) {
    const auto& last = traj.back();
    double dir = std::atan2(goal.second - last.y, goal.first - last.x);
    double err = wrap_to_pi(dir - last.theta);
    return std::cos(err); // [-1,1]
}

static double speed_score(double v, const Config& cfg) {
    return (v - cfg.v_min) / std::max(1e-9, (cfg.v_max - cfg.v_min));
}

static bool admissible(double v, double clearance, const Config& cfg) {
    double d_stop = (std::max(0.0, v) * std::max(0.0, v)) / std::max(1e-9, 2.0 * cfg.a_brake);
    return clearance > (d_stop + cfg.robot_radius);
}

static void normalize(std::vector<double>& vals) {
    double lo = std::numeric_limits<double>::infinity();
    double hi = -std::numeric_limits<double>::infinity();
    for (double v : vals) { lo = std::min(lo, v); hi = std::max(hi, v); }
    if (std::fabs(hi - lo) < 1e-12) {
        for (auto& v : vals) v = 0.0;
        return;
    }
    for (auto& v : vals) v = (v - lo) / (hi - lo);
}

struct Candidate {
    double h;
    double c;
    double s;
    double v;
    double w;
    std::vector<State> traj;
};

static std::tuple<double,double,std::vector<State>>
dwa_control(const State& s, const std::pair<double,double>& goal,
            const std::vector<std::pair<double,double>>& obs, const Config& cfg) {

    auto [vL, vU, wL, wU] = dynamic_window(s, cfg);

    std::vector<Candidate> cand;
    for (double v = vL; v <= vU + 1e-12; v += cfg.v_res) {
        for (double w = wL; w <= wU + 1e-12; w += cfg.w_res) {
            auto traj = rollout(s, v, w, cfg);
            double clear = min_clearance(traj, obs) - cfg.robot_radius;
            if (!admissible(v, clear, cfg)) continue;
            Candidate c;
            c.h = heading_score(traj, goal);
            c.c = clear;
            c.s = speed_score(v, cfg);
            c.v = v;
            c.w = w;
            c.traj = std::move(traj);
            cand.push_back(std::move(c));
        }
    }

    if (cand.empty()) {
        return {0.0, 0.0, std::vector<State>{s} };
    }

    std::vector<double> hs, cs, ss;
    hs.reserve(cand.size()); cs.reserve(cand.size()); ss.reserve(cand.size());
    for (const auto& c : cand) { hs.push_back(c.h); cs.push_back(c.c); ss.push_back(c.s); }
    normalize(hs); normalize(cs); normalize(ss);

    double bestJ = -1e18;
    size_t bestIdx = 0;
    for (size_t i = 0; i < cand.size(); ++i) {
        double J = cfg.w_heading * hs[i] + cfg.w_clear * cs[i] + cfg.w_speed * ss[i];
        if (J > bestJ) { bestJ = J; bestIdx = i; }
    }

    return {cand[bestIdx].v, cand[bestIdx].w, cand[bestIdx].traj};
}

int main() {
    Config cfg;
    State s;
    std::pair<double,double> goal{8.0, 6.0};

    std::vector<std::pair<double,double>> obs;
    for (int i = 0; i < 15; ++i) obs.push_back({2.0 + i*(5.0/14.0), 3.0});
    for (int i = 0; i < 12; ++i) obs.push_back({4.0, 1.0 + i*(4.0/11.0)});

    for (int k = 0; k < 600; ++k) {
        auto [v, w, traj] = dwa_control(s, goal, obs, cfg);
        s = motion_step(s, v, w, cfg.dt);
        double dist = std::hypot(s.x - goal.first, s.y - goal.second);
        if (k % 20 == 0) {
            std::cout << "k=" << k << " x=" << s.x << " y=" << s.y << " dist=" << dist << " v=" << v << " w=" << w << "\n";
        }
        if (dist < 0.3) break;
    }
    std::cout << "Final: x=" << s.x << " y=" << s.y << " theta=" << s.theta << "\n";
    return 0;
}
      

7.3 Java Implementation

Code: Chapter15_Lesson3.java

/*
Chapter15_Lesson3.java
Dynamic Window Approach (DWA) — minimal Java implementation (no ROS required).
Compile:
  javac Chapter15_Lesson3.java
Run:
  java Chapter15_Lesson3
*/

import java.util.ArrayList;
import java.util.List;

public class Chapter15_Lesson3 {

    static class State {
        double x, y, theta, v, w;
        State(double x, double y, double theta, double v, double w) {
            this.x = x; this.y = y; this.theta = theta; this.v = v; this.w = w;
        }
    }

    static class Config {
        double vMin = -0.2, vMax = 1.0;
        double wMin = -1.5, wMax = 1.5;

        double aMax = 0.8;
        double aBrake = 1.0;
        double alphaMax = 2.0;

        double dt = 0.1;
        double T = 2.0;
        double vRes = 0.05;
        double wRes = 0.1;

        double robotRadius = 0.3;

        double wHeading = 0.4;
        double wClear = 0.4;
        double wSpeed = 0.2;
    }

    static class Candidate {
        double h, c, s, v, w;
        List<State> traj;
        Candidate(double h, double c, double s, double v, double w, List<State> traj) {
            this.h = h; this.c = c; this.s = s; this.v = v; this.w = w; this.traj = traj;
        }
    }

    static double wrapToPi(double a) {
        double pi = Math.PI;
        a = (a + pi) % (2.0 * pi);
        if (a < 0) a += 2.0 * pi;
        return a - pi;
    }

    static State motionStep(State s, double v, double w, double dt) {
        double nx = s.x + v * Math.cos(s.theta) * dt;
        double ny = s.y + v * Math.sin(s.theta) * dt;
        double nt = s.theta + w * dt;
        return new State(nx, ny, nt, v, w);
    }

    static double[] dynamicWindow(State s, Config cfg) {
        double vLowDyn = s.v - cfg.aBrake * cfg.dt;
        double vHighDyn = s.v + cfg.aMax * cfg.dt;
        double wLowDyn = s.w - cfg.alphaMax * cfg.dt;
        double wHighDyn = s.w + cfg.alphaMax * cfg.dt;

        double vL = Math.max(cfg.vMin, vLowDyn);
        double vU = Math.min(cfg.vMax, vHighDyn);
        double wL = Math.max(cfg.wMin, wLowDyn);
        double wU = Math.min(cfg.wMax, wHighDyn);
        return new double[]{vL, vU, wL, wU};
    }

    static List<State> rollout(State s0, double v, double w, Config cfg) {
        int n = (int)Math.floor(cfg.T / cfg.dt);
        List<State> traj = new ArrayList<>();
        traj.add(s0);
        State s = s0;
        for (int i = 0; i < n; i++) {
            s = motionStep(s, v, w, cfg.dt);
            traj.add(s);
        }
        return traj;
    }

    static double minClearance(List<State> traj, double[][] obs) {
        double best = Double.POSITIVE_INFINITY;
        for (State st : traj) {
            for (double[] o : obs) {
                double dx = st.x - o[0];
                double dy = st.y - o[1];
                double d = Math.hypot(dx, dy);
                if (d < best) best = d;
            }
        }
        return best;
    }

    static double headingScore(List<State> traj, double gx, double gy) {
        State last = traj.get(traj.size() - 1);
        double dir = Math.atan2(gy - last.y, gx - last.x);
        double err = wrapToPi(dir - last.theta);
        return Math.cos(err);
    }

    static double speedScore(double v, Config cfg) {
        return (v - cfg.vMin) / Math.max(1e-9, (cfg.vMax - cfg.vMin));
    }

    static boolean admissible(double v, double clear, Config cfg) {
        double vv = Math.max(0.0, v);
        double dStop = (vv * vv) / Math.max(1e-9, 2.0 * cfg.aBrake);
        return clear > (dStop + cfg.robotRadius);
    }

    static double[] normalize(double[] vals) {
        double lo = Double.POSITIVE_INFINITY, hi = Double.NEGATIVE_INFINITY;
        for (double v : vals) { lo = Math.min(lo, v); hi = Math.max(hi, v); }
        double[] out = new double[vals.length];
        if (Math.abs(hi - lo) < 1e-12) return out; // all zeros
        for (int i = 0; i < vals.length; i++) out[i] = (vals[i] - lo) / (hi - lo);
        return out;
    }

    static Object[] dwaControl(State s, double gx, double gy, double[][] obs, Config cfg) {
        double[] win = dynamicWindow(s, cfg);
        double vL = win[0], vU = win[1], wL = win[2], wU = win[3];

        List<Candidate> cand = new ArrayList<>();
        for (double v = vL; v <= vU + 1e-12; v += cfg.vRes) {
            for (double w = wL; w <= wU + 1e-12; w += cfg.wRes) {
                List<State> traj = rollout(s, v, w, cfg);
                double clear = minClearance(traj, obs) - cfg.robotRadius;
                if (!admissible(v, clear, cfg)) continue;
                double h = headingScore(traj, gx, gy);
                double sp = speedScore(v, cfg);
                cand.add(new Candidate(h, clear, sp, v, w, traj));
            }
        }

        if (cand.isEmpty()) return new Object[]{0.0, 0.0, List.of(s)};

        double[] hs = new double[cand.size()];
        double[] cs = new double[cand.size()];
        double[] ss = new double[cand.size()];
        for (int i = 0; i < cand.size(); i++) {
            hs[i] = cand.get(i).h;
            cs[i] = cand.get(i).c;
            ss[i] = cand.get(i).s;
        }
        hs = normalize(hs);
        cs = normalize(cs);
        ss = normalize(ss);

        double bestJ = -1e18;
        int bestIdx = 0;
        for (int i = 0; i < cand.size(); i++) {
            double J = cfg.wHeading * hs[i] + cfg.wClear * cs[i] + cfg.wSpeed * ss[i];
            if (J > bestJ) { bestJ = J; bestIdx = i; }
        }
        Candidate best = cand.get(bestIdx);
        return new Object[]{best.v, best.w, best.traj};
    }

    public static void main(String[] args) {
        Config cfg = new Config();
        State s = new State(0.0, 0.0, 0.0, 0.0, 0.0);
        double gx = 8.0, gy = 6.0;

        List<double[]> obsList = new ArrayList<>();
        for (int i = 0; i < 15; i++) obsList.add(new double[]{2.0 + i*(5.0/14.0), 3.0});
        for (int i = 0; i < 12; i++) obsList.add(new double[]{4.0, 1.0 + i*(4.0/11.0)});
        double[][] obs = obsList.toArray(new double[0][0]);

        for (int k = 0; k < 600; k++) {
            Object[] out = dwaControl(s, gx, gy, obs, cfg);
            double v = (double)out[0];
            double w = (double)out[1];
            s = motionStep(s, v, w, cfg.dt);
            double dist = Math.hypot(s.x - gx, s.y - gy);
            if (k % 20 == 0) {
                System.out.printf("k=%d x=%.3f y=%.3f dist=%.3f v=%.2f w=%.2f%n", k, s.x, s.y, dist, v, w);
            }
            if (dist < 0.3) break;
        }
        System.out.printf("Final: x=%.3f y=%.3f theta=%.3f%n", s.x, s.y, s.theta);
    }
}
      

7.4 MATLAB / Simulink Implementation

MATLAB can prototype DWA quickly; for deployment, toolchains typically use Robotics System Toolbox and code generation (when applicable). The file below provides: (i) a runnable DWA demo, and (ii) a script to programmatically build a Simulink scaffold.

Code: Chapter15_Lesson3.m

% Chapter15_Lesson3.m
% Dynamic Window Approach (DWA) — MATLAB reference implementation + Simulink build script.
%
% Usage:
%   Chapter15_Lesson3();
%
% Notes:
% - Obstacles are point obstacles (Mx2). Replace with costmap distance queries in real AMR stacks.
% - The Simulink section programmatically builds a simple model for unicycle + DWA controller.

function Chapter15_Lesson3()
    cfg = dwa_config();

    s = struct('x',0,'y',0,'theta',0,'v',0,'w',0);
    goal = [8; 6];

    obs = [];
    obs = [obs; [linspace(2,7,15)' 3*ones(15,1)]];
    obs = [obs; [4*ones(12,1) linspace(1,5,12)']];

    path = [s.x, s.y];
    for k = 1:600
        [v, w, traj] = dwa_control(s, goal, obs, cfg); %#ok<ASGLU>
        s = motion_step(s, v, w, cfg.dt);
        path(end+1,:) = [s.x, s.y]; %#ok<AGROW>
        if norm([s.x; s.y] - goal) < 0.3
            break;
        end
    end

    fprintf('Final state: x=%.3f y=%.3f theta=%.3f (steps=%d)\n', s.x, s.y, s.theta, size(path,1));

    figure; hold on; grid on; axis equal;
    plot(path(:,1), path(:,2), 'LineWidth', 1.5);
    scatter(obs(:,1), obs(:,2), 20, 'filled');
    scatter(goal(1), goal(2), 80, 'p', 'filled');
    legend('trajectory','obstacles','goal');
    title('DWA demo (MATLAB)');
end

function cfg = dwa_config()
    cfg.v_min = -0.2; cfg.v_max = 1.0;
    cfg.w_min = -1.5; cfg.w_max = 1.5;

    cfg.a_max = 0.8;
    cfg.a_brake = 1.0;
    cfg.alpha_max = 2.0;

    cfg.dt = 0.1;
    cfg.T  = 2.0;
    cfg.v_res = 0.05;
    cfg.w_res = 0.1;

    cfg.robot_radius = 0.3;

    cfg.w_heading = 0.4;
    cfg.w_clear   = 0.4;
    cfg.w_speed   = 0.2;
end

function s2 = motion_step(s, v, w, dt)
    s2 = s;
    s2.x = s.x + v*cos(s.theta)*dt;
    s2.y = s.y + v*sin(s.theta)*dt;
    s2.theta = wrapToPi(s.theta + w*dt);
    s2.v = v;
    s2.w = w;
end

function [v_best, w_best, traj_best] = dwa_control(s, goal, obs, cfg)
    [vL, vU, wL, wU] = dynamic_window(s, cfg);

    cand = struct('h',{},'c',{},'sp',{},'v',{},'w',{},'traj',{});
    idx = 0;

    for v = vL:cfg.v_res:(vU + 1e-12)
        for w = wL:cfg.w_res:(wU + 1e-12)
            traj = rollout(s, v, w, cfg);
            clear = min_clearance(traj, obs) - cfg.robot_radius;
            if ~admissible(v, clear, cfg)
                continue;
            end
            idx = idx + 1;
            cand(idx).h = heading_score(traj, goal); %#ok<AGROW>
            cand(idx).c = clear;
            cand(idx).sp = speed_score(v, cfg);
            cand(idx).v = v;
            cand(idx).w = w;
            cand(idx).traj = traj;
        end
    end

    if isempty(cand)
        v_best = 0; w_best = 0; traj_best = {s};
        return;
    end

    hs = normalize([cand.h]);
    cs = normalize([cand.c]);
    ss = normalize([cand.sp]);

    Jbest = -inf; best = 1;
    for i = 1:numel(cand)
        J = cfg.w_heading*hs(i) + cfg.w_clear*cs(i) + cfg.w_speed*ss(i);
        if J > Jbest
            Jbest = J; best = i;
        end
    end

    v_best = cand(best).v;
    w_best = cand(best).w;
    traj_best = cand(best).traj;
end

function [vL, vU, wL, wU] = dynamic_window(s, cfg)
    v_low_dyn = s.v - cfg.a_brake * cfg.dt;
    v_high_dyn = s.v + cfg.a_max * cfg.dt;
    w_low_dyn = s.w - cfg.alpha_max * cfg.dt;
    w_high_dyn = s.w + cfg.alpha_max * cfg.dt;

    vL = max(cfg.v_min, v_low_dyn);
    vU = min(cfg.v_max, v_high_dyn);
    wL = max(cfg.w_min, w_low_dyn);
    wU = min(cfg.w_max, w_high_dyn);
end

function traj = rollout(s0, v, w, cfg)
    n = floor(cfg.T / cfg.dt);
    traj = cell(n+1,1);
    traj{1} = s0;
    s = s0;
    for i = 1:n
        s = motion_step(s, v, w, cfg.dt);
        traj{i+1} = s;
    end
end

function dmin = min_clearance(traj, obs)
    pts = zeros(numel(traj),2);
    for i = 1:numel(traj)
        pts(i,:) = [traj{i}.x, traj{i}.y];
    end
    % pairwise distances N x M
    D = sqrt((pts(:,1) - obs(:,1)').^2 + (pts(:,2) - obs(:,2)').^2);
    dmin = min(D(:));
end

function h = heading_score(traj, goal)
    last = traj{end};
    dir = atan2(goal(2) - last.y, goal(1) - last.x);
    err = wrapToPi(dir - last.theta);
    h = cos(err);
end

function sp = speed_score(v, cfg)
    sp = (v - cfg.v_min) / max(1e-9, (cfg.v_max - cfg.v_min));
end

function ok = admissible(v, clear, cfg)
    dstop = (max(0,v)^2) / max(1e-9, 2*cfg.a_brake);
    ok = clear > (dstop + cfg.robot_radius);
end

function y = normalize(x)
    lo = min(x); hi = max(x);
    if abs(hi - lo) < 1e-12
        y = zeros(size(x));
    else
        y = (x - lo) ./ (hi - lo);
    end
end

function a = wrapToPi(a)
    a = mod(a + pi, 2*pi) - pi;
end

% -------------------------------------------------------------------------
% Simulink: Build a minimal DWA closed-loop model programmatically
% -------------------------------------------------------------------------
function Chapter15_Lesson3_build_simulink()
    mdl = 'Chapter15_Lesson3_DWA_Simulink';
    if bdIsLoaded(mdl); close_system(mdl,0); end
    new_system(mdl); open_system(mdl);

    % Blocks: Integrator for x,y,theta (unicycle), MATLAB Function for DWA,
    % and a "To Workspace" for logging.
    add_block('simulink/Sources/Constant', [mdl '/Goal'], 'Value','[8;6]');
    add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/DWA_Controller']);
    add_block('simulink/Continuous/Integrator', [mdl '/Int_x']);
    add_block('simulink/Continuous/Integrator', [mdl '/Int_y']);
    add_block('simulink/Continuous/Integrator', [mdl '/Int_theta']);

    add_block('simulink/Sinks/To Workspace', [mdl '/x_log'], 'VariableName','x_log');
    add_block('simulink/Sinks/To Workspace', [mdl '/y_log'], 'VariableName','y_log');

    % Positioning (rough)
    set_param([mdl '/Goal'], 'Position', [40 40 80 70]);
    set_param([mdl '/DWA_Controller'], 'Position', [140 30 280 90]);
    set_param([mdl '/Int_x'], 'Position', [340 20 370 50]);
    set_param([mdl '/Int_y'], 'Position', [340 70 370 100]);
    set_param([mdl '/Int_theta'], 'Position', [340 120 370 150]);
    set_param([mdl '/x_log'], 'Position', [420 20 480 50]);
    set_param([mdl '/y_log'], 'Position', [420 70 480 100]);

    % Fill MATLAB Function code (simplified DWA: outputs v,w based on x,y,theta)
    mf = Simulink.findBlocks(mdl, 'BlockType', 'MATLABFunction');
    mf = mf{1};
    code = [
        "function [v,w] = f(goal,x,y,theta)" newline ...
        "%#codegen" newline ...
        "% Minimal placeholder: replace with full dwa_control logic if desired." newline ...
        "dx = goal(1) - x; dy = goal(2) - y;" newline ...
        "dir = atan2(dy,dx);" newline ...
        "err = atan2(sin(dir-theta), cos(dir-theta));" newline ...
        "v = min(0.8, sqrt(dx^2+dy^2));" newline ...
        "w = max(-1.0, min(1.0, 2.0*err));" newline ...
        "end" newline
    ];
    set_param([mdl '/DWA_Controller'], 'Script', code);

    % Wiring: Goal to controller, controller to dynamics, states back
    add_line(mdl, 'Goal/1', 'DWA_Controller/1');
    add_line(mdl, 'Int_x/1', 'DWA_Controller/2');
    add_line(mdl, 'Int_y/1', 'DWA_Controller/3');
    add_line(mdl, 'Int_theta/1', 'DWA_Controller/4');

    % Unicycle dynamics: xdot=v*cos(theta), ydot=v*sin(theta), thetadot=w
    % Use Gain blocks and Trigonometric function blocks for a complete model
    % (omitted for brevity). Students can complete this as an exercise.

    save_system(mdl);
    fprintf('Built Simulink model: %s\n', mdl);
end
      

7.5 Wolfram Mathematica Implementation

Code: Chapter15_Lesson3.nb

(* ::Package:: *)

(* Chapter15_Lesson3.nb
   Dynamic Window Approach (DWA) — Wolfram Language reference notebook content.
   Open this file in Mathematica.
*)

ClearAll["Global`*"];

(* Configuration *)
cfg = <|
  "vMin" -> -0.2, "vMax" -> 1.0,
  "wMin" -> -1.5, "wMax" -> 1.5,
  "aMax" -> 0.8, "aBrake" -> 1.0, "alphaMax" -> 2.0,
  "dt" -> 0.1, "T" -> 2.0,
  "vRes" -> 0.05, "wRes" -> 0.1,
  "rRobot" -> 0.3,
  "wHeading" -> 0.4, "wClear" -> 0.4, "wSpeed" -> 0.2
|>;

wrapToPi[a_] := Mod[a + Pi, 2 Pi] - Pi;

motionStep[s_, v_, w_, dt_] := Module[{x, y, th},
  {x, y, th} = s[[{1, 2, 3}]];
  {x + v Cos[th] dt, y + v Sin[th] dt, wrapToPi[th + w dt], v, w}
];

dynamicWindow[s_, cfg_] := Module[{v, w, dt, aBrake, aMax, alphaMax,
   vL, vU, wL, wU},
  {v, w} = s[[{4, 5}]];
  dt = cfg["dt"]; aBrake = cfg["aBrake"]; aMax = cfg["aMax"]; alphaMax = cfg["alphaMax"];
  vL = Max[cfg["vMin"], v - aBrake dt];
  vU = Min[cfg["vMax"], v + aMax dt];
  wL = Max[cfg["wMin"], w - alphaMax dt];
  wU = Min[cfg["wMax"], w + alphaMax dt];
  {vL, vU, wL, wU}
];

rollout[s0_, v_, w_, cfg_] := Module[{n, dt, s = s0},
  n = Floor[cfg["T"]/cfg["dt"]];
  dt = cfg["dt"];
  NestList[motionStep[#, v, w, dt] &, s, n]
];

minClearance[traj_, obs_] := Module[{pts},
  pts = traj[[All, {1, 2}]];
  Min[Flatten[EuclideanDistance @@@ Tuples[{pts, obs}]]]
];

headingScore[traj_, goal_] := Module[{last, dir, err},
  last = traj[[-1]];
  dir = ArcTan @@ Reverse[(goal - last[[{1, 2}]])];
  err = wrapToPi[dir - last[[3]]];
  Cos[err]
];

speedScore[v_, cfg_] := (v - cfg["vMin"]) / Max[10^-9, (cfg["vMax"] - cfg["vMin"])];

admissibleQ[v_, clear_, cfg_] := Module[{dStop},
  dStop = (Max[0, v]^2) / Max[10^-9, 2 cfg["aBrake"]];
  clear > (dStop + cfg["rRobot"])
];

normalize[list_] := Module[{lo = Min[list], hi = Max[list]},
  If[Abs[hi - lo] < 10^-12, ConstantArray[0., Length[list]], (list - lo)/(hi - lo)]
];

dwaControl[s_, goal_, obs_, cfg_] := Module[
  {vL, vU, wL, wU, vs, ws, cand = {}, traj, clear, h, sp, hs, cs, ss, J, best},
  {vL, vU, wL, wU} = dynamicWindow[s, cfg];
  vs = Range[vL, vU, cfg["vRes"]];
  ws = Range[wL, wU, cfg["wRes"]];

  Do[
    traj = rollout[s, v, w, cfg];
    clear = minClearance[traj, obs] - cfg["rRobot"];
    If[admissibleQ[v, clear, cfg],
      h = headingScore[traj, goal];
      sp = speedScore[v, cfg];
      AppendTo[cand, <|"h" -> h, "c" -> clear, "s" -> sp, "v" -> v, "w" -> w, "traj" -> traj|>]
    ],
    {v, vs}, {w, ws}
  ];

  If[cand === {}, Return[{0., 0., {s} }]];

  hs = normalize[cand[[All, "h"]]];
  cs = normalize[cand[[All, "c"]]];
  ss = normalize[cand[[All, "s"]]];

  J = cfg["wHeading"] hs + cfg["wClear"] cs + cfg["wSpeed"] ss;
  best = Ordering[J, -1][[1]];
  {cand[[best, "v"]], cand[[best, "w"]], cand[[best, "traj"]]}
];

(* Demo *)
s = {0., 0., 0., 0., 0.};
goal = {8., 6.};

obs1 = Transpose[{Subdivide[2., 7., 14], ConstantArray[3., 15]}];
obs2 = Transpose[{ConstantArray[4., 12], Subdivide[1., 5., 11]}];
obs = Join[obs1, obs2];

path = {s[[{1, 2}]]};
Do[
  {v, w, traj} = dwaControl[s, goal, obs, cfg];
  s = motionStep[s, v, w, cfg["dt"]];
  path = Append[path, s[[{1, 2}]]];
  If[Norm[s[[{1, 2}]] - goal] < 0.3, Break[]],
  {k, 1, 600}
];

Show[
  ListLinePlot[path, PlotRange -> All, AspectRatio -> 1],
  ListPlot[obs, PlotStyle -> PointSize[0.012]],
  ListPlot[{goal}, PlotMarkers -> {"\[Star]", 16}]
]
      

Problem 1 (Dynamic Window Derivation): Starting from kinematic limits \( v \in [v_{\min}, v_{\max}] \):, \( \omega \in [\omega_{\min}, \omega_{\max}] \): and acceleration bounds \( \dot v \in [-a_{\text{brake} }, a_{\max}] \):, \( \dot\omega \in [-\alpha_{\max}, \alpha_{\max}] \):, derive the window bounds \( v_{\text{low} }, v_{\text{high} }, \omega_{\text{low} }, \omega_{\text{high} } \): used by DWA for one control period \( \Delta t \):.

Solution:

Over a short interval \( \Delta t \):, acceleration feasibility implies \( v \in [v_t - a_{\text{brake} }\Delta t,\ v_t + a_{\max}\Delta t] \): and \( \omega \in [\omega_t - \alpha_{\max}\Delta t,\ \omega_t + \alpha_{\max}\Delta t] \):. Intersecting with kinematic bounds yields:

\[ \begin{aligned} v_{\text{low} } &= \max\!\left(v_{\min},\, v_t - a_{\text{brake} }\Delta t\right),\quad v_{\text{high} } = \min\!\left(v_{\max},\, v_t + a_{\max}\Delta t\right),\\ \omega_{\text{low} } &= \max\!\left(\omega_{\min},\, \omega_t - \alpha_{\max}\Delta t\right),\quad \omega_{\text{high} } = \min\!\left(\omega_{\max},\, \omega_t + \alpha_{\max}\Delta t\right). \end{aligned} \]

Problem 2 (Stop-Safety Inequality): Assume the robot can brake with constant deceleration magnitude \( a_{\text{brake} } \):. Derive the stopping distance \( d_{\text{stop} }(v) \): from initial forward speed \( v \ge 0 \): and state the admissibility condition using the minimum clearance \( d_{\min}(\tau) \):.

Solution:

With constant deceleration, kinematics give \( v^2 = 2a_{\text{brake} }d \):. Solving for distance:

\[ d_{\text{stop} }(v) = \frac{v^2}{2a_{\text{brake} } }. \]

If clearance along the rollout is \( d_{\min}(\tau) \): measured from the robot boundary, the stop-safe condition is: \( d_{\min}(\tau) > d_{\text{stop} }(v) + r_{\text{robot} } \):.

Problem 3 (Rollout Discretization Error): Consider forward Euler simulation of the unicycle model with step \( \Delta t \):. Give a bound on the one-step local truncation error order and explain qualitatively how this affects DWA performance.

Solution:

Forward Euler has local truncation error \( \mathcal{O}(\Delta t^2) \): and global error \( \mathcal{O}(\Delta t) \): over a fixed horizon. If \( \Delta t \): is too large, the simulated trajectory deviates from the true executed motion, which can violate clearance predictions and destabilize scoring. Practical DWA uses small \( \Delta t \): consistent with the controller update rate and includes conservative safety margins.

Problem 4 (Candidate Set Complexity): Suppose the dynamic window spans \( \Delta v \): in linear velocity and \( \Delta\omega \): in yaw rate, and you sample with steps \( \delta v \): and \( \delta\omega \):. With rollout length \( N = T/\Delta t \):, give the asymptotic time complexity per control cycle.

Solution:

The number of candidates is approximately \( n_v \approx \Delta v/\delta v \): and \( n_\omega \approx \Delta\omega/\delta\omega \):. Each candidate rollout costs \( \mathcal{O}(N) \): for state propagation plus obstacle checks. Therefore total cost is \( \mathcal{O}(n_v n_\omega N) \):, plus the per-step distance-query cost (often optimized via distance fields or spatial indices).

Problem 5 (Weight Tuning as a Constrained Optimization): Consider the normalized score \( J = w_h\widehat{H} + w_c\widehat{C} + w_v\widehat{V} \): with \( w_h,w_c,w_v \ge 0 \): and \( w_h+w_c+w_v = 1 \):. Show that maximizing \( J \): over weights for a fixed candidate is achieved by placing all weight on the largest normalized component, and explain why this is undesirable in practice.

Solution:

For a fixed candidate, \( \widehat{H},\widehat{C},\widehat{V} \): are constants in \( [0,1] \):. Over the simplex, \( J \): is linear in the weights, so its maximum occurs at an extreme point: \( (w_h,w_c,w_v) \in \{(1,0,0),(0,1,0),(0,0,1)\} \):. Hence all weight goes to whichever component is largest. In practice, this is undesirable because it makes behavior brittle: a tiny change in normalization can flip the chosen extreme, causing oscillations (e.g., switching between “go fast” and “stay safe”). Therefore DWA fixes weights (or adapts them smoothly) to balance objectives robustly.

We formulated DWA as a real-time velocity selection method that (i) constructs a dynamically feasible search region in \( (v,\omega) \): space, (ii) forward-simulates short-horizon trajectories under a mobile kinematic model, (iii) enforces a stop-safety admissibility constraint, and (iv) chooses commands by multi-objective scoring with normalization. This sets the foundation for more advanced local planners such as TEB in the next lesson.