每日算法 — 使用java实现赛车:物理运动学与动态路径规划算法

赛车游戏是电子游戏史上最经典的类型之一,从早期的《Pole Position》到现代的模拟驾驶游戏,其核心始终围绕物理运动学与路径规划。本文将使用Java实现一个简化版赛车系统,重点讲解车辆运动学模型、曲线路径计算以及贪心动态避障策略。

赛车物理模型

真实车辆运动遵循牛顿力学。为简化计算,我们将赛车建模为刚体,主要考虑:

  1. 位置与速度:二维平面上的位移与速率
  2. 加速度:引擎推力与摩擦阻力的合力
  3. 转向:前轮角度决定转弯半径
  4. 离心力:高速过弯时的侧向力

简化运动学方程

ax = (thrust * cos(heading) - drag * vx) / mass
ay = (thrust * sin(heading) - drag * vy) / mass
vx += ax * dt
vy += ay * dt
x += vx * dt
y += vy * dt
heading += steer * speed * dt / wheelbase

核心类实现

赛车实体

/**
 * 赛车实体类
 * 包含物理属性与运动学状态
 */
public class RaceCar {

    // 物理常量
    private static final double MAX_SPEED = 80.0;        // 最大速度 (m/s)
    private static final double MAX_THRUST = 15.0;       // 最大加速度 (m/s^2)
    private static final double DRAG_COEFFICIENT = 0.3;  // 空气阻力系数
    private static final double FRICTION = 0.1;          // 地面摩擦
    private static final double MAX_STEER = Math.PI / 4; // 最大转向角 (45度)
    private static final double WHEELBASE = 2.5;         // 轴距 (m)
    private static final double MASS = 800.0;            // 质量 (kg)
    private static final double BRAKE_FORCE = 25.0;      // 刹车力

    // 状态变量
    private double x;           // 位置X
    private double y;           // 位置Y
    private double vx;          // 速度X分量
    private double vy;          // 速度Y分量
    private double heading;     // 朝向角度(弧度,0为右方向)
    private double speed;       // 速率大小
    private double steerAngle;  // 转向角
    private double thrust;      // 当前油门(-1到1,负数为刹车)

    // 赛道信息
    private double trackWidth;  // 赛道宽度

    public RaceCar(double startX, double startY, double startHeading) {
        this.x = startX;
        this.y = startY;
        this.heading = startHeading;
        this.vx = 0;
        this.vy = 0;
        this.speed = 0;
        this.steerAngle = 0;
        this.thrust = 0;
        this.trackWidth = 10.0;
    }

    /**
     * 物理更新
     * 使用欧拉积分更新车辆状态
     * 
     * @param dt 时间步长(秒)
     */
    public void update(double dt) {
        // 计算当前速度大小和方向
        speed = Math.sqrt(vx * vx + vy * vy);

        // 1. 计算引擎推力产生的加速度
        double engineForce = thrust * MAX_THRUST;
        double ax_thrust = (engineForce * Math.cos(heading)) / MASS;
        double ay_thrust = (engineForce * Math.sin(heading)) / MASS;

        // 2. 计算阻力(与速度方向相反)
        double dragMagnitude = DRAG_COEFFICIENT * speed * speed + FRICTION * speed;
        double ax_drag = 0;
        double ay_drag = 0;

        if (speed > 0.001) {
            ax_drag = -(vx / speed) * dragMagnitude / MASS;
            ay_drag = -(vy / speed) * dragMagnitude / MASS;
        }

        // 3. 计算刹车力(当thrust < 0时)
        double ax_brake = 0;
        double ay_brake = 0;
        if (thrust < 0 && speed > 0.001) {
            double brake = Math.abs(thrust) * BRAKE_FORCE;
            ax_brake = -(vx / speed) * brake / MASS;
            ay_brake = -(vy / speed) * brake / MASS;
        }

        // 4. 总加速度
        double ax = ax_thrust + ax_drag + ax_brake;
        double ay = ay_thrust + ay_drag + ay_brake;

        // 5. 更新速度
        vx += ax * dt;
        vy += ay * dt;

        // 限制最大速度
        double newSpeed = Math.sqrt(vx * vx + vy * vy);
        if (newSpeed > MAX_SPEED) {
            vx = (vx / newSpeed) * MAX_SPEED;
            vy = (vy / newSpeed) * MAX_SPEED;
        }

        // 6. 更新位置
        x += vx * dt;
        y += vy * dt;

        // 7. 更新朝向(转向影响)
        // 转向速率与车速成正比(阿克曼转向几何简化)
        if (speed > 0.5) {
            double turnRate = (speed * Math.tan(steerAngle)) / WHEELBASE;
            heading += turnRate * dt;
        }

        // 归一化heading到[0, 2PI)
        heading = normalizeAngle(heading);
    }

    /**
     * 归一化角度到[0, 2π)
     */
    private double normalizeAngle(double angle) {
        while (angle < 0) angle += 2 * Math.PI;
        while (angle >= 2 * Math.PI) angle -= 2 * Math.PI;
        return angle;
    }

    /**
     * 设置控制输入
     * @param throttle 油门 [-1, 1],负数表示刹车
     * @param steer 转向 [-1, 1],-1为左,1为右
     */
    public void setInput(double throttle, double steer) {
        this.thrust = Math.max(-1.0, Math.min(1.0, throttle));
        this.steerAngle = steer * MAX_STEER;
    }

    // Getters
    public double getX() { return x; }
    public double getY() { return y; }
    public double getVx() { return vx; }
    public double getVy() { return vy; }
    public double getHeading() { return heading; }
    public double getSpeed() { return speed; }
    public double getSteerAngle() { return steerAngle; }

    /**
     * 获取车辆前方向量
     */
    public double[] getForwardVector() {
        return new double[]{Math.cos(heading), Math.sin(heading)};
    }

    /**
     * 获取车辆右方向量(垂直于前进方向)
     */
    public double[] getRightVector() {
        return new double[]{-Math.sin(heading), Math.cos(heading)};
    }

    @Override
    public String toString() {
        return String.format("Car(x=%.1f, y=%.1f, speed=%.1f, heading=%.1f°)",
                            x, y, speed, Math.toDegrees(heading));
    }
}

赛道与路径规划

import java.util.*;

/**
 * 赛道路径点
 * 使用贝塞尔曲线或简单路径点序列定义赛道中心线
 */
public class RaceTrack {

    private final List<TrackPoint> waypoints;  // 路径点序列
    private final double width;                 // 赛道宽度

    public RaceTrack(double width) {
        this.waypoints = new ArrayList<>();
        this.width = width;
    }

    /**
     * 添加路径点
     */
    public void addWaypoint(double x, double y) {
        waypoints.add(new TrackPoint(x, y));
    }

    /**
     * 获取最近路径点索引
     */
    public int findClosestWaypoint(double x, double y) {
        int closest = 0;
        double minDist = Double.MAX_VALUE;

        for (int i = 0; i < waypoints.size(); i++) {
            double dist = waypoints.get(i).distanceTo(x, y);
            if (dist < minDist) {
                minDist = dist;
                closest = i;
            }
        }

        return closest;
    }

    /**
     * 获取前方N个路径点
     */
    public List<TrackPoint> getLookAheadPoints(int fromIndex, int count) {
        List<TrackPoint> ahead = new ArrayList<>();
        for (int i = 1; i <= count && (fromIndex + i) < waypoints.size(); i++) {
            ahead.add(waypoints.get(fromIndex + i));
        }
        return ahead;
    }

    /**
     * 计算某点相对于赛道的横向偏移
     * 正值为右侧,负值为左侧
     */
    public double getLateralOffset(double x, double y, int waypointIndex) {
        if (waypointIndex >= waypoints.size() - 1) return 0;

        TrackPoint current = waypoints.get(waypointIndex);
        TrackPoint next = waypoints.get(waypointIndex + 1);

        // 赛道方向向量
        double dx = next.x - current.x;
        double dy = next.y - current.y;
        double len = Math.sqrt(dx * dx + dy * dy);

        if (len < 0.001) return 0;

        // 单位方向向量
        double ux = dx / len;
        double uy = dy / len;

        // 点到赛道方向的垂直距离(叉积)
        double offsetX = x - current.x;
        double offsetY = y - current.y;

        return offsetX * (-uy) + offsetY * ux;  // 垂直投影
    }

    /**
     * 检查点是否在赛道范围内
     */
    public boolean isOnTrack(double x, double y, int waypointIndex) {
        double offset = Math.abs(getLateralOffset(x, y, waypointIndex));
        return offset <= width / 2.0;
    }

    public double getWidth() { return width; }
    public List<TrackPoint> getWaypoints() { return waypoints; }

    /**
     * 路径点内部类
     */
    public static class TrackPoint {
        public final double x;
        public final double y;

        public TrackPoint(double x, double y) {
            this.x = x;
            this.y = y;
        }

        public double distanceTo(double px, double py) {
            return Math.sqrt((x - px) * (x - px) + (y - py) * (y - py));
        }

        @Override
        public String toString() {
            return String.format("(%.1f, %.1f)", x, y);
        }
    }
}

贪心路径规划策略

/**
 * 贪心赛车AI控制器
 * 每帧基于当前状态贪心选择最优控制输入
 */
public class GreedyRacingAI {

    // 控制参数
    private static final double LOOK_AHEAD_DISTANCE = 30.0;  // 前瞻距离
    private static final double CORNERING_SPEED_FACTOR = 0.6; // 过弯速度系数

    /**
     * 计算下一帧的控制输入
     * 
     * @param car 当前车辆状态
     * @param track 赛道信息
     * @return [throttle, steer] 控制数组
     */
    public double[] computeControl(RaceCar car, RaceTrack track) {
        int currentIdx = track.findClosestWaypoint(car.getX(), car.getY());

        // 1. 寻找目标路径点(前瞻)
        RaceTrack.TrackPoint target = findTargetPoint(car, track, currentIdx);
        if (target == null) {
            return new double[]{0, 0};  // 到达终点
        }

        // 2. 计算到目标的方向
        double dx = target.x - car.getX();
        double dy = target.y - car.getY();
        double targetAngle = Math.atan2(dy, dx);

        // 3. 计算转向
        double angleDiff = targetAngle - car.getHeading();
        // 归一化到[-π, π]
        while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
        while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;

        double steer = Math.max(-1.0, Math.min(1.0, angleDiff * 2.0));

        // 4. 计算速度策略(贪心:根据弯道曲率决定速度)
        double targetSpeed = computeTargetSpeed(car, track, currentIdx);
        double speedDiff = targetSpeed - car.getSpeed();

        double throttle;
        if (speedDiff > 2.0) {
            throttle = 1.0;  // 需要加速
        } else if (speedDiff < -2.0) {
            throttle = -1.0; // 需要刹车
        } else {
            throttle = speedDiff / 2.0;  // 微调
        }

        return new double[]{throttle, steer};
    }

    /**
     * 寻找前瞻目标点
     * 贪心策略:选择前方一定距离处的路径点
     */
    private RaceTrack.TrackPoint findTargetPoint(RaceCar car, RaceTrack track, int currentIdx) {
        List<RaceTrack.TrackPoint> waypoints = track.getWaypoints();

        double accumulatedDist = 0;
        for (int i = currentIdx + 1; i < waypoints.size(); i++) {
            double dist = waypoints.get(i).distanceTo(
                waypoints.get(i - 1).x, waypoints.get(i - 1).y
            );
            accumulatedDist += dist;

            if (accumulatedDist >= LOOK_AHEAD_DISTANCE) {
                return waypoints.get(i);
            }
        }

        // 返回最后一个点
        return waypoints.isEmpty() ? null : waypoints.get(waypoints.size() - 1);
    }

    /**
     * 计算目标速度
     * 基于前方路径的曲率:弯道越急,目标速度越低
     */
    private double computeTargetSpeed(RaceCar car, RaceTrack track, int currentIdx) {
        List<RaceTrack.TrackPoint> ahead = track.getLookAheadPoints(currentIdx, 5);
        if (ahead.size() < 2) {
            return RaceCar.MAX_SPEED;  // 直道全速
        }

        // 计算前方路径的弯曲程度(用角度变化近似)
        double totalAngleChange = 0;
        for (int i = 1; i < ahead.size(); i++) {
            double dx1 = ahead.get(i).x - (i == 1 ? track.getWaypoints().get(currentIdx).x : ahead.get(i - 2).x);
            double dy1 = ahead.get(i).y - (i == 1 ? track.getWaypoints().get(currentIdx).y : ahead.get(i - 2).y);
            double dx2 = ahead.get(i).x - ahead.get(i - 1).x;
            double dy2 = ahead.get(i).y - ahead.get(i - 1).y;

            double angle1 = Math.atan2(dy1, dx1);
            double angle2 = Math.atan2(dy2, dx2);
            totalAngleChange += Math.abs(angle2 - angle1);
        }

        // 曲率越大,速度越低
        double curvature = totalAngleChange / ahead.size();
        double speedFactor = Math.max(CORNERING_SPEED_FACTOR, 1.0 - curvature * 3.0);

        return RaceCar.MAX_SPEED * speedFactor;
    }

    /**
     * 紧急避障检测
     * 检测前方是否有障碍物并计算避让转向
     */
    public double avoidObstacles(RaceCar car, List<Obstacle> obstacles) {
        double avoidSteer = 0;

        for (Obstacle obs : obstacles) {
            double dx = obs.x - car.getX();
            double dy = obs.y - car.getY();
            double dist = Math.sqrt(dx * dx + dy * dy);

            if (dist < 20.0 && dist > 0.1) {
                // 计算障碍物相对于车辆的角度
                double obsAngle = Math.atan2(dy, dx);
                double angleDiff = obsAngle - car.getHeading();

                // 归一化
                while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
                while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;

                // 如果障碍物在前方,计算避让方向
                if (Math.abs(angleDiff) < Math.PI / 3) {
                    // 障碍物在车辆右侧则向左避让,反之向右
                    double avoidDirection = (angleDiff > 0) ? -1.0 : 1.0;
                    double urgency = (20.0 - dist) / 20.0;  // 距离越近越紧急
                    avoidSteer += avoidDirection * urgency;
                }
            }
        }

        return Math.max(-1.0, Math.min(1.0, avoidSteer));
    }

    /**
     * 障碍物类
     */
    public static class Obstacle {
        public final double x;
        public final double y;
        public final double radius;

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

完整模拟系统

/**
 * 赛车模拟器主程序
 */
public class RacingSimulator {

    public static void main(String[] args) {
        // 创建赛道(椭圆形测试赛道)
        RaceTrack track = new RaceTrack(12.0);

        // 生成椭圆形路径点
        int numPoints = 60;
        double centerX = 200, centerY = 200;
        double radiusX = 150, radiusY = 100;

        for (int i = 0; i < numPoints; i++) {
            double angle = 2 * Math.PI * i / numPoints;
            double x = centerX + radiusX * Math.cos(angle);
            double y = centerY + radiusY * Math.sin(angle);
            track.addWaypoint(x, y);
        }

        // 创建赛车(起点)
        RaceCar car = new RaceCar(
            centerX + radiusX, centerY, 
            Math.PI / 2  // 朝上
        );

        // 创建AI控制器
        GreedyRacingAI ai = new GreedyRacingAI();

        // 模拟参数
        double dt = 0.05;  // 50ms时间步
        int totalSteps = 5000;
        int printInterval = 50;

        System.out.println("===== 赛车物理模拟 =====");
        System.out.println("赛道: 椭圆形,长轴=" + (radiusX * 2) + "m,短轴=" + (radiusY * 2) + "m");
        System.out.println("时间步长: " + (dt * 1000) + "ms\n");

        double totalTime = 0;
        int currentWaypoint = 0;
        int laps = 0;
        int lastWaypoint = -1;

        for (int step = 0; step < totalSteps; step++) {
            // 1. AI计算控制输入
            double[] control = ai.computeControl(car, track);
            double throttle = control[0];
            double steer = control[1];

            // 2. 应用控制
            car.setInput(throttle, steer);

            // 3. 物理更新
            car.update(dt);
            totalTime += dt;

            // 4. 检测路径点通过(用于圈数统计)
            int wp = track.findClosestWaypoint(car.getX(), car.getY());
            if (wp < lastWaypoint && lastWaypoint > numPoints - 5) {
                laps++;
                System.out.println("\n🏁 完成第 " + laps + " 圈!用时: " + String.format("%.1f", totalTime) + "s");
            }
            lastWaypoint = wp;

            // 5. 输出状态
            if (step % printInterval == 0) {
                boolean onTrack = track.isOnTrack(car.getX(), car.getY(), wp);
                String status = onTrack ? "赛道内" : "⚠ 偏离";
                System.out.printf("t=%5.1fs | %s | throttle=%4.1f | steer=%5.2f | %s%n",
                    totalTime, car, throttle, steer, status);
            }
        }

        System.out.println("\n模拟结束,总时间: " + String.format("%.1f", totalTime) + "s");
        System.out.println("完成圈数: " + laps);
    }
}

复杂度分析

操作 时间复杂度 空间复杂度 说明
物理更新 O(1) O(1) 每帧常量计算
最近路径点 O(n) O(1) n为路径点数量
前瞻计算 O(k) O(k) k为前瞻点数
目标速度 O(k) O(1) 基于前方曲率
避障检测 O(m) O(1) m为障碍物数量

扩展方向

  1. 龙格-库塔积分:使用RK4替代欧拉积分,提高物理精度
  2. A*全局路径规划:结合赛道边界,规划最优行驶线(racing line)
  3. 遗传算法调参:自动优化AI的lookahead距离、速度系数等参数
  4. 轮胎模型:引入温度磨损影响抓地力的复杂物理

总结

本文从牛顿运动学出发,构建了简化的赛车物理模型,通过贪心策略实现实时路径跟踪和速度控制。虽然真实赛车模拟涉及更复杂的车辆动力学,但本文提供的框架展示了如何将物理公式转化为可运行的Java代码,为游戏开发和物理仿真提供了算法基础。