Robotic Tracking and Formation
Movement using Ultrasonic Trilateration
Chris Johnson, Mike Irvine,
Saeid Azimi Moghadam, David Bauslaugh
Abstract — This document outlines the development and
implementation of a robotic formation as a culminating, final year, design
project. The research was primarily focused on robotic formations using
trilateration: the location of an unknown point using three distances and three
known points. The trilateration algorithm was implemented by combining radio
communication and ultrasonic transmission, effectively exploiting the time delay
between the signals, and accurately measuring distance and direction to a
This project will utilize analog, digital, and software
systems to develop a swarm of inexpensive robots that interact with one another
and behave as a single cohesive unit. A remote-controlled master robot will
serve as a human interface and ultimately as a leader for the swarm. Ideally,
the slave robots will accompany the master by maintaining specific individual
positions within a defined swarm formation. The robots will communicate over a
2.4 GHz wireless radio while detecting and calculating position using sensors
at three distinct points on each robot, a process called trilateration. The
main robot board will have two distinct functionalities: master mode and slave
mode. While in master mode the Main Control Board will communicate directly
with a handheld receiver. In slave mode, the Main Control Board will
communicate with the master robot and perform trilateration calculations. In
both modes the Main Control Board will also communicate with the Motor Control
Board. The Motor Control Board accepts a distance and direction value from the Main
Control Board and computes the appropriate output for the left and right motor.
The motor speed is controlled by pulse width modulation performed on the Motor
This combination of master/slave
behavior allows a single node for human contact that can control a potentially
limitless group of robots.
II. Location Mechanism using Trilateration
With robots moving autonomously in a swarm,
the ability to accurately locate other units is paramount. From preliminary research
on existing robotic swarms, trilateration was selected as the ideal method for
the process of locating a point using the coordinates of three known points,
and their respective Euclidian distances from the unknown point. The method uses
individual distances between the known and unknown points to construct a
network of intersecting circles. The calculated intersection of the circles is
the location of the uknown point. Figure 1 shows the
process of trilateration in its graphical form.
To simplify the calculations that
would eventually be ported to relatively low-power, mobile devices, a
constraint was placed on the location of the known trilateration points. The
center of the robot was set, computationally, as the origin, with the three
known sensor points arranged to form an orthogonal basis, with each sensor
having only a single non-zero x or y component.
Equations (1) and (2) show the
simplified solutions for locating an unknown point, P(x,y), that is distance a,
b, and c from three known points .
Figure 1: Solving the location
of point P(x,y) using calculated distances a, b, and c, alongside a known,
fixed distance, d.
The trilateration calculations
will undoubtedly come from noisy sensors, and as a result, extra care must be
taken to minimize the sensitivity to error. The most easily controlled variable
in the trilateration calculation is the fixed distance between trilateration
sensors. Several trial simulations were run to chart the effect of increasing
and decreasing fixed sensor distance. Figure 2 shows the simulation results,
with error-prone calculated positions converging on the actual robot position
as long as sensors are placed more than 0.2m apart.
A. Ultrasonic Channel Model
The chosen method for distance
measurement was a combination of radio frequency (RF) communication and
ultrasonic transceivers. Alongside an RF pulse that travels almost instantly,
the time delay inherent to the speed of sound gives a predictable,
easily-implemented method of measuring distance.
The distance measurement is a
function of the time delay associated with the ultrasonic pulse traveling
through air. Figure 3 shows the time delay mechanism that makes distance
measurement possible with ultrasonic waves.
Figure 3: Demonstration of distance measurement. RF
pulse signals timers to start at the green line and transmission (red wave)
begins. As the wave travels through air, it reaches each receiver at different
times (blue waves).
The delays obtained from each
ultrasonic receiver are multiplied by the speed of sound (taken as 343.89 m/s) to
yield the distance from ultrasonic transmitter to receiver.
In order to calculate the
distance and direction to a robot, regardless of their orientation or location,
the traditional ultrasonic transceiver had to be modified. In many cases,
sensors labeled “omni-directional” had a teardrop dispersion pattern of no more
than 45° off the centre axis. A key requirement to the successful
implementation of ultrasonic distance sensing, and ultimately trilateration,
was the dispersion of an ultrasonic signal into a plane. This dispersion was
accomplished with a parabolic cone defined in (3) .
A parabolic cone allows all
incoming planar sound waves to be collected to a single point, the focal point
of the parabola. The converse is true for transmission, where the single
ultrasonic source is dispersed into a planar direction. Figure 4 shows the
dispersion characteristics of a parabolic cone on incoming and outgoing sound
Figure 4: Parabolic cone used for ultrasonic
dispersion and collection .
After fabricating the cones from
two part epoxy, a single ultrasonic receiver/transmitter pair was tested for
accuracy and channel characteristics.
Figure 5: Frequency of errors in ultrasonic distance measurement.
The sensors exhibited an error in
linear distance measurement of about +/- 6 cm centered around 2 cm. Theoretically,
an overshoot in distance measurement would occur in the scenario where a timing
delay was artificially lengthened. This timing delay that caused a 2 cm offset
in distance measurement was attributed to the delay associated with overcoming
the inertia associated with the ultrasonic transmitter/receiver cone. The delay
associated with the sensor inertia was accounted for and mitigated in software.
B. Swarm Simulation Environment
In order to better understand
swarm dynamics, a MATLAB environment (Figure 6) capable of real-time
behavioural analysis was developed. The environment implemented the following
· variable swarm size
· variable formation shape
· variable robot speed (linear and rotational)
· simulated measurement errors based on measured error data
· real-time robot movement display
· real-time robot tracking (historical path tracing of user-defined
The environment was implemented to
be as complete and flexible as possible for use as a development and decision
tool. All major algorithm decisions from formation configuration, to motor
control, to trilateration were executed first in the simulation environment and
adapted for implementation on the robot hardware.
Figure 6: MATLAB simulation
III. Hardware Implementation
The robot hardware implementation
consists of three major components: the Remote Control Board, the Main Control
Board, and the Motor Control Board. The remote is responsible for sending
wireless radio commands from a human operator to a master robot. The Main
Control Board handles trilateration calculations and the Motor Control board
hanles the conversion from distance and direction of travel to outputs to the
motor. The most important part of the hardware implementation comes from the
communication links. Without communication between all three major components,
the robot is unable to function in a normal manner.
1) Serial Peripheral Interface (SPI) Bus
Communication between the radio
and the Main Control Board was accomplished with the Serial Peripheral
Interface (SPI) Bus. SPI is a master slave protocol where the master can talk
to multiple peripherals, but the slaves do not normally communicate with each
other. SPI uses three control signal pins, plus one extra pin per device that
acts as an enable. The control signals that are used in the SPI protocol
include Clock (SCK), Serial Data Out (SDO) or Master Out Slave In (MOSI),
Serial Data In (SDI) or Master In Slave Out (MISO). To see the direction of
communication refer to Figure 7.
The Clock signal is controlled
only by the master. During a transmission, the desired device enable,
controlled by the master is set active (usually low), and the master starts the
clock. The device and master exchange data one character at a time until the
transmission is complete, when the device enable is disabled (usually set high).
A diagram of SPI communication with a Nordic semiconductor radio peripheral is
shown in Figure 8. In this diagram the master is sending a single character to
the slave and the slave is responding with multiple characters back to the
master. The CSN signal (enable) is set active(low) for the entire duration of
the communication Note that the clock is still controlled by the master even
after it has finished sending data.
2) Inter-Integrated Circuit (I2C) Bus
Communication between the Main
Control Board and the motor control board was done using the Inter Integrated
Circuit (I2C) communication protocol. I2C is also a
master slave protocol but uses a bidirectional data channel. It also does not
have an enable pin for each of its slaves, reducing the number of required
communication wires to two.
The signals are Serial Clock
(SCL), and Serial Data (SDA). These signals are shared to create a bus between
multiple devices. With the I2C protocol, multiple masters may be
established giving each device the ability to communicate with any other device
on the I2C bus. To differentiate between devices, each slave has a
distinct address that is accessed by the current master.
The structure of an I2C
command is a seven
bit address followed by the data to be sent to the device. The address is left
shifted by one, and a control is shifted into the least significant bit. The
control bit is a zero if the master is writing to the slave or one if the
master is going to read from the slave. Like SPI, I2C transmits one
character (eight bits) at a time.
Figure 9: I2C connection diagram.
3) Wireless Protocol
For inter-robot communication and
Remote Control Board commands to the robots, a wireless protocol was developed.
A simple command structure was implemented to determine the action each robot
had to take after their radio interrupt service routine was activated. The
structure is a single character followed by a variable-length argument list of data.
A table of instructions is shown in Table 1. For identification control, each
robot was dynamically assigned a different ID when it became active and joined
a formation. To ensure that the robots could always talk to at least the master
a standard master radio ID was used. The wireless protocol uses the following
Table 1: Wireless Radio Command Table
This sends a desired percentage of left and right motor power.
This command has the robot perform trilateration (receiver
This command is used to configure the trilateration delay
from the radio, a fixed point is used to set delay values.
This commands comes from the master device to the slave
device, the data bits contain more information.
This command comes from the slave to the master device the
data bits contain more information.
This command changes the formation
This command announces that a robot has just become active
and is answered by the master
This command either establishes or relinquishes control as
B. Remote Control Board
Figure 10: Remote Control Board render.
The Remote Control Board was the
first circuit designed; it was created as a launch point for all of the other
circuit boards that would be created. As a result, locations for many expansion
devices were incorporated into the design but were not implemented (see Appendix
A: Schematic diagrams). The Remote Control Board is driven by the a 28-pin Microchip
PIC 18F Microcontroller . This devices was chosen because it has a built in
SPI bus, as well as a separate UART, with sufficient pins for other basic input
output devices. The devices that were implemented included the Nordic
Semiconductor Radio (nrf24L01+), four input pushbuttons, the on-chip UART, and
output debugging LEDs. The choice of these devices allowed for the initial
coding and debugging of the Wireless Radio (nrf24L01+) over the SPI protocol
using a second controller board. By doing this, the radio communication
protocol was established and a custom radio library was written for future use
with the Microchip family of microcontrollers.
The Remote Control Board operates
in a standby mode, it continuously checks both the input peripherals (selection
buttons), and the radio Interrupt Flag Pin, which is asserted when data has
arrived and is ready to be read. If any of the buttons are pressed then the
radio sends to the master a left and right percentage for the left and right
motor respectively. This left and right percent directly control the master robot,
creating a radio-controlled device that the other slave robots will follow. The
Control Board is the sole source of human input to the robotic swarm.
C. Ultrasonic Trilateration Modules
After choosing an appropriate
method for distance detection, and constraining the final size of the robot
based on error sensitivity, the main distance sensor designs were developed. The
platform required three individual receiver (Rx) sensors, and a single
centralized transmitter (Tx). To eliminate the need for four ultrasonic
sensors, the sensor at the origin was designed as both a transmitter and
receiver whose operation mode was controlled via an output pin on the Main
To transmission functionality of
the Tx/Rx board (Figure 11) uses a 555 timer as an astable multivibrator
operating at 40 KHz and provides a low voltage signal for a MAX232 driver chip.
The MAX232 uses charge capacitors to turn a single +5V supply into +9V and -9V
signals that are used to drive the ultrasonic sensor. The MAX232 chip is
normally used in communication and is not designed to drive loads so the actual
output in testing was ±6V. In order to get the most out of the MAX232, the 555
timer signal was inverted and supplied to the second input of the MAX232 and
used to drive the negative pin of the ultrasonic sensor.
Figure 11: Transmitter Board layout.
A number of components were
implemented to provide the dual-mode operation of the Tx/Rx board. The first
step in dual mode operation involved disabling the transmitter. A PNP
transistor was added to control the power to the MAX232. The PNP was used to
cut off the power supply to the MAX232 and pull the signal to ground through a
pull down resistor. Although the MAX232 supply had been tied to ground, the
charge capacitors still had enough charge to emit almost two volts to the
ultrasonic sensors. To remove the voltage a decoupling capacitor was added.
The second element of dual-mode
operation is an enable signal. An enable signal was used to start and stop the
40 KHz output from the 555 timer. Finally, an NPN transistor was used to ground
the negative pin of the ultrasonic sensor when trying to receive and during
transmission the signal was cut-off from ground to allow it to travel between 6
volts and 0 volts. The resultant output wave in transmit mode can be seen in Figure 12.
Figure 12: Transmitter output signal.
The receiving portion of the
Tx/Rx board and the Rx board itself were identical and were implemented using a
gain stage, a gain and filter stage, and finally a comparator stage. The first
gain stage inverts the signal and amplifies it by a factor of 66 using an LF412
operational amplifier. As it is running a 40 kHz signal and the gain bandwidth
of the op-amp is 4 GHz, the maximum gain possible is 75. The second gain stage,
shown in Figure 13, uses the second op-amp in the LF412 IC. It implements an
inverting gain of 30 and a low pass filter with cut-off frequency of 75 KHz.
The frequency response is shown in Figure 14.
Figure 13: Second gain-stage circuit.
Figure 14: Frequency response of second stage (gain is
The third and final stage of the
receiving circuit is a simple LM311 voltage comparator. Using a voltage divider
of 40 KΩ and 100 KΩ the reference voltage going to the negative input
of the LM311 is 1.4 volts. With the previous two gain stages of 66 and 30 the
total amplification is 1980 and therefore any error of 0.7 mV will cause a
false output signal. To eliminate this possibility a capacitor was added in
series with the output to the voltage comparator to remove any low frequency
noise that might have been amplified. Testing was also done on the ultrasonic
receivers to ensure no noise was created from the motors or the environment.
D. Main Control Board
Figure 15: Main Control Board render.
The Main Control Board was created
as the workhorse of the robotic design. The PIC32 microcontroller was chosen
for a number of reasons. The PIC32 provides the ability to perform floating
point operations which are crucial for trilateration calculation , . The PIC32 also has
multiple UARTs allowing more flexible I/O configurations as devices are attached
and pin configurations are changed.
The PIC32 has multiple I2C
and SPI ports, these were used to route the connections to the nrf24L01+
wireless radio, and to the Motor Controller Board . The traces for the
radio were run as short as possible to help eliminate interference in the
communication lines. The I2C connector was routed as close as
possible to the location where the motor board would be mounted. This minimized
the length of wire needed for the connector, and increased the robustness of
the device. Because the I2C, SPI, and UART were configured with
separate pins debugging was possible while the robot was fully assembled and
connected without having to resort to bit banging, or basic status information
The PIC32 was set to operate at 80MHz
or 12.5ns per clock cycle. The peripheral clock divider was set so that the
external and internal peripheral devices were configured to run at the same
high frequency. Running the microcontroller at 80Mhz allowed for an increase in
the resolution of the timers used for counting ultrasonic delays.
For future expansion, it was
imperative that as many pins as possible were broken out from the board. Using
a 64 pin device allowed for a 26 pin expansion port that was named the I/O
Expander. The I/O expander can be used to implement any number of devices. It
allows access to several entire ports on the PIC32 including analog to digital
pins, another I2C bus, another UART, and general purpose I/O.
Currently the only device that exist for the I/O Expander is the debug board (see
Appendix A: Schematic diagrams).
The PIC32 was also chosen for the
free ANSI C-compatible compiler . The C programming language allowed for faster development
on Microchips PIC Microcontroller devices, and it also allowed for the creation
and sharing of a common library for the nrf24L01+ wireless radio link. SPI was
used because the nrf24L01+ wireless radio communicates via an SPI connection. I2C
was chosen for inter-board communication because of the low pin count and
simple interface and support as an industry standard. It also allows for
hardware abstraction, if another propulsion device was implemented the Main
Control Board would not have to be changed. The I2C bus was clocked
at 400KHz, which was sufficient for data transfer of motor commands to the
Motor Controller board occurring at most four times per second.
The trilateration interface on
the Main Controller board consisted of three interrupts from each of the
trilateration boards, the power connections needed for each trilateration board
(-5V, 5V, GND), and an active high enable for the transmitter board.
Like the Remote Control board,
the Main Control Board does very little while running. Most of the control is
done through the use of interrupts.
Current radio interrupts include
a trilateration start that triggers a timer and enables the interrupts for each
trilateration sensor. When the ultrasonic pulse is received at the sensor the
interrupt line is brought high and the interrupt service routine samples the
timer, storing the value. When all three interrupt routines have been run then
the board processes the three times and computes the location of the sending
robot. This is then used to find the desired direction and distance based on
further processing from the formation control routine. The new direction and
distance data is finally passed to the Motor Control Board via the I2C
Another radio interrupt sets the
left and right motors duty cycle percent. This command forwards the information
to the Motor Controller Board again through the I2C bus.
Configuration of trilateration
timing based on radio delay is also initialized through a radio interrupt. The
Main Control Board uses a specified distance and direction (0o at
50cm) and configures the timing offset so that the trilateration result is the
same location, this allows for accurate trilateration timing information to be
acquired. A specialized configuration routine was created for each robot to
auto-configure the individual delays, this was based off a two robot procedure
including a configuring robot (configurer) and robot that was being configured (configuree).
The configuree would be started and would then wait for a configuration radio
command. When the configurer robot was started it would send out a
configuration pulse and start transmitting an ultrasonic pulse. The configuree
then calculates the offset and sends back a done command, and the configurer repeats
the process. This allows for many configuration values to be averaged together,
ideally these values would then be stored onboard using the built in SD card
reader. The main board layout can be seen in Figure 16.
A start up algorithm was
developed so that the bots would be able to dynamically assign identification
numbers and positions in the swarm. The first step involves sending out a
command looking for the master using the “I’m active” command. If no master is
found, the current robot becomes the master. If a master receives the “I’m
active” command then it responds with the current robot’s new identification
number. Currently it assigns robots in sequential order stating at one. Zero is
reserved for the master. Once the individual robot knows its identification
number, it changes its radio address and can join the swarm. Further
implementation would include having the new robot receive, in addition to its
identification number, the current formation.
Figure 16: Board layout for Main Control Board.
E. Motor Control Board
Figure 17: Motor Control Board
The Motor Control Board pictured above used the dsPIC33 microcontroller. This microcontroller has the 2-pin I2C
module which was set up as the slave and received commands from the Main
Control Board. This was routed at the top right of the board (see Figure 18) to reduce the noise inherent with the rotating magnetic field of the motors. This also
allowed for a shorter wiring harness to be run from the Main Control Board to
the Motor Control Board.
Figure 18: Board Layout of Motor Control Board.
The dsPIC33 was chosen because it
incorporated two Pulse Width Modulation (PWM) channels. Each PWM channel was
implemented to control the duty cycle of a motor. By varying the duty cycle,
the amount of power that is delivered to the motor is increased or decreased. However,
the relationship between the duty cycle and the amount of power delivered to
the motor are nonlinear, since power is related to the square of the voltage.
The PWM channels were connected to the enables of the motor driver chip. This
allowed the motor to coast during the off portion of the PWM wave as the output
terminals were set in high impedance mode see Figure 19.
Figure 19: Motor driver truth table .
For position feedback, the dsPIC33 included two Quadrature
Encoder Interfaces, these were used for velocity feedback control. The dsPIC33
received on off pulses from the IR sensors on the Quadrature Encoder Board
shown in Figure 20. The board was designed to fit directly onto the robot
chassis surrounding the axle.
Figure 20: Quadrature Encoder Board render.
1) Movement algorithm
Each robot has the ability to become the slave and follow
the master, or become the master and follow commands from the Remote Control
Board. The master robot, by default, is configured to run at 75 percent of its
maximum speed, to allow slave robots to catch up and assemble into formations.
Under slave mode, the desired
angle and distance are communicated to the Motor Control Board from the Main
Control Board via I2C protocol. The motor control algorithm compares
the values of the desired angle and distance against preset values and executes
one of the following actions:
Stop: If the desired
distance is below the minimum distance threshold, the enable pin on the
H-Bridge for both motors are disabled.
Go straight: If the
desired angle is within the threshold value (|θ|<5) the motor will
enter a forward drive mode where drive pins on the H-Bridge are forward enabled
and the PWM value across the enable pins is set to maximum speed.
Gradual turn: If the
desired angle is less than 45° off the robot’s centre line, the PWM ratio
between the two motors is set as 25%:50% according to the direction of the
turn. In this mode, each robot will turn and move forward simultaneously,
turning gradually to the right or left depending on the sign of the desired
Stop and turn: If the
desired angle is greater than 45° off the robot’s centre line, the PWM ratio
between the two motors is set as -37%:37% according to the direction of the
turn. In this mode, each robot will turn on the spot.
The point of reference for
calculating the desired angle is in front of the robot (on the bisector of the
trilateration arms). A positive angle translates to a turn left command where a
negative angle requires a turn right. The direction update frequency is
configured so that there will be multiple updates within the time needed for
the motor to complete an individual directives thus providing a smooth
transition on tracking and movement of each robot.
2) Quadrature Encoder Interface
Velocity feedback is implemented
with the use of the built-in Quadrature Encode Interface (QEI) on the dsPIC33.
The QEI inputs are driven from an optical encoder circuit with two reflective
object sensors positioned 90° out of phase, and looking at the encoder disc on
the inner face of the wheels.
Each tick of the QEI will trigger
a timer on the dsPIC33 which is used to calculate the length of n discrete
encoder pulses .
The accumulated length of the pulses is then divided by the number of the
pulses to provide the velocity of each motor. The calculated velocity is fed
back to the controller and checked against predetermined reference values for
each PWM value. This enables all robots to maintain identical speed while
preserving the formation.
Since the encoder pulses are not
necessarily synchronized with the polling period, each polling interval sees a
loss of data that can approach the size of one full encoder tick. For this
reason, the quadrature encoder was not enabled for final demo purposes.
F. Mechanical Design
The main robot mechanical design
was modeled and constructed completely in a software CAD package before
construction (Figure 21). Adjustments were made on the model, and the main
chassis was exported for computer-aided milling.
Figure 21: CAD model of main robot assembly.
The robot chassis was made of 1/10”
clear Plexiglas to provide a lightweight, sturdy design for the chassis that
can be easily CNC milled.
The power sources are connected
to the boards through a double-pole double-throw switches which allow a single
switch to operate two individual power supplies (digital and motor supply) simultaneously
and without interference. All the wire connections were electrically isolated
with heat-shrink tubing to help keep any failures in the software domain where
they can be easily debugged and resolved.
A two-wheel design was used to
eliminate the need for any dedicated steering component. This reduced cost,
complexity, and allowed for an independent height adjustment on the front of
the robot. This design also allowed the use of a Tamiya Double Gearbox motor with
left/right independent 4-speed functionality. Set with gear ratio 114.7:1, each
motor provides maximum of 115 rpm with a maximum torque of 1618 N.cm across
both motors. Different gear configuration provide flexibility in mechanical
adjustments of speed and torque. This in return allows the use of a cheaper
SN754410 H-bridge or equivalent motor driver instead of more expensive L298 driver.
It also eliminates the need for a heat sink on the Motor Control Board.
In conclusion, each individual
module of the project was able to perform as designed. The cause of most
failures, however, came from the unknown effects associated with connecting
working modules together (ie. connecting Main Control Board to trilateration
modules). Repeated hardware issues plagued the project and ranged from faulty
components, to broken wires, to conductive solder flux, to dead batteries.
The other major stumbling block with the project came
from the modularity of the design. While a modular design is a better practice
in theory, the overhead and complexity associated with module communication can
provide a substantial source of unnecessary error and uncertainty.
At the time of project completion,
the issues with module communication were resolved, and plans to move forward
with expansion and improvement are in discussion. Expanding on the features
currently built into the robot, improvements and advancements can be easily
added with a minimal amount of effort.