一种经纬度轨迹数据计算物理弯道的方法，轨迹弯道算法

前言

        本文使用了纯数学计算的方法计算识别了轨迹中的弯道位置，整体思路较为简单，不过最终效果十分可观，本算法已经在百万级用户的平台稳定运行，在实现过程中对GPS漂移、连续弯道、回旋弯道进行了处理。不过弯道数据仅供页面展示，不能用于准确判断。

实现思路

• 轨迹预处理
• 简化轨迹点（抽稀）
• 获取弯道
• 计算弯道特征数据
• 弯道过滤与合并

 轨迹预处理

        读取原始轨迹文件并格式化为对象，使用简单的速度判断排除异常GPS点。我们业务中是从高德平台拿到的定位数据，其中包含经纬度、速度、朝向、海拔、时间戳维度，客户端定时获取数据后写入文件中再上传到后台。后台格式化解析各项数据，排除速度小于5km/h的定位点，然后链式计算定位点之间的经纬度速度，排除速度大于360km/h的定位点。上述步骤是为了排除GPS飘逸点，保证最终数据的准确性。关键代码如下：

//读取本地文件
String posPath = "E:\\posFiles\\1666768903631.pos";
String content = Util.copyToString(posPath);
//得到原始轨迹点
List<Position> formatList = posDataFormat(content);
assert formatList != null;

List<Position> list = new ArrayList<>();
Position last = formatList.get(0);
for (Position position:formatList) {
    //速度有效
    if((position.speed != null && position.speed>5) || position==formatList.get(formatList.size()-1)){
        // 链式计算当前点
        // 计算理论速度m/s   100 米/秒=360 千米/时
        // 避免GPS点异常漂移导致里程计算结果过大
        double mile = Util.getDistance(position,last);
        double time = (position.time - last.time)/1000.0;
        if(time>0.0 && mile/time < 100){
            position.countMile = last.countMile + mile;
            position.countTime = last.countTime + (position.time - last.time);
            position.maxSpeed = Math.max(position.speed==null?0:position.speed, last.maxSpeed);
            last = position;
            list.add(position);
        }
    }
}
//转换为数组方便后续简化点计算
Position[] array = new Position[list.size()];
for (int i = 0; i < list.size(); i++) {
    array[i] = list.get(i);
}

 简化轨迹点（抽稀）

         原始轨迹中的数据点是高频且无用的，我们需要简化原有轨迹数据保留关键拐点数据，这样可以节省我们的计算时间，而且对后面的弯道计算很有帮助。简化点有一个很经典的算法，该算法以递归方式划分线。最初，它给出了第一个点和最后一个点之间的所有点。它会自动标记要保留的第一个和最后一个点。然后找到以第一和最后一个点为终点的距线段最远的点。该点显然在曲线上离端点之间的近似线段最远。如果该点比ε靠近线段，则可以丢弃当前未标记为要保留的任何点，而简化曲线的质量不会比ε差。 如果距离线段最远的点大于近似值的ε，则必须保留该点。该算法以第一个点和最远的点递归调用自身，然后以最远的点和最后一个点进行递归调用，最后一个点包括被标记为保留的最远点。 递归完成后，可以生成一条新的输出曲线，该曲线包括所有标记点和仅那些标记为保留的点。文字描述总是不那么直观，直接看图就能很清楚的知道原理。

附上该算法的博文地址：Ramer–Douglas–Peucker_algorithm

获取弯道

        基本思路就是通过三点计算夹角，上文已经将密集的点进行抽稀保留了关键拐点数据，我们可以使用滑动窗口的思维获取三个点计算之间的夹角，如果角度满足弯道的定义就创建弯道对象。计算夹角使用夹角余弦公式【cosA=(AB*AC)/(|AB|*|AC|)】，java实现如下：

/**
 * 查找弯道中心点
 */
private static List<CurveData> findAngle(Position[] simplifyList){
    List<CurveData> list = new ArrayList<>();
    for (int i = 0; i < simplifyList.length-2; i++) {
        //滑动窗口：每次取三个点计算夹角
        int angle = getAngle(simplifyList[i], simplifyList[i+1], simplifyList[i+2]);
        //角度满足条件则认为是弯道
        if(angle>PositionConst.CURVE_ANGLE_MIN && angle<PositionConst.CURVE_ANGLE_MAX){
            CurveData curveData = new CurveData();
            curveData.angle = angle;
            curveData.center = simplifyList[i+1];
            list.add(curveData);
        }
    }
    return list;
}

/**
 * 三点获取夹角
 */
private static int getAngle(Position a, Position b, Position c){
    return getAngle(a, b, b, c);
}

/**
 * 获取两向量的夹角
 */
private static int getAngle(Position a, Position a1, Position b, Position b1){
    return Math.round(
            (float) (
                    Math.acos(
                            ((b1.lat-b.lat)*(a.lat-a1.lat) + (b1.lng-b.lng)*(a.lng-a1.lng))
                            /
                            (getDistanceMath(a, a1) * getDistanceMath(b, b1))
                    ) / 0.0174533
            )
    );
}

/**
 * 数学方法计算两点间距离 - 经纬度当成普通数值
 */
private static double getDistanceMath(Position a, Position b){
    return Math.sqrt((a.lat-b.lat)*(a.lat-b.lat) + (a.lng-b.lng)*(a.lng-b.lng));
}

计算弯道特征数据

        我们需要获取弯道的速度、时间、出入弯坐标等信息用于后文的弯道合并，这些数据在业务端上传的轨迹文件中就已经存在，我们只需要做一下简单的遍历获取即可，代码如下：

/**
 * 计算弯道数据
 * 获得速度、时间、出入弯坐标等信息
 */
private static List<CurveData> getCurveData(List<CurveData> curveList, List<Position> list){
    int listSize = list.size();
    int curveListSize = curveList.size();
    //结果列表
    List<CurveData> curveDataList = new ArrayList<>();

    if(listSize <= 0 || curveListSize <= 0){
        return curveDataList;
    }

    Position before, after;
    for (CurveData curveData : curveList) {
        Position curr = curveData.center;
        //查找弯心点在原始数据中的位置
        int index = list.indexOf(curr);
        //排除开始和结尾的数据点
        if (index > 10 && index < listSize - 10) {
            //向前递推5秒
            int indexTemp = index - 1;
            before = list.get(indexTemp);
            double maxSpeed = before.speed;
            while (indexTemp > 0 && curr.time - before.time < 5000) {
                indexTemp = indexTemp - 1;
                before = list.get(indexTemp);
                maxSpeed = (maxSpeed < before.speed) ? before.speed : maxSpeed;
            }

            //向后递推5秒
            indexTemp = index + 1;
            after = list.get(indexTemp);
            maxSpeed = (maxSpeed < after.speed) ? after.speed : maxSpeed;
            while (indexTemp < listSize - 1 && after.time - curr.time < 5000) {
                indexTemp = indexTemp + 1;
                after = list.get(indexTemp);
                maxSpeed = (maxSpeed < after.speed) ? after.speed : maxSpeed;
            }

            //数据更新
            curveData.before = before;
            curveData.center = curr;
            curveData.after = after;
            curveData.maxSpeed = maxSpeed;
            curveData.countMile = after.countMile - before.countMile;
            curveData.countTime = after.countTime - before.countTime;
            curveData.speed = (curveData.countMile / (curveData.countTime / 1000.0)) * 3.6;
            curveDataList.add(curveData);
        }
    }
    return curveDataList;
}

弯道过滤与合并

        上文我们得到了基础的弯道数据，但是现实场景中有很多异常情况，比如一个大弯道会被分成几个小弯道，比如环形螺旋弯道。这些场景是在业务中必须考虑的，我们使用弯道朝向和弯心点距离判断是否是同一个弯道，从而进行弯道合并。

大弯道对比图：

螺旋弯道对比图： 

         上图可以看到效果是显著的，我们业务对于螺旋弯道的判断是属于一个弯道，所以螺旋弯的角度数据是不准确的，这个具体要看业务需求定义。

        弯道合并的主要是对于弯道方向的定义，上文知道我们是三点求夹角，即ABC求B点位置的角度。我们弯道方向使用AB作为基准直线，判断C点处于AB直线的上方还是下方（朝内还是朝外），从而确定弯道的方向。弯道合并过滤的具体实现代码如下：

/**
 * 过滤弯道数据
 * 整个多个弯道数据到一个弯道
 */
private static List<CurveData> filterCurveData(List<CurveData> curveDataList){
    int curveListSize = curveDataList.size();
    //同一弯道数据暂存
    List<CurveData> sameCurveData = new ArrayList<>();
    //结果列表
    List<CurveData> filterList = new LinkedList<>();

    if(curveListSize <= 0){
        return filterList;
    }

    //初始化第一个弯道的方向数据
    boolean directionTemp = getDirection(curveDataList.get(0));
    sameCurveData.add(curveDataList.get(0));
    for (int i = 1; i < curveListSize; i++) {
        //获取弯道方向
        boolean direction = getDirection(curveDataList.get(i));
        //获取与上一个弯心点的距离
        double distance = Util.getDistance(curveDataList.get(i).center, curveDataList.get(i-1).center);
        //判断是否不是同一弯道：即方向不一致 且 间隔距离不满足条件
        if(directionTemp != direction || distance > PositionConst.CURVE_DISTANCE_MAX){
            //不是同一弯道则保存暂存列表中累积的弯道数据
            CurveData curveData = createBySameCurveData(sameCurveData);
            if(curveData != null){
                filterList.add(curveData);
            }
            //清空弯道缓存
            sameCurveData.clear();
        }
        //更新暂存数据
        directionTemp = direction;
        sameCurveData.add(curveDataList.get(i));
    }
    //避免最后一个弯道丢失
    if(sameCurveData.size() > 0){
        CurveData curveData = createBySameCurveData(sameCurveData);
        if(curveData != null){
            filterList.add(curveData);
        }
    }
    return filterList;
}

/**
 * 将相同弯道数据合并
 * @param sameCurveData 弯道列表
 * @return 弯道
 */
private static CurveData createBySameCurveData(List<CurveData> sameCurveData){
    CurveData curveData = new CurveData();
    curveData.maxSpeed = 0;
    curveData.speed = 0;
    //计算暂存列表中统计数据
    sameCurveData.forEach(it->{
        curveData.maxSpeed = Math.max(curveData.maxSpeed, it.maxSpeed);
        curveData.speed += it.speed;
    });
    curveData.speed = curveData.speed / sameCurveData.size();
    //第一个弯的起始点
    curveData.before = sameCurveData.get(0).before;
    //中间弯的中间点
    curveData.center = sameCurveData.get(sameCurveData.size()/2).center;
    //最后一个弯的结尾点
    curveData.after = sameCurveData.get(sameCurveData.size()-1).after;
    //默认第一个弯的角度
    curveData.angle = sameCurveData.get(0).angle;
    if(sameCurveData.size() > 1){
        //只有两个弯道时，取弯心中间点，避免弯心偏向某一边
        if(sameCurveData.size() == 2){
            Position position = pointsCenter(sameCurveData.get(0).center, sameCurveData.get(1).center);
            curveData.center.lat = position.lat;
            curveData.center.lng = position.lng;
        }
        //有多个弯时重新计算角度
        curveData.angle = getAngle(sameCurveData.get(0).before, sameCurveData.get(0).center,
                sameCurveData.get(sameCurveData.size()-1).center, sameCurveData.get(sameCurveData.size()-1).after);
        return curveData;
    }else if(curveData.angle < PositionConst.CURVE_ANGLE_MAX){
        //只有一个弯时如果角度满足才加入结果集
        return curveData;
    }
    return null;
}

private static boolean getDirection(CurveData curveData){
    return getDirection(curveData.before, curveData.center, curveData.after);
}

/**
 * 获取弯道弯心朝向，同一个弯道朝向应该相同 - 正负
 */
private static boolean getDirection(Position a, Position b, Position c){
    //根据直线方程计算x对应的y
    //lat对应x lng对应y
    double y = (c.lat - a.lat)/(b.lat - a.lat)*(b.lng - a.lng)+a.lng;
    //以x轴判断方向，判断y处于直线的上方还是下方
    if(b.lat > a.lat){
        return c.lng > y;
    }else{
        return c.lng <= y;
    }
}

总结

        经过上文的算法，我们可以通过GPS坐标信息计算地理平面上的弯道数据，对于弯道的定义是三点的夹角值满足范围即可。整体思路简单，实现难度低，不依赖其它库，对一些基本的异常进行了处理判断。需要注意的是其中弯道夹角计算时未进行大地坐标格式化，存在一定误差，对于简化参数的选择也需要根据实际情况调整。

整体预览图：

算法完整实现代码： aloneCurve