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