Waypoint Seeking

Range Filtering

Since waypoint projection generates a range estimate on every cycle, the location of the actual waypoint becomes overdetermined. More recent projections will tend to be better (or at least no worse) because they will made closer to the actual waypoint. Here's some supporting analysis.

However, the current projection will be somewhat noisy:

The projection method can change between driving cycles, thus the range estimate might change drastically.
The result of flat earth projection will itself be noisy because it is based on noisy inclinometer readings.

Ideally we would use a Kalman filter, but it may be adequate to run the range estimates through a low-pass filter (e.g. an averaging process.) The time constant of the filter is adjusted based on the projected range from the current location to the target so that a step change now will have settled to its new value when have closed a fixed fraction of the currently projected remaining distance (say 50%.)

Basic Concepts

Once each waypoint has had its position estimated, then these locations define a sequence of line segments which we will call the path polyline. The purpose of waypoint seeking is to drive a smooth path which generally follows this polyline. The basic driving algorithm is to find the point which is the lookahead distance down the path polyline from our current location and to steer toward that point.

The lookahead distance implies a scale of desirable turn radius (or acceptable degree of sloppiness.) In practice it seems to work well to make this depend on the current speed:

lookahead time: The lookahead distance is made a function of the speed by specifying a constant lookahead time which is multiplied by the current forward speed to generate a lookahead distance.
minimum lookahead distance: This parameter sets a lower bound on the lookahead distance computed from speed. It will typically be set to a few vehicle lengths to discourage the use of drastic turns to get precisely onto the path polyline.

Driving Tolerances

There are several parameters which don't primarily control the path itself, but are used to determine whether we have arrived, or if we have failed to follow the path.

path tolerance: If the robot strays farther than the path tolerance from the path polyline, then we halt the robot and wait for new instructions from the operator. The path tolerance must be larger than the lookahead distance in order to prevent spurious failure. It may make sense to specify the path tolerance as a multiple of the lookahead distance.
critical point tolerance: If the next waypoint is critical, the lookahead distance is disabled. Instead, we steer toward the waypoint itself until we come within the critical point tolerance distance. We then resume normal waypoint following. This provides a servo deadband which prevents us from circling around hunting for the exact waypoint location. The terminal point is treated as a critical point, so this tolerance also determines how close we must be to the terminal point in order for us to decide that we've arrived.

Steering to the Line

The projected waypoint does not in general lie on the plane of our current orientation. We must take the ray to the projected waypoint, vertically project it down onto our current plane, and head in that direction. If the waypoint happened to be flat-earth projected, then it will lie in the current plane, in which case the projection does not cause any change.

The input format to the arbiter also introduces some issues with steering commands. As implemented in the past, waypoint following outputs turn radii as a way to implement smoothly turning onto the desired path. If the arbiter instead takes a heading as input, and does no smoothing itself, then we'd need to compute the heading that would in one driving cycle take us to where we'd be if we did travel on the desired arc.

Stopping Point Determination

If the next waypoint is terminal, then waypoint seeking uses the range projection to determine if it is time to stop. When we have closed within the critical point tolerance of the projected waypoint location, we stop. This will tend to cause us to stop somewhat short of the point, which may not be a bad thing, see possible user interface issue.

If there is no range projection (line following mode), then stop time is determined according to the maximum travel distance.

Critical Points

[ram: issues:

What if a waypoint is infinite and critical?
What if we are unable to reach a critical waypoint? How do we know to give up?
Need to specify how to determine a waypoint is used up. See this.

]

Infinite Waypoints

It may be that we cannot estimate any range to a waypoint. In this case, the waypoint is considered to be at infinity along the designated ray, and is known as an infinite waypoint.

An infinite waypoint that is not the last waypoint designated is still temporarily considered to be terminal until it either acquires a range projection or the maximum travel distance is exceeded.

We will also discard the current infinite waypoint and go on to the next one if we pass within the lookahead distance of the next waypoint (which could only happen if we do have range info for it.

Home | Mail to CMU TMR-members