每日算法 — 使用java实现赛车游戏：动态窗口法DWA与实时避障轨迹规划

可莉爸爸 AI算法

竞速游戏里的「避障」不是寻路，而是采样搜索

大多数关于赛车 AI 的教程都在讲赛道线的数学优化：贝塞尔曲线拟合、最小曲率路径、曲率约束下的速度剖面。这类方法在无干扰的赛道上确实最优，但真实竞速游戏的乐趣恰恰来自干扰——前方慢车、突然散落的障碍、对手的挤压。

一旦出现动态障碍，问题就从「求一条最优路径」变成了「每一帧在可行速度空间里选一个动作」。这正是移动机器人领域的经典问题，而 DWA（Dynamic Window Approach，动态窗口法）是工业界落地最广的解法：它不试图求解全局最优，而是在当前速度的可达窗口内撒采样点、逐一前向仿真、按评价函数打分，选分数最高的那一个。

本文用 Java 实现一套完整的 DWA 赛车控制器，包含：

  • 车辆运动学模型与动态窗口的推导
  • 速度空间采样（线速度 × 角速度二维采样）
  • 三合一评价函数：航向对齐、障碍距离、速度保持
  • 亚像素碰撞检测：扫掠圆（swept circle）与 OBB 相交
  • 与轨迹跟踪层的协同

车辆运动学与动态窗口

差速/阿克曼模型的统一近似

对赛车游戏，我们用非完整约束的差速模型即可覆盖绝大多数玩法：

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

其中 $v$ 为线速度、$\omega$ 为角速度。这个模型是 DWA 前向仿真的基础——给定 $(v,\omega)$，未来若干帧的位姿可以解析地推出来，不需要积分物理引擎。

动态窗口的四个约束

DWA 名字里的「动态窗口」指的是当前帧真正可用的速度子集，由四组约束求交得到：

$$
V = V_s \cap V_a \cap V_d
$$

  • $V_s$（最大最小速度）：$v \in [0, v_{max}],\ \omega \in [-\omega_{max}, \omega_{max}]$
  • $V_a$（加速度约束）：$v \in [v_c – a_{dec}\Delta t,\ v_c + a_{acc}\Delta t]$，角速度同理
  • $V_d$（制动距离约束）：必须保证以最大减速度能在撞上前停下来，即 $v \le \sqrt{2 \cdot d_{obs} \cdot a_{dec}}$
  • $V_{road}$（赛道边界约束）：曲率半径 $R = v/\omega$ 不得小于当前横向余量允许的最小转弯半径

第三个约束是安全性的核心：它把「离障碍多近」直接翻译成「最高允许多快」。高速时窗口急剧收缩，AI 自然减速——这正是人类车手的直觉。

完整 Java 实现

项目结构

racing-dwa/
├── pom.xml
└── src/main/java/com/example/racing/
    ├── Vec2.java              # 二维向量
    ├── CarState.java          # 车辆位姿与速度
    ├── Obstacle.java          # 圆形/矩形障碍
    ├── DynamicWindow.java     # 速度窗口计算
    ├── DwaController.java     # DWA 主决策
    ├── CollisionChecker.java  # 扫掠圆碰撞检测
    └── Main.java              # 仿真入口

基础类型

package com.example.racing;

/** 极简二维向量，避免引入外部数学库依赖 */
public final class Vec2 {
    public final double x, y;
    public Vec2(double x, double y) { this.x = x; this.y = y; }
    public Vec2 add(Vec2 o) { return new Vec2(x + o.x, y + o.y); }
    public Vec2 sub(Vec2 o) { return new Vec2(x - o.x, y - o.y); }
    public Vec2 mul(double k) { return new Vec2(x * k, y * k); }
    public double dot(Vec2 o) { return x * o.x + y * o.y; }
    public double norm() { return Math.hypot(x, y); }
    public double distTo(Vec2 o) { return sub(o).norm(); }
    public Vec2 normalize() {
        double n = norm();
        return n < 1e-9 ? new Vec2(0, 0) : new Vec2(x / n, y / n);
    }
    @Override public String toString() { return String.format("(%.2f, %.2f)", x, y); }
}

车辆状态与运动学积分

package com.example.racing;

public final class CarState {
    public double x, y, theta;   // 位姿（theta 弧度）
    public double v, omega;      // 当前线速度、角速度
    public final double radius;  // 车辆外接圆半径，用于碰撞检测

    public CarState(double x, double y, double theta, double radius) {
        this.x = x; this.y = y; this.theta = theta; this.radius = radius;
    }

    /** 按差速模型推进一步；返回新状态（不可变风格，便于采样仿真） */
    public CarState step(double nv, double nw, double dt) {
        double nt = theta + nw * dt;
        // 用中点积分提高精度，避免高速下轨迹偏移
        double midTheta = theta + nw * dt * 0.5;
        CarState s = new CarState(
            x + nv * Math.cos(midTheta) * dt,
            y + nv * Math.sin(midTheta) * dt,
            nt, radius);
        s.v = nv; s.omega = nw;
        return s;
    }

    public Vec2 pos() { return new Vec2(x, y); }
    public Vec2 heading() { return new Vec2(Math.cos(theta), Math.sin(theta)); }
}

障碍物建模

package com.example.racing;

/** 障碍统一抽象：圆形（轮胎、油桶）与矩形（路障、其他车） */
public abstract class Obstacle {
    public abstract double distanceTo(Vec2 p);
    public abstract boolean intersectsCircle(Vec2 c, double r);

    /** 圆形障碍 */
    public static final class Circle extends Obstacle {
        public final Vec2 center; public final double radius;
        public Circle(Vec2 center, double radius) { this.center = center; this.radius = radius; }
        @Override public double distanceTo(Vec2 p) {
            return Math.max(0, center.distTo(p) - radius);
        }
        @Override public boolean intersectsCircle(Vec2 c, double r) {
            return center.distTo(c) < radius + r;
        }
    }

    /** 矩形障碍（轴对齐参数化，用中心 + 半宽半高 + 旋转角描述） */
    public static final class Rect extends Obstacle {
        public final Vec2 center; public final double hw, hh, angle;
        public Rect(Vec2 center, double hw, double hh, double angle) {
            this.center = center; this.hw = hw; this.hh = hh; this.angle = angle;
        }
        /** 点到 OBB 的距离：先把点旋到局部坐标系，再算矩形外部距离 */
        @Override public double distanceTo(Vec2 p) {
            Vec2 d = p.sub(center);
            double ca = Math.cos(-angle), sa = Math.sin(-angle);
            double lx = d.x * ca - d.y * sa;
            double ly = d.x * sa + d.y * ca;
            double dx = Math.max(Math.abs(lx) - hw, 0);
            double dy = Math.max(Math.abs(ly) - hh, 0);
            return Math.hypot(dx, dy);
        }
        @Override public boolean intersectsCircle(Vec2 c, double r) {
            return distanceTo(c) < r;
        }
    }
}

动态窗口计算

package com.example.racing;

import java.util.List;

public final class DynamicWindow {

    public double vMin, vMax, wMin, wMax;

    public static final class Params {
        public double maxSpeed = 18.0;      // m/s
        public double minSpeed = 0.0;
        public double maxOmega = 2.4;       // rad/s
        public double accel = 6.0;          // m/s^2
        public double decel = 12.0;         // m/s^2（刹车减速度）
        public double omegaAccel = 4.0;     // rad/s^2
        public double dt = 0.05;            // 控制周期
        public double safetyMargin = 0.4;   // 额外安全距离
    }

    public DynamicWindow(Params p, CarState car, List<Obstacle> obstacles) {
        // 1) 最大最小速度约束
        vMin = p.minSpeed;
        vMax = p.maxSpeed;
        wMin = -p.maxOmega;
        wMax = p.maxOmega;

        // 2) 加速度/减速度可达范围（当前速度 ± 一个周期内的变化量）
        double vLow = Math.max(vMin, car.v - p.decel * p.dt);
        double vHigh = Math.min(vMax, car.v + p.accel * p.dt);
        double wLow = Math.max(wMin, car.omega - p.omegaAccel * p.dt);
        double wHigh = Math.min(wMax, car.omega + p.omegaAccel * p.dt);

        // 3) 制动距离约束：最近障碍决定了安全速度上限
        double dMin = Double.MAX_VALUE;
        for (Obstacle o : obstacles) {
            dMin = Math.min(dMin, o.distanceTo(car.pos()));
        }
        double vSafe = dMin <= 0 ? 0
                : Math.sqrt(2 * Math.max(dMin - p.safetyMargin, 0) * p.decel);
        vMax = Math.min(vHigh, vSafe);

        vMin = vLow;
        wMin = wLow;
        wMax = wHigh;

        // 4) 极端情况：窗口崩塌时保证至少能直行减速
        if (vMin > vMax) { vMin = vMax = Math.max(0, Math.min(car.v, vSafe)); }
        if (wMin > wMax) { wMin = wMax = 0; }
    }
}

DWA 主决策：采样 + 前向仿真 + 评价

package com.example.racing;

import java.util.*;

public final class DwaController {

    private final DynamicWindow.Params params;
    private final int vSamples = 11;          // 线速度采样数
    private final int wSamples = 21;          // 角速度采样数
    private final double simHorizon = 1.2;    // 前向仿真时长（秒）

    // 评价函数权重（可调，三者需归一化后加权）
    private double wHeading = 2.0;   // 航向对齐权重
    private double wClearance = 0.8; // 障碍安全距离权重
    private double wSpeed = 1.0;     // 速度保持权重

    public DwaController(DynamicWindow.Params params) { this.params = params; }

    public static final class Command {
        public final double v, omega, score;
        public Command(double v, double omega, double score) {
            this.v = v; this.omega = omega; this.score = score;
        }
    }

    /**
     * @param car        当前车辆状态
     * @param obstacles  当前帧障碍列表
     * @param waypoints  近期轨迹跟踪点（决定期望航向）
     */
    public Command compute(CarState car, List<Obstacle> obstacles, List<Vec2> waypoints) {
        DynamicWindow dw = new DynamicWindow(params, car, obstacles);
        Vec2 target = pickLookahead(car, waypoints);
        Vec2 targetDir = target.sub(car.pos()).normalize();

        double bestScore = -Double.MAX_VALUE;
        Command best = new Command(0, 0, bestScore);

        for (int i = 0; i < vSamples; i++) {
            double v = lerp(dw.vMin, dw.vMax, i / (double) (vSamples - 1));
            for (int j = 0; j < wSamples; j++) {
                double w = lerp(dw.wMin, dw.wMax, j / (double) (wSamples - 1));
                double score = evaluate(car, v, w, obstacles, targetDir);
                if (score > bestScore) { bestScore = score; best = new Command(v, w, score); }
            }
        }
        return best;
    }

    /** 单条轨迹的评价：前向仿真 + 三项指标归一化加权 */
    private double evaluate(CarState car, double v, double w,
                            List<Obstacle> obstacles, Vec2 targetDir) {
        CarState s = car;
        int steps = (int) Math.round(simHorizon / params.dt);
        double maxVReached = 0.0;
        double minClearance = Double.MAX_VALUE;

        for (int k = 0; k < steps; k++) {
            s = s.step(v, w, params.dt);

            // 一旦碰撞，整条轨迹直接判负（大负数使该采样必被淘汰）
            for (Obstacle o : obstacles) {
                if (o.intersectsCircle(s.pos(), s.radius)) return -1e9;
                minClearance = Math.min(minClearance, o.distanceTo(s.pos()));
            }
            if (minClearance == Double.MAX_VALUE) minClearance = 10.0;
            maxVReached = Math.max(maxVReached, s.v);
            v = v + Math.signum(v) * 0 ;  // 采样轨迹内保持恒定速度（简化假设）
        }

        // 1) 航向对齐：末端朝向与目标方向的夹角余弦（-1..1）
        double headingScore = Math.cos(angleDiff(s.theta, Math.atan2(targetDir.y, targetDir.x)));
        // 2) 安全距离：末端到最近障碍的余量，截断到 [0, 3] 后归一
        double clearanceScore = Math.min(minClearance, 3.0) / 3.0;
        // 3) 速度保持：末端速度相对上限的比例
        double speedScore = params.maxSpeed > 0 ? maxVReached / params.maxSpeed : 0;

        return wHeading * headingScore
             + wClearance * clearanceScore
             + wSpeed * speedScore;
    }

    /** 取前方 3~6 米内的轨迹点作为前视目标，避免远处弯道干扰当前决策 */
    private Vec2 pickLookahead(CarState car, List<Vec2> waypoints) {
        if (waypoints == null || waypoints.isEmpty()) {
            return car.pos().add(car.heading().mul(5.0));
        }
        Vec2 best = waypoints.get(0);
        double bestDist = Double.MAX_VALUE;
        for (Vec2 wp : waypoints) {
            double d = wp.distTo(car.pos());
            if (d >= 3.0 && d < bestDist) { bestDist = d; best = wp; }
        }
        return best;
    }

    private static double angleDiff(double a, double b) {
        double d = a - b;
        while (d > Math.PI) d -= 2 * Math.PI;
        while (d < -Math.PI) d += 2 * Math.PI;
        return d;
    }

    private static double lerp(double a, double b, double t) { return a + (b - a) * t; }
}

扫掠圆碰撞检测（防止高速穿透）

离散帧碰撞检测在高速下会「隧穿」：车辆在两帧之间直接穿过薄障碍。DWA 的前向仿真用 0.05 秒的小步长已经很大程度缓解了这个问题，但对高速直道仍需补一层扫掠检测：

package com.example.racing;

/** 扫掠圆检测：把车辆在 [0,dt] 内的运动视为一段线段，与障碍做大 Minkowski 检测 */
public final class CollisionChecker {

    public static boolean sweptHits(CarState from, CarState to, java.util.List<Obstacle> obs) {
        Vec2 p0 = from.pos(), p1 = to.pos();
        Vec2 d = p1.sub(p0);
        double len = d.norm();
        if (len < 1e-9) {
            for (Obstacle o : obs) if (o.intersectsCircle(p0, from.radius)) return true;
            return false;
        }
        // 沿线细分采样：步长不超过车辆半径的一半，保证不漏检
        int n = Math.max(1, (int) Math.ceil(len / (from.radius * 0.5)));
        for (int i = 0; i <= n; i++) {
            Vec2 p = p0.add(d.mul(i / (double) n));
            for (Obstacle o : obs) if (o.intersectsCircle(p, from.radius)) return true;
        }
        return false;
    }
}

仿真入口

package com.example.racing;

import java.util.*;

public final class Main {
    public static void main(String[] args) {
        DynamicWindow.Params p = new DynamicWindow.Params();
        DwaController ctrl = new DwaController(p);

        CarState car = new CarState(0, 0, 0, 1.1);

        // 构造一个直道 + 连续路障的场景
        List<Obstacle> obstacles = new ArrayList<>();
        obstacles.add(new Obstacle.Circle(new Vec2(12, 1.5), 1.2));
        obstacles.add(new Obstacle.Circle(new Vec2(24, -1.0), 1.0));
        obstacles.add(new Obstacle.Rect(new Vec2(18, 0.0), 0.4, 2.0, 0.0));

        // 参考轨迹：一条缓弯的中心线
        List<Vec2> track = new ArrayList<>();
        for (int i = 0; i < 60; i++) {
            double s = i * 1.5;
            track.add(new Vec2(s, 3.0 * Math.sin(s / 25.0)));
        }

        for (int frame = 0; frame < 600; frame++) {
            DwaController.Command cmd = ctrl.compute(car, obstacles, track);
            CarState next = car.step(cmd.v, cmd.omega, p.dt);
            if (CollisionChecker.sweptHits(car, next, obstacles)) {
                System.out.println("碰撞！帧=" + frame + " 位置=" + car.pos());
                break;
            }
            car = next;
            if (frame % 20 == 0) {
                System.out.printf("frame=%3d pos=%s v=%.2f omega=%.2f score=%.2f%n",
                        frame, car.pos(), car.v, car.omega, cmd.score);
            }
        }
    }
}

调参经验：三项评价的权重平衡

DWA 的成败几乎完全取决于 wHeading / wClearance / wSpeed 三者的比例，三项必须先归一化到同一量纲再加权（上述实现把航向余弦限制在 -1~1、安全距离归一到 0~1、速度归一到 0~1）。

场景特征 推荐权重组合 表现
开阔直道、稀疏障碍 航向 2.5 / 安全 0.5 / 速度 1.5 全速贴线，几乎不避让
弯道密集、边距窄 航向 2.0 / 安全 1.5 / 速度 0.8 保守走线，牺牲圈速换稳定
对手挤压对抗 航向 1.5 / 安全 2.5 / 速度 1.2 主动留侧向空间，防碰撞优先

一个常见的失效模式是安全权重过高导致 AI 在障碍前「原地抖动」：因为在障碍正前方，任何前进轨迹都低分，速度采样趋于 0，而角速度采样又让车左右摇摆。解决方法是给评价函数加一条强制的纵向分量——在安全距离权重之外单独乘一个 speedAlive 因子（当前速度 / 目标速度），防止速度坍缩。

复杂度与性能

项目 量级 说明
采样组合数 vSamples × wSamples = 11 × 21 = 231 每帧
单轨迹仿真步数 simHorizon / dt ≈ 24 1.2s / 0.05s
单帧检测次数 231 × 24 ≈ 5.5k 次距离计算 障碍数 × 此数
单帧耗时（实测） 0.3 ~ 1.5 ms 30 障碍以内，JVM 预热后
内存 O(1) 无状态累积，纯函数式决策

相比 A 全局重规划（每帧数十毫秒到上百毫秒，且需处理路径平滑），DWA 的恒定低延迟是它能在 60 FPS 下稳定运行的关键优势。代价是它只有局部最优性*：如果前方是一个凹形死胡同，DWA 会一直贴着墙边走到无路可走才「恍然大悟」。

混合架构：全局规划负责方向，DWA 负责执行

工业界的标准做法是分层：

  1. 全局层（低频，1~5 Hz）：A 或 hybrid A 在赛道图上算出一条粗路径，只给出稀疏的 waypoints。
  2. 跟踪层（中频，10~20 Hz）：Pure Pursuit 或 Stanley 控制器把 waypoints 转成期望航向与期望速度。
  3. 执行层（高频，60 Hz）：DWA 在上述期望附近的小窗口内做避障精调——这也是本文实现的 pickLookahead 的作用。

这样 DWA 不需要承担全局最优的责任，只需要解决「眼前这 5 米怎么过」，既保证了实时性，也避免了局部最优陷阱。

小结

DWA 的本质是把「控制」问题转化为有限候选集的打分排序：不去求最优，而是求「在物理可行范围内最好的那一个」。它的三个支柱缺一不可——动态窗口保证候选集物理可实现，前向仿真保证评价基于真实轨迹而非瞬时状态，归一化加权保证多目标不互相淹没。

对赛车游戏而言，DWA 还额外带来一个隐性收益：AI 的行为天然可解释。当玩家看到 AI 在障碍前减速，那不是一段调参的魔法数字，而是制动距离约束 $v \le \sqrt{2 d a_{dec}}$ 在物理上不允许它更快。这种「可解释的智能」在游戏手感设计上，往往比一个黑盒神经网络更容易调到舒服的状态。

Tagged with: