I think you basically understand what I’m saying, we’re just using different terminology. In my explanation, N+1 would be the red waypoint, N would be the previous waypoint and N+2 would be the waypoint after the red one.
The way I envisioned using this is as follows:
A path from N->N+1 is going to have an optimal solution - a straight line, if you only include distance in your optimization measure. In my assumption, the heading data you include for N+1 is going to be the heading that points directly at it. The data you would be including about N+2 would be the heading for the optimal path from N+1->N+2. If the robot knows where its next optimal heading (N+1->N+2) is and the location of N+2 it knows the straight line that is the optimal path from N+1->N+2. This is information you can use that to preemptively adjust your angle or to recover from a position such as you described above where the robot veers off in the wrong direction.
At each point where the robot re-evaluates its location on the field you consider your current path and the optimal path. If your angle of approach to waypoint N+1 is sufficiently interior to the angle of approach of the optimal path from N->N+1 you attempt to correct towards the optimal approach angle gradually, so you keep moving towards the waypoint but try to “reacquire” the optimal path. As you get closer to a waypoint, the correction should become more extreme - however, if you reach a certain distance from the waypoint (let us say 3 ft) with an approach angle sufficiently interior to the optimal approach, assume that you can no longer acquire the optimal path to waypoint N+1, and and adjust your goal to one that can be achieved. In other words, consider N+1 visited and find a new intermediate waypoint along the path from N+1->N+2.
I suppose this could be said a bit more succinctly by saying “If your current angle of approach is sufficiently poor and you are within a certain tolerance of waypoint N+1, consider waypoint N+1 ‘visited’ and assign a new intermediate waypoint along the optimal path from N+1->N+2”. Basically you are re-assigning N+1’s position to (N+1)’ within a tolerance allowable by design such that your current path is CLOSE ENOUGH to the optimal path from N->(N+1)’ that your robot will not overcompensate and veer off path completely.
There are a number of heuristic approaches you could take to determining the position of a new waypoint dynamically, as well as some algorithmic approaches. I don’t know what would work best for your situation, or even in the above approach will work with your physical system (it should be pretty easy to implement in software, however). Provided you plan your waypoints such that your tolerance will not cause the robot to plot a path through the lane divider (there are some ways you could avoid the planning issues, such as special rules for any two connected waypoints that represent a cross-field movement), you should be able to use a system such as I’ve described to reacquire optimal paths with minimal difficulties. I think this should solve the problem you observed.
However, as I said, I’ve not implemented this before, so this is purely logical/algorithmic thought on my part and some significant consideration of behavior that should be observed using the system.
And, hey, even if you don’t find this useful it provided an excuse for me to think more fully through the dynamics and implementation challenges of an idea I’ve had sitting around for a while.
//Dillon
