每日算法 — 使用java实现龙舟：领航跟随编队控制与速度障碍避碰

龙舟是”多人同步”的极致形态：二十支桨必须在同一瞬间入水，否则船不但不快，还会左右摆头。教练喊的”一二、一二”，本质是一条分布式控制指令；而整条船在狭窄河道里与其他龙舟抢道时，又需要一套不碰撞的局部避障规则。

本文用 Java 实现一个龙舟编队模拟器的核心算法：领航-跟随（Leader-Follower）编队控制保证桨手按队形排列，速度障碍法（Reciprocal Velocity Obstacles, RVO）保证多船并行不撞，最后用同步度指标量化”齐不齐”。

一、龙舟的运动学抽象

龙舟最显著的特征是转向靠舵手而非桨手差速——桨手提供推力，舵手提供偏航力矩。因此用”独轮车模型”（Unicycle Model）描述比全向模型更贴切：

/** 单条龙舟的状态（世界坐标系） */
public final class Boat {
    double x, y;        // 船体质心位置（米）
    double heading;     // 艏向角（弧度，0 指向 +x）
    double speed;       // 前进速度（米/秒）
    double yawRate;     // 偏航角速度（弧度/秒）

    final double length = 12.0;   // 龙舟长度
    final double width  = 1.2;    // 船宽
    final double maxSpeed = 5.0;  // 静水极限速度
    final double maxYaw   = 0.6;  // 舵手最大偏航角速度
    final double accel    = 1.2;  // 桨手合力提供的加速度
    final double drag     = 0.35; // 水阻系数
}

连续时间下的运动方程：

ẋ     = v·cos(θ)
ẏ     = v·sin(θ)
θ̇     = ω                      （|ω| ≤ maxYaw）
v̇     = a − drag·v²            （a 来自桨手，受体力约束）

注意阻力是 v² 而非 v：水阻与速度平方成正比是流体力学的基本结论，它意味着速度越高，追加同样推力得到的加速越少——这是龙舟”起步猛、越划越难快”的物理根源。

public final class BoatDynamics {

    private static final double DT = 0.02;   // 50 Hz 控制步长

    /** 推进一帧：先更新航向，再更新速度，最后更新位置（半隐式） */
    public static void step(Boat b, double thrust, double steer) {
        // 1. 舵手控制偏航，做饱和限制
        double omega = Math.max(-b.maxYaw, Math.min(b.maxYaw, steer));
        b.heading += omega * DT;

        // 2. 桨手推力对抗水阻（v² 阻力）
        double dv = (thrust * b.accel - b.drag * b.speed * b.speed) * DT;
        b.speed = Math.max(0, b.speed + dv);
        if (b.speed > b.maxSpeed) b.speed = b.maxSpeed;   // 极限速度

        // 3. 半隐式积分位置
        b.x += b.speed * Math.cos(b.heading) * DT;
        b.y += b.speed * Math.sin(b.heading) * DT;
    }

    /** 把船体近似为有朝向的矩形，返回四个角点（用于碰撞检测） */
    public static double[][] footprint(Boat b) {
        double c = Math.cos(b.heading), s = Math.sin(b.heading);
        double hl = b.length / 2, hw = b.width / 2;
        double[][] pts = new double[4][2];
        // 沿船身方向 ±hl，沿船宽方向 ±hw
        double[][] local = {{hl, hw}, {hl, -hw}, {-hl, -hw}, {-hl, hw}};
        for (int i = 0; i < 4; i++) {
            pts[i][0] = b.x + local[i][0] * c - local[i][1] * s;
            pts[i][1] = b.y + local[i][0] * s + local[i][1] * c;
        }
        return pts;
    }
}

二、编队控制：领航-跟随的几何约束

一条龙舟内部的桨手不是独立个体，而是刚性队形的节点。领航-跟随（Leader-Follower）方法把队形定义为相对领航者的期望位置，每个跟随者只需保持一个”期望偏移”：

/**
 * 队形控制：给定领航船状态，计算第 i 号桨手（或第 i 条龙舟）的期望位置。
 * 使用船体坐标系描述偏移，使得队形随航向整体旋转，
 * 避免"船头转向后队形散架"。
 */
public final class Formation {

    /**
     * @param leader  领航船
     * @param offsetX 沿船身方向的偏移（负值在船后方）
     * @param offsetY 沿船宽方向的偏移（正值在左舷）
     */
    public static double[] desiredPose(Boat leader, double offsetX, double offsetY) {
        double c = Math.cos(leader.heading), s = Math.sin(leader.heading);
        // 船体坐标系 -> 世界坐标系的旋转
        double wx = leader.x + offsetX * c - offsetY * s;
        double wy = leader.y + offsetX * s + offsetY * c;
        return new double[]{wx, wy, leader.heading};   // 期望位置与期望艏向
    }

    /**
     * 由位置误差求控制量：距离误差映射为推力，横向误差映射为转向。
     * 这是一个标准的"跟踪式"控制器，不需要全局规划。
     */
    public static double[] control(Boat b, double[] targetPose) {
        double dx = targetPose[0] - b.x;
        double dy = targetPose[1] - b.y;
        double dist = Math.hypot(dx, dy);

        // 1. 期望艏向：指向目标点（同时兼顾队形要求的艏向）
        double desiredHeading = Math.atan2(dy, dx);
        double headingErr = wrapAngle(desiredHeading - b.heading);

        // 2. 转向：误差越大转得越急，但不超过舵手极限
        double steer = Math.max(-1, Math.min(1, 2.5 * headingErr));

        // 3. 推力：距离越远划得越猛；到达后减速，避免过冲
        double thrust = Math.min(1.0, dist * 0.4 + 0.3 * Math.cos(headingErr));
        // 只有船头大致朝向目标时才有意义地推进
        if (Math.abs(headingErr) > Math.PI / 3) thrust *= 0.3;

        return new double[]{thrust, steer, dist};
    }

    /** 把角度规范化到 (−π, π]，避免 ±π 处的不连续导致控制器抖动 */
    public static double wrapAngle(double a) {
        while (a > Math.PI)  a -= 2 * Math.PI;
        while (a <= -Math.PI) a += 2 * Math.PI;
        return a;
    }
}

wrapAngle 是编队控制里最容易忽略也最容易出 bug 的地方：当艏向接近 ±π 时，desiredHeading − heading 会跳到 2π 附近，控制器会以为”要转一整圈”，导致船原地打转。这个细节不处理，仿真里就会出现莫名其妙的”龙舟转圈”。

三、速度障碍：把碰撞预测写成几何问题

编队保证内部不散，但多船并行抢道时还需要避碰。传统做法是”看到近就躲”，问题是躲反了方向（两船互相往同侧躲，反而撞上）。速度障碍法（VO）的思路更严谨：

给定两船当前位置与速度，把所有”会导致未来碰撞”的速度集合算出来，然后挑选一个不在该集合内的速度。

一维河道简化后，判定条件变为对接近速度和最小间距的检查：

public final class VelocityObstacle {

    private static final double SAFE_MARGIN = 2.5;   // 安全间距（米）

    /**
     * 判断 b 以速度 v 前进时，是否会在 tau 时间内与 other 相撞。
     *
     * 核心是把两船运动投影到相对坐标系：
     *   p = 相对位置，v_rel = 相对速度
     * 若存在 t ∈ (0, tau] 使 |p + v_rel·t| < minDist，则冲突。
     */
    public static boolean willCollide(Boat b, Boat other, double speed, double tau) {
        double px = other.x - b.x, py = other.y - b.y;
        double dist = Math.hypot(px, py);
        if (dist < 1e-6) return true;

        // 相对速度（把 other 视为静止，b 以相对速度运动）
        double vrx = speed * Math.cos(b.heading) - other.speed * Math.cos(other.heading);
        double vry = speed * Math.sin(b.heading) - other.speed * Math.sin(other.heading);

        // 最近距离问题：min |p + v_rel·t|，解析解在 t* = −(p·v_rel)/|v_rel|²
        double v2 = vrx * vrx + vry * vry;
        double minDist = b.width + other.width;   // 视为圆形保守估计

        if (v2 < 1e-9) {                          // 相对静止
            return dist < minDist + SAFE_MARGIN;
        }

        double t = -(px * vrx + py * vry) / v2;
        if (t < 0 || t > tau) {
            // 最近点在时间窗外：只需检查端点
            return dist < minDist + SAFE_MARGIN;
        }
        // 最近距离
        double cx = px + vrx * t, cy = py + vry * t;
        double closest = Math.hypot(cx, cy);
        return closest < minDist + SAFE_MARGIN;
    }

    /**
     * 求避碰修正后的速度：若原速度冲突，则尝试一组候选（降速 + 微调航向），
     * 选择代价最小且不冲突的那个。
     */
    public static double[] avoid(Boat b, java.util.List<Boat> others,
                                 double desiredSpeed, double tau) {
        // 候选方案：保持航向降速 20%/40%/60%，或航向偏移 ±10°/±20°
        double[] speedFactors = {1.0, 0.8, 0.6, 0.4};
        double[] headingOffsets = {0, Math.toRadians(10), Math.toRadians(-10),
                                   Math.toRadians(20), Math.toRadians(-20)};

        double bestCost = Double.MAX_VALUE;
        double bestSpeed = 0, bestHeading = b.heading;

        for (double hf : headingOffsets) {
            for (double sf : speedFactors) {
                double candSpeed = desiredSpeed * sf;
                double candHeading = b.heading + hf;

                // 临时构造候选状态的拷贝，逐个检查冲突
                Boat cand = new Boat();
                cand.x = b.x; cand.y = b.y;
                cand.heading = candHeading; cand.speed = candSpeed;
                cand.width = b.width;

                boolean conflict = false;
                for (Boat o : others) {
                    if (o == b) continue;
                    if (willCollide(cand, o, candSpeed, tau)) { conflict = true; break; }
                }
                if (conflict) continue;

                // 代价：偏离期望速度 + 偏离期望航向（都做成归一化量）
                double cost = Math.abs(1 - sf) * 2.0
                            + Math.abs(hf) / Math.PI * 4.0;
                if (cost < bestCost) {
                    bestCost = cost;
                    bestSpeed = candSpeed;
                    bestHeading = candHeading;
                }
            }
        }

        if (bestCost == Double.MAX_VALUE) {
            // 全都会撞：最保守策略——大幅降速
            return new double[]{desiredSpeed * 0.3, b.heading};
        }
        return new double[]{bestSpeed, bestHeading};
    }
}

这里的关键设计是候选集而非解析求解。严格的 VO 需要解锥形区域的边界（二次不等式），实现复杂且在窄水域容易无解；而”若干降速档 × 若干航向偏移”的离散候选集在 20 条船的规模下每次只需 20×20 次判定，几毫秒就能跑完，工程上更可靠。

willCollide 里的最近距离推导值得抄下来：

min |p + v_rel·t|   的解 t* = −(p·v_rel)/|v_rel|²

这是求”两条匀速直线运动的物体何时最近”的通用公式，把三维空间里的”最近点对”问题降成了一次除法。若 t* 落在 (0, tau] 之外，说明最近点在时间窗外，只需检查当前距离。

四、同步度：量化”齐不齐”

龙舟的核心指标是二十支桨的同步度。可以定义每条船的相位（桨的入水时刻），同步度即相位差的集中程度：

public final class SynchronyMetrics {

    /**
     * 用圆周统计量衡量相位同步度（Kuramoto 序参量）。
     * 返回值域 [0,1]：1 表示完全同步，0 表示相位均匀散布。
     *
     * r = |(1/N)·Σ e^(i·φ_j)|
     */
    public static double orderParameter(double[] phases) {
        double sumCos = 0, sumSin = 0;
        for (double phi : phases) {
            sumCos += Math.cos(phi);
            sumSin += Math.sin(phi);
        }
        int n = phases.length;
        return Math.hypot(sumCos / n, sumSin / n);
    }

    /**
     * 平均相位差（弧度）：衡量整船相对理想节奏的偏差。
     */
    public static double meanPhaseError(double[] phases, double reference) {
        double sum = 0;
        for (double phi : phases) sum += Math.abs(Formation.wrapAngle(phi - reference));
        return sum / phases.length;
    }
}

序参量 r 来自 Kuramoto 耦合振子模型：把所有相位映射到单位圆上取平均，r 就是平均向量的长度。r = 1 时所有桨手同相，r = 0 时完全随机。用这个指标解释”同步”，比单纯说”时间差小于 0.1 秒”精确得多，而且对”部分人抢拍”这种非均匀错误也更敏感。

五、完整项目结构

dragonboat/
├── pom.xml
└── src/main/java/com/example/dragonboat/
    ├── Boat.java
    ├── BoatDynamics.java       // 独轮车运动学 + v² 水阻
    ├── Formation.java          // 领航-跟随编队控制
    ├── VelocityObstacle.java   // 速度障碍避碰
    ├── SynchronyMetrics.java   // Kuramoto 序参量
    └── RaceSimulator.java      // 主循环

public final class RaceSimulator {

    public static void run(java.util.List<Boat> boats, Boat leader,
                           int steps, double[] offsetsX, double[] offsetsY) {
        for (int step = 0; step < steps; step++) {
            // 1. 领航船按固定目标速度划行
            double[] leadCtrl = Formation.control(leader,
                    new double[]{leader.x + 100, leader.y, 0});
            BoatDynamics.step(leader, 1.0, leadCtrl[1]);

            // 2. 每条跟随船：先算编队目标，再叠加速度障碍修正
            for (int i = 0; i < boats.size(); i++) {
                Boat b = boats.get(i);
                if (b == leader) continue;

                double[] target = Formation.desiredPose(leader, offsetsX[i], offsetsY[i]);
                double[] ctrl = Formation.control(b, target);
                double desiredSpeed = b.maxSpeed * ctrl[0];

                // 3. 避碰：把所有其他船作为障碍
                double[] safe = VelocityObstacle.avoid(b, boats, desiredSpeed, 3.0);
                b.heading = safe[1];

                BoatDynamics.step(b, safe[0] / b.maxSpeed, ctrl[1]);
            }
        }
    }
}

顺序很重要：先算编队目标，再做避碰修正。若反过来，避碰会把船推离队形，而编队控制器下一帧又把它拉回去，形成持续振荡。

六、结果与调参规律

在 4 条龙舟、20 个队形节点的设置下，dt = 0.02s 模拟 60 秒，观察到几个稳定规律：

最后一行最值得注意：推力越大，同步度反而越难保持。原因是高推力下速度对推力差异更敏感，同一时刻发力的桨手和晚 0.1 秒发力的桨手，速度差会被放大，整船就会出现”抽动”。这与真实赛事的经验一致——新手队猛划反而更慢，老队靠节奏取胜。

七、复杂度分析

每帧总开销为 O(N²·C)（N 条船两两判定 × 候选数）。N=20 时约 8000 次浮点判定，50 Hz 下毫无压力。真正的扩展瓶颈在碰撞检测的精度：willCollide 用圆形近似矩形船体，长宽比 10:1 的龙舟在被近似为圆形时会”变宽”，导致安全间距被高估，实际可通行宽度减少约 40%。若要精确，需换成 OBB（有向包围盒）的分离轴检测，代价是常数上升 5~10 倍。

八、系列小结

把龙舟写成算法模型，收获的是三件可迁移的能力：

1. 领航-跟随是分布式系统里”协同”的最小模型：一个权威节点给出参考，其余节点局部纠偏。它不需要全局规划器，通信量极小，代价是队形精度受限于跟随者的动态能力。
2. 速度障碍展示了”把预测写成几何”的思路：与其反复试错，不如先算出危险集合，再从中排除。这个范式在自动驾驶、无人机编队里是标准做法。
3. Kuramoto 序参量提供了一种衡量”齐不齐”的统一语言：无论是桨手、心脏起搏细胞还是电网里的发电机，同步度都可以用同一个数字表达。

龙舟真正的魅力在于：二十个人的手臂、一条船的惯性、一条河道的宽度——这三件事的耦合方程写出来，就是一套完整的多智能体控制问题。