前言
本文使用了纯数学计算的方法计算识别了轨迹中的弯道位置，整体思路较为简单，不过最终效果十分可观，本算法已经在百万级用户的平台稳定运行，在实现过程中对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
