Setting up speed and guidance commands just before second run
SparkFun score board. Final result: 7th of 13.
Checking radio-link and open/closed-loop switching at start line
Robots lined up at mass start, ready for carnage
First off the line at the mass start means nothing if you crash into the first curb
Elmo, in a fury of road rage, takes out another competitor. Nate is called in to
calm him down.
|
3 - Control Architecture
Cross-Track Waypoint Navigation
The basic element of the navigation design is the cross-track waypoint following,
which sets the desired path as the line connecting the previous and next waypoint.
This differs fundamentally from the more common approach of a waypoint-direct navigation,
which guides the vehicle in the direction of the next waypoint irrespective of the vehicle
position. Whereas the waypoint-direct method provides the shortest path to the waypoint,
the cross-track approach constrains the path taken to a corridor and will guide the vehicle
back to the desired path should it be displaced.
Cross-track navigation is a direct extension of waypoint-direct and uses a course-
correction term to achieve the line following. The desired heading is set to the
bearing from the current vehicle position to the next waypoint plus a heading correction
toward the cross track. If the vehicle is navigating along the track, the heading correction
is obviously zero and desired heading is set directly toward the waypoint. As the vehicle
deviates from the desired path, the heading correction is computed as the inverse tangent
cross track distance error scaled both by a gain and by the ratio of the cross track distance
error to the waypoint distance.
When the vehicle is far from the waypoint (waypoint distance >> cross track error),
the inverse tangent provides a softened heading correction in response to cross track
errors. For instance, a small error of 5 feet off course generates only a few degrees of
heading correction, whereas a large error of 50 feet generates nearly 90 degrees of correction.
For large errors, the desired heading is directly toward the track, rather than toward the
waypoint.
In the case of the SparkFun competition, the cross-track method seemed to be a better
approach given the narrowness of the parking lot course, particularly along the east side.
I had some concern that an overshot turn could lead to navigation into a curb or the pond
using waypoint-direct, but using cross-track would force the vehicle to compensate for
the overshoot and return to the line connecting waypoints.
The proximity of the course to the relatively-tall SparkFun building meant that GPS
shadowing could be a factor in the accuracy of the position fix. On the west side of
the building, satellites low in the eastern sky are occluded, giving the vehicle access to
half of a hemisphere of sky. Each side of the building causes satellite blocking in a
different quadrant. The building corners permit access to 75% of the sky, blocking only
one corner. Given this shadowing, my approach was then to set a waypoint from the GPS output
at each corner and each midpoint of the building. Such an arrangement of waypoint accounts
for all combinations of GPS shadowing - and conceivably the vehicle could transition gracefully
between different groups of satellites while moving around the building.
Heading and Yaw Rate Tracking
Heading commands from the navigation outer-loop drive the heading and
yaw rate tracking inner loops. The heading error (desired - measured) is
scaled by the K_rc_psi gain to generate a yaw rate command. This yaw rate command
is then limited by a kinematic filter which only allows rates that result in a
lateral acceleration less than 1.5G. As the vehicle speed increases, the maximum
allowable yaw rate decreases. The resulting yaw rate error is scaled by gain
K_ds_r (itself scheduled on speed) and generates the desired steering servo command.
Speed Control
Speed tracking is achieved using a simple, single-gain reference tracking controller
with GPS velocity used as a speed measurement. The commanded speed varies between the
maximum setpoint, used between waypoints, and the minimum setpoints, used during turns.
The commanded speed varies smoothly between the two setpoints in response to distance
from waypoint and heading error. In the former case, the commanded speed begins
to decrease from the maximum setpoint as the vehicle closes within a set distance
threshold from the waypoint. The desired speed continues to decrease linearly
with distance to waypoint and reaches the minimum speed at the waypoint.
Throttle command to the electronic speed controller is computed by scaling the
speed error (desired - measured) by gain K_dt_V. For positive errors, where the
desired speed exceeds the measured speed, the throttle is positive and the torque
to the wheels is in the driving direction. During reductions in speed command
on approach to waypoints, the speed error can become negative as the desired speed
decreases below the measured speed. In such instances, the vehicle commands negative
throttle, which is used for both braking and reverse. A 1-second limit on negative
throttle is set to ensure that only braking is engaged. Beyond this limit, the minimum
allowable throttle is 0 (neutral), which causes the vehicle to slow by coasting.
Desired speed is also reduced for large heading errors, preventing the vehicle
from moving quickly in any direction except toward the waypoint along the desired
cross-track. Two heading angle error thresholds, inner and outer, control the
behavior of this speed reduction. For absolute heading errors less than the inner
limit, the speed is set to the maximum value, since the vehicle is presumably
heading in the desired direction. Between the limits, the speed command scales
linearly with heading error between the maximum speed at the inner limit to the
minimum speed at the outer limit. The speed command remains fixed at the minimum
value for absolute heading errors greater than the outer limit.
In practice, the speed limiting appears to work well and provides a logical
variation of speed throughout the course. As the vehicle approaches a waypoint,
the controller brakes briefly, coasts until the waypoint is reached, then turns at
the minimum speed to intercept the cross-track to the next waypoint. The speed limit
on heading error applies to the combined bearing to waypoint plus cross track correction,
which allows the vehicle to travel at the maximum speed when intercepting the cross track.
Obstacle Avoidance
Obstacle avoidance is the least developed of my controllers, as evidenced by
my vehicle's tendency to crash into curbs. My initial goal was to use dual scanning
ultrasonic sensors to build a simple model of the external environment and command
the steering based on the distribution of passable and unpassable zones. I ran
with simplified version of this, which uses two fixed ultrasonic sensors to detect
objects slightly left and right of the vehicle centerline. A detection of any
object within the defined threshold disables navigation and engages obstacle avoidance
mode, which commands a steering angle equal to the scaled difference between the
left and right sensor measurements.
Approaching a curb to the right of the vehicle would cause the right sensor to
detect an obstacle while the left sensor detects nothing. The steering command
would then be a left turn, which steers the vehicle away from the obstacle.
Obstacle detection also causes a reduction in throttle in proportion to the
obstacle distance. The throttle command is computed simply as the obstacle
distance multiplied by a gain, so the vehicle does not enter braking (positive
throttle values only) and the throttle does not correspond to any desired speed.
|
