MECHENG 706 – Autonomous SLAM Robot
Hayden Ng Charles Bridger Mandeep Singh Dave D’sa
2
3
EXECUTIVE SUMMARY
This report details our solution to the SLAM problem with regards to an autonomous robot
designed to cover a given area while avoiding any obstacles in the shortest time possible. Given
a chassis attached with mecanum and a combination of sensors, we successfully implemented
tested the required tasks through a combination of iterative methods with our main philosophy
revolving around keeping the design as simple as possible. We executed our solution with the
robot avoiding the obstacles and covering a large area while at the same time producing an
accurate map.
Contents Contents .......................................................................................................................................... 1
Table of Figures ............................................................................................................................... 2
Table of Tables ................................................................................................................................ 2
1.0. Introduction ......................................................................................................................... 3
2.0. Problem Description & Specifications ................................................................................. 4
3.0. Hardware/Physical Design: .................................................................................................. 5
3.1. Pin Mapping ..................................................................................................................... 5
3.2. Sensor & Sonar layout: ..................................................................................................... 7
3.3. Sensors ............................................................................................................................. 8
3.4. Sonar Calibration .............................................................................................................. 9
3.5. Battery problems .............................................................................................................. 9
3.6. SONAR PLACEMENT ......................................................................................................... 9
3.7. SERVO MOTOR ............................................................................................................... 10
3.8. MECANUM Wheels ........................................................................................................ 10
3.9. BLUETOOTH AND MPU PLACEMENT: ............................................................................. 11
3.10. MPU SETUP & OPERATION ......................................................................................... 12
4.0. Systems Design & Testing .................................................................................................. 14
4.1. Fuzzy Logic ...................................................................................................................... 14
4.2. BANG BANG control ....................................................................................................... 16
4.3. Initialisation of Robot ..................................................................................................... 17
4.4. Planned path .................................................................................................................. 17
................................................................................................................................................... 19
4.5. Motion Control for Planned Path ................................................................................... 19
4.6. TURNING: ....................................................................................................................... 20
4.7. STRAIGHTENING: ............................................................................................................ 21
4.8. Detecting Obstacles ....................................................................................................... 22
4.9. Obstacle Avoidance ........................................................................................................ 23
4.10. equence of Control ..................................................................................................... 24
4.11. SETUP FUNCTION: ....................................................................................................... 24
4.12. MAIN LOOP: ................................................................................................................ 24
4.13. Mapping ...................................................................................................................... 25
4.13.1. Custom C# Program: ........................................................................................... 26
2
4.13.2. Developing Program: ........................................................................................... 27
4.13.3. Mapping Path: ..................................................................................................... 29
4.13.4. Mapping Obstacles .............................................................................................. 30
5.0. Final Demonstration .......................................................................................................... 31
5.1. Mapping ......................................................................................................................... 31
5.2. Result .............................................................................................................................. 31
6.0. Discussion and Future improvements ............................................................................... 33
7.0. Conclusion .......................................................................................................................... 34
8.0. Reference ........................................................................................................................... 35
Table of Figures Figure 1: Pin Mapping of Arduino Mega Sensor Shield .................................................................. 5
Figure 2: Sensor Configurations. a) left. b) middle c) right ............................................................. 7
Figure 3: IR Sensor testing results. .................................................................................................. 8
Figure 4: Exploded view of ultrasonic sensor casing .................................................................... 10
Figure 5: Mouting of a) Bluetooth module(left), b) MPU module (right) ..................................... 12
Figure 6: Fuzzy Logic membership functions ................................................................................ 16
Figure 7: Possible area covering methods. a) (Left) Outside-to-inside spiral, b) (right) zig-zag... 18
Figure 8: Final area covering method, with obstacle avoidance implementation ....................... 19
Figure 9: Obstacle avoidance. a)(left) obstacle triangle area, b) (middle) normal orientation,
c)(right) wall angle detection ........................................................................................................ 23
Figure 10: Map of path and obstacles producecd from final robot run. ...................................... 26
Figure 11: C# program developed for SLAM ................................................................................. 27
Figure 12: Data read and stored from robot ................................................................................ 28
Figure 13: Serial prints sent from robot via bluetooth, used for mapping................................... 29
Figure 14: Robot path captured by overhead camera from first run ........................................... 31
Figure 15: Robot path captured by overhead camera from second run, with overlaid colour key
....................................................................................................................................................... 33
Table of Tables Table 1: Fuzzy rule implementation ............................................................................................. 15
3
1.0. Introduction Simultaneous Localisation and Mapping (SLAM) involves the the tracking of an robot’s
movement, while continuously mapping an unknown environment. This is essentially a
‘chicken-and-egg’ problem, as to generate a map the position of the robot must be known, but
to know the position of the robot, a map must be referenced. It is an increasing useful
technology used alongside the rise of mobile robots in complex environments such as driverless
vehicles. Another example of autonomous robots is the market of household vacuum cleaning
robots which operate by moving around rooms without colliding with any objects, such as the
Roomba.
This report covers how we approached this open-ended problem, and designed a robot to
perform as a “room vacuum cleaner”. The robot is designed and programmed to cover the
entire area of an arena, in the fastest time possible, while avoiding obstacles and implementing
simultaneous localisation and mapping. The following pages of this report detail the specific
tasks required throughout the iterative process for a functioning robot. A discussion of the
performance is also undertaken, analysing the strengths and weaknesses of the final robot.
4
2.0. Problem Description & Specifications While this assignment presented a less-defined project brief, there were still certain crucial
requirements that needed to be met:
- Mobile and autonomous: As the robot chassis received already achieved mobility, our
focus meant a robot running without human intervention once it is started.
- Fastest coverage: The robot should be capable of starting from anywhere in the arena
and be able to decide how to begin its path, and achieving the complete coverage of the
“room” in the shortest time.
- Obstacle avoidance: There were a number of cylindrical objects randomly placed on the
platform which the robot must navigate around without collision. Collision with the
walls is also not permitted.
- Localisation and mapping: A map of the paths the robot will have travelled actually
along, is recorded by the robot, in order to show the complete coverage of the “room”,
which is the arena in this case. It also needs to build the map of the “room”, by
outlining the boundary walls of the arena as well mapping where all the obstacles are
within the arena.
In order to meet the target requirements, a variety of technologies and platforms were
required, integrating the electronics (sensors, actuators, microcontroller, logic) with the
mechanical setup.
The project demands that all components are either supplied by, or designed within the
University of Auckland’s Mechanical Department. The following are hardware components
received at the start of the project :
1. Partially assembled robot chassis
2. Four VEX motors.
3. Four mecanum wheels, each driven by one motor.
4. Arduino Mega 2560 microcontroller board, with a Sensor Shield.
5. Rechargeable Lithium-ion Polymer (LiPo) battery
6. One small servo motor.
7. Two 2Y0A21 Medium Range IR Sensors
8. Two 2Y0A02 Long Range IR Sensors
9. One HC-SR04 Ultrasonic Sensor Module
10. One SparkFun 9 Degree Of Freedom MPU-9150 (Motion Processing Unit)
11. One HC-05 Bluetooth Module, and a Bluetooth Dongle
5
3.0. Hardware/Physical Design: 3.1. Pin Mapping
Using the Arduino Mega Sensor Shield that was provided with the robot, the pins were used as
follows, with the ground and power pins being used as needed for each component , seen in
Figure 1 above:
Figure 1: Pin Mapping of Arduino Mega Sensor Shield
6
Motor Connections:
Pin 46 - Left Front Motor
Pin 47 - Left Rear Motor
Pin 48 - Right Rear Motor
Pin 49 - Right Front Motor
Sensors:
Pin A4 - Medium range IR sensor facing forward, on the left of the robot
Pin A5 - Medium range IR sensor facing forward, on the right of the robot
Pin A6 - Long range IR sensor facing right, on the back of the robot
Pin A7 - Long range IR sensor facing right, on the front of the robot
Pin 44 - Ultrasonic trigger pin (used for the ultrasonic sensor)
Pin 45 - Ultrasonic echo pin (used for the ultrasonic sensor)
BlueTooth Pins:
Pin 18 - TX1 - Transfer pin for the bluetooth module
Pin 19 - RX1 - Receive pin for the bluetooth module
MPU Pins:
Bluetooth pin - provides 3.3V necessary for the MPU-9150 module
Pin 20 - SDA - Serial data line needed for the MPU’s I2C communication with the Arduino
Pin 21 - SCL - Serial clock line also needed for I2C communication with the Arduino
7
3.2. Sensor & Sonar layout: 1. Seen in Figure 2a
The initial layout chosen for our robot was designed so that two sensors were always
facing the wall that the car is following. They were also used for obstacle detection on
the right hand side of the car. The long range sensor at the back of the car also meant
that when the car had completely passed an obstacle, could move back into it’s original
position. This meant that one long range sensor and the sonar sensor was facing
forwards so that it could detect any objects obstructing the robot’s path.
The left sensor was used for detection of any obstacles, as well as detecting when the
robot strayed too far from the wall allowing it to realign with the wall.
2. Seen in Figure 2b
The layout was changed to two medium sensors facing forwards to create a three point
collision and obstacle detection system. This made our obstacle detection a lot more
accurate and consistent. The long range sensor was moved to face the left to serve the
same purpose as the left facing medium range sensor in the previous layout, and the
right facing long range sensor remained in the same place to detect when an obstacle is
passed.
3. Seen in Figure 2c
After further testing, it was determined that one sensor was ineffective at keeping the
car aligned to the wall, and that the left sensor was being ineffectively and underused so
it was repositioned for our final design. The long range left facing sensor was moved to
face right, which meant that the two sensors facing right were used to realign the robot
and give a more accurate reading of the robots position relative to the wall. The long
Figure 2: Sensor Configurations. a) left. b) middle c) right
8
range sensors were both shifted away from the right edge to the left to eliminate the
‘dead zone’ that exists when measuring a distance smaller than 20cm.
3.3. Sensors Initial calibration of the sensors, seen in Figure 3, was needed due to the raw readings from the
sensors being inaccurate. Calibration was carried out by measuring the output from the sensors
at set distances from a wall. Once these readings were recorded, they were plotted against
their ideal values, giving an equation of the relationship between the two. Using this equation
in our code, we were able to calibrate the sensors to give accurate readings based on the
distance from an object. Re-calibration was required because the sensors were very sensitive to
movement, and would change if the sensors were altered by the smallest angle. This also
meant that every time the sensor layout changed, re-calibration was also required. These
graphs below show the resulting calibration equation with all the data plotted. The distances
were chosen inside of ‘dead zones’ of the sensors, were readings were very unstable and
inconsistent.
Figure 3: IR Sensor testing results.
9
3.4. Sonar Calibration The sonar sensor provided was an HC-R04 model which provided very accurate distance
sensing. One major change that was made was creating a limit on the time the sensor waited
when trying to retrieve data. Although the sonar sensor could provide a range of 0-6 meters,
the maximum needed for this project was 2 meters. Hence, the duration of the time allowed for
the sensor (high pulse) was capped off at 10,000 microseconds and this would allow it to obtain
a distance of 1.7 meters if needed. By speeding up this part, the whole process ran much more
efficiently.
𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 (𝑚) = 𝑠𝑝𝑒𝑒𝑑 𝑜𝑓 𝑠𝑜𝑢𝑛𝑑 𝑥 𝑡𝑖𝑚𝑒
= 340 𝑥 10,000 𝑢 /2
= 1.7m
This value obtained was more than sufficient.
3.5. Battery problems The battery of the robot did actually affect the overall performance of the robot, since it would
turn too much or too little if it had a full or low charge. This did not prove a problem during the
final demonstration though, as the code was adapted to not be affected as significantly by the
battery voltage, as the MPU headings were not affected by the battery voltage.
3.6. Sonar placement The sonar sensor was mounted in front of the robot as it provided the most accurate and
consistent readings out of all the other position sensors. A laser cut case was design so that the
sonar sensor were mounted safely and securely. CREO was used in the development of this
case to accurately model the design so that the mounting holes on the vex robot lined up
perfectly with the case. An exploded view of the case with the sonar sensor shown in Figure 4.
Two plates; front and back were mounted on both sides of the sensor to fasten it securely. The
front case had fittings which allowed the sonar sensor to fit well. A drawing of this case is
provided in the Appendices.
10
3.7. SERVO MOTOR While in the design phase, the servo motor was considered as a possible feature that could
benefit the robot. The servo was considered as a means for actuating the sonar sensor, so that
it could rotate and be used to detect obstacles based on that.
However, a simpler approach was decided upon, which did not use the servo motor at all,
simply mounting the sonar directly to the front of the robot, where the robot's movement and
sensor layout would be used to determine obstacles and walls.
3.8. MECANUM Wheels Mecanum wheels were an important feature of the four
wheeled robot. They provided a wide variety of
movement due to their design, where individual rollers
are attached to a wheel hub. A certain combination of
four of these wheels placed in ‘pairs’ in terms of their
orientation, as seen in Figure 5, allow the robot motion in
multiple degrees of freedom.
Figure 4: Exploded view of ultrasonic sensor casing
1 2
3
FRONT
4
Vx
Vy
ωz
Figure 5: Mecanum Wheel setup
11
By turning specific wheels in certain directions, it was possible to move the car in many
directions, such as forwards, backwards, strafing left, strafing right, and turning clockwise or
anticlockwise. This was due to the rollers on each wheel being at an angle to the car, as seen in
the force reactions in the above figure.
In this project, through the use of forward kinematic equations for mecanum wheeled robot's,
it was possible to derive the movement of the robot, from the angular velocity of the four
motors. This was done using the following equation:
(1)
(2)
(3)
Where Rw = 2.75cm (the radius of the wheel hub),
L = 7.5cm (length from the centre of the wheel to the middle of the robot's side length),
l = 8.75cm (length from the centre of the wheel's width to the centre of the robot's width).
Therefore, as the different angular velocities can be known through testing, it was possible to
determine the movement of the robot using equations 1, 2 and 3 above.
3.9. BLUETOOTH AND MPU PLACEMENT: The Bluetooth module was placed on the side of the robot using double sided tape to attach it
directly to one of the IR sensor mounts. This kept it at a reasonable height, away from any
possible interference from the other components of the robot.
Similarly, for the MPU, it was important to have it placed in such a manner that it would not be
affected by any interference from the other electrical components, such as the motors or the
microcontroller board. Additionally, it had to be placed in a location where it would be less
exposed to the harsh vibrations of the robot, and the rough accelerations of the motors. This
meant the MPU needed to be placed in a higher, more stable area, in order for it to be able to
receive and compute reliable data. The MPU was attached to the top of the mounting between
the two front facing medium range IR sensors, with double sided tape. This resulted in a secure
12
connection between the MPU and the robot, as will be discussed later in the report. The
connections of both the Bluetooth module and the MPU can be seen below in Figure 6.
3.10. MPU SETUP & OPERATION The MPU 9150 sensor was used to provide information about the heading of the robot
whenever it was polled. With this data available, the robot could be turned relatively
accurately, within +- 2 degrees of a specified angle. MPU9150 contains 3 axis of gyroscope, 3
axis of magnetometer and 3 axis of accelerometer. The gyroscope provided the angular rate of
change of the robot, while the magnetometer provided a method to measure strength of
magnetic field in each axis and the accelerometer provided the rate change of velocity. The
onboard mpu has a Digital Motion Processing Unit (DPM) that allows data fusion and it can be
used as a tool to minimize power and and increase processing speed. Its main communication
with the Arduino Mega is over the I2C bus.
For setting up the MPU and its registers two main library created by Jeff Rowberg [3] was used;
“MPU6050” and MPU6050_6Axis_MotionApps20. The main codes were kept as it was but a few
changes were made to it. An interrupt service routine which used INT0 pin from the MPU to the
arduino mega interrupt pin was removed as it caused too many errors and delayed the time of
the loop execution. There were massive wait times of >7 seconds involved which caused severe
cascading effects to the rest of the programs execution. Rather than using an interrupt
whenever the MPU had a new reading, the MPU was constantly polled . The polling process
checked whether the First In First Out(FIFO) count was more then the packet size. A FIFO count
register keeped track of the number of bytes in the FIFO buffer. The packet size referred to the
Figure 6: Mouting of a) Bluetooth module(left), b) MPU module (right)
13
needed amount of data for the DMP to execute; in this case it was 42 bytes. If true, sensor data
was read from the sensor into the FIFO buffer. The FIFO buffer acted as a temporary storage
register where it read data from the sensor and output it to the user. The DMP obtained raw
data from the gyroscope, accelerometer and magnetometer and converted the readings to roll
pitch and yaw angles. The DMP then fuses the accelerometer and the gyroscope data to obtain
accurate output. The gyroscope sampling rate was set to 8000 Hz and the gyroscope full scale
range was set to +-2000 ‘/second.
Another problem that was encountered was that while polling the the mpu for data, overflow
of the FIFO buffer occurred. While polling, if the FIFO rate (rate of obtaining new values from
sensor) was higher than the host processing rate, it caused an overflow which then caused
incorrect yaw headings to be generated, causing the robot to execute incorrect turns. To solve
this issue three solutions were implemented. First, the FIFO buffer was reset when the FIFO
count reached 1024; the maximum size of FIFO buffer. Next, the FIFO rate was decreased to
66.67 Hz and finally MPU was polled more often.
Calibration was also required for the MPU’s DMP in order to decrease the stabilising time of the
DMP’s yaw output. One way of doing this was by putting the MPU flat and on a horizontal
surface then modify the RAW output program to so that the output of the gyro in the Z axis was
equal to “0” and acceleration in the Z axis was equal to “+16384” [3]. This could be done by
using commands such as “setZGyroOffset” and “setZAccelOffset” . With testing the offsets
obtained for the MPU were:
ZAccelOffset= 1688
ZGyroOfset= -7.9
With these offset, the MPU stabilised much faster than normal and eliminated yaw drift.
The DMP’s yaw output varied from -180 to +180 which was incompatible with the rest of our
code. Simple modular arithmetic was implemented to convert the output range from 0-359.
The final thing to consider was the placement of the MPU on the robot. To get accurate
readings from the MPU, it was placed away from the motors to avoid electromagnetic
interference, elevated above and in the middle of the robot.
14
4.0. Systems Design & Testing
This next section of the report outlines the approach used for each main problem within this
project. Discussed here are the concepts and ideas developed in order to implement the entire
system envisioned by the team. Project requirements were broken down into independent
components which would then be tested and integrated together to provide the final system.
4.1. Fuzzy Logic A Fuzzy logic controller was initially implemented for obstacle avoidance and motion control
but due to the unreliable values given by the IR sensors and the difficulty in effectively
modifying the fuzzy controller to give desirable outputs, it ended up being removed. The
implemented fuzzy logic controller lacked precision and was difficult to debug when any
problems were encountered due to the large amount of variables that the errors could be
attributed to. Described below are the fuzzy sets, membership function and fuzzy rule that
were involved in this controller.
Three sets of membership functions were constructed and it consisted of sensor readings
(input), speed (output), and direction/heading (output). Five sensors were used to provide the
distance of an object from the robot; two long range IR sensors, two short range IR sensor and
one sonar sensor. The membership function for distance for each sensor were divided into
three fuzzy sets; short, medium and far. A graph of this is shown in Figure 7. With each sensor
having a different sensor range, the sensor values were normalized before integrating them
with the graphs. The code for this is displayed in Appendices. The fuzzy logic controller utilises
sensor configuration 3.
With each sensor outputting a fuzzy value, these values were then feed into a speed and
direction membership function. Fuzzy rules are displayed in Table 1. The strategy implemented
here was to avoid obstacles at a safe distance while maintaining an optimum robot speed. A
safe factor scale of two was taken into account when deciding the obstacles distance as the
sensors reading tended to fluctuate. The fuzzy controller also made sure that the robot will turn
left the majority of time as the planned path was an anticlockwise spiral unless it specified to
certain conditions.
Speed defuzzification were divided into three fuzzy sets, slow, medium and fast , as shown in
Figure 7, with the output speed being normalised. Maximum of minimum fuzzy rule is applied
on the output of the distance fuzzification. This is followed with the center of area calculation
for speed defuzzification. Direction defuzzification were also obtained through the same
method as speed defuzzification. However, directional outputs were divided into five sets;
extreme left (0) , extreme right (4) , angled right (3) , angled left (1) and stay on course (1). The
15
maximum of extreme left and extreme right degrees were capped at +- 90. Upon running the
robot with this controller, it was seen that the robot’s performance in avoiding obstacles and
following the track was inconsistent. Also, the fuzzy control did not have the ability to
differentiate between a wall and an obstacle. A separate section would be needed to handle
this.
Rules Right (Long IR)
Right (Long IR)
Forward Middle (Sonar)
Forward Left (Short IR)
Forward Right (Short IR)
Speed Direction
A Far Far Far Far Far Fast 2
B Medium Far Far Far Far Fast 1
C Far Medium Far Far Far Fast 1
D Near Far Far Far Far Slow 0
E Far Near Far Far Far Slow 0
F Far Far Medium Medium Medium Slow 0
G Far Far Near Near Near Slow 1
H Far Far Far Near Far Medium 4
I Near Near Far Far Medium Medium 1
J Near Far Near Medium Far medium 3
K Medium medium Far Far Far Fast 2
L Near Far Far Medium Medium Fast 2
M Far Far Medium Near Far Medium 3
N Near Far Near Far Far Slow 4
Table 1: Fuzzy rule implementation
16
Figure 7: Fuzzy Logic membership functions
4.2. BANG BANG control After discarding the fuzzy controller, using a bang bang controller was attempted due to its
simplicity and speed to design, implement and test. The Bang-Bang controller for speed and
direction only had two outputs. For speed it was either Moving at a constant speed or stopped
and for turning it was either turning at constant speed or stopped. There was no significant
overshoot present in the speed bang bang controller and for the turning bang bang controller
overshoot was accounted for by adding a realigning function. The realigning function was also
just a bang bang controller which took the wall facing sensors as inputs rather than the MPU
and had a smaller constant turning speed. It checked if the robot was aligned parallel to the
wall, and if not, rotated the robot at a constant slower speed until it was aligned. The
implementation of these three controllers were deemed successful as they were easy to
implement and tweak as well as generating the results desired.
17
4.3. Initialisation of Robot
As it was not certain if control of where the robot was placed for the demonstration run, an
initialisation state was implemented. This function was to simply find the robot a corner from
where it can begin the main area covering function.
This initialisation, called ‘FindCorner()’ , followed a structure as this:
- First the robot would travel to the closest wall, by comparing the data in front of it with
the data from the right facing sensors.
- If the wall to the right was closer, it would turn clockwise towards it and go forward. If
the wall in front was closer, it would go forward, until it reaches the wall.
- The robot would then turn anticlockwise and go forward again.
- When the robot reached a wall once more, this would place the robot in a corner, facing
into the corner.
- Therefore, the final step of this function would be to turn the robot anticlockwise again,
to have the robot in an orientation that is ready to begin its main path.
This code was implemented in a similar manner to the main path covering function, such as not
getting too close to a wall, and straightening up after a turn. Both these features are discussed
in the Motion Control for Planned Path segment of this report.
4.4. Planned path
As it was known the basic size and shape of the area in which the robot would be required to
traverse, it was possible to develop methods for covering the area. There were three main
considerations: a zig-zag pattern, an outside-to-inside spiral and an inside-to-outside spiral.
The inside-to-outside spiral was quickly removed from consideration, as it would be difficult for
the robot to find the centre of the arena, and the zig-zag pattern would require accurate sensor
data as it would be travelling across areas of the arena where it would be getting limited sensor
data, due to its distance from the walls. For a zig-zag, it would also be more challenging to make
the robot move a small distance with limited sensor data, which is an important part for that
method, during its ‘zag’ step. The two paths are shown below in Figure 8.
18
Therefore, the final planned path decided upon was an anticlockwise outside-to-inside spiral
and was chosen over a clockwise spiral purely based on the current layout of the sensors. An
anticlockwise spiral path was chosen on over a zig-zag path based on our sensor setup, after
comparing the two pathways, as this implementation provided constant sensor data that could
be used to stabilise the robot and more importantly, align it. With the spiral path, the idea was
to follow a wall at a set distance for the entire route, whereas a zig zag would have had to
continually run consistent parallel lines along the arena.
Based on the sensor setup, and the anticlockwise spiral path, below is a representation of how
the robot would approach and complete covering the arena. As the sensors were aimed in front
and to the right, if an obstacle was detected, the only possible avoidance method was to move
the robot to the left and drive around the obstacle from the left.
Figure 8: Possible area covering methods. a) (Left) Outside-to-inside spiral, b) (right) zig-zag
19
4.5. Motion Control for Planned Path Following finding a corner and initialising the robot, the robot needed to then begin its main
path procedure. This was designed and initially implemented as a system on its own, with a
focus only on completing the entire area of the arena. After measuring the dimensions of the
arena, the planned spiral path was broken down into three different ‘loops’: the outside loop,
the middle loop, and the final loop.
The outside loop was the circular section in which the robot followed the walls of the arena,
and covered that area. The middle loop was the next circular section, one step inside from the
walls, and the final loop, would be the last circular section that the robot would need to cover.
However, after initial testing, it was determined that the final loop was not exactly a full loop,
and instead half a loop, as that was all that was necessary for the robot to have covered the
entire area. At the end of each loop, the robot would prepare for the next section, by turning
anticlockwise to an orientation from which the next loop could begin.
The algorithm implemented for the robot to complete the full path, was a switch case
statement used for the robot to traverse a loop, and reuse the similar code structure for the
next few loops. The basic concept for the code was as follows, as can be seen in the
Appendices:
- If the robot is within an acceptable range from the wall and there is no obstacle in front,
it will travel forward.
Figure 9: Final area covering method, with obstacle avoidance implementation
20
- If the robot is not within an acceptable range from the wall it will adjust its position. If it
is too close to the wall, it will strafe left, and if it is too far from the wall, it will strafe
right.
- If the robot is within an acceptable range from the wall but there is a wall in front of it, it
will stop, and set a flag which will result in the robot turning the next time the code is
run.
- Therefore, if the robot is in a corner, it will turn anticlockwise. It will then reset its
turning flag, and increment a counter, which counts the number of corners that have
been encountered.
- Therefore, this similar flow is repeated, as all the robot does is travel straight, then turn.
- Once the robot has reached 4 corners, it will stop a specific distance from the wall, from
where it can straight away begin the next loop of the full path. From here, it turns
anticlockwise, and switches state into the next loop.
- The middle loop follows the same process, except the tolerances for the wall distances
to the side and to the front would be increased to cater to the robot being further from
the wall.
- Finally, when the robot finishes its middle loop and initialises itself for the final loop, the
final loop only has two straight line segments, with a turn separating the two.
Integrating this algorithm with the rest of the system required a constant iterative process, and
testing resulted in a series of improvements and adjustments being made in order to meet the
requirements. Two important features and implementations that required significant changes
for the path control were turning the robot, and straightening the robot.
4.6. TURNING: Initially, the robot simply relied on its sensors to turn. It would begin turning when the front
sensors detected a wall, and stop when the side sensors detected the robot to be with a
suitable distance from the wall. This method was intended to align the robot with the wall,
using the sensors during the turn. However, due to the synchronous sequential process of
implementing, sometimes by the time the sensors detected the wall being within the desired
range, there would be overshoot, and sometimes undershoot, as the angle of the walls in the
corner created problems for accurate sensor readings. Additionally, when the robot was further
from the walls, such as during the middle and final loop, due to the unreliability of the sensors
at greater distances, sometimes the robot would not stop in its desired position.
Therefore, the MPU was focused on to be used for turning purposes, instead of using only
sensor data. Instead of turning and stopping based on sensor data, the robot would turn until
the MPU gyro heading had reached its desired heading position, which was negative 90
21
degrees, for an anticlockwise turn. After extensive testing, it was clear that the MPU only
method was significantly more reliable than using sensors, and the robot turned and stopped in
better positions. However, often due to the initial stopped position at the wall, it became clear
that there needed to be a function for the robot to ‘straighten’ itself after a turn, in order to
realign it with the wall to its side. This was reinforced by the fact that the robot had relied on
the ideal assumption that it would turn perfectly and be aligned ideally with the wall, which was
an unrealistic target. A ‘straighten’ function was developed in order to solve this problem. It is
also important to note that except for one possible clockwise turn in the initialise function, all
turns undertaken were anticlockwise, and were only ever programmed to turn 90 degrees with
the MPU. The code for these turning algorithms can be seen in the Appendices.
4.7. STRAIGHTENING: As a solution to the robot travelling alongside the wall at an angle, and therefore having to
strafe often in order not to collide with the wall, a straightening algorithm was integrated into
the process flow of the code. The straightening function is always called after a turn, and
depending on which loop the robot is in, it will straighten the robot to the wall with a tolerance
for the distance from the wall that it is set from.
After a turn, the integer variable ‘s’ is set to a number (1,2 or 3) depending on which loop the
robot is in. When the program goes through the main loop, it will call the straighten function.
The function then uses calibrated distances from the wall which the robot should be within,
based on test results on the arena. The robot will then use its right-facing long range sensors to
determine if it is within a tolerance that is straight enough for the robot to move forward
without straying from its path. Once it has straightened, it will next strafe to a distance from the
wall that is suitable for that specific loop. After this is done, the straightening flag variable is set
to 0, therefore the straightening code will not be run again until the robot has turned again.
Please refer to Appendices for the Arduino code.
The addition of straightening and turning based on the MPU helped make the robot run more
efficiently, accurately and consistently. In testing, it was confirmed that these added functions
made the path control system work better, and the robot completed the outside loop and the
middle loop repeatedly, turning and straightening well. However, while testing, the robot
struggled to successfully enter the final loop. This was determined to be due to the
inconsistencies from the sensor data at longer distances.
As the final loop consisted of just two segments, two straight lines, it was decided to complete
the final loop without checking whether the robot was at an acceptable distance away from the
side walls, and relying solely on the from sensors and the MPU. One method was to go straight
22
until the robot was less than 50cm away from the wall, and to turn anticlockwise without any
straightening, and drive straight using the heading from the MPU to steer the robot straight,
until it was also 50cm away from the wall in front. This would bring the robot to a complete
end, having covered the entire area of the arena. However there were errors when
implementing the robot to drive straight based only on the MPU heading, and therefore the
robot was then programmed to simply drive straight only checking the front sensors for the
final loop. This provided successful results, where the robot would complete both segments of
the final loop and stop at a good distance. Once the robot had completed all loops, a flag would
be set, which meant that neither the obstacle avoidance function nor the path control function
would be called again, and therefore the robot would remain stopped until it was restarted.
4.8. Detecting Obstacles Obstacles were detected using a triangular area calculation involving the three front facing
sensors (sonar and two medium range sensors). It collected these three values and executes
the “Get triangle area” function. The function calculates the area of a given triangle using the
Heron’s formula , which takes the lengths of a triangle. As the distance was known between
each of the sensors placed on the robot, and the sensors output the distance that they could
detect anything at, the distances between each point could be calculated. If each point landed
on a wall or empty space there would be no area within the three points otherwise there would
be with an obstacle in the way, seen in Figure 10. Due to inaccuracies in the sensors, we used a
value of 42cm^2 rather than zero, to determine if an obstacle was present. If the area was
below this value, the robot carries on with its current function. 42 was the area from
continuous testing of our robot up until the demonstration.
One of the main problems encountered with this function was that when the robot is on an
angle to the wall, instead of the sensors all returning the distance to the wall, giving an area of
the triangle to be zero, the sonar sensor returned a much larger distance due to the way it
functions, by sending out a signal and waiting for it to bounce back, using the speed of sound to
calculate the distance the object is a way. The angle reflects the sonars signal away from the
receiver increasing the time it takes to return, increasing the distance, causing the three points
to lie in a triangular shape, artificially increasing the given area. To fix this problem, a wall angle
function was called, which uses simple trigonometry to calculate the angle the robot makes
with any wall. If this angle was above 20 degrees (0 is facing the wall, 90 is parallel to the wall)
then we’ve counted that as a wall and break out of the loop to then straighten the robot with
the wall.
23
4.9. Obstacle Avoidance If an obstacle was detected while the robot was driving its path, it stored the current distances
of the long range sensors, facing the wall (savedSLF, savedSLB1, savedSLB2). Next the robot
entered the first state of obstacle avoidance.
The first stage checked whether the robot was still detecting an object, and if it was it proceeds
to strafe left to move around the obstacle. This constantly checked the triangle area, and once
the object had left its field of view (triangle area decreases below 42), the new distance from
the wall was stored. The next stage in obstacle avoidance was stage three.
The second stage analysed the back long range sensor reading facing the edge of the arena
comparing the data to the previously saved data. When it detects the object was in its path (i.e.
the reading is smaller than the previously saved data) it saved the side distance to the object
and moved onto the third stage of obstacle avoidance.
In the third stage, the robot continued to move forward, while checking the current long range
sensor reading with the variable saved from the previous state (savedSLB2). When the robot
passes the object (the back sensor reading a larger value than the previously saved value) the
robot entered the the fourth and final stage of obstacle avoidance.
Figure 10: Obstacle avoidance. a)(left) obstacle triangle area, b) (middle) normal orientation, c)(right) wall angle detection
24
In the fourth stage, the robot started to strafe right to return to its original path. This is where
the readings saved in the first stage are used. When the current sensor readings equal to, or fall
below the saved readings, the robot exited the fourth stage and re-entered the main loop to
continue along its path.
It is also important to note that in every stage of avoidance, the robot checked for a wall and if
one was present, it broke out of the avoidance function to realign with the wall ahead of it,
updating at which stage of the path it was in. Every state also checked the current triangle area,
and if it was above 42, then it re-entered obstacle avoidance from the first stage to strafe left
around the newly detected obstacle.
4.10. Sequence of Control The control sequence used in implementing all the above control systems makes use of the
Arduino’s loop() function which consecutively runs the code within that function. As
mentioned, while integrating the different types of code written, the best method of having it
work together was to use the individual functions for each task, and have them get called from
the main loop when they were needed. As the Arduino Mega operates at a clock speed of
16MHz, this was suitable for use in this project, as all tasks were able to be completed within
their time constraints so that the system was successful, without any critical failures. After the
Arduino is turned on and the setup function runs, this is the synchronous control sequence (as
is displayed in Appendices, that was executed:
4.11. SETUP FUNCTION: The setup function only runs once, and set variables as well as loading important data, such as
setting pins, and defining values. Its more significant features are:
1. Setting and opening the Serial port, with a baud rate of 115200.
2. The next thing in the loop is that the MPU is set up. This includes setting the offsets for
the gyro and the accelerometer, and waiting for the MPU to stabilise before beginning
the main program. Once this is completed, the Arduino runs the main loop.
4.12. MAIN LOOP: 1. If the robot has not finished covering the map, the function to check for obstacles is
called. Sensor data isn’t read first, because the obstacle checking function has its own
calls to read the data from the sensors. Depending on whether there is an obstacle or
not, it will run further obstacle related code, or it will return back to the main loop.
25
2. If there is no obstacle detected, the sensor data is acquired again, by running the
GetReadings() function. As detailed previously, this code simply reads the data from the IR
sensors and averages the set of readings to provide a more accurate reading.
3. The robot then checks to see if it needs to straighten itself to align well with the wall. This is
only done after a turn, and therefore if the robot has turned prior to the microcontroller
returning to the main loop, a ‘flag’ (in this case an integer variable ‘s’) will be set to 1 or 2,
depending on what distance it is set to align from the wall. If there is no need to straighten, it
will not call the straighten function.
4. Finally, the last sequence of the main loop, is what for controlling the main motion and area
covering for the robot, only if it doesn’t need to straighten. If the robot has just been turned on,
it will be set up to initialise the robot to a corner. This calls the FindCorner() function which will
find one of the closer corners for the robot to align itself with. Once this initialisation sequence
is complete, it will then run the main pathPlanning() function, which governs the robot's motion
around the arena, the outside to inside spiral. The logic for these uses if statements and
variables which are used as flags. If the robot has reached the end of the path and covered the
entire area of the arena, it will set a flag which will effectively keep the robot permanently
stopped.
This is the sequence that the robot used in order to control all its motion and sensor functions,
in order to cover the entire map accurately and without intervention or collision.
4.13. Mapping
The map of the Arena, robot's path within the Arena and the objects placed in the arena were
generated by looking at the data the sensors were producing as well as the state that the robot
was currently in. Assumptions made in mapping was that the map would be situated within a
rectangular arena with all four walls present. The data was then sent via bluetooth to a
computer running our custom C# program to draw the map in realtime. Using our C# program
we were also able to identify bugs in our Arduino Code as it reported what the robot was doing
at any given instant.
26
4.13.1. Custom C# Program:
The custom C# Program, seen in Figure 12, contained a UI for displaying the sensor values, the
state the robot was in, the serial data stream as well as the map that was drawn live in front of
the user. The controls available to the user were the connect button, for connecting to the
robot, a disconnect button, a load data button, for loading previously recorded data, record and
live radio buttons for switching between recording data and displaying data as it was read, a
save image button, for saving the current map drawn as a png file, a clear data button for
refreshing the program to a clean state and an exit button. The program required a port and
baud rate to be entered by the user before connecting to the robot via bluetooth or cable. The
C# program ran a separate thread for reading in data over bluetooth or cable asynchronously,
the code for the background thread can be seen in Appendices, while updating the UI
synchronously with the processed data, seen in Appendices. A detailed picture of the program
with further explanation of the GUI can be seen in Appendices.
Figure 11: Map of path and obstacles producecd from final robot run.
27
Figure 12: C# program developed for SLAM
4.13.2. Developing Program:
To develop the program a combination of mock data as well as real time tests were used to
develop, tweak and test the algorithms it uses. As well as being able to map the course the
robot took, it was also extremely useful for identifying bugs in the Arduino code, as the
program would show whenever the robot was stuck in any of the available states as well as
show whenever the sensors were reported incorrect data. Because the program could record
any test run, a library of test runs were generated for analysis of our robots procedures, an
example of the data generated can be seen in figure 13 with the live data from our first run
seen in figure 14. This was more effective using the provided serial monitors to detect any
errors , since the serial monitors couldn’t format or save the incoming live data, decoding what
was happening at any given instant was extremely difficult and time consuming.
28
Figure 13: Data read and stored from robot
29
Figure 14: Serial prints sent from robot via bluetooth, used for mapping
4.13.3. Mapping Path:
To solve the SLAM problem a combination of assumptions and methods were used. As it was
known that the arena was going to be contained within four walls, they were able to be used as
a reference for the pathing when combined with the dead reckoning method. The dead
30
reckoning method involves calculating the current position from a previously known position.
Dead reckoning accumulates errors, as any error in determining the known position is added to
determining the next position, but when combined with current sensor data and the knowledge
that the walls are straight with 90 degree angles at each corner, accumulated error can be
effectively reduced if not eliminated. Because the speed of the robot was held constant while it
was driving, the program was able to determine where it was with reference to the walls.
Whenever the robot was moving forward, the current state would be transmitted to the
program letting it know to move the representation of the robot forward at the same rate as
the robot. To map the walls the outer sensor data was plotted against the path. As the robot
constantly readjusted to keep the wall within ~10cm of its side position on its outer loop the
walls were able to be accurately plotted as seen in Figure 11. The current angle of the robot
was constantly updated by the MPU, the robot was only ever required to change its direction
by 90 degrees and after it had completed a turn, used the side sensors to line itself up with the
wall adjacent to it to eliminate any error in the MPU data. This ensured a complete 90 degree
turn as it was known that every wall was perpendicular to the next wall, with this knowledge
the pathing knew that any time the robot turned, it would be a 90 degree counter-clockwise
turn. The only exception to this rule was in the final inner loop that the robot ran. As in the
inner loop, the side sensors were too far from the wall to give accurate readings, it purely relied
on the MPU to tell it when a 90 degree turn had been made which is where one of the major
differences can be seen in the map the C# program came up with, and the map that the Camera
produced. Seen in figure X it can be seen that the robot drove at an angle to the walls rather
than parallel, as the robot turned more than 90 degrees due to drift in the MPU. This isn't seen
in the C# program as it assumed a perfect 90 degree turn would be made. The same
assumptions are also made regarding strafing of the robot. When the robot strafes it changes
its direction of movement to be perpendicular to its forward movement, any slippage in the
mecanum wheels are not accounted for, nor are any difference in the motors powers
accounted for, which means that errors would accumulate over time in the mapping. As the
robot is constantly looking for reference points and constantly realigning to these reference
points this eliminates the error in real time. When the states and data is sent to the mapping
program, any accumulated error is already reduced, creating an accurate map.
4.13.4. Mapping Obstacles
The obstacles were mapped whenever the robot went into the obstacle avoidance mode, as
soon it went into obstacle avoidance mode it would send this data to the computer and the
program would mark where the object was detected with a blue circle. Although the robot
would check for obstacles in its initial state, where it looks for the first corner to start in, the
program wouldn’t map any obstacles found in this mode. It assumes that the robot would pick
up those objects again running one of the loops, outer, middle or inner for mapping later.
31
Figure 15: Robot path captured by overhead camera from first run
5.0. Final Demonstration 5.1. Mapping
Comparing the final map provided against the map that the C# program generated, it can be
seen that they are very similar. The main difference between the two was that the map
provided had distortion due to the fisheye lens on the camera. Another difference was that on
the final loop, due to the robot not being able to check its distance from the walls, it drove at
an angle to the walls. The provided map accurately captured this path, whereas the generated
map wasn’t able to represent this error. Finally, the distances covered weren’t as accurate in
our generated map due to the dead reckoning method being used alongside the assumption
that the robot moves exactly the same distance each time segment. In reality it doesn’t due to
slippage and inconsistencies between the motors and wheels.
5.2. Result For the demonstration, the robot's performance was up to standard, by covering the arena
both times and doing it safely. As our design specification indicated that there should be no
collisions with either the walls or the obstacles, these final runs used the calibrated tolerances
which ensured that the robot would avoid the walls, and attempt to avoid the obstacles. Both
32
runs had the robot start in the middle of the arena, at an angle, facing one of the longer side-
walls of the arena:
Run 1: The first run saw the robot traverse across the arena in its desired path structure. The
logic and control algorithms kept the robot at a constant pace, while ensuring care of its
surroundings. The robot initialised itself to the closest corner, and then begin its outer loop. In
this outer loop , it avoided the first obstacle in its path, and proceeded on its way. However,
after the third corner, the wires connected to one of its sensors, brushed against one of the
obstacles as it drove past, which was counted as a collision. After this incident, the robot
carried on through the rest of the arena without any problems, completing the map in a time of
1:40. After reviewing the coverage, it was noted that the robot finished its inner loop early, as
can be seen by comparing the maps of the two runs. This was a pleasing result for a first run, as
it completed the map with no collisions with the wall, nor any real collision with the obstacle.
The map captured by the overhead camera can be seen in Figure 15. With the paths captured
by the overhead camera, it is important to keep in mind the result can be distorted due to the
fisheye lens effect and the robot path is indicated by the black dots, with obstacles being the
big red circles.
Run 2: The second run was also similar to the first run in many ways. It started in a similar
position, and covered the entire area of the arena. However, in this run, the robot
unfortunately hit the first obstacle encountered in its outer loop. After this incident the robot
performed exceedingly well, by avoiding all other obstacles and walls, and finishing its planned
path in a time of 1:50. This run covered more area than the first run, as it fully covered the final
loop, as can be seen from the maps, and this is why the run took 10 seconds more to
finish..Aside from the obstacle collision, this too was a good result. The map generated from
the data the robot relayed to the computer via bluetooth for SLAM purposes can be seen above
in FIGURE 16 for this run. Also the map generated by the overhead camera can be seen below,
bearing in mind the considerations with this map as discussed above. In this map, the path has
been overlaid with coloured lines as an indication of what the robot's position meant in the
overall pathway algorithm.
33
Figure 16: Robot path captured by overhead camera from second run, with overlaid colour key
6.0. Discussion and Future improvements Upon completion of the project, one function that could’ve been improved was the obstacle
avoidance of the robot. If implemented correctly, we may not have needed to strafe around an obstacle,
and simply turned around it. This would involve supplying the motors with specific voltages and after
vigorous testing instead of the equal and opposite voltages we send to the motors, enabling the strafing
mechanism. This would decrease the overall time spent on obstacle avoidance, and in turn should
decrease the overall run time.
Something else we could’ve implemented was to check around an obstacle once it approaches one, to
see if it is safe to pass. At the moment, we somewhat have this implemented, but the robot does not
check anything directly to the left of it. This means that if the robot strafes, it may collide with another
obstacle right beside it. We could have mounted the any one of the sensors (but most probably the
sonar) on the servo motor, and used it to rotate the sonar to check for any obstructions, and if it’s safe
to strafe. This was not the case in the final demonstration, but with an additional feature like this, it
would a lot safer.
34
Mapping also could have been improved by making less assumptions about the behaviour of the robot
based on what states it was in. Although though it read in the current angle and the desired angle of the
robot it assumed the robot was going to make a perfect 90 degree turn regardless of these values, and
in some cases it didn’t, degrading the accuracy of the generated map. The mapping generated the walls
of the area based on the robot's first loop, and then assumed that the second and third loop were also
at the correct distance from the wall, rather than checking against the provided data. These
assumptions were made to decrease the time required on developing accurate models of the robot's
path but the trade off was decreased accuracy. With the knowledge gained from this project, next time
more complex algorithms could be developed to further increase the accuracy of the mapping as well as
increase the robot's ability to cover more area of a given arena.
Additionally, visual feedback could have been added to the robot which indicated the state the
robot was in and if there were any errors. Therefore that would help debug the robot and help
the process for programming the robot and testing.
7.0. Conclusion A fully autonomous robot was created which effectively covered and mapped a given area,
while at the same time navigating and avoiding any possible collisions with all walls and any
obstacles. The robot was able to cover a 2.4m^2 area in on average 1 minute and 45 seconds.
Mapping was also created live during the demonstration.
35
8.0. Reference
[1] http://www.nexuscyber.com/mega-sensor-shield-for-arduino-mega
[2] https://en.wikipedia.org/wiki/Heron%27s_formula
[3] https://www.sparkfun.com/products/retired/11486
36
9.0. Appendix ● Parts list
● Cost and source?
● Design Sketches
● Programs Circuitry
● Design Drawings
● C# Program wall straightening, cornering)
Commented [1]: ?
37
38
39
40
41
42
43
DRAWN BY
TUTOR
DATE SCALEALL DIMENSIONS IN MILLIMETRES
TITLE
A4SHEETQty.
GROUP No.
ALL TOLERANCES ARE 0.1mm UNLESS OTHERWISE SPECIFIED MATERIAL
DEPARTMENT
FILE LOCATIONFILENAME
30
60
11
5
3
3
Change No. Name of Item Changes Made OK Date
Back Mount Sonar Sensor
Mechanical Engineering
1 1 : 1 1 of 1
Mandeep Singh
5/05/2016
13
DRAWN BY
TUTOR
DATE SCALEALL DIMENSIONS IN MILLIMETRES
TITLE
A4SHEETQty.
GROUP No.
ALL TOLERANCES ARE 0.1mm UNLESS OTHERWISE SPECIFIED MATERIAL
DEPARTMENT
FILE LOCATIONFILENAME
60
50
16.5
5
3
3
Change No. Name of Item Changes Made OK Date
Front Plate Sonar Mount
Mechanical Engineering
1 1 : 1 1 of 1
Mandeep Singh
13
5/05/2016