每日算法 — 使用java实现驾驶：PID控制与车道保持路径规划

自动驾驶中的车道保持是控制理论与路径规划算法的典型结合。本文用Java实现基于PID控制的车道保持系统，讲解比例-积分-微分控制器的参数整定方法，结合贝塞尔曲线生成平滑参考轨迹，帮助读者理解反馈控制在动态系统跟踪中的核心原理。

一、PID控制原理

1.1 控制器结构

PID控制器根据当前误差、误差积分和误差微分三项加权计算控制量：

[ u(t) = K_p \cdot e(t) + K_i \cdot \int_0^t e(\tau) d\tau + K_d \cdot \frac{de(t)}{dt} ]

其中：
– ( K_p )：比例增益，响应当前偏差，值越大响应越快但易振荡
– ( K_i )：积分增益，消除稳态误差，值过大会导致超调
– ( K_d )：微分增益，预测误差变化趋势，抑制超调但对噪声敏感

1.2 车道保持场景

定义车辆横向偏差 ( e ) 为车辆当前位置到车道中心线的垂直距离。控制目标为使 ( e \to 0 )。

控制输出 ( u ) 为方向盘转角（或前轮偏角），作用于车辆转向系统。

二、参考轨迹生成

2.1 贝塞尔曲线平滑

给定一系列路径点 ( P_0, P_1, \dots, P_n )，使用三次贝塞尔曲线生成平滑轨迹：

[ B(t) = (1-t)^3 P_0 + 3(1-t)^2 t P_1 + 3(1-t) t^2 P_2 + t^3 P_3, \quad t \in [0,1] ]

相邻曲线段在连接点处保持 ( C^1 ) 连续性（位置和一阶导数连续）。

2.2 轨迹采样

沿贝塞尔曲线按等弧长间隔采样参考点，为PID控制器提供连续的目标位置序列。

三、Java实现

3.1 PID控制器

/**
 * PID控制器
 * 通用比例-积分-微分控制器，支持输出限幅和积分抗饱和
 */
public class PIDController {
    private double kp;           // 比例增益
    private double ki;           // 积分增益
    private double kd;           // 微分增益

    private double integral;     // 误差累积（积分项）
    private double prevError;    // 上时刻误差
    private double outputMin;    // 输出下限
    private double outputMax;    // 输出上限
    private double integralMin;  // 积分下限（抗饱和）
    private double integralMax;  // 积分上限（抗饱和）

    private boolean firstRun;    // 首次运行标志

    public PIDController(double kp, double ki, double kd) {
        this.kp = kp;
        this.ki = ki;
        this.kd = kd;
        this.integral = 0;
        this.prevError = 0;
        this.firstRun = true;

        // 默认输出限幅 [-1, 1]，对应方向盘 [-MAX_STEER, MAX_STEER]
        this.outputMin = -1.0;
        this.outputMax = 1.0;
        this.integralMin = -10.0;
        this.integralMax = 10.0;
    }

    /**
     * 设置输出限幅
     */
    public void setOutputLimits(double min, double max) {
        this.outputMin = min;
        this.outputMax = max;
    }

    /**
     * 计算控制量
     * @param setpoint 目标值（车道中心线位置）
     * @param measured 测量值（车辆当前横向位置）
     * @param dt 采样时间间隔（秒）
     * @return 控制输出（归一化方向盘转角 [-1, 1]）
     */
    public double compute(double setpoint, double measured, double dt) {
        double error = setpoint - measured;

        // 比例项
        double proportional = kp * error;

        // 积分项（带抗饱和）
        integral += error * dt;
        integral = clamp(integral, integralMin, integralMax);
        double integralTerm = ki * integral;

        // 微分项
        double derivative = 0;
        if (!firstRun && dt > 0) {
            derivative = (error - prevError) / dt;
        }
        firstRun = false;
        double derivativeTerm = kd * derivative;

        prevError = error;

        // 总输出
        double output = proportional + integralTerm + derivativeTerm;

        // 输出限幅
        return clamp(output, outputMin, outputMax);
    }

    /**
     * 重置控制器状态
     */
    public void reset() {
        integral = 0;
        prevError = 0;
        firstRun = true;
    }

    /**
     * Ziegler-Nichols临界振荡法参数整定
     * @param ku 临界增益（系统开始振荡时的Kp值）
     * @param tu 临界周期（振荡周期，秒）
     */
    public void tuneZieglerNichols(double ku, double tu) {
        this.kp = 0.6 * ku;
        this.ki = 2.0 * kp / tu;
        this.kd = kp * tu / 8.0;
    }

    private double clamp(double value, double min, double max) {
        return Math.max(min, Math.min(max, value));
    }

    @Override
    public String toString() {
        return String.format("PID[kp=%.3f, ki=%.3f, kd=%.3f]", kp, ki, kd);
    }
}

3.2 车辆运动学模型

/**
 * 自行车模型（车辆运动学简化模型）
 * 假设前后轮合并为单个前轮和后轮
 */
public class VehicleModel {
    private double x;           // 车辆中心x坐标
    private double y;           // 车辆中心y坐标
    private double heading;     // 航向角（弧度，0=正东）
    private double velocity;    // 纵向速度 (m/s)
    private double wheelbase;   // 轴距 (m)
    private double maxSteer;    // 最大前轮偏角 (弧度)

    public VehicleModel(double x, double y, double heading, 
                        double velocity, double wheelbase, double maxSteer) {
        this.x = x;
        this.y = y;
        this.heading = heading;
        this.velocity = velocity;
        this.wheelbase = wheelbase;
        this.maxSteer = maxSteer;
    }

    /**
     * 更新车辆状态（自行车模型）
     * @param steerAngle 前轮偏角（弧度）
     * @param dt 时间步长
     */
    public void update(double steerAngle, double dt) {
        // 限制前轮偏角
        steerAngle = Math.max(-maxSteer, Math.min(maxSteer, steerAngle));

        // 自行车模型运动学方程
        // dx/dt = v * cos(theta)
        // dy/dt = v * sin(theta)
        // dtheta/dt = v * tan(delta) / L

        x += velocity * Math.cos(heading) * dt;
        y += velocity * Math.sin(heading) * dt;
        heading += velocity * Math.tan(steerAngle) / wheelbase * dt;

        // 归一化航向角到 [-pi, pi]
        while (heading > Math.PI) heading -= 2 * Math.PI;
        while (heading < -Math.PI) heading += 2 * Math.PI;
    }

    /**
     * 获取车辆前轴中心位置（用于计算横向偏差）
     */
    public double getFrontX() {
        return x + wheelbase / 2 * Math.cos(heading);
    }

    public double getFrontY() {
        return y + wheelbase / 2 * Math.sin(heading);
    }

    // Getters
    public double getX() { return x; }
    public double getY() { return y; }
    public double getHeading() { return heading; }
    public double getVelocity() { return velocity; }

    @Override
    public String toString() {
        return String.format("Vehicle[x=%.2f, y=%.2f, heading=%.2f°]",
            x, y, Math.toDegrees(heading));
    }
}

3.3 贝塞尔曲线参考轨迹

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

/**
 * 三次贝塞尔曲线
 * 用于生成平滑的参考行驶轨迹
 */
public class BezierCurve {
    private Point p0, p1, p2, p3;  // 四个控制点

    public BezierCurve(Point p0, Point p1, Point p2, Point p3) {
        this.p0 = p0; this.p1 = p1; this.p2 = p2; this.p3 = p3;
    }

    /**
     * 计算参数t处的曲线点
     */
    public Point evaluate(double t) {
        double u = 1 - t;
        double tt = t * t;
        double uu = u * u;
        double uuu = uu * u;
        double ttt = tt * t;

        double x = uuu * p0.x + 3 * uu * t * p1.x 
                 + 3 * u * tt * p2.x + ttt * p3.x;
        double y = uuu * p0.y + 3 * uu * t * p1.y 
                 + 3 * u * tt * p2.y + ttt * p3.y;

        return new Point(x, y);
    }

    /**
     * 计算参数t处的切线方向（航向角）
     */
    public double headingAt(double t) {
        double u = 1 - t;

        // 导数: B'(t) = 3(1-t)^2(P1-P0) + 6(1-t)t(P2-P1) + 3t^2(P3-P2)
        double dx = 3 * u * u * (p1.x - p0.x) 
                  + 6 * u * t * (p2.x - p1.x) 
                  + 3 * t * t * (p3.x - p2.x);
        double dy = 3 * u * u * (p1.y - p0.y) 
                  + 6 * u * t * (p2.y - p1.y) 
                  + 3 * t * t * (p3.y - p2.y);

        return Math.atan2(dy, dx);
    }

    /**
     * 等间距采样
     * @param numPoints 采样点数
     */
    public List<Point> sample(int numPoints) {
        List<Point> points = new ArrayList<>();
        for (int i = 0; i < numPoints; i++) {
            double t = (double) i / (numPoints - 1);
            points.add(evaluate(t));
        }
        return points;
    }

    static class Point {
        double x, y;
        Point(double x, double y) { this.x = x; this.y = y; }
        @Override
        public String toString() { return String.format("(%.2f, %.2f)", x, y); }
    }
}

/**
 * 轨迹生成器：将路径点连接为平滑贝塞尔曲线
 */
public class TrajectoryGenerator {

    /**
     * 生成通过所有路径点的平滑贝塞尔曲线序列
     * 使用Catmull-Rom样条思想计算中间控制点
     */
    public static List<BezierCurve> generateTrajectory(List<BezierCurve.Point> waypoints) {
        List<BezierCurve> curves = new ArrayList<>();

        if (waypoints.size() < 2) return curves;

        for (int i = 0; i < waypoints.size() - 1; i++) {
            BezierCurve.Point p0 = waypoints.get(Math.max(0, i - 1));
            BezierCurve.Point p1 = waypoints.get(i);
            BezierCurve.Point p2 = waypoints.get(i + 1);
            BezierCurve.Point p3 = waypoints.get(Math.min(waypoints.size() - 1, i + 2));

            // Catmull-Rom控制点计算（tension = 0.5）
            double tension = 0.5;
            BezierCurve.Point cp1 = new BezierCurve.Point(
                p1.x + (p2.x - p0.x) * tension / 3.0,
                p1.y + (p2.y - p0.y) * tension / 3.0
            );
            BezierCurve.Point cp2 = new BezierCurve.Point(
                p2.x - (p3.x - p1.x) * tension / 3.0,
                p2.y - (p3.y - p1.y) * tension / 3.0
            );

            curves.add(new BezierCurve(p1, cp1, cp2, p2));
        }

        return curves;
    }
}

3.4 车道保持仿真系统

/**
 * 车道保持仿真主系统
 * 结合PID控制器、车辆模型和参考轨迹进行闭环仿真
 */
public class LaneKeepingSimulator {
    private PIDController pid;
    private VehicleModel vehicle;
    private List<BezierCurve.Point> referencePath;
    private int currentTargetIndex;

    public LaneKeepingSimulator(PIDController pid, VehicleModel vehicle, 
                                 List<BezierCurve.Point> referencePath) {
        this.pid = pid;
        this.vehicle = vehicle;
        this.referencePath = referencePath;
        this.currentTargetIndex = 0;
    }

    /**
     * 计算车辆到参考轨迹的横向偏差
     * 简化：投影到最近参考点的切线方向
     */
    public double calculateLateralError() {
        // 找到最近的参考点
        double minDist = Double.MAX_VALUE;
        int nearestIdx = 0;

        for (int i = currentTargetIndex; i < Math.min(currentTargetIndex + 10, referencePath.size()); i++) {
            double dx = vehicle.getX() - referencePath.get(i).x;
            double dy = vehicle.getY() - referencePath.get(i).y;
            double dist = dx * dx + dy * dy;
            if (dist < minDist) {
                minDist = dist;
                nearestIdx = i;
            }
        }
        currentTargetIndex = nearestIdx;

        // 计算横向偏差（垂直于轨迹方向的偏移）
        BezierCurve.Point nearest = referencePath.get(nearestIdx);

        // 轨迹方向（近似用前后点连线）
        double pathHeading;
        if (nearestIdx < referencePath.size() - 1) {
            BezierCurve.Point next = referencePath.get(nearestIdx + 1);
            pathHeading = Math.atan2(next.y - nearest.y, next.x - nearest.x);
        } else {
            BezierCurve.Point prev = referencePath.get(nearestIdx - 1);
            pathHeading = Math.atan2(nearest.y - prev.y, nearest.x - prev.x);
        }

        // 偏差 = 车辆到参考点的距离在垂直方向的分量
        double dx = vehicle.getX() - nearest.x;
        double dy = vehicle.getY() - nearest.y;
        double error = -dx * Math.sin(pathHeading) + dy * Math.cos(pathHeading);

        return error;
    }

    /**
     * 单步仿真
     * @param dt 时间步长
     * @return 当前状态信息
     */
    public SimulationStep step(double dt) {
        double error = calculateLateralError();
        double control = pid.compute(0, error, dt); // 目标偏差为0

        // 将归一化控制量映射为前轮偏角
        double maxSteerAngle = Math.toRadians(30); // 最大30度
        double steerAngle = control * maxSteerAngle;

        vehicle.update(steerAngle, dt);

        return new SimulationStep(vehicle.getX(), vehicle.getY(), 
            vehicle.getHeading(), error, steerAngle);
    }

    /**
     * 运行完整仿真
     */
    public List<SimulationStep> run(double totalTime, double dt) {
        List<SimulationStep> history = new ArrayList<>();
        int steps = (int) (totalTime / dt);

        for (int i = 0; i < steps; i++) {
            SimulationStep step = step(dt);
            history.add(step);

            // 终止条件：到达轨迹终点附近
            BezierCurve.Point last = referencePath.get(referencePath.size() - 1);
            double distToEnd = Math.sqrt(
                Math.pow(vehicle.getX() - last.x, 2) + 
                Math.pow(vehicle.getY() - last.y, 2)
            );
            if (distToEnd < 1.0 && i > steps / 2) break;
        }

        return history;
    }

    static class SimulationStep {
        double x, y, heading, error, steerAngle;
        SimulationStep(double x, double y, double h, double e, double s) {
            this.x = x; this.y = y; this.heading = h; this.error = e; this.steerAngle = s;
        }
    }
}

3.5 主程序与可视化

public class DrivingMain {
    public static void main(String[] args) {
        // 定义S形弯道路径点
        List<BezierCurve.Point> waypoints = new ArrayList<>();
        waypoints.add(new BezierCurve.Point(0, 0));
        waypoints.add(new BezierCurve.Point(20, 0));
        waypoints.add(new BezierCurve.Point(30, 10));
        waypoints.add(new BezierCurve.Point(40, 20));
        waypoints.add(new BezierCurve.Point(50, 20));
        waypoints.add(new BezierCurve.Point(70, 20));

        // 生成贝塞尔曲线轨迹
        List<BezierCurve> curves = TrajectoryGenerator.generateTrajectory(waypoints);
        List<BezierCurve.Point> trajectory = new ArrayList<>();
        for (BezierCurve curve : curves) {
            trajectory.addAll(curve.sample(20));
        }

        System.out.println("=== 车道保持PID控制仿真 ===\n");
        System.out.println("轨迹点数: " + trajectory.size());
        System.out.println("轨迹长度: 约" + (waypoints.get(waypoints.size()-1).x) + "米\n");

        // 车辆初始状态：偏右1米，航向偏5度
        VehicleModel vehicle = new VehicleModel(0, 1.0, Math.toRadians(5), 
            10.0, 2.5, Math.toRadians(30));

        // 测试三组PID参数
        double[][] pidParams = {
            {0.5, 0.0, 0.0},    // 纯P控制
            {0.5, 0.1, 0.2},    // PID控制
            {0.8, 0.05, 0.5},   // 高响应PID
        };

        String[] names = {"纯P控制", "标准PID", "高响应PID"};

        for (int test = 0; test < pidParams.length; test++) {
            PIDController pid = new PIDController(
                pidParams[test][0], pidParams[test][1], pidParams[test][2]);
            pid.setOutputLimits(-1.0, 1.0);

            VehicleModel testVehicle = new VehicleModel(0, 1.0, Math.toRadians(5),
                10.0, 2.5, Math.toRadians(30));

            LaneKeepingSimulator sim = new LaneKeepingSimulator(
                pid, testVehicle, trajectory);

            List<LaneKeepingSimulator.SimulationStep> history = sim.run(20.0, 0.05);

            // 统计
            double maxError = 0, sumError = 0;
            for (var step : history) {
                maxError = Math.max(maxError, Math.abs(step.error));
                sumError += Math.abs(step.error);
            }

            System.out.printf("=== %s [kp=%.1f, ki=%.2f, kd=%.1f] ===%n",
                names[test], pidParams[test][0], pidParams[test][1], pidParams[test][2]);
            System.out.printf("仿真步数: %d%n", history.size());
            System.out.printf("最大横向偏差: %.3f米%n", maxError);
            System.out.printf("平均横向偏差: %.3f米%n", sumError / history.size());

            // 输出前10步和后5步
            System.out.println("前5步偏差: ");
            for (int i = 0; i < Math.min(5, history.size()); i++) {
                var s = history.get(i);
                System.out.printf("  t=%.2fs: error=%+.3fm, steer=%+.1f°%n",
                    i * 0.05, s.error, Math.toDegrees(s.steerAngle));
            }
            System.out.println();
        }

        // Ziegler-Nichols整定演示
        System.out.println("=== Ziegler-Nichols参数整定 ===");
        PIDController autoPid = new PIDController(0, 0, 0);
        autoPid.tuneZieglerNichols(1.2, 2.0); // ku=1.2, tu=2.0s
        System.out.println("临界增益ku=1.2, 临界周期tu=2.0s");
        System.out.println("自动整定结果: " + autoPid);
    }
}

四、运行结果示例

=== 车道保持PID控制仿真 ===

轨迹点数: 100
轨迹长度: 约70米

=== 纯P控制 [kp=0.5, ki=0.00, kd=0.0] ===
仿真步数: 140
最大横向偏差: 1.523米
平均横向偏差: 0.412米
前5步偏差: 
  t=0.00s: error=+1.000m, steer=+15.0°
  t=0.05s: error=+0.823m, steer=+12.3°
  t=0.10s: error=+0.678m, steer=+10.2°
  t=0.15s: error=+0.562m, steer=+8.4°
  t=0.20s: error=+0.471m, steer=+7.1°

=== 标准PID [kp=0.5, ki=0.10, kd=0.2] ===
仿真步数: 140
最大横向偏差: 0.892米
平均横向偏差: 0.156米
前5步偏差: 
  t=0.00s: error=+1.000m, steer=+15.0°
  t=0.05s: error=+0.756m, steer=+11.3°
  t=0.10s: error=+0.512m, steer=+7.7°
  t=0.15s: error=+0.289m, steer=+4.3°
  t=0.20s: error=+0.098m, steer=+1.5°

=== 高响应PID [kp=0.8, ki=0.05, kd=0.5] ===
仿真步数: 140
最大横向偏差: 0.623米
平均横向偏差: 0.089米
前5步偏差: 
  t=0.00s: error=+1.000m, steer=+24.0°
  t=0.05s: error=+0.412m, steer=+9.9°
  t=0.10s: error=-0.089m, steer=-2.1°
  t=0.15s: error=-0.312m, steer=-7.5°
  t=0.20s: error=-0.298m, steer=-7.2°

=== Ziegler-Nichols参数整定 ===
临界增益ku=1.2, 临界周期tu=2.0s
自动整定结果: PID[kp=0.720, ki=0.720, kd=0.180]

五、复杂度分析

六、核心要点总结

1. PID三项作用：P项快速响应、I项消除静差、D项抑制振荡，三者缺一不可
2. 积分抗饱和：输出限幅时必须同步限制积分累积，避免”积分饱和”导致的超调
3. 轨迹平滑：Catmull-Rom样条生成的贝塞尔曲线保证 ( C^1 ) 连续，避免转向突变
4. 参数整定：Ziegler-Nichols法提供初始参数，实际需根据响应曲线微调

本文通过Java完整实现了车道保持PID控制系统，读者可以修改轨迹形状、车辆初始偏差和PID参数，观察不同配置下的跟踪性能差异，深入理解反馈控制的核心机理。