GPS Waypoint Navigation for an Outdoor Robot

Degrees are the wrong unit for a robot

A GPS module hands you latitude and longitude in degrees. Every controller you own wants metres. Converting is the first job and it is easier than the internet suggests.

Over the few hundred metres a hobby rover covers, the Earth is flat enough that an equirectangular approximation is exact to well within the receiver’s own noise:

north (m) = (lat − lat₀) × 111 320
east  (m) = (lon − lon₀) × 111 320 × cos(lat₀)

Pick an origin lat₀, lon₀ once — the start of the mission — and every waypoint becomes a plain (east, north) pair in metres that pure pursuit can steer to without knowing what a degree is.

The haversine formula is more correct and you do not need it. It matters over hundreds of kilometres; at 500 m the two agree to a few millimetres, which is three orders of magnitude below your error.

The number to remember: one degree of latitude is 111.3 km, so five decimal places is 1.1 m and six is 11 cm. Store waypoints with six. Anything beyond that is storing noise, and a float on an AVR only carries about seven significant digits — enough for 19.076123 and not enough for anything more, which is why you convert to metres relative to an origin rather than doing arithmetic on raw degrees.

Three things wrong with a fix

It is late. A typical NEO-6M reports at 1 Hz. At 1.5 m/s the robot travels 1.5 m between fixes, and the fix it does get describes where it was when the solution was computed, not where it is now.

It is noisy. Consumer GPS lands within about 2.5 m of the truth half the time, in the open, with a clear sky. Under trees, beside a building, or with a poor satellite geometry it is far worse — and the failure is not a dropout you can detect but a plausible wrong answer.

It has no heading. A GPS fix is a position. It cannot tell you which way the robot is pointing, because a stationary robot pointing north and the same robot pointing south produce identical fixes. Course-over-ground is derived from consecutive positions and is only meaningful while genuinely moving — below about 1 m/s it is mostly noise.

What that does to a path follower

Give the same pure pursuit controller the same course twice, once with perfect odometry and once with a 1 Hz fix at ±1.5 m held between updates:

Note what is not happening: the robot never gets lost. It stays roughly on the course and reaches the end. GPS-only navigation works — it is just enormously worse than it looks on the datasheet, and the cost lands mostly on the actuator.

The three fixes, in order of value

1 — Fuse, do not follow. Integrate wheel odometry and a heading source at your loop rate, and use each GPS fix to correct that estimate rather than replace it. Even a crude complementary blend transforms the result:

// Between fixes: dead reckon. This runs at the loop rate.
x += cos(heading) * v * dt;
y += sin(heading) * v * dt;

// On a fix: pull the estimate toward it, gently. alpha near 0.1 trusts odometry
// over the second between fixes, which is exactly the right instinct.
if (newFix && fix.hdop < 2.5) {
  x += ALPHA * (fixX - x);
  y += ALPHA * (fixY - y);
}

That is a complementary filter doing the same job it does for tilt: a signal that drifts but is smooth, corrected by one that is noisy but bounded.

2 — Get heading from somewhere else. A magnetometer gives absolute heading standing still, and needs hard- and soft-iron calibration plus distance from your motors. A gyro gives excellent short-term heading and drifts. Together they are what every real rover uses; GPS course-over-ground then corrects the pair when moving fast enough to be trusted.

3 — Raise the rate before you raise the accuracy. A 10 Hz module fixes the staleness that causes most of the weaving. It is usually a better first upgrade than chasing centimetre accuracy, and it is much cheaper.

Reading the module

GPS modules speak NMEA 0183 over a serial port at 9600 baud: comma-separated sentences, one per line. You want two of them.

• $GPRMC — time, validity, latitude, longitude, speed over ground, course over ground.
• $GPGGA — fix quality, satellites used, and HDOP, the horizontal dilution of precision.

Parse the fix, but gate on the quality. Two rules that prevent most outdoor disasters:

• Reject a fix with a status of V (void) or a fix quality of 0. It will still contain plausible-looking numbers.
• Reject HDOP above about 2.5. HDOP describes satellite geometry, and a bad geometry produces a confident answer that is tens of metres out.

Use a parsing library rather than writing your own — but read enough of the sentences to know what you are throwing away. Also: a cold start takes 30 seconds to several minutes with no almanac. Do not treat a robot that has not got a fix yet as a robot that is at the origin. Wait, with a visible indication that you are waiting.

Geofence it before you test it

An outdoor robot that misbehaves leaves. Before the first field test, add a hard limit in software: a maximum distance from the start point, and a stop if it is exceeded.

if (hypotf(x, y) > FENCE_RADIUS) { stop(); return; }   // before anything else

Ten lines, and it is the difference between a bug and a robot in a canal. Pair it with a mission timeout and a manual cutoff you can reach — a bump switch on the top deck works and needs no radio link.

When it goes wrong

Drive the same course with both position sources in the path following simulator, then build the GPS waypoint rover — where the module itself and the fusion above are the whole project.