Several years ago, Brian Tischler developed his own solution for using GPS to record farm machinery coverage in a field and to automatically turn things on and off as the machine crossed into areas that it had already been in. This is known in the industry as "section control." Brian's program, AgOpenGPS, and the community that surrounds it, is incredibly useful, education, and affordable. Already quite a number of farmers throughout the world have build their own GPS solutions for their farm machines including using GPS to steer with, providing straight rows and saving money reducing overlap of costly applications of fertilizer, seed, or other chemicals.
Initially, AgOpenGPS acted as if the implement a farmers was pulling was fixed to the back of the tractor, which is often the case when using 3-point hitch equipment. However if a trailing implement was being pulled, AgOpenGPS would think it was straight back from the tractor, when it fact it was trailing and swinging shorter during turns. My first, and so far only, contribution to AgOpenGPS was an algorithm for approximating where the trailer is tracking
The algorithm
The algorithm I came up with is not based on any research or literature, and it's only an approximation of the real path. It does not take into account side drafting or slipping of the trailer. There are three things that factor into the computation. First, the new GPS position of the tractor. Second, the length of the tongue of the trailer, from where it attaches to the tractor to the center of yaw of the trailer, which is usually the axle. Third, the old computed position of the center of yaw point on the trailer, from the last iteration. In general terms, the algorithm is composed of three steps, repeated each time a new GPS position is known.
- Calculate the angle or bearing from the new GPS position to the position where the trailer was before.
- Along that bearing, find the location that is the distance equal to the length of the tongue away from the new GPS position.
- Assign the trailer position to be that position calculated in step 2.
The more often you measure position and perform these calculations, the calculated trailer position will more closely approximate the real trailer's path.
Whether we are dealing with longitude and latitude, and using Haversines methods and ellipsoidal or spherical means of calculating distances, bearings, and new positions after a given bearing and distance, or if we are dealing with rectangular grid coordinates and simple trigonometry, the effect is the same.
Rectangular coordinates
Here's a diagram showing the original state of a trailer and the result of the first iteration overlaid to illustrate how this works using rectangular coordinates, where e is for "easting" (x coordinate) and "n" is for "northing" (y coordinate):
In the diagram, the first blue cross represents the axle and tongue of the trailer. The green line represents where the GPS location reported for the tractor moved from V0 to V1. The new trailer position can be calculated using trigonometry for rectangular coordinates. But since we're really dealing with similar triangles here, it can also be calculated by using nothing other than the Pythagorean Theorem and applying a ratio, as can be seen in the diagram.
Using latitude and longitude (spherical coordinates)
Using latitude, longitude, the haversine formula, and another position algorithm, it looks like this in C++:
void move_distance_bearing( double &lat, double &lon,
double heading, double distance)
{
double offset = distance / EARTH_RADIUS;
double latr = RADIANS(lat);
double lonr = RADIANS(lon);
double lat1sin = sin(latr);
double lat1cos = cos(latr);
double distcos = cos(offset);
double distsin = sin(offset);
heading = RADIANS(heading);
lat = asin( lat1sin * distcos +
lat1cos * distsin * cos(heading) );
lon = lonr + atan2( sin(heading) * distsin * lat1cos,
distcos - lat1sin * sin(lat) );
lat = DEGREES(lat);
lon = DEGREES(lon);
}
void trailer_move(double lat, double lon,
double &trailer_lat, double &trailer_lon,
double heading, double hitch_length)
{
//step 1. Calculate bearing from new GPS position to
// last trailer position
if (trailer_lat >= -90) {
//if last trailer position was known
heading = haversine_bearing_degrees(
lon, lat, trailer_lon, trailer_lat );
} else {
//otherwise just translate backwards along
//the current GPS heading (assume trailer is
//straight behind).
heading = heading + 180;
}
//step 2. Find location that along that bearing that is
// hitch_length from the current GPS position.
move_distance_bearing (lat, lon, heading, hitch_length);
//step 3. That location becomes the new trailer position
trailer_lat = lat;
trailer_lon = lon;
}
The function, haversine_bearing_degrees() is not given here, but it's an implementation of the standard haversine formula. Also not known are macros DEGREES(), RADIANS(), and the EARTH_RADIUS defines, which anyone can implement. The radius should be in whatever units you use for the other distance and hitch_length variables.
The Haversine and the new position formulas above are based on a spherical approximation of the Earth, which has inaccuracies. An ellipsoidal method would be more accurate. However, compared to working in UTM coordinates which use an ellipsoidal model, at my elevation the differences are often within a cm or two, which is enough for our purposes, especially given that this is just an approximation anyway.
Only an approximation
As said before, this algorithm is only an approximation. As you can see from the diagram above, the calculated path the trailer makes will not be a smooth, continuous arc. Rather it will be a bunch of little straight lines that track to the inside of the real path somewhat. The more often you iterate the calculation, provided you had very fine GPS position updates, the closer it would be come to the actual arc. In real farming experience, it's close enough to work for coverage mapping and section control, even with spraying.
Links
