One of the biggest problems with precise motion in robotics is localization. Encoders are only so accurate and tend to drift over time. Absolute positioning systems like GPS have error margins measured in meters. The solutions to this is often complicated (e.g. SLAM) or expensive (e.g. RTK GPS). As a cheap and simple alternative to this, I'm going to attempt to use incremental trilateration to enable the robots to determine their own position relative to their siblings.

Why It's Important

An accurate map of where the robots are is critical for proper planning of their movements. Without it they could get in each other's way or fall off of a cliff! It's also necessary for them to perform tasks that require precise motion such as moving debris or laying brick. While the robots will also be teleoperated and have an onboard camera, it's hard for the human eye to properly determine depth and estimate measurements from a 2D image.

The most common method for robot positioning is using encoders to measure the distance the wheels have traveled by counting rotations. There are some caveats to this method, however. If a wheel slips the encoder's accuracy will suffer and it will think it's traveled farther than it has. Over time the encoder's measurement will drift farther and farther from reality. This problem will only be exacerbated by the rough and slippery terrain that these robots will eventually be operating on. However, with incremental trilateration the robot will recalculate an absolute position every time it moves. This absolute measurement won't be susceptible to location drift.

Incremental Trilateration

The method I'm proposing for robot localization is based on trilateration, which is the same method GPS satellites use to determine position. This method, however, will be able to attain greater accuracy with cheaper and less complex hardware. Rather than the speed-of-light radio signals that GPS satellites use for trilateration, I plan on using much slower sound waves to do the same calculations. This makes it possible to do signal detection and calculation with a relatively slow microcontroller instead of high speed DSPs and custom silicon. I also plan on having an IR blast in the beginning to synchronize all of the robots before each sound pulse is sent. This sync signal will provide a trigger to the robots acting as base points so that they don't always need to be waiting for sound pulses.

Incremental trilateration consists of four steps:

1. Movement
2. Sync Transmit
3. Signal Transmit
4. Calculation

Initial Conditions

For this method to work the robots need to start off at set points. This means that the robot master and two of the robots need to start at known locations. Trilateration requires the knowledge of three base points for calculating the fourth point and so absolute positions of the three base points need to be known.

Steps 1-3

The animation above covers the movement, sync transmit, and signal transmit steps of the process. The big square and two smaller circles on the sides represent the base station and two of the robots at known points.

Movement

In this step the robot that's currently active moves, either towards a target or to explore. In the example above the robot in front moves along the Y axis from its position directly in front of the base station to an indeterminate position in front and to the right of where it was before.

Sync Transmit

The expanding, red circle sent out from the robot represents a sync signal that will be sent from the robot at an unknown position to the three other robots at known locations. In my planned implementation this will be a 40Khz IR signal. Once the three receiving robots receive this signal they know to start counting and waiting for the signal transmit. It should be noted that I'm ignoring the travel time of the IR pulse because the speed of light is so fast that it can be considered insignificant over the small distances that the robots will be moving.

Signal Transmit

Once the sync IR signal has been received, the robots will start counting until they see the sound pulse. I plan on using 40Khz ultrasonic transducers to handle this and generate an inaudible sound wave. Once the robots see the sound pulse they will stop counting and save the difference in time between the sync and signal transmissions. Using the speed of sound they can then calculate the distance to the robot that sent the transmissions.

Step 4: Calculation

Once each of the base robots calculates the distance to the moving robot they can then effectively draw a circle around themselves with a radius of the distance they've calculated. The base robots can definitively know that the moving robot is somewhere on the edge of the circle.

Robot 1 calculates the distance.
Base station calculates the distance. Robot 2 calculates the distance.

All three overlapping circles show the location of the moving robot.

Using the circles drawn by each of the base robots and the known direction that the robot traveled in, it can then be determined that the moving robot is sitting at the point where each of the three circles overlap, as displayed in the image above.

Limitations

There are a few limitations to the incremental trilateration method that I'd like to explain and propose solutions to.

Initial Conditions

The first and most inconvenient is that the robots will require known initial conditions. This means that at least three nodes in the localization network need to be at predefined positions, otherwise it's impossible to calculate the position of the moving robot. This makes setup a little harder and introduces accuracy problems if the initial conditions aren't perfect.

Some of this can be mitigated by the fact that each of the nodes can determine the location to the master node using the sync and signal method. If each robot is placed along a single straight line (which can be considered a line perpendicular to the Y axis at a known value of Y), it can send sync and signal transmissions to the master to determine its X offset.

Another possibility would be adding three IR and ultrasonic receivers to the master at predefined locations so that the master itself can act as the three reference points for the moving robot. This introduces some complexity but may ultimately be worth it.

Turn based movement

Another limitation is that in the above scenario with four nodes, only one robot can move at a time as it needs the three reference points to be stationary. This is less limiting in larger networks as the necessity of three reference points means (N - 3) robots can be moving with N being the number of robots in the network. For large values of N the limitation is less. However, because the IR and ultrasound are using the air as a common bus the actual transmissions will need to be kept to one at a time to prevent collisions.

Accuracy

Accuracy is the biggest concern with this system. Until I test this I won't be sure the exact accuracy of the system but there is a lot of variability that can cause problems. The reason I'm using ultrasound for the signal transmission instead of light is because the speed of light is much too fast and the clock speed of microprocessors is much too low to properly detect the signal over small distances. However the ultrasound still has the same limitations, albeit to a lesser degree.

The Arduino micros timer measurement has a resolution of four microseconds. Since the speed of sound is 343 m/s, the ultrasonic pulse will be able to travel 0.001372 meters (343 m/s * 4e-6 seconds) or 1.372 millimeters for one increment of the micros() counter. This is only the maximum theoretical resolution, however, since it doesn't take into account things such as digital read or sensor latency. Ultimately the actual resolution is something I'll have to determine experimentally. I'm hoping for a 1cm accuracy for my initial implementation and will have to search for optimizations should that not be immediately achievable.

Another thing to take into account is that the speed of sound changes with temperature. However this can be fixed by using a temperature sensor to more accurately calculate the speed of sound as shown here.

Next up is planning out and designing the actual hardware!