Background

The Overbot suffers from terrible sensor myopia. We can't see more than about 10-12 meters ahead. It's not that the LIDAR can't see further than that; we can usually get range information out to 20-25 meters. But small amounts of vibration in the vehicle, especially in the pitch axis, result in large range errors, as the scan hits the ground in the wrong place. This causes the detection of false obstacles.

Our original approach, as described in Note #8, works by comparing successive scan lines, rather than trying to extract absolute elevation from range information. Because the LIDAR is on shock mounts, vibration above some frequency (supposedly 20Hz, but we need to check this) is filtered out. The LIDAR scans at 75Hz. This provides some noise immunity, enough to get us to where we are today. But it's not good enough.

New approach

Here's a new approach - using the LIDAR range data itself to correct for vibration.

For the first version of this, we have the following assumptions:

• If we're looking out to maximum range, we're going reasonably straight on a reasonably flat road, which might contain occasional obstacles or potholes.. So that's the case for which this correction must work best. On difficult terrain, we're moving slowly, and this is less of an issue.
• Only pitch needs to be corrected. We will assume that position, roll and yaw data from GPSINS are good enough to use. Pitch gives us the most trouble, because what we want is accurate elevation to within a few centimeters. Roll and yaw may have us looking in the wrong place.

Given those assumptions, a typical scan, profiling the ground from an angle above the ground, looks like this.

Typical LIDAR scan on near-flat ground

As the scanner moves in the pitch axis, the range line moves relative to the vehicle. We know the approximate pitch of the scanner from an encoder in the tilt head. But because the scanner is on shock mounts and the vehicle frame has some flexibility, a few degrees of error are introduced into the pitch measurement as the vehicle bounces around. It is that error which we need to remove.

Basic algorithm

Our best source of information about where the scanner is pointed is the scanner range data itself. We could mount an extra tilt sensor on the scanner frame, but that introduces additional problems.

For each incoming scan line

• Start with a row of raw range data R(n) from the scanner, representing scan line n.
• Convert to a vector in the scan plane.
• Extract X coordinate of that vector. For a flat ground surface, all values will now be the same.
• Apply some low-pass filtering across the scan, to remove noise.
• Compute average value.
• Throw out values "too far" from average value (need sound statistical method for this) and re-average.
• Call this result L(n), for line n. This is a scalar value.

Scan line filtering

• Maintain a queue of the last N scan lines. (N should cover about two cycles of the slowest vibration mode.)
• Examine the values of L(n) for the scan lines in the queue. Ideally, all values of L(n) are the same. If the scanner is vibrating in pitch, the sequence of L(n) values will oscillate. If the terrain is going uphill or downhill, or the scanner is tilting, there will be a trend in L(n). Compute some reasonable smoothed value for the L(n) values which removes the oscilation and noise. Call these Ls(n). (Again, we need a sound statistical method for this. A line fit or a low pass filter is probably sufficient, but we need to check this against the data.)
• When a new line enters the queue, the oldest line is removed from the queue and passed to the next processing step. The line has a range correction C(n) = L(n) - Ls(n). The range error must be used in conjunction with the scanner tilt and scanner height to compute a tilt correction. The corrected tilt is then passed, along with the scan line, to the next processing step. Note that the measured range values are passed through unchanged, because they are correct. The correction applies to where the scanner is pointed, which is the data contaminated by noise.

We need to extract the C, L and Ls values from logged scanner data and see what they look like. We have a Python script which attempts to load scanner data into Blender to build a 3D model, but it's not working correctly for data taken from the moving vehicle. This needs to be fixed and augmented with the correction algorithms shown here, so that we can debug the algorithms off-line.

Scan line reordering

The scan lines from the previous step may need to be reordered for proper map updating. As the scanner pitches up and down and move forward, some sequential scan line pairs may be unsuitable for pairwise updating.

Map updating

Previously, elevation values for individual cells weren't very accurate, and we were basing our driving decisions strictly on difference information between successive scan line pairs. With this new adjustment system, we should have good elevation information.

***MORE***

Geometry of tilt correction

Tilt correction geometry h: scanner height L: raw (unsmoothed) range Ls: smoothed range θ scanner tilt from encoder α: error in scanner tilt caused by vibration We know h, L, Ls, and the angle θ. We need the angle α.

Derivation

General formula for perimeter: K=ab sin(c)/2

perimeter = h + L + B = h*L*sin(θ)/2

B = h*L*sin(θ)/2 - L - h

similarly

Bs = h*Ls*sin(θ + α)/2 - Ls - h

Bc = L*Ls*sin(α)/2 - Ls - L

Bs = B + Bc

so

h*Ls*sin(θ + α)/2 - Ls - h = h*L*sin(θ)/2 - L - h + L*Ls*sin(α)/2 - Ls - L

h*Ls*sin(θ + α)/2 = h*L*sin(θ)/2 + L*Ls*sin(α)/2- 2*L

solving for α, we get

***MORE***

Vehicle elevation estimation

We now need smooth elevation information for the vehicle. GPS elevation alone is not enough. We must use smoothed pitch and dead reckoning to compute a smoothed elevation value. Absolute elevation is not critical, but within the scale of the map (100m), relative elevation must be good to within 1% of the distance moved. (i.e. 20 m of movement should produce less than 20cm of elevation error.)

***MORE***