GPS 定位點道路貼合修正算法：垂足投影與里程插值實現

問題背景

車載 GPS 上傳座標常因訊號漂移、多徑效應偏離實際道路。若直接落圖，軌跡會呈現「鑽籬笆」狀態。需將原始點在容差範圍內「貼合」到最近道路中心線，並計算對應里程（樁號），供後續軌跡回放、偏離報警使用。

核心算法流程

1. 依道路 ID 讀取該路段所有形狀點（有序節點）。
2. 遍歷相鄰節點構成的線段，計算 GPS 點到線段的最短距離（Line2D.ptSegDist）。
3. 找出距離最小的線段（兩端點 P1、P2）。
4. 計算 GPS 點在該線段上的垂足（垂直投影點）。
5. 判斷垂足是否落在線段內：
   • 是 → 採用垂足座標，並依比例插值計算里程。
   • 否 → 取距離較近的端點座標與里程。

Java 完整實現

import java.awt.geom.Line2D;
import java.util.List;

public class GpsSnapToRoad {

    /** 道路形狀點 POJO */
    static class Point {
        double x, y;      // 經度/緯度或投影坐標
        double mile;      // 累計里程（樁號）
        // getter/setter 略
    }

    /** 入口：根據道路 ID 與 GPS 座標，返回貼合後的點 */
    public static Point snapToRoad(String roadId, double gpsX, double gpsY) {
        List<Point> points = RoadlineDao.getPoints(roadId); // 從 DB/緩存讀取
        if (points == null || points.size() < 2) return null;

        double minDist = Double.MAX_VALUE;
        Point segP1 = null, segP2 = null;

        // 1. 遍歷所有線段，找最近線段
        for (int i = 1; i < points.size(); i++) {
            Point p1 = points.get(i - 1);
            Point p2 = points.get(i);
            double dist = Line2D.ptSegDist(p1.x, p1.y, p2.x, p2.y, gpsX, gpsY);
            if (dist < minDist) {
                minDist = dist;
                segP1 = p1;
                segP2 = p2;
            }
        }
        if (segP1 == null || segP2 == null) return null;

        // 2. 計算垂足並判斷是否在線段內
        return projectPoint(segP1, segP2, gpsX, gpsY);
    }

    /** 垂足投影與里程插值 */
    private static Point projectPoint(Point p1, Point p2, double x, double y) {
        Point foot = new Point();
        double dx = p2.x - p1.x;
        double dy = p2.y - p1.y;

        // 垂足座標計算（向量投影公式）
        if (dx == 0) {                 // 垂直線段
            foot.x = p1.x;
            foot.y = y;
        } else if (dy == 0) {          // 水平線段
            foot.x = x;
            foot.y = p1.y;
        } else {
            double k = dy / dx;        // 斜率
            foot.x = (k * k * p1.x + k * (y - p1.y) + x) / (k * k + 1);
            foot.y = k * (foot.x - p1.x) + p1.y;
        }

        // 3. 判斷垂足是否在線段矩形包絡盒內
        boolean onSegment = (Math.min(p1.x, p2.x) <= foot.x && foot.x <= Math.max(p1.x, p2.x))
                         && (Math.min(p1.y, p2.y) <= foot.y && foot.y <= Math.max(p1.y, p2.y));

        Point result = new Point();
        if (onSegment) {
            result.x = foot.x;
            result.y = foot.y;
            // 里程線性插值
            double dm = p2.mile - p1.mile;
            if (dx != 0) {
                result.mile = (float) (p1.mile + (foot.x - p1.x) / dx * dm);
            } else {
                result.mile = (float) (p1.mile + (foot.y - p1.y) / dy * dm);
            }
        } else {
            // 取較近端點
            double d1 = Math.hypot(p1.x - foot.x, p1.y - foot.y);
            double d2 = Math.hypot(p2.x - foot.x, p2.y - foot.y);
            result = (d1 <= d2) ? p1 : p2;
        }
        return result;
    }
}

關鍵點補充

1. 坐標系統一致性

GPS 經緯度（WGS84）不可直接用於歐氏距離計算。實際專案需先投影至平面坐標系（如 UTM、高斯-克魯格），或使用球面距離公式（Haversine）配合局部平面近似。

2. 性能優化

• 道路網建立空間索引（R-Tree、Grid），避免全表掃描。
• 熱門道路形狀點緩存至 Redis/本地記憶體。
• 批量處理時，預先按 bounding box 過濾候選道路。

3. 容差閾值

若 minDist > THRESHOLD（如 50 米），視為異常點，不強制貼合，保留原始座標並標記低信心度。

總結

「垂足投影 + 線段包絡盒判斷 + 里程線性插值」是 GPS 道路貼合的經典輕量級方案。工程落地時，需關注坐標系統一、空間索引加速、異常點過濾三大工程化課題。對於高精度需求（如自動駕駛），可進一步引入隱馬爾可夫模型（HMM）或 Kalman Filter 進行軌跡平滑與地圖匹配。