algorithm - 是否有已知的车辆路线覆盖算法?
问题描述
我有车辆路线作为输入:Lon1,Lat1;经度2,纬度2;经度3,纬度3;等等,我有白天的车辆运动坐标。
我需要找出车辆经过的路线的哪一部分。在我开始实现自己的算法之前,是否有一些现成的算法?
这是任务的插图:
解决方案
我不知道有任何明确的算法可以直接应用于这个问题。如果我必须自己编程:
很明显,问题在于计算预期路线的交点和实际车辆轨迹段的并集(我引入了并集以去除可能的重复段)。这可以通过以下方式完成:
首先计算车辆轨迹的并集,然后从预期路线中减去
每次有新的车辆位置到达时,只需从剩余的预期路线中减去每个车辆轨迹段并计算减去的距离。
我想我会选择第二种选择,乍一看似乎更有效。假设,挑战主要减少计算当前剩余路线和每个到达轨迹段之间的差异。该差异的计算方法将根据采样时间、所需精度等而有所不同。
一个非常幼稚的代码来说明这个想法(在 C# 中)
class RouteChecker
{
List<RouteSegment> MissingRoute = null;
double TotalCoveredDistance = 0.0;
Point LastVehiclePoint = default(Point);
public RouteChecker(List<Point> expectedRoute)
{
MissingRoute = new List<RouteSegment>(expectedRoute.Count - 1);
Point previousPoint = expectedRoute[0];
for (int i = 1; i < expectedRoute.Count; i++)
{
MissingRoute.Add(new RouteSegment(previousPoint, expectedRoute[i]));
previousPoint = expectedRoute[i];
}
}
public void SetStartPoint(Point startPoint)
{
TotalCoveredDistance = 0.0;
LastVehiclePoint = startPoint;
}
public double CheckRouteCovered(Point realRouteNextPoint)
{
RouteSegment vehSegment = new RouteSegment(LastVehiclePoint, realRouteNextPoint);
LastVehiclePoint = realRouteNextPoint;
for (int i = 0; i < MissingRoute.Count; i++)
{
if (!MissingRoute[i].isActive)
continue;
RouteSegment[] remainingSegments;
double removedDistance;
bool otherSegmentisFullyContained;
if (MissingRoute[i].Difference(vehSegment, out remainingSegments, out removedDistance, out otherSegmentisFullyContained))
{
if (remainingSegments != null)
{
MissingRoute[i] = remainingSegments[0];
if (remainingSegments.Length > 1)
MissingRoute.Add(remainingSegments[1]);
}
else
MissingRoute[i].isActive = false;
TotalCoveredDistance += removedDistance;
if(otherSegmentisFullyContained)
break;
}
}
return TotalCoveredDistance;
}
}
所以相应的对象将被创建为
List<Point> expectedRoute = new List<Point>() { new Point(Lon1, Lat1),
new Point(Lon2, Lat2),
new Point(Lon3, Lat3),
new Point(Lon4, Lat4),
new Point(Lon5, Lat5)
//[...]
};
RouteChecker myChecker = new RouteChecker(expectedRoute);
myChecker.SetStartPoint(new Point(vehicleStartLon, vehicleStartLat));
然后,每 5 秒
double CoveredDistance = myChecker.CheckRouteCovered(new Point(vehicleNextLon, vehicleNextLat));
RouteSegment 类只是一个管理所有带有点和线段的几何操作的类。
private class RouteSegment
{
const double ParallelMinAngleRadians = Math.PI / 180;
const double MaxOnRouteDist = 10;
Point P1, P2;
public bool isActive = true;
public RouteSegment(Point p1, Point p2)
{
P1 = p1;
P2 = p2;
}
public bool Difference(RouteSegment otherSegment, out RouteSegment[] RemainingRouteSegments, out double removedDistance,out bool otherSegmentisFullyContained)
{
otherSegmentisFullyContained = false;
if(!IsParallelTo(otherSegment))
{
removedDistance = 0.0;
RemainingRouteSegments = null;
return false;
}
if (Contains(otherSegment.P1))
{
if (Contains(otherSegment.P2))
{
removedDistance = Distance(otherSegment.P1, otherSegment.P2);
if (Distance(otherSegment.P2, P1) < Distance(otherSegment.P2, P2))
RemainingRouteSegments = new RouteSegment[] { new RouteSegment(P1, otherSegment.P2), new RouteSegment(otherSegment.P1, P2) };
else
RemainingRouteSegments = new RouteSegment[] { new RouteSegment(P1, otherSegment.P1), new RouteSegment(otherSegment.P2, P2) };
otherSegmentisFullyContained = true;
return true;
}
else
{
if(Distance(otherSegment.P2,P1)< Distance(otherSegment.P2, P2))
{
RemainingRouteSegments = new RouteSegment[] { new RouteSegment(otherSegment.P1, P2) };
removedDistance = Distance(otherSegment.P1, P1);
return true;
}
else
{
RemainingRouteSegments = new RouteSegment[] { new RouteSegment(P1,otherSegment.P1) };
removedDistance = Distance(otherSegment.P1, P2);
return true;
}
}
}
else if (Contains(otherSegment.P2))
{
if (Distance(otherSegment.P1, P1) < Distance(otherSegment.P1, P2))
{
RemainingRouteSegments = new RouteSegment[] { new RouteSegment(otherSegment.P2, P2) };
removedDistance = Distance(otherSegment.P2, P1);
return true;
}
else
{
RemainingRouteSegments = new RouteSegment[] { new RouteSegment(P1, otherSegment.P2) };
removedDistance = Distance(otherSegment.P2, P2);
return true;
}
}
else if (otherSegment.Contains(P1))
{
RemainingRouteSegments = null;
removedDistance = Distance(P1, P2);
return true;
}
else
{
removedDistance = 0.0;
RemainingRouteSegments = null;
return false;
}
}
bool Contains(Point p)
{
double A = P1.Y - P2.Y;
double B = P2.X - P1.X;
double dist = Math.Abs(A * (p.X - P1.X) + B * (p.Y - P1.Y)) / Distance(P1, P2);
return dist < MaxOnRouteDist;
}
bool IsParallelTo(RouteSegment otherSegment)
{
double v1X = P2.X - P1.X;
double v1Y = P2.Y - P1.Y;
double v2X = otherSegment.P2.X - otherSegment.P1.X;
double v2Y = otherSegment.P2.Y - otherSegment.P1.Y;
return Math.Abs(v1X * v2X + v1Y * v2Y) / Distance(P1, P2) / Distance(otherSegment.P1, otherSegment.P2) < Math.Cos(ParallelMinAngleRadians);
}
static double Distance(Point p, Point q)
{
double dX = p.X - q.X;
double dY = p.Y - q.Y;
return Math.Sqrt(dX * dX + dY * dY);
}
}
抱歉这么多代码。我发现编写它比解释所有情况更容易。