From dad7477726f41a13d89f6f7c02998d4d3850f247 Mon Sep 17 00:00:00 2001 From: GNSS-Stylist Date: Tue, 23 Apr 2024 16:25:46 +0300 Subject: [PATCH] Change automatic steering to follow "local" nearest point instead of "global" while following a curve The previous implementation led the vehicle to sometimes pick a wrong part of a curve when the curve was intersecting itself (AB-curve) or there were more than one parallel lines near to each other (typically occurs when driving along an inner curve formed from a boundary curve and the field's shape is not convex). This changes the curve-following algorithm to use a search for "local" nearest point instead of "global" (= taking all the points on the curve into account) when already following the curve. Look ahead-distance is used when searching the nearest "local" point to prevent problems in tight hairpins caused by the search sticking to a local minimum "on the wrong side" of the hairpin (especially when using large look ahead values causing the vehicle to take a shortcut). Fixes #391 --- SourceCode/GPS/Classes/CABCurve.cs | 165 ++++++++++++++++++++++++----- 1 file changed, 139 insertions(+), 26 deletions(-) diff --git a/SourceCode/GPS/Classes/CABCurve.cs b/SourceCode/GPS/Classes/CABCurve.cs index 700730ff3..a87f75dc2 100644 --- a/SourceCode/GPS/Classes/CABCurve.cs +++ b/SourceCode/GPS/Classes/CABCurve.cs @@ -58,6 +58,9 @@ public class CABCurve public double inty; + // Should we find the global nearest curve point (instead of local) on the next search. + private bool findGlobalNearestCurvePoint = false; + public CABCurve(FormGPS _f) { //constructor @@ -402,7 +405,7 @@ public void BuildCurveCurrentList(vec3 pivot) refPoint1 = mf.trk.gArr[idx].ptA; //cross product - isHeadingSameWay = ((mf.pivotAxlePos.easting - refPoint1.easting) * (mf.steerAxlePos.northing - refPoint1.northing) + isHeadingSameWay = ((mf.pivotAxlePos.easting - refPoint1.easting) * (mf.steerAxlePos.northing - refPoint1.northing) - (mf.pivotAxlePos.northing - refPoint1.northing) * (mf.steerAxlePos.easting - refPoint1.easting)) < 0; //how far are we away from the reference line at 90 degrees - 2D cross product and distance @@ -410,8 +413,8 @@ public void BuildCurveCurrentList(vec3 pivot) distanceFromRefLine -= (0.5 * widthMinusOverlap); - double RefDist = (distanceFromRefLine - + (isHeadingSameWay ? mf.tool.offset : -mf.tool.offset) + double RefDist = (distanceFromRefLine + + (isHeadingSameWay ? mf.tool.offset : -mf.tool.offset) + mf.trk.gArr[idx].nudgeDistance) / widthMinusOverlap; if (RefDist < 0) howManyPathsAway = (int)(RefDist - 0.5); @@ -423,7 +426,7 @@ public void BuildCurveCurrentList(vec3 pivot) //build the current line curList?.Clear(); - double distAway = widthMinusOverlap * howManyPathsAway + double distAway = widthMinusOverlap * howManyPathsAway + (isHeadingSameWay ? -mf.tool.offset : mf.tool.offset) - mf.trk.gArr[idx].nudgeDistance; distAway += (0.5 * widthMinusOverlap); @@ -434,7 +437,7 @@ public void BuildCurveCurrentList(vec3 pivot) //distAway += mf.trk.gArr[idx].nudgeDistance; - vec3 currentPos = new vec3(refPoint1.easting-distAway, refPoint1.northing, 0); + vec3 currentPos = new vec3(refPoint1.easting - distAway, refPoint1.northing, 0); while (currentPos.heading < glm.twoPI) { @@ -466,6 +469,7 @@ public void BuildCurveCurrentList(vec3 pivot) } lastSecond = mf.secondsSinceStart; + findGlobalNearestCurvePoint = true; } public void GetCurrentCurveLine(vec3 pivot, vec3 steer) @@ -484,6 +488,10 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer) if (curList.Count > 0) { + // Update based on autosteer settings and distance from line + double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(); + bool ReverseHeading = mf.isReverse ? !isHeadingSameWay : isHeadingSameWay; + if (mf.yt.isYouTurnTriggered && mf.yt.DistanceFromYouTurnLine())//do the pure pursuit from youTurn { //now substitute what it thinks are AB line values with auto turn values @@ -513,14 +521,21 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer) //close call hit int cc = 0, dd; - for (int j = 0; j < curList.Count; j += 10) + if (findGlobalNearestCurvePoint) { - dist = glm.DistanceSquared(pivot, curList[j]); - if (dist < minDistA) - { - minDistA = dist; - cc = j; - } + // When not already following some line, find the globally nearest point + + cc = findNearestGlobalCurvePoint(pivot, 10); + + findGlobalNearestCurvePoint = false; + } + else + { + // When already "locked" to follow some line, try to find the "local" nearest point + // based on the last one. This prevents jumping between lines close to each other (or crossing lines). + // As this is prone to find a "local minimum", this should only be used when already following some line. + + cc = findNearestLocalCurvePoint(pivot, currentLocationIndex, goalPointDistance, ReverseHeading); } minDistA = double.MaxValue; @@ -556,14 +571,21 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer) } else { - for (int j = 0; j < curList.Count; j++) + if (findGlobalNearestCurvePoint) { - dist = glm.DistanceSquared(pivot, curList[j]); - if (dist < minDistA) - { - minDistA = dist; - A = j; - } + // When not already following some line, find the globally nearest point + + A = findNearestGlobalCurvePoint(pivot); + + findGlobalNearestCurvePoint = false; + } + else + { + // When already "locked" to follow some line, try to find the "local" nearest point + // based on the last one. This prevents jumping between lines close to each other (or crossing lines). + // As this is prone to find a "local minimum", this should only be used when already following some line. + + A = findNearestLocalCurvePoint(pivot, currentLocationIndex, goalPointDistance, ReverseHeading); } currentLocationIndex = A; @@ -579,7 +601,6 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer) curList[B].easting, curList[B].northing, pivot.easting, pivot.northing)) goto SegmentFound; - A = currentLocationIndex; //step back one if (A == 0) { @@ -666,11 +687,6 @@ public void GetCurrentCurveLine(vec3 pivot, vec3 steer) rNorthCu = curList[A].northing + (U * dz); manualUturnHeading = curList[A].heading; - //update base on autosteer settings and distance from line - double goalPointDistance = mf.vehicle.UpdateGoalPointDistance(); - - bool ReverseHeading = mf.isReverse ? !isHeadingSameWay : isHeadingSameWay; - int count = ReverseHeading ? 1 : -1; vec3 start = new vec3(rEastCu, rNorthCu, 0); double distSoFar = 0; @@ -879,7 +895,7 @@ public void DrawCurve() GL.Color3(0.95f, 0.2f, 0.95f); GL.Begin(PrimitiveType.LineStrip); - GL.Vertex3(mf.trk.gArr[mf.trk.idx].ptA.easting, mf.trk.gArr[mf.trk.idx].ptA.northing, 0) ; + GL.Vertex3(mf.trk.gArr[mf.trk.idx].ptA.easting, mf.trk.gArr[mf.trk.idx].ptA.northing, 0); for (int h = 0; h < curList.Count; h++) GL.Vertex3(curList[h].easting, curList[h].northing, 0); GL.End(); @@ -1250,5 +1266,102 @@ public void ResetCurveLine() mf.trk.idx = -1; } + // Searches for the nearest "global" curve point to the refPoint by checking all points of the curve. + // Parameter "increment" added here to give possibility to make a "sparser" search (to speed it up?) + // Return: index to the nearest point + private int findNearestGlobalCurvePoint(vec3 refPoint, int increment = 1) + { + double minDist = double.MaxValue; + int minDistIndex = 0; + + for (int i = 0; i < curList.Count; i += increment) + { + double dist = glm.DistanceSquared(refPoint, curList[i]); + if (dist < minDist) + { + minDist = dist; + minDistIndex = i; + } + } + return minDistIndex; + } + + // Searches for the nearest "local" curve point to the refPoint by traversing forward and backward on the curve + // startIndex means the starting point (index to curList) of the search. + // Return: index to the nearest (local) point + private int findNearestLocalCurvePoint(vec3 refPoint, int startIndex, double minSearchDistance, bool reverseSearchDirection) + { + double minDist = glm.DistanceSquared(refPoint, curList[startIndex]); + int minDistIndex = startIndex; + + int directionMultiplier = reverseSearchDirection ? 1 : -1; + double distSoFar = 0; + vec3 start = curList[startIndex]; + + // Check all points' distances from the pivot inside the "look ahead"-distance and find the nearest + int offset = 1; + + while (offset < curList.Count) + { + int pointIndex = (startIndex + (offset * directionMultiplier) + curList.Count) % curList.Count; // Wrap around + double dist = glm.DistanceSquared(refPoint, curList[pointIndex]); + + if (dist < minDist) + { + minDist = dist; + minDistIndex = pointIndex; + } + + distSoFar += glm.Distance(start, curList[pointIndex]); + start = curList[pointIndex]; + + offset++; + + if (distSoFar > minSearchDistance) + { + break; + } + } + + // Continue traversing until the distance starts growing + while (offset < curList.Count) + { + int pointIndex = (startIndex + (offset * directionMultiplier) + curList.Count) % curList.Count; // Wrap around + double dist = glm.DistanceSquared(refPoint, curList[pointIndex]); + if (dist < minDist) + { + // Getting closer + minDist = dist; + minDistIndex = pointIndex; + } + else + { + // Getting farther, no point to continue + break; + } + offset++; + } + + // Traverse from the start point also into another direction to be sure we choose the minimum local distance. + // (This is also needed due to the way AB-curve is handled (the search may start one off from the last known nearest point)). + for (offset = 1; offset < curList.Count; offset++) + { + int pointIndex = (startIndex + (offset * (-directionMultiplier)) + curList.Count) % curList.Count; // Wrap around + double dist = glm.DistanceSquared(refPoint, curList[pointIndex]); + if (dist < minDist) + { + // Getting closer + minDist = dist; + minDistIndex = pointIndex; + } + else + { + // Getting farther, no point to continue + break; + } + } + + return minDistIndex; + } } } \ No newline at end of file