10-Report-Spring

1

Drexel University | 3141 Chestnut Street, Philadelphia, PA 19104

Design of a Robot Guide/Usher Project 10

Adviser M. Ani Hsieh

Design Team Tyler Aaron John Burchmore Ian DeOrio Jeff Gao

Drexel University Mechanical Engineering and Mechanics Senior Design MEM 493

Spring 14

08 Fall

2

Abstract

Several Mechanical Engineering and Mechanics (MEM) research labs are located within the Science

Center, but access is restricted only to ID card holders. As a direct result, visitors and a few lab members

require the assistance of an ID card holder to gain access to the building every visit. While the option of

employing a dedicated ID card usher is plausible, it just isn’t very feasible and the job itself would be

very menial. Autonomous robots with the ability to identify and guide human visitors would solve this

issue. Designed as a case study with broader implications, this project will develop a robotic guide/usher

to perform this task.

3

Table of Contents

Abstract ......................................................................................................................................................... 2

List of Figures ................................................................................................................................................ 6

List of Tables ................................................................................................................................................. 8

Introduction .................................................................................................................................................. 9

Stakeholders and Needs ............................................................................................................................. 10

Problem Statement ..................................................................................................................................... 11

Methodology ............................................................................................................................................... 12

Background ............................................................................................................................................. 12

Perception Subsystem: Asus Xtion Pro ................................................................................................... 13

Mobility Subsystem: iRobot Create ........................................................................................................ 15

Manipulation Subsystem: Keycard ......................................................................................................... 16

Control Architecture Subsystem............................................................................................................. 17

Mapping ............................................................................................................................................. 17

Background .................................................................................................................................... 17

Process ........................................................................................................................................... 18

Localization ......................................................................................................................................... 22

Theory ............................................................................................................................................ 22

Background ................................................................................................................................ 22

Edge/Corner Detection .............................................................................................................. 23

Blob Detection ........................................................................................................................... 24

Translational (XYZ) ......................................................................................................................... 25

Background ................................................................................................................................ 25

Object Detection ....................................................................................................................... 25

Implementation ......................................................................................................................... 27

Bearing ........................................................................................................................................... 29

Overview .................................................................................................................................... 29

Flat Wall Assumption ................................................................................................................. 29

Template Method ...................................................................................................................... 30

Template Method - Processing ................................................................................................. 31

Obstacle Detection ............................................................................................................................. 36

Process ........................................................................................................................................... 36

Obstacle Avoidance ............................................................................................................................ 39

Theory ............................................................................................................................................ 39

Execution........................................................................................................................................ 40

Elevator Navigation ............................................................................................................................ 42

4

Process ........................................................................................................................................... 42

Exit Elevator ................................................................................................................................... 45

Subject Identification ......................................................................................................................... 46

Process Explanation ....................................................................................................................... 46

QR Code System ............................................................................................................................. 47

Code Decryption ............................................................................................................................ 48

Design Overview ..................................................................................................................................... 49

Codes and Standards .......................................................................................................................... 49

Model Concept ................................................................................................................................... 50

Design............................................................................................................................................. 50

Physics ............................................................................................................................................ 52

Parts List ......................................................................................................................................... 54

Simulation / Experimental Validation Plan ................................................................................................. 55

Description of the Test Bed .................................................................................................................... 55

Bearing Localization ........................................................................................................................... 55

Translational XYZ Localization ............................................................................................................ 56


Elevator .............................................................................................................................................. 57

Object Avoidance/Detection .............................................................................................................. 57

Validation Plan ........................................................................................................................................ 57

Bearing ............................................................................................................................................... 57

Translational XYZ ................................................................................................................................ 58


Elevator .............................................................................................................................................. 60

Object Avoidance/Detection .............................................................................................................. 60

Validation Data ....................................................................................................................................... 60

Rotational Localization ....................................................................................................................... 60

Translational XYZ ................................................................................................................................ 61

Guest Recognition .............................................................................................................................. 62

Elevator .............................................................................................................................................. 63

Obstacle Avoidance ............................................................................................................................ 63

Context and Impact ..................................................................................................................................... 65

Economic Analysis .................................................................................................................................. 65

Environmental Analysis .......................................................................................................................... 66

Social Impact Analysis ............................................................................................................................. 66

Ethical Analysis ....................................................................................................................................... 67

Discussion/Future Work ............................................................................................................................. 68

5

Conclusions ............................................................................................................................................. 68

Future Work............................................................................................................................................ 69

Hardware Upgrades ........................................................................................................................... 69

Facial Recognition .............................................................................................................................. 69

Speech Recognition ............................................................................................................................ 71

Speaker Recognition ........................................................................................................................... 72

Elevator .............................................................................................................................................. 73

Service Panel .................................................................................................................................. 73

Elevator Door Detection ................................................................................................................ 73

Elevator Speech Recognition ..................................................................................................... 73

Template Matching ................................................................................................................... 73

Project Management .................................................................................................................................. 75

SVN Repository ....................................................................................................................................... 75

References .................................................................................................................................................. 76

Appendices .................................................................................................................................................. 80

Appendix A ............................................................................................................................................. 80

Appendix B .............................................................................................................................................. 92

Appendix C .............................................................................................................................................. 93

Appendix – QRCodeDetection.m .......................................................................................................... 101

Appendix – CreateTemplate.m ............................................................................................................. 103

Appendix – RunComparison.m ............................................................................................................. 104

Appendix – XYZTemp_Angle.m ............................................................................................................ 106

Appendix – Xlocalization_Angle.m ....................................................................................................... 108

Appendix – ObstacleDetection.m ......................................................................................................... 110

Appendix – PotFieldDrive.m ................................................................................................................. 114

Appendix – ButtonDetection.m ............................................................................................................ 117

Appendix – ExitElevator.m ................................................................................................................... 120

Appendix – Demo.m ............................................................................................................................. 121

Appendix – QRLocal.m .......................................................................................................................... 125

Appendix – Rlocal.m ............................................................................................................................. 127

Appendix – Init.m ................................................................................................................................. 130

Appendix – DepthTemp.m .................................................................................................................... 131

Appendix – parsepoints.m .................................................................................................................... 132

6

List of Figures

Figure 1 - OpenNI SDK Architecture [3] ...................................................................................................... 13

Figure 2 - Occupancy Grid with obstacles [8] ............................................................................................. 18

Figure 3 - Occupancy grid mapping code .................................................................................................... 19

Figure 4 - Sample hallway map ................................................................................................................... 19

Figure 5 - SetLoc code ................................................................................................................................. 20

Figure 6 - SetGoal code ............................................................................................................................... 20

Figure 7 - AdjustLoc code ............................................................................................................................ 21

Figure 8 - FindDel code ............................................................................................................................... 21

Figure 9 – Microsoft Kinect Subcomponents .............................................................................................. 22

Figure 10 – ASUS Xtion Pro Specifications under OpenNI .......................................................................... 22

Figure 11 - Harris Feature Extraction .......................................................................................................... 23

Figure 12 - SURF Feature Extraction ........................................................................................................... 24

Figure 13 - Sample Code ............................................................................................................................. 25

Figure 14 - Matched Points (Including Outliers) ......................................................................................... 26

Figure 15 - Matched Points (Excluding Outliers)......................................................................................... 26

Figure 16. Localization "checkpoints" ......................................................................................................... 27

Figure 17 - Overlayed images of a template versus current ....................................................................... 28

Figure 18 - Flat wall assumption ................................................................................................................. 29

Figure 19 - Unknown pose .......................................................................................................................... 30

Figure 20 - Example of a template captured by the Xtion Pro.................................................................... 31

Figure 21 - Code application of the functions ............................................................................................. 32

Figure 22 - Example of Harris features extracted from an image ............................................................... 32

Figure 23 - Code application of the functions ............................................................................................. 33

Figure 24 - Result of identifying the Harris features and finding matching features in the images ........... 34

Figure 25 - Visual representation of the relative pixel displacements used to find the function .............. 34

Figure 26 - Mapped Depth Map of a Hallway ............................................................................................. 36

Figure 27 - Respective Image Capture of Depth Capture ........................................................................... 36

Figure 28 - Ground Plane Extraction ........................................................................................................... 37

Figure 29 - Obstacle Extraction ................................................................................................................... 38

Figure 30 - Colored Map with Obstacles ..................................................................................................... 38

Figure 31 - Example of valid and invalid path from grid pathing [16] ........................................................ 39

Figure 32 - Vector field analysis and path of movement from Potential Field method ............................. 40

Figure 33 - Image Acquisition ..................................................................................................................... 42

Figure 34 - Elevator Panel Template ........................................................................................................... 43

Figure 35 - Template Matching with the Captured Image .......................................................................... 43

7

Figure 36 - Cropping Matrix ........................................................................................................................ 44

Figure 37 - First Floor Crop ......................................................................................................................... 44

Figure 38 - Black and White Conversion ..................................................................................................... 44

Figure 39 - NNZ Function ............................................................................................................................ 44

Figure 40 - Standard QR Code ..................................................................................................................... 46

Figure 41 - (a) 10 character code vs (b) 100 character code ...................................................................... 47

Figure 42 – a) 3D Creo Model of Robot System b) Final Proof of Concept Design ................................... 50

Figure 43 – a) Creo Model of Housing b) Physical Model of Housing ...................................................... 51

Figure 44 – a) Close up View of Web Camera b) Close up View of Xtion Pro and platform ..................... 52

Figure 45 – a) Creo Model Close up of housing spacers b) Physical Model Close up of Housing Spacers

.................................................................................................................................................................... 52

Figure 46 - iRobot Create Coordinate System ............................................................................................ 53

Figure 47 - Printed Degree Chart for Validation ......................................................................................... 58

Figure 48 - Robot Localization Testing ........................................................................................................ 59

Figure 49 - Object Avoidance Example ....................................................................................................... 60

Figure 50 - Outline of facial recognition system [24] .................................................................................. 70

Figure 51 - Rotating Elevator Bank Views ................................................................................................... 74

Figure 52 - Flow Chart – Elevator to Lobby and Back ................................................................................. 88

Figure 53 - Flow Chart - Elevator to Room .................................................................................................. 89

Figure 54 - Fall Term Gantt Chart ................................................................................................................ 90

Figure 55 – Winter Term Gantt Chart ......................................................................................................... 91

Figure 56 - Detail Model Drawing ............................................................................................................... 92

8

List of Tables

Table 1 - Project Needs ............................................................................................................................... 10

Table 2 - Database Example ........................................................................................................................ 47

Table 3 - Mass Properties of Model Assembly ............................................................................................ 53

Table 4 - Final Parts List .............................................................................................................................. 54

Table 5 - Overall Rotational Localization Data ............................................................................................ 61

Table 6 - Overall Z Depth localization Data ................................................................................................. 61

Table 7 - Overall X Displacement Localization Data .................................................................................... 62

Table 8 - Overall Guest Recognition Data ................................................................................................... 63

Table 9 - Overall Object Avoidance Data .................................................................................................... 63

Table 10 - Parts List for Assembly ............................................................................................................... 80

Table 11 - Budget Table .............................................................................................................................. 81

Table 12 - Decision Matrix .......................................................................................................................... 82

Table 13 - Project Needs (1 of 2) ................................................................................................................. 82

Table 14 - Project Needs (2 of 2) ................................................................................................................. 83

Table 15 - Specifications and Metrics (1 of 2) ............................................................................................. 85

Table 16 - Specifications and Metrics (2 of 2) ............................................................................................. 86

Table 17 - Event Breakdown ....................................................................................................................... 86

Table 18 - Complete Z Depth Localization Trial Data .................................................................................. 93

Table 19 - Complete X Displacement Trial Data (1/2)................................................................................. 94

Table 20: Complete X Displacement Trial Data (2/2) .................................................................................. 94

Table 21 - Complete X Displacement Trial Data (2/2)................................................................................. 95

Table 22 - Complete Rotational Localization Trial Data (1/4) ..................................................................... 96




Table 26 - Complete Obstacle Avoidance Trial Data ................................................................................... 99

Table 27 - Complete QR Code Reader Trial Data ...................................................................................... 100

9

Introduction

Many movies today regarding the future feature robots living, working, and interacting with humans on

a daily basis. Often the programming and design of some of these robots have reached the level of near

complete autonomy, as seen in the movie “iRobot”. Autonomous mobile robots are developed to

perform tasks with little to no help from humans. This means that a robot will have to be able to

complete tasks given to it without becoming stuck either physically or in a programming loop. The use of

these robots has the potential to simplify and improve the quality of life for everyone on the planet.

These types of robots are not just a far off dream. Current work in robotics is progressing to the point

that robots play active roles in the current workforce. Robots are being used for manufacturing,

healthcare, and service industries with plans for application in military and space programs [1]. Although

the robots of today aren’t what one would expect when the term “robot” is used, the tasks performed

are similar.

There have been a growing number of instances where robots have been used to interact with humans

over the years. The Smithsonian Museum of American History installed MINERVA mobile robots that

acted as tour guides for certain exhibits. MINERVA was designed to interact with groups of guests and

provide information on the current exhibits [2]. California hospitals are purchasing RP-VITA telepresence

robots for doctors to be able to check on patients from home. The doctor connects to the robots

monitor and can then automatically drive to rooms or take manual control of to get closer views.

Amazon’s shipping warehouses utilize Kiva robots to transport shelves and boxes at the request of a

human worker. The human will be at a command terminal giving prompts to the robot while they

perform the tasks. All these robots have demonstrated the continuing development of the robotics field.

A problem with some of the current market robots is the minimal human interaction with the robots

while they carry out their tasks. The Kiva robots work very well in factory settings, however human

interaction is kept to a command terminal basis. The telepresence robots are good for long distance

communication of doctors and patients however many accounts say that it took some time to get used

to having their doctor just be a screen in front of them. Additionally a nurse or assistant practitioner has

to follow these robots in case extra interaction with the patients is needed. The MINERVA tour guides

showed that robots could adapt and show a general understanding of simple emotion in a group. This

robot did work well, but the outward design was on the functional side and not aesthetically pleasing.

Now that the technology allowing robots to perform simple tasks is growing, development on the

human interaction side is needed to make them more compatible in normal society. Responding to

voice commands instead of computer prompts could be the precursor to simple speech. The design of

an autonomous robotic guide in a building can clearly exemplify the steps necessary for successful

human robot interaction.

10

Stakeholders and Needs

Needs are created from stakeholder input and are used to guide the project in the correct direction. The

current stakeholders include Science Center MEM Lab members, visitors and building management,

however as the project progresses, more stakeholders may become involved. Table 1 below shows four

of the overarching needs relevant to the project.

Table 1 - Project Needs

Design Category Need

S1 Safety Potential for injuries during normal operation mitigated

Op1 Operation Minimize need for human input

P1 Programming Robust guest detection

P2 Programming Modular software components

The needs listed above are all important because they are key driving factors that shape the design.

Safety is of the utmost concern whenever humans interact with robots, with is addressed by S1. The

goal of the project is to create an autonomous system, therefore minimizing the need for superfluous

human input, which is represented by OP1. Guest verification is a need expressed directly by the

stakeholders. P2 is a need expressed by team members to introduce modularity and increase operating

efficiency. A full list of needs can be found at Table 13 and Table 14 in Appendix A.

11

Problem Statement

Certain secure buildings have workers take their minds off of their work to retrieve/sign in guests that

arrive. This wastes precious time and effort that the workers could be putting towards finishing up

projects. Also, many secure buildings now have multiple security checkpoints that further slow the

process of picking up guests. If this task could be given to a robot instead, company workers could

continue working until the guests are delivered to the designated area.

The core mission of this senior design project is to design and build a proof of concept for a fully

autonomous robotic guide/usher. The robot will have to be able to work its way through hallways,

elevators, and lobbies while avoiding stationary obstacles and variably moving objects. Elevator controls

will have to be operable through some means. The robot will also have to be able to recognize the

appropriate guests that will be delivered the specific destination. There could be two or three different

groups of guests waiting to be picked up for different departments in said building. Once delivery of

guests occurs, the robot should move to a designated wait location for its next command.

12

Methodology

Background

The robotic guide/usher in question will be an autonomous robot; comprised of perception, mobility,

manipulation, and control architecture systems. The perception subsystem is used to distinguish and

extract information from within the robot’s “visual” range. The mobility subsystem controls the robot’s

locomotion in terms of heading and velocity. The manipulation subsystem in this particular case consists

of the keycard system. The control architecture subsystem would be used as the central management

and control console with regards to mobility and guest verification. These four subsystems in

combination with each other form the structure that the autonomous robot relies on to complete the

tasks assigned to it.

13

Perception Subsystem: Asus Xtion Pro

Modern day machine vision systems employ multiple vision sensors working concurrently to capture

depth, image, and audio for post-processing and general interpretation. The most basic suite of sensors

utilized in order to enable vision for autonomous systems typically include a depth camera and a RGB

(Red/Green/Blue) camera. Depth cameras come mainly in two different variants. Time-of-flight (ToF)

depth cameras are based on utilizing the phase difference between the emitted and reflected IR signal

in order to calculate the depth of a targeted object. Structured pattern depth cameras emit a structured

IR pattern onto target objects and then utilize triangulation algorithms to calculate depth. Standard RGB

cameras are used to provide color vision of the environment. Industrial grade depth cameras are often

very expensive and cost anywhere from $3000 to $9000. RGB CMOS color cameras are an established

and mature technology and therefore inexpensive. The integration of the Xtion Pro sensor to the system

provides a cheap and established solution as a complete vision package. The Asus Xtion Pro is a motion

sensing input device for use with the Windows PCs and is built from the same hardware as the Microsoft

Kinect. From a developmental standpoint, utilizing the Asus Xtion Pro as the system’s vision suite is

extremely cost efficient due to the fact that the Asus Xtion Pro contains an RGB camera, depth sensor,

and a multi-array microphone all within one package.

The integration of the Asus Xtion Pro is a complex and multilayered issue that involves a multitude of

techniques, and modern engineering tools for successful integration. MATLAB is the desired Integrated

Development Environment (IDE) because of staff and team familiarity. For integration of the Xtion Pro

vision system, several software components need to be put in place to facilitate accurate data capture.

Figure 1 - OpenNI SDK Architecture [3]

14

In Figure 1 above, the Xtion Pro sensor is represented by the “Depth Physical Acquisition”. This step of

the Software Development Kit (SDK) architecture constitutes the physical sensor’s ability to capture raw

data. Next, the PrimeSense SoC operates the underlying hardware by performing functions such as

depth acquisition dedicated calculations, matching depth and RGB images, down sampling, and various

other operations. Then, the OpenNI framework takes over as the most popular open source SDK for use

in the development of 3D sensing middleware libraries and applications. From there, a C++ wrapper was

found which would allow the use of MATLAB as the primary IDE.

The proper method for creating a data processing system for the Xtion Pro involves numerous steps. A

Kinect MatLAB C++ wrapper was developed by Dirk-Jan Kroon from Focal Machine Vision en Optical

Systems in January 31st, 2011. [4] This particular MATLAB C++ wrapper is utilized alongside OpenNI

2.2.0, NITE 2.2.0, Microsoft Kinect SDK v1.7, and Microsoft Visual C++ compiler to create the functional

data processing system.

15

Mobility Subsystem: iRobot Create

An important part of a mobile autonomous robot is the components that allow for locomotion. This

motion system changes the system’s velocity and trajectory via a closed-loop control. There are two

specific types of motion systems that need to be considered. Limb based locomotion and wheel based

locomotion are the two most common types of motion systems.

The iRobot Corporation, the makers of the iRobot Roomba, created the iRobot Create Programmable

Robot as a mobile robot platform for educators, students, and developers as a part of their contribution

and commitment to the Science, Technology, Engineering, and Math (STEM) education program.

Utilizing the iRobot Create as the motion system is the most straightforward cost efficient solution. The

iRobot Create features three wheels, a designated cargo bay, 6-32 mounting cavities, an omnidirectional

IR receiver, sensors, a rechargeable 3000mAh battery, and a serial port for communication. By using this

platform, the overall focus of the project can be turned toward developing new functionalities between

the iRobot Create and the Asus Xtion Pro without having to worry about mechanical robustness or low-

level control.

The iRobot Create uses an Open Interface (OI) comprised of an electronic interface as well as a software

interface for programmatically controlling the Create’s behavior and data collection capabilities. The

Create communicates at 57600 baud via a numeric command system. For example, the command code

that commands the Create to drive in reverse at a velocity of -200mm/s while turning at a radius of

500mm is [137] [255] [56] [1] [244] [5]. It becomes clear that this command code structure is unintuitive

and unwieldy. In order to resolve and simplify this issue, MATLAB can be used to better facilitate

communication between a laptop and the iRobot Create via a RS-232 to USB converter. MATLAB is the

desired Integrated Development Environment (IDE) due to staff and team familiarity. Therefore,

MATLAB will be used to issue commands to alter the robot’s heading and velocity. The MATLAB Toolbox

for the iRobot Create (MTIC) replaces the native low-level numerical drive commands embedded within

the iRobot Create with high level MATLAB command functions that act as a “wrapper” between MATLAB

and the Create [6].

16

Manipulation Subsystem: Keycard

The Science Center building is a secure facility. Therefore in order to operate the elevators, a keycard

must be swiped over a reader to grant access. A human being will always be accompanying the robot

when it enters the elevator, eliminating the need for a complex swiping mechanism. Instead, the robot

will utilize a heavy duty retractable key reel system. When entering the elevator, the robot will prompt

the human via an audio cue, to swipe and press the corresponding elevator button. The human

accompanying the robot will be able to pull the card to the proper location to swipe, then release the

card, which will retract back into its starting location. In order to prevent possible theft, the reel will use

heavy duty Kevlar wire fixed to robot. This design be the fastest method to swipe, and will prevent

delays for any other people in the elevator.

17

Control Architecture Subsystem

Mapping

Background

Mapping is a fundamental issue for a mobile robot moving around in space. A map is relied on for

localization, path planning, and obstacle avoidance. A robot needs a map when performing localization

in order to give itself a location on that map. Otherwise, the robot could be at any location in an infinite

space. According to DeSouza and Kak’s article on vision-based robot navigation systems, three broad

groups in which indoor vision-based robot navigation systems can be categorized into are map-based

navigation, map-building-based navigation, and map-less navigation. Map-based navigation systems are

reliant on geometric modeling to element modeling of the environment, which the navigation system

can rely on. Map-building navigation systems utilize the onboard sensors to actively generate the

modeling of the environment for use with active navigation. Map-less navigation systems are typically

more sophisticated systems based on recognizing objects found in the environment in order to orient

and continue along their path.

Several challenges arise when mapping. To start, the area of all possible maps is infinitely large. Even

when using a simple grid there can be a multitude of different variables. This can lead to very large and

complex maps. Noise in the robot’s sensors also poses a problem. As a robot is navigating around errors

accumulate, which can make it difficult to obtain an accurate position. Also, according to the

Probabilistic Robotics textbook [7], when different places look very similar, for instance in an office

hallway, it can be very difficult to differentiate between places that have already been traveled at

different points in time, otherwise known as perceptual ambiguity.

Challenges aside, depending on the type of navigation system chosen, the complexity and sophistication

of sensors needed varies. 3-dimensional mapping can be more complicated and is not always needed

depending on the application. Instead a 2-dimensional map or occupancy grid can be created. “The basic

idea of the occupancy grids is to represent the map as a field of random variables, arranged in an evenly

spaced grid [7]“. The random variables are binary and show if each specific location is occupied or

empty.

The main equation that makes up occupancy grid mapping calculates the posterior over maps with

available data, as seen in Equation 1.

𝑝(𝑚 |𝑧1:𝑡 , 𝑥1:𝑡)

Equation 1

18

Here, m is the overall map, z1:t is the set of all measurements taken up to time t, and x1:t is the set of all

the robots poses over time t. The set of all poses make up the actual path the robot takes.

The occupancy grid map, m, is broken up into a finite number of grid cells, mi as seen in Equation 2.

𝑚 = ∑ 𝑚𝑖

𝑖

Equation 2

Each grid cell has a probability of occupation assigned to it: p(mi=1) or p(mi=0). A value of 1 corresponds

to occupied, and 0 corresponds to free space. When the robot knows its pose and location on the

occupancy grid, it is able to navigate due to the fact that it knows where obstacles are since they are

marked as a 1. Figure 2 shows a basic occupancy grid with a robot located at the red point, the white

points are empty space, and the black spaces are walls or obstacles.

Figure 2 - Occupancy Grid with obstacles [8]

The obstacles on the occupancy grid are considered to be static only. These obstacles are considered to

be stationary for the particular iteration that they are being used. Dynamic obstacles are considered to

be within this classification; however the robustness of the algorithm to calculate the optimal path

around these obstacles needs further investigation.

Process

In order to implement the 2D occupancy grid into MATLAB, a simple 2D array is needed. As an example a

simple straight hallway can be mapped. The empty space is made up of a unit 8 variable type array of

0’s, while the walls are made up of an array of 1’s. The map, m, is comprised of both the space and the

walls. Example code for a simple map can be seen in Figure 3.

19

Figure 3 - Occupancy grid mapping code

In this basic code, an empty hallway is mapped using the basis of a 2D occupancy grid as described

previously. This hallway is about 5.5 feet (170cm) by about 10 feet (310cm). Here the walls are 15cm

thick, which was just chosen to give the walls some thickness. The unit8 variable type is used to

minimize memory usage while running the code. This is important since a much larger occupancy grid

will have many more data points. A basic example of what the occupancy grid of zeros and ones would

look like for a straight hallway can be seen in Figure 4.

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1

Figure 4 - Sample hallway map

Now that the basic example of the occupancy grid is defined, further steps can be taken to convert a

fully 3D space into a functional 2D occupancy grid. This occupancy grid can be used to keep track of

goals, positions, and obstacles. In order to clean up the syntax for map operations, several functions

must be defined.

The first function is the setLoc.m function. The code for this is shown on the next page.

20

Figure 5 - SetLoc code

The SetLoc function accepts three inputs and outputs the modified m variable. The first input is the

initial map variable, the second input is the value of the y location in centimeters, and finally the last

input is the value of the x location in centimeters. Next, the function locations the last known x and y

position of the robot and resets this location to 0. And finally, it uses the two input location variables to

set the new position variable before updating the m output variable. This function allows for efficient

and clean redefinition of the robot’s position on the map.

The next function is the setGoal.m function. The code for this particular function is shown below.

Figure 6 - SetGoal code

The SetGoal function accepts three inputs and outputs the modified m variable. The first input is the

initial map variable, the second input is the value of the y goal location in centimeters, and finally the

last input is the value of the x goal location in centimeters. Similar to the SetLoc function, the function’s

next step is to locate the previous goal location and to reset this location’s occupancy grid value to 0.

And finally, it redefines the new goal position’s occupancy grid value to 3. This designates the goal.

21

The next function is the AdjustLoc function. The code for this particular function is shown below.

Figure 7 - AdjustLoc code

The AdjustLoc function again accepts three inputs and outputs the modified m variable. The first input is

the initial map variable to be adjusted, the second input is the centimeter value of adjustment of the y

position, and the third input is the centimeter value of adjustment of the x position. Similar to the

SetLoc function, the function’s next step is to locate the previous goal location and then to reset the

previous location’s occupancy grid value to 0. Finally, it calculates the new position by summing the old

position with the adjustments specified in the inputs. This allows for adjustment of the position of the

robot due to localization algorithms that will be defined later on.

The last function for performing operations on the map is the FindDel.m function. The code for this

particular function is shown below.

Figure 8 - FindDel code

The FindDel function simply calculates the distance in Y and X between the goal and the current position

of the robot. It then converts this distance from centimeters to millimeters so that the input to the

Roomba is simplified.

22

Localization

Theory

Background

“Mobile robot localization is the problem of determining the pose of a robot relative to a given map of

the environment [9]”. Otherwise known as position tracking or position estimation, the problem of

determining the position and pose of a robot is a fundamental problem that needs to be solved before

any level of autonomy can be claimed. When given a map of the surrounding environment, the robot

must be able to traverse the environment to the desired goal. However, without accurate and timely

position data, the robot can quickly accumulate error leading to undesirable actions.

However, there are many steps before successful self-localization can be accomplished. Because the

Asus Xtion Pro has been selected as the platform from which the data from the surrounding

environment will be collected, there are immediate parameters and values from which things can be

worked around are needed.

Figure 9 – Microsoft Kinect Subcomponents

Figure 10 – ASUS Xtion Pro Specifications under OpenNI

23

Utilizing the IR Depth Sensor, the IR Emitter, and the Color Sensor along with their corresponding

components, the data captured by the various sensors can be used. It is important to mention that both

position and pose need to be approximated using the information derived from the sensors. As such, the

next two sections will detail the various techniques used to calculate this information with respect to

the global coordinate system. However, before position and pose can be solved for, the basic algorithms

that facilitate this capability need to be detailed first. It is important to note that the ASUS Xtion Pro is

the developer’s version of the Microsoft Kinect. It is a compact version with the exact same hardware.

Edge/Corner Detection

One of the feature extraction methods utilized is the corner detection algorithm named Harris &

Stephens’s method. This particular algorithm is a further improvement on Moravec’s corner detection

algorithm introduced in 1977. Moravec’s algorithm operates on detecting the intensity contrast from

pixel to pixel within a black and white image. By taking the sum of squared differences (SSD) of the

intensity from pixel to pixel, patches with a large SSD denote regions of interest whereas patches with a

small SSD denote regions of minimal interest. Harris & Stephens take this concept a step further by

taking the differential of the intensity score with respect to direction into account. The general form of

this weighted SSD approach can be found in Equation 3, where an image region (𝐼) of area (𝑢, 𝑣) is

shifted by (𝑥, 𝑦), a value representing the weighted SSD is denoted by 𝑆(𝑥, 𝑦).

𝑆(𝑥, 𝑦) = ∑ ∑ 𝑤(𝑢, 𝑣)(𝐼(𝑢 + 𝑥, 𝑣 + 𝑦) − 𝐼(𝑢, 𝑣))2

𝑣𝑢

Equation 3

Harris & Stephens method’s implementation is built into MATLAB’s Computer Vision Toolbox and the

results are shown in Figure 11.

Figure 11 - Harris Feature Extraction

24

Blob Detection

Blob detection methods are algorithms that are used in computer vision systems to detect and

subsequently describe local features in images. These detection algorithms are very complex ways of

detecting points of interest. These points of interest are then filtered for stability and reliability based on

their various properties. A robust local feature detector named SURF was first presented by Herbert Bay

in 2006. SURF (Speeded-Up Robust Features) is a feature extraction algorithm that was inspired by SIFT

(Scale-invariant feature transform). Although SURF is the algorithm that ultimately was utilized, it is

important to describe the concept behind SIFT which SIFT uses to extract important features from an

image. SIFT is an algorithm published by David Lowe. Lowe's method is based on the principle that

distinct features can be found in areas of an image that are located on high contrast regions known as

an edge. However, beyond edge detection the SIFT method also accounts for the relative positions of

these detected points of interest. The SURF method’s implementation is built into MATLAB’s Computer

Vision Toolbox and the results are shown in Figure 12. [10] [11] [12] [13] [9]

Figure 12 - SURF Feature Extraction

25

Translational (XYZ)

Background

Self-localization is the act of determining the position and pose of a mobile body relative to a reference

map. Self-localization is a very important component in almost every function of an autonomous robot.

As previously mentioned, both the position and the pose of a mobile body are important pieces of

information to track. For ideal organizational division of labor, this section will detail the methods in

which the problem of self-localization with respect to placement is solved.

By utilizing the Asus Xtion Pro, measurements of the local environment can be made and used toward

solving this problem. However, pure measurements without any distinctive properties do not serve

much use when a translation or rotation relative to the map is performed. As such, effort must be taken

to withdraw distinctive features of the surrounding environment. A multitude of computer vision

systems rely on corner detection or feature extraction algorithms to detect various points of interest

that can be associated to the environment rather than just an individual image.

Object Detection

Either of the aforementioned feature extraction methods can be used to quickly ascertain the similarity

of each image based their features. Once the strongest feature points from each image are gathered,

they are compared with each other using the following code.

Figure 13 - Sample Code

The strongest feature points from each image are matched for similarities. The feature points with

matching distinct identical features can be plotted. An example of this can be seen in Figure 14.

26

Figure 14 - Matched Points (Including Outliers)

The next step is to eliminate any erroneous data points from misaligning or skewing of the object by

conducting a geometric transformation relating the matched points. The final result is shown below.

Figure 15 - Matched Points (Excluding Outliers)

Once the erroneous data points have been eliminated, the pixel location of the object in the scene can

be approximated and finally data can be taken. By using OpenNI1, the exact pixel location can be used

calculate the x, y, and z coordinates of the object. The OpenNI framework utilizes two different

coordinate systems to specify depth. The first coordinate system is called Depth. This coordinate system

is the native coordinate system as the X, and Y values represent the pixel location relative to the frame

of the camera. The Z value in the depth coordinate system represents the depth between the camera

plane and the object. Using a few functions, OpenNI allows for the ability to quickly translate the Depth

coordinate system to the Real World coordinate system. Here, the x, y, and z values represent the more

familiar 3D Cartesian coordinate system. Where the camera lens is the origin, and the x, y, and z values

represent the distance in those dimension the object is away from the origin. With the knowledge that

these XYZ real world coordinates are from the camera’s location, the relative location of the camera in

accordance with the global map can be calculated. If the object’s placement is previously given in the

global map; the raw difference in X and Z can be used to calculate the actual location of the camera

compared with the approximate location. [10] [11] [13]

27

Implementation

This method is not infallible by any means; the object matching utilizing a template requires a very high

resolution camera. While the Xtion Pro has a decent RGB camera, the effective range of the RGB camera

is about one meter. Therefore, when selecting landmarks for localization, care must be taken to select

large landmarks with distinctive features that are unique to that specific landmark. For example, the use

of a door would prove to be inadequate due to the fact that a door would typically not have distinctive

features when compared with another door in the environment.

Because of the aforementioned difficulties, the usage of QR Codes alongside the concept of localization

“checkpoints” were used to perform positional localization with the degree of accuracy that is desired.

Figure 16. Localization "checkpoints"

These QR Codes serve two different purposes. By utilizing unique QR Codes at every checkpoint, the

difficulty of distinguishing different checkpoints from one another is eliminated. The next goal of using

QR Codes is to provide a distinct object from which measurements can be made.

The localization file used to obtain a fix on the relative position of the robot with respect to the map is

named Appendix. This localization subroutine enables the ability for the robot to obtain an accurate x

and y dimension reading with respect to the map. Because the QR codes are strategically placed ninety

degrees to the ideal path toward the goal, the x position of the robot can be calculated and corrected

simply by measuring the depth of the wall that the QR code is posted on. If an x position of 80cm is

desired, the localization subroutine will simply iterate the logic until a depth of 80cm is recorded. The

28

next objective of the localization subroutine is to obtain the y position of the robot with respect to the

map. Before navigation can be conducted, the system must have a template image associated to that

specific QR code and localization checkpoint to use as a reference.

Figure 17 - Overlayed images of a template versus current

The image above is an example of utilizing the template image to compare the current position of the

robot with the ideal position of the robot. The red circles represent feature points detected by the SURF

method. These are representative of the feature points that will be tracked. Next, the green plus marks

are those same feature points but at a different location. The translation can be calculated directly. This

allows for calculation of the y position by comparing the template image and the current image and

leveraging the QR code as a point of reference. Templates are created using Appendix Code.

29

Bearing

Overview

The localization subroutine used to determine the pose of the robot has undergone a multitude of

changes over the course of the project. The two main methods that emerged from the development

phase that became the two primary methods of determining pose. The first method utilized a template

matching concept in order to match the exact viewing angle that the template was created within. The

next method involved using simple trigonometry as well as the fact that each localization checkpoint

directly faces a large flat wall.

Flat Wall Assumption

The simplest and most consistent method of determining pose involves assuming that every localization

checkpoint can be placed within a section of the wall that has a wide and flat surface.

Figure 18 - Flat wall assumption

The green block represents the wall, the white space represents the open space, the blue grid

represents the map grid, the red circle represents the robot, the arrow indicates our current

heading/pose, and finally the dotted line represents the plane from which the Xtion Pro takes

measurements. Figure 18 assumes that our heading is exactly facing the wall, and the dotted line which

represents the measurement plane and the wall are exactly parallel. However, this assumption cannot

be immediately made with some initial calculations and measurements.

Let the pose scenario for the robot be presented in the figure below. The measurement plane is skewed

an unknown angle, and the pose of the robot is now unknown. As such, the correction angle is also

unknown. The correction angle can actually easily be solved for with some simple trigonometry.

30

Figure 19 - Unknown pose

Measurements are taken from the measurement angle for depth directly to the green wall. If the robot’s

measurement plane is truly parallel to the wall, both measurements will be equal. Otherwise, the

difference between the two measurements can be utilized to calculate the correction angle needed to

adjust the pose to the desired state.

𝑡𝑎𝑛−1 (∆Y

∆𝑋) = 𝜃

Equation 4. Correction Angle

The code listed in Appendix uses this exact methodology to calculate for the correction angle if needed.

Template Method

In order to properly localize itself, the system must have a template image to use as a reference. This

reference image should be a photo taken from a position where the robot would be ideally lined up.

Whether it is aimed at a specific landmark or just down a hallway, it needs to be an image of its

destination orientation. To optimize the likelihood of success of the system, an image with some activity

should be used, activity meaning items hung up on walls or doors; something that is not just a plain

hallway. Figure 20 below shows a strong example image of a template.

31

Figure 20 - Example of a template captured by the Xtion Pro

While technically an image from any camera could be used, given it shoots in the same resolution,

ideally the rgb camera on the Xtion should be used because the comparison images will be shot from it.

To facilitate the creation of a template image, the script shown in Appendix – QRCodeDetection.m, is

used to initialize the camera, snap a picture and then store the outputted image in the folder. This

function allows for templates to be created easily, which is beneficial for testing, but as well for if the

environment in which the robot is functioning, changes.

Template Method - Processing

For the system to orient itself, it compares the current image the camera is seeing with the precaptured

template image. The script analyzes the two images finds a correlation between the two and then

rotates the system respectively. If the script finds no relation between the two pictures, it rotates 58°

(the field of view of the camera), to give itself an entirely new image to try. Once it has a relation

between the two, the script repeats itself until the system is lined up with the template.

Thanks to MATLAB’s Computer Vision Toolbox, the script is fairly straightforward. The script relies

heavily on the toolbox’s built in function’s corner point finding abilities. While the toolbox comes with a

variety of different feature detection functions including “BRISKFeatures”, “FASTFeatures”, and

“SURFFeatures”; the chosen method for this project was “HARRISFeatures” which utilized the Harris-

Stephens algorithm. This method was chosen because of its consistent positive performance in testing.

The “detectHARRISFeatures()” function takes a 2D grayscale image as an input and outputs a

cornerPoints object, ‘I’, which contains information about the feature points detected. Not all of the

points found by the Harris-Stephens algorithm however and further processing needs to be done to find

viable points. The “extractFeatures()” function is then used and takes an input of the image as well as

32

the detected cornerPoints to find valid identification points. The function takes a point (from ‘I’) and its

location on the image and examines the pixels around the selected point. The function than outputs

both a set of ‘features’ and ‘validPoints’. The ‘features’ object consists of descriptors, or information

used that sets the points apart from other points. ‘validPoints’ is an object that houses the locations of

those relevant points; it also removes points that are too close to the edge of the images for which

proper descriptors cannot be found [14]. Figure 21 below shows how the code is executed whileFigure

22 Figure 22 shows the results of using these functions to identify Harris features.

Figure 21 - Code application of the functions

Figure 22 - Example of Harris features extracted from an image

33

This process is run for both the template image and the comparison image, which brings about the

importance of using the same camera for both images. Different cameras, although same resolution, can

capture an image in different color, making the feature matching process more difficult. With the two

images processed, they must then be compared. Using the built in function, “matchFeatures()”, the two

sets of features can be compared to and determined as to whether or not they represent the same

feature, but from a different angle. The output consists of an index of points that contain features most

likely to correspond between the two input feature sets [15]. Plugging the locations of those specific

points back into the validPoints allows for an object consisting solely of the coordinates of matched

pairs. Figure 23 shows the applications of the functions in the code while Figure 24 shows the result of

comparing the Harris features on the two images. From the image, it is clear to see the translations of

the different points from template to comparison, as represented by the line connecting the two

different datasets. This translation is crucial to the script because that is the key to determining the

amount the robot must rotate to properly line up.

Figure 23 - Code application of the functions

34

Figure 24 - Result of identifying the Harris features and finding matching features in the images

The pixel difference between a specific set of points is relative to the angle the robot must rotate to line

up. Because there is usually some discrepancies when it comes to comparing all the different pixel

displacements, an average, ‘d”, is taken of the lot to get a better overall displacement. A multiplier is

created, using the average displacement over the horizontal resolution as a ratio. That ratio is multiplied

by the sensors field of view to get the necessary turn angle, as represented by θ in Equation 5.

Figure 25 - Visual representation of the relative pixel displacements used to find the function

35

𝑑

𝑥𝑟𝑒𝑠∗ 𝐹𝑂𝑉 =

𝑑

640∗ 58 = 𝜃

Equation 5

The robot is then turned the calculated value of θ degrees to try and line up in the right direction. The

entire process is set in a loop to repeat until the robot is as close to lined up as can be. This is made

possible by utilizing the average displacement data from between the valid points. The code is set to

repeat itself, capturing a new comparison image, finding features and comparing them, until the average

difference between points is below a certain number. That chosen number is 20, which relates to an

angle of 1.82 degrees. Most tests, with the upper bound set to 20, would result in an average

displacement much less than that actually being the result of the rotation. Testing also showed that

when the chosen number was lower, the system would try and make it into the bounds and wind up

oscillating until it was no longer remotely lined up. The complete code used to perform the comparison

can be found in Appendix – CreateTemplate.m at the end of the paper.

36

Obstacle Detection

Obstacle detection is extremely important because without it, the system cannot be considered

autonomous. It allows the system to differentiate between what is a clear and travelable location and

what is obstructed and needs to be avoided, on the fly. Once the obstacles are detected, they are able

to be transformed to the map, allowing the robot to move safely and freely.

Process

Before any scripts can be run, a depth image is first captured of the system’s current field of view. The

image is stored as a 3D matrix that holds real world XYZ coordinate data, for each pixel of the camera’s

resolution, i.e. a 640 x 480 x 3 matrix. The XYZ data represents the length, height and depth for each

pixel, in millimeter units, hence the 3D matrix. Figure 26 below shows a graphical representation of the

depth map; where every pixel that is not black, has XYZ data. Figure 27 provides the respective visual

image of the depth capture for clarification purposes. It is important to note that the smooth reflective

tile floor caused issues with depth collection, shown by the close black pixels.

Figure 26 - Mapped Depth Map of a Hallway

Figure 27 - Respective Image Capture of Depth Capture

37

Once the depth image is captured, a ground plane extraction can be run on the image. Due to the low

resolution of the camera, only the bottom 100 pixels of the image were analyzed; anything above those

pixels was too inaccurate and caused issues with extraction.

The ground plane extraction runs on the principle that each step taken down a hallway takes you further

than where you started. This means that each point on the floor should have a point behind it, further

away. To do this, the depth portion (Z) of the depth map is analyzed and each cell is examined. If the

cells vertical neighbor is a larger number, meaning further from the camera, it is not considered an

obstacle. Any cell that is not considered an obstacle, has its location saved to a new matrix. Plotting that

matrix on top of the original image produces Figure 28 below.

Figure 28 - Ground Plane Extraction

The lack of pixels in certain areas is again due to a combination of having a low resolution camera as well

as the material that the floor is made of. However, it is still easy to see in the figure, where the ground is

clear; marked by the green pixels. The pixel selection is then inverted, causing the unselected to now

become selected and vice versa. This is done in order to now highlight and detect what is an obstacle.

In order to try and increase the green pixel density, the data is smoothed using MATLAB’s built in

smoothing function. Further manual smoothing is then performed. Any column of the image that only

has one pixel is removed, as it doesn’t provide enough data. Each column that has two or more green

pixels, is filled with green from the columns minimum value to its highest. This manually removes all the

small missing pixel areas. Lastly, if a column of green has no green columns on either side of it, then it is

also removed because it is more inaccurate data. With the ground plane extracted, all of the opposite

pixels are selected and made into an “obstacle” array. Figure 29 below shows the results of the obstacle

extraction. Green means that the area obstructed, while no marking means the path is clear.

38

Figure 29 - Obstacle Extraction

Each marked pixel’s locations (on the image) are updated in the obstacle array. Because the obstacle

array was created to overlay on top of the image, the marked pixel locations are directly related to XYZ

data. The real world location for each pixel is then extracted from the depth image, which is then

written to the already existing map. Because it is a top down two dimensional map the length values (X)

are used as the horizontal distances and the depth values (Z) are used as the vertical distance. All the

values are written from the origin, which is wherever in the map the robot currently is. The process of

writing values to the map consists of assigning the specific cell, determined from the coordinates, a

value of 4, indicating at that location there is an obstacle. Figure 30 below shows a zoomed out matrix

image of the map after the obstacles have been added.

Figure 30 - Colored Map with Obstacles

Because of the zoom, the values of zero are not shown. Anything visible has a value of 1; the black lines

are the walls, red are the detected walls and green indicates the trashcan. The origin of the figure,

where the points are measured from, is the bottom center of the image. The full code can be found in

Appendix – ObstacleDetection.m.

39

Obstacle Avoidance

Theory

Robotic pathing, or motion planning, is the ability of a robot to compute its desired movement

throughout the known area by breaking down the movement into smaller steps. The path is calculated

such that the robot could complete travel to its end destination in one large motion. Starting position

and heading of the robot are assumed to be known from either specific placement of the robot or

localization algorithms. A map is imported into the system showing the end destination of the intended

motion along with all known obstacles within the map area. A specific pathing algorithm is then chosen,

using the map as the basis of the path calculation.

Grid-Base Search pathing sees a similar functionality to the occupancy grid in that the map is broken up

into grid that is represented as being filled with either free or occupied points. The robot is allowed to

move to adjacent grid points as long as the cell has a value of free associated with it. Constant checking

from a collision avoidance program is needed for this type of pathing to make sure the grid points along

the planned path are truly free. One consideration with this type of pathing is the size of the grid

overlaid on the map. An adequate resolution has to be chosen depending on the availability of the area

of motion as free. Courser grids with cells representing a larger area offer quicker computational time

but sacrifice precision. A cell that is deemed to be free might actually be occupied in 40 percent of the

cell area. This could lead to potential collision in narrow sections of the map. The other side of the coin

has a fine grid with more precise cell values. More time is needed to complete the path before

operation could start.

Figure 31 - Example of valid and invalid path from grid pathing [16]

40

Potential Fields method sees the use of pseudo-charges to navigate to the destination. The end

destination of the path is given a positive, or opposite, charge that the robot would like to move to.

Obstacles along the robots motion toward the goal will assigned negative, or like, charges. The path is

then determined as the trajectory vector from the addition of all the charges. The potential field method

has advantages that the trajectory is a simple and quick calculation. The problem lies in becoming

trapped in the local minima of the potential field and being unable to find a path. Local minima can be

planned and adjusted for by a number of methods. Adding small amount of noise into the system will

bump out of the minima area and back into useable calculation. Obstacles could also be given a

tangential field to again move the robot out of the minima to recalculate the path [17] .

Figure 32 - Vector field analysis and path of movement from Potential Field method

Execution

As seen in the Obstacle Detection section, several subroutines allow for the robot to map the

unforeseen obstacles within its path onto the map. These obstacles are represented by numerical 1s. As

previously described in the Theory section of Obstacle Avoidance, there are a multitude of techniques

can that be utilized to dynamically calculate the path to avoid these obstacles. The method that was

chosen for this particular project is called the Potential Field method. Adopting behaviors from nature,

obstacles and goals are modeled with pseudo-charges and the contribution of each pseudo-charge is

calculated and the resulting velocity vector is used to drive the robot.

As described in Michael A. Goodrich’s tutorial for the implementation of potential fields method, the

functional behavior of goals and obstacles within the operating environment of the robot are carefully

defined with a few parameters. For 2D navigation, (𝑥𝑔, 𝑦𝑔) define the position of the goal, and (x, 𝑦)

define the position of the robot. The variable r will define the radius of the goal. The direct distance as

the angle between the goal and the robot is calculated. By utilizing a set of if statements various

41

behaviors can be defined with respect to the operating boundaries of the robot and its interaction with

the goal.

{

∆𝑥 = ∆𝑦 = 0

∆𝑥 = 𝛼(𝑑 − 𝑟) cos(𝜃) , ∆𝑦 = 𝛼(𝑑 − 𝑟)sin (𝜃)

∆𝑥 = 𝛼 cos(𝜃) , ∆𝑦 = 𝛼 sin(𝜃)

Where 𝛼 is the scaling factor of the goal seeking behavior, d is the distance between the robot and the

goal, r is the radius of the goal, and theta is the angle between the goal and the robot. The first equation

is used if the distance between the goal and the robot is less than the radius of the goal. This defines the

behavior of the robot when the goal has been reached. The second equation defines the behavior of the

robot when the radius of the goal is less than or equal to the distance between the goal of the robot and

that is less than the influence field of the goal plus the radius of the goal. This condition is met when the

robot is nearing the goal, and the equation will scale the velocity vector that the robot experiences until

the goal is reached. And finally, the last equation defines the behavior of the robot when it is nowhere

near the goal.

The obstacles are defined in the exact opposite manner. Both the distance and the angle between the

robot and the obstacle are calculated and used in the following equations. These equations are

contained within a set of if statements. These if statements define the behavior of the robot in various

scenarios involving these obstacles.

{

∆𝑥 = −(cos(𝜃))∞, ∆𝑦 = −(sin(𝜃))∞

∆𝑥 = −𝛽(𝑠 + 𝑟 − 𝑑) cos(θ) , ∆y = −β(s + r − d)sin (θ)∆𝑥 = ∆𝑦 = 0

Where 𝛽 is the scaling factor of the obstacle avoiding behavior, d is the distance between the robot and

the goal, r is the radius of the goal, and theta is the angle between the goal and the robot. The first

equation is used if the robot’s position and the obstacle’s position are infinitely small. As such, the

robot’s behavior will be to move away from the obstacle at very large velocity vector. Again, the second

equation is to scale the velocity vector of the robot as it approaches the obstacle, and finally the robot

experiences no contributions to its velocity vector if it is nowhere near the obstacle.

The overall velocity vector is a summation of the contributions of the attractive and repulsive forces

exerted by the goals and obstacles within the robot’s operating environment. This exact method is

utilized can be found in Appendix – PotFieldDrive.m.

42

Elevator Navigation

The process behind the elevator navigating component involves a modified and extended template

matching system. The logic behind the system is that if it can tell that it’s in front of the panel, it can

discern between the different buttons and therefore monitor which are pressed are which are not. The

system first scans to capture the current panel and determines whether or not the desired floor button

is pressed. If the button is not pressed, it prompts the user to press it and will continue scanning until

the button is detected as pressed. The exact detection process will be detailed in the following section.

With the button verified as pressed, the system then checks constantly until it sees that the button is no

longer pressed. This signifies that the correct floor has been reached and the robot will proceed to exit

the elevator.

Process

The first step in the process is capturing a template image. This process is only done once and can be

applied to all of the elevators that use the same panel. This image is captured using the Image

Acquisition Toolbox with the webcam selected. Figure 33 below shows how the call is made to the

webcam. The color space is set to gray scale to simplify the template matching process as well as the

frames per trigger set to one to make it so the program only captures one image every time it’s called.

Figure 33 - Image Acquisition

The template is created and cropped to feature only the actual panel itself, as shown in Figure 34. This

template image is stored and is used for every run; it does not change.

43

Figure 34 - Elevator Panel Template

With the template captured, the process is able to run. With the robot in place facing the panel, it

begins to capture new images. Each image is matched against the template image and compared to find

similar points, using the same method as the localization. However in order to analyze the current

image, a geometric transformation is performed. This automatically scales and crops the captured image

to match that of the template [18]. Figure 35 shows MATLAB actually matching the different images.

Figure 35 - Template Matching with the Captured Image

This is done so that the individual buttons can be analyzed. The image is sectioned off into different

rectangles, one for each button. This is done by creating a matrix of coordinates consisting of the pixel

locations of rectangles that encompass each button. Doing this allows the function to look for a certain

flow, and know how to crop the image accordingly. The cropping matrix for this elevator panel is shown

in Figure 36 and only works on this template.

44

Figure 36 - Cropping Matrix

Setting the desired floor to one, so that the system gets off at the lobby, causes the program to crop the

image like as shown in Figure 37.

Figure 37 - First Floor Crop

With the image cropped correctly, the actually button needs to be analyzed to be determined whether

or not it is pressed. In order to do this, the image is converted to strictly black and white. The

illuminated button creates a large amount of white pixels as shown in Figure 38 , which are nonexistent

when the button is not pressed.

Figure 38 - Black and White Conversion

With the white pixels showing, it is easy to count the amount and use that number to determine

whether or not the button is pressed. This is done by using the number of non-zero (nnz) function built

into MATLAB as demonstrated in Figure 39. The nnz function analyzes a matrix, in this case the black and

white image, and counts the number of elements that are non-zero, in this case white pixels [19]. If that

number is large enough, it tells the program that the button is pressed and continues running.

Figure 39 - NNZ Function

45

This process is nested within itself and after running and determining that the button is pressed, it

repeats itself. It uses the same exact process to determine when the button is no longer pressed,

signaling that it is time to exit the elevator. The full code can be found in Appendix – ButtonDetection.m.

Exit Elevator

Part of navigating the elevator involves being able to physically leave. The system is set to drive out

using prewritten commands. Originally, depth measurements were going to be used, however other

visitors in the elevator threw off the ability to find the walls and therefore the only way to constantly

move was with scripted steps. The full code can be found in

46

Subject Identification

The use of a QR code to determine a visitor’s identity is the current choice for verification. The use of QR

code was chosen for multiple reasons. Because it is basically a barcode, QR codes are very simple to

generate, especially with the help of different internet sites. Now that most people carry around

smartphones that can decode the codes with the use of an app, they are becoming a more and more

popular method to share information. Figure 40 below shows an example of a standard code that is

used.

Figure 40 - Standard QR Code

A code, in this case a static id, can be inputted and a high resolution image of the code can quickly be

created and emailed to a client. The QR code was also chosen for ease of use, mainly with respect for

the client. With more well-known methods, such as facial recognition, a database of client faces would

need to be created. Not only would this be extremely tedious, it could make clients who do not want

their photograph taken feel awkward. Because it is a barcode, the QR Code can easily be decoded into

strings that can be stored in a simple database, much easier than having to sort through a gallery of

photographs. The last reason it was chosen was due to the availability of free software that is

compatible with MATLAB. The current working code for the QR scanning can be found in the Appendix –

QRCodeDetection.m.

Process Explanation

The system is designed to be as user friendly as possible. Due to the lack of a video display, audio cues

are used to guide the visitor through the process. Once the robot reaches the lobby, it begins to scan for

a QR code. The visitor, being told prior to the visit of the process, displays the code on their phone (or a

piece of paper) in front of the camera. The system then scans and either confirms who they are or

rejects the code. If it accepts, it will prompt the user with an audio cue, greeting them by name and

asking to follow to the elevator. If it rejects, it will prompt the user to call and schedule an appointment,

or to call for assistance.

47

QR Code System

The versatility of the QR code allows for unique codes to be easily generated. This project takes

advantage of this and allows someone to send out unique codes to all their expected visitors. These

unique codes not only allow for personalization for visitors, but also allows for an added layer of

security. The same code, while it could scan for anyone, could easily raise a flag when double scanned.

The process of setting up the code is very simple in order to make sure that the entire ordeal is no more

difficult than sending out a normal email. If it takes forever to set up the entire system, you wind up

losing time just like having to have someone go greet the visitor in person, defeating the purpose of the

project.

First a code is created, whether it be randomly or by design, and assigned to someone’s name, like as

shown in Table 2 below. The codes can be normally up to a length of 160 characters, but as shown in

Figure 41, the larger the code the more complex the image becomes. While some cameras can

accommodate the small image, the RGB camera on the Xtion Pro cannot consistently, so smaller 10

character codes are used. The spreadsheet, which could be hosted on a website, or in a private folder on

a service such as Dropbox, or for this proof of concept directly on the laptop, holds these codes and

names.

Table 2 - Database Example

Tyler Aaron 13689482

John Burchmore 2432158

Ian DeOrio 59824

Jeff Gao 209

Figure 41 - (a) 10 character code vs (b) 100 character code

48

Using the “xlsread()” function built into MATLAB, the program is able to easily take the spreadsheet and

convert it into a useable matrix. Using a “for loop”, the program compares the value from the decoded

image, to the matrix from the spreadsheet. If a code matches one found in the matrix, then the system

considers the run a success, speaks aloud the person’s name and prompts the user to follow. In order to

get the system to be able to read out a visitor’s name, MATLAB must use Microsoft Windows built in

Text To Speech libraries. Through the use of a wrapper, written by Siyi Deng © 2009 [20], MATLAB is

able to directly access these libraries and use them to read out strings, i.e., the corresponding names

from the matrix.

Code Decryption

Because QR codes are not a closed source technology, there are many different programs and softwares

available for people to use when it comes to encoding and decoding them. One of the most popular

open source libraries is one developed by ZXing (“Zebra Crossing”). ZXing ("zebra crossing") is an open-

source, multi-format 1D/2D barcode image processing library implemented in Java, with ports to other

languages [21]. They have android clients, java clients, web clients, but unfortunately, do not have a

simple way to incorporate into MATLAB.

In order for MATLAB to properly use the ZXing libraries, it needs a wrapper. Fortunately, Lior Shapira

created and published an easy to use wrapper for MATLAB [22]. Shapira’s wrapper consists of two

simple MATLAB functions, “encode” and “decode”, which call to specific java files to either encode or

decode a code. The “encode” function takes a string as an input and uses ZXing’s library to create a

figure that displays the resulting QR. However, the figure produced is not the most visually appealing,

hence why a different online service is used to generate the code to email to visitors. The “decode”

function takes an image as an input, in this case captured with the RGB sensor on the camera, and

outputs the resulting message.

49

Design Overview

Codes and Standards

The field of robotics is a new and emerging field and standards are still being put in place. Two very

relevant ISO standards are below.

“ISO/FDIS 13482: Safety requirements for personal care robots

ISO/AWI 18646-1: Performance criteria and related test methods for service robot -- Part 1: Wheeled

mobile servant robot”

It is important to note that these standards are still in early stages of development and are constantly

being updated and modified. Just like the project, they are a work in progress, and new standards and

codes will be used as they are instituted.

50

Model Concept

Design

Figure 42a shows a Creo Parametric 3D model of the planned design for the robot. Figure 42b shows the

completed assembly. The goal of the design was simple yet secure. The model features a 1:1 scale

iRobot Create1 and Xtion Pro2. The aluminum support bar3 can also be changed to a different size if need

be. The assembled robot design changed slightly so the height of the Xtion Pro could be changed if

needed. A web camera was also places on top of the aluminum support bar for QR code scanning and

elevator detection.

Figure 42 – a) 3D Creo Model of Robot System b) Final Proof of Concept Design

The current housing, which can be seen in Figure 43, is designed to fit most small sized

laptops/netbooks, but can be modified if the laptop to be used changes. The housing is round not only

to match more aesthetically with the Create, but also to minimize sharp corners poking out from the

system. The open design was chosen because the reduction in materials will keep the weight at a

1 Courtesy of Keith Frankie at http://www.uberthin.com/hosted/create/ 2 Courtesy of GrabCAD user Kai Franke at https://grabcad.com/library/asus-xtion-pro-live 3 Courtesy of 80/20 Inc. at http://www.3dcontentcentral.com

http://www.uberthin.com/hosted/create/

https://grabcad.com/library/asus-xtion-pro-live

http://www.3dcontentcentral.com/

51

minimum as well as because the fabrication of the system will be much easier without having to create a

round wall. Each peg is screwed into the top and bottom plates. There are four smaller pegs that screw

into the iRobot base and the bottom acrylic piece. The gap between all the pegs is roughly six inches, not

enough to remove a laptop. Because of this, the back peg can be easily removed, the laptop slid in and

replaced.

Figure 43 – a) Creo Model of Housing b) Physical Model of Housing

The laptop housing is made of two 0.25” thick clear acrylic pieces with a diameter of about 12”. The top

and bottom pieces are essentially the same aside from the opening on the bottom piece for easy access

to the iRobot Create’s buttons. The six pegs are made from 2” long threaded aluminum standoffs. This

allows the pegs to be screwed on between the acrylic pieces. On the top piece of the acrylic housing the

aluminum t-slotted extrusion extends about 36”, which is attached using two 90-degree brackets. On

the t-slotted frame another 90-degree bracket is used to attach the Xtion 0.25” thick acrylic platform.

This allows for varying the height of the Xtion camera. A close-up of the Xtion and its platform can be

seen in Figure 44. The web camera on top of the aluminum extrusion is attached with a secured 90-

degree bracket to allow for easy removal. The aluminum extrusion is also used for cord management.

52

Figure 44 – a) Close up View of Web Camera b) Close up View of Xtion Pro and platform

Between the iRobot Create and the bottom side of the laptop housing are four small nylon standoffs to

allow for some spacing for cords and modularity, which can be seen in Figure 45.

Figure 45 – a) Creo Model Close up of housing spacers b) Physical Model Close up of Housing Spacers

Physics

In order to calculate the center of gravity a couple assumptions need to be made. To start, the Asus

Xtion Pro is assumed to be a uniform solid piece. In reality, it is not completely solid and its actual center

of gravity could be different depending on how the internals are oriented. For ease of calculation, since

its total weight and dimensions are known, the volume and in turn density of the camera can be

calculated for the “solid uniform” piece. A similar assumption needs to be made for the iRobot Create.

Since the exact weights and positions of all the internals of the robot are unknown, the robot is also

assumed to be a uniform solid piece. Models of these two parts were created in PTC Creo and put into

the assembly in order to do the overall center of gravity calculation.

The known densities and calculated mass properties of all the materials used in the model can be seen

in Table 3. PTC Creo uses these densities to calculate the volume and mass of each component. After

running an analysis on the mass properties of the entire assembly, Creo outputs the x, y, and z

coordinates for the center of mass of each component, as well as an overall center of gravity.

53

Table 3 - Mass Properties of Model Assembly

Part Density

(lb/in^3)

Mass

(lb)

Center of Gravity (w/ respect to

Create_Robot_Axis)

X Y Z

iRobot Create 0.0353 8.8000 0.0000 1.0831 0.8225

Xtion Pro 0.0249 0.4784 0.0000 18.6422 0.0000

Aluminum

Support 0.0979 1.3553 0.0000 11.4080 0.0002

Xtion Platform 0.0434 0.1049 0.0000 17.5326 0.0000

Laptop Housing 0.0434 2.4994 0.0000 4.1820 -0.0074

Base Spacer 1 0.0434 0.0010 4.3884 2.6580 -0.1022

Base Spacer 2 0.0434 0.0010 4.3884 2.6580 -2.8522

Base Spacer 3 0.0434 0.0010 -4.3884 2.6580 -0.1022

Base Spacer 4 0.0434 0.0010 -4.3884 2.6580 -2.8522

The center of gravity coordinates listed above are with respect to the main axis of the iRobot Create

model. That axis is located at (0, 0, 0) directly in the bottom center of the create model as seen in Figure

46.

Figure 46 - iRobot Create Coordinate System

54

According to the Creo analysis, the calculated center of gravity for the entire assembly is located at: (X,

Y, Z)(0.0000, 3.4898, 0.5448). These numbers do seem to make sense when looking at the model.

With the direction of the x-axis being in the absolute center and all components being centered on the

model, that x-coordinate would be zero. The y-coordinate is expected to be just slightly above the

Create due to the extra weight above, and since the majority of the weight is in the Create itself. The z-

coordinate should be slightly biased toward the front of the robot since more weight is located there.

Parts List

All of the major components and raw materials used in the design of the robot are readily available to

consumers. The iRobot Create, Asus Xtion Pro, and Acer Ferrari are the three major components that are

physically off the shelf products. The Creative Live Web Camera is also readily available off the shelf. The

laptop housing and Xtion support materials required fabricating in the machine shop to the desired

specifications. The acrylic pieces were all modeled in AutoCad and laser cut to the required

specifications. A complete list of all parts and materials can be seen in Table 4.

Table 4 - Final Parts List

55

Simulation / Experimental Validation Plan

Description of the Test Bed

Bearing Localization

The Xtion Pro is to take a template photo about 80cm from the wall in the SAS lab; this is the goal

orientation of the system. For the template photo, the robot is aligned with the wall such that the Xtion

camera plane is parallel with the wall. This original orientation of the robot corresponds with a heading

of zero degrees. With the template taken, MATLAB is then used to turn the robot to a random angle in

the positive and/or negative direction. Because the field of view of the camera is 58 degrees, that

bounds the random number. Using the function shown in the Localization-Bearing Template Method

section, the robot then captures a new image to compare to the template image. Feature detection is

used to determine any differences from the template and new images. The displacement of matched

features is then related to an angle, and the robot will turn that angle back towards the zero degree

heading. After the turn has been made, a new comparison image is taken and the comparison process

repeated. If the measured pixel displacements between features are small enough, as explained in the

validation section of the report, the process will end. If the resulting displacement is too large, the

displacement will again be turned into an angle and the robot turned. The process is repeated until the

system reaches that zero degree heading; within a certain level of uncertainty.

Using the flat wall assumption program described in the Localization-Bearing section of this report,

another method of determining the pose of the robot is performed. The robot is set at a distance of

80cm from the wall, with an orientation so that the plane of the Xtion camera is parallel to the wall. This

validation will only work efficiently if the robot is facing a wall without any major protrusions. The robot

is then commanded to turn a random number of degrees in the positive and/or negative direction. As

described earlier, the program takes measurements of two different points, one on either side of the

robot. It then uses trigonometry to calculate the angle to turn in order to make both those

measurements equal. The program runs in a loop until the measurements are determined to be equal,

and the validation is over. The corrected angle is output in MATLAB, which can be compared to the

initial angle of offset.

56

Translational XYZ Localization

A sample map of the testing area is created as a matrix in MATLAB. A landmark object is placed within

the mapping area at a known location. The landmark used is a QR code that is placed on the wall and its

specific code data is stored in MATLAB. Due to the fact that the landmark must have enough

distinguishable features to be recognized by the working code and large enough seen by camera, a QR

code on an 8.5”x11” piece of paper is used. The map is then updated to have the QR code at its precise

location. The robot is initially placed with the Xtion camera parallel to the wall, facing the QR code. The

Xtion is used to take a template photo of the QR code at a distance of 80cm away from the wall.

With respect to XYZ data obtained by the Xtion, the Z distance (X on the map), and the X distance (Y on

the map) will be tested. For this testing description, the X and Y map distances will be mentioned. In

order to do this validation, the robot will be offset from it’s original position in the X direction for one

test, and in the Y direction for another.

To test for the X direction, the robot will be commanded to move various distances away and toward

the wall. Using the functions described in the Localization-Translational XYZ section, the camera

captures a new image, locates the QR code, and compares the image to the template. When the QR

code is found, a single pixel on the code has XYZ data extracted and converted to Cartesian real world

distances. The distance between the camera and the QR code is then calculated from the new Cartesian

XYZ data. The distance is stored and used to update the map with the location of the robot relative to

the QR code. The program uses this distance to calculate how far to move to get back to the origin

location. The robot moves that distance, while the program is looped to correct for error, and the robot

will correct itself until it reaches the original location. Manual measurements are taken at each step to

verify the movements are accurate.

To test for the Y direction, a similar approach is used. Instead, the robot is turned 90/-90 degrees and

moved to the positive/negative Y direction (i.e. to the left/right of the QR code). The code outputs the

difference in the position of the single pixel on the QR code that is matched in the new image and

template image. This distance is also manually measured to determine accuracy.


A QR code that is stored in the QR recognition program will be taped to a wall in the SAS lab. The Robot

is placed so the Xtion camera is facing the QR code at varied distances. The program is run to scan the

code, showing if the code was successfully scanned. The tic-toc function in MATLAB is used to time the

start and stop of each run to see how long the “quest recognition” took. This is performed at several

distances.

57

Elevator

The elevator button detection program is tested using the actual elevators in the building. The robot is

placed facing the elevator buttons, about 70cm away from the panel. Once the desired floor button is

pressed the program is run, with the desired floor already set in the program. The timer starts when the

program starts and will run until the program outputs that the correct floor has been reached. Data on

whether or not the correct floor was detected is to be taken.

Object Avoidance/Detection

In order to validate the obstacle detection program described earlier, several tests need to be

performed. Obstacles with heights varying from 2cm to 100cm are placed in a hallway setting at varying

distances from the robot. The obstacle detection program is run to determine which obstacles are

actually detected and translated to the map. Obstacles of each different height being used are tested at

each distance away to find the bounds of detection and any error involved.

For obstacle avoidance, a setup on the open floor of the SAS lab is performed. The robot is placed at a

known and specified start position. An obstacle, say a trash can, is placed directly between the robot

and its goal. The goal and trash can have varying fields of influence input to the code as well as specified

locations. Multiple tests are performed to measure the offset of the robot from the desired goal, as well

as testing to see if the obstacle is hit.

Validation Plan

Bearing

The first bearing localization program that uses template matching was not successfully validated due to

time constraints and minor bugs in the code. The program using the flat wall assumption was tested

however. A successful trial needs to be within the design specifications of less than 1 degree of error.

58

After each run, the robot should be lined up, within one degree, to have the Xtion parallel to the wall.

This appears to be a large amount of error especially for small degree increments, but when the code is

looped the error becomes much lower. At least 100 trials are needed, more depending on variances and

outliers in the data. The minimum, maximum, and average degree offset output from the code can be

seen in the validation data section.

The iRobot create introduces error with each turn it makes. It can either not fully rotate to the desired

degree or not rotate far enough. There is also some lateral movement when multiple rotations occur.

When the system is set to rotate at a random angle, that angle is recorded and stored as a value, theta.

Because it is not enough to simply eyeball the robots position to verify that it turned correctly, the

MATLAB script will output the calculated return turn’s respective angle, theta prime. As long as theta

prime is less than 1 degree plus or minus of theta, the trial will be successful.

In an attempt to verify the turn angles of both theta and theta prime, a printed degree chart was taped

to the floor beneath the robot as seen in Figure 47. Testing showed it was extremely difficult to get the

absolute center of the wheels lined up at the 0-180 degree line, so fluctuations occurred. Due to theses

fluctuations, combined with the inherent error with each turn of the iRobot Create, the error in these

manual measurements was very high. Therefore, use of the degree chart was stopped, and only the

angles output from the program, along with visual confirmation of alignment were used for validation

purposes.

Figure 47 - Printed Degree Chart for Validation

Translational XYZ

With regards to the XYZ localization, a successful trial means that the system is able to accurately

localize itself within a 2-D map. In order to meet the design specifications, accurate implies within 3cm

of error for the distance calculations for X and Y on the map. 3cm relates to the higher end of the error

from initial tests. When this error is related to the distance of use of the camera, 0.8m to 3.5m, it is a

59

3.8% error on the minimum value but a 0.9% error in the maximum value. The number of trials run is

determined by the variances in data during testing. If the error from the distance calculated exceeds

3cm, the trial will be considered a failure. If the script also fails to localize the robot in the proper

location on the map, the trial will be considered a failure.

Measuring tapes were used to measure the actual distances traveled by the robot. Measurements were

taken from the center point of the Xtion camera. When the robot was commanded to move a specified

number, either in the X or Y direction, the actual distances were recorded as can be seen in the setup in

Figure 48. The actual distances were compared to the distances corrected and output by the robot

program. Minimum, maximum, and average error for both the X and Y distances are analyzed in the

validation data section.

Figure 48 - Robot Localization Testing


With regard to the subject recognition software, the QR code scanner should be able to recognize the

guest (i.e. scan QR code) within 15 seconds of viewing the code. A successful trial will be under the 15-

second recognition time set by the design specifications. Varying distances will be tested repeatedly to

find the limitations of the program and Xtion sensor. The program will also be tested with QR codes that

60

are not in the database. If the QR code that is not in the database is determined to be accepted, then

the trial is a failure. Minimum, maximum, and average times of detection are analyzed in the validation

data section.

Elevator

Initial testing of the elevator program was very promising, and numerous trials were successfully

performed. Due to time constraints and limitations in performing the tests in the elevator during

operating hours, completed validation data was unable to be collected.

Object Avoidance/Detection

In order to validate the obstacle avoidance program, a specific setup was used. The robot was placed at

a start position on the map of (50,50). The trashcan to be avoided was placed at (165,186), while the

goal was located at (220,306). The scaling factor for the goal was β=0.4, and that for the obstacle was

α=0.3. These parameters were held constant throughout the testing. The error found between the

measured (X,Y) final location and the code output (X,Y) final location was determined. The

minimum, maximum, and average error can be seen in the validation data section.

Figure 49 - Object Avoidance Example

Validation Data

Rotational Localization

The validation for the Rotational Localization code was done by performing 10 trials of the code at each

angle ranging from -25° to 25°. This testing totaled 110 trials with changing the angle run at 5°

61

increments. Each of the trials had the robot correcting itself back to a zero angle set being

perpendicular from a wall. The final degree was measured using a large protractor setup and the error

calculated. Complete trial data for Rotational Localization can be seen in Table 22 in Appendix C.Table

22 - Complete Rotational Localization Trial Data

Table 5 - Overall Rotational Localization Data

Direction Error (degrees)

Average 0.9

Min. 0.0

Max. 4.0

The trials of this code showed a maximum error of 4° off from perpendicular and a minimum error of 0°

off. The error was averaged across all trials showing an average value of 0.9° off from the perpendicular.

Our design specifications state that the robot should be able to localize within 1° of error which some of

the data does not fall into but this degree of error comes in for just one iteration of the code. When

iterated, limits can be set to make sure the robot is under a degree and putting the robot within

specification.

Translational XYZ

The validation for the Translational XYZ code was split into two sections for both the of the codes

functions. 25 trials were run of the Z-offset distance from the wall at various distances. The robot was

placed at distances ranging from 70cm to 120cm to the wall and was commanded to adjust back to an

80cm distance. Each time the position of the robot was then measured and the error was calculated.

Complete trial data for Z Depth Localization can be seen in Table 18 in Appendix C.

Table 6 - Overall Z Depth localization Data

Depth Error (cm)

Average 0.74

Min. 0

62

Max. 2

The error from this test showed a max error value of 2cm and a minimum error of 0cm. The error was

also averaged from all trials showing an averaged value of 0.74cm. Seeing the maximum value of 2cm is

within our design specifications of less than 3cm of error.

The second part of the code checked of the robots X displacement from where it was to where it was

supposed to be. 50 trials were run ranging the displacement from 20cm to the left of the QR code to

20cm to the right of the code. At each location test data output from the robot on where it was. This

was compared to the actual measured distance and error was calculated. Complete trial data for X

Displacement Localization can be seen in Table 19 in Appendix C.

Table 7 - Overall X Displacement Localization Data

Displacement Error (cm)

Average 1.39

Min. 0.10

Max. 3.12

The error from this test showed a max error value of 3.1cm and a minimum error value of 0.1cm. The

error was averaged across all trials showing an average value of 1.4cm. Though the average value is

within the design specification of less than 3cm, the maximum error was 0.1cm over this value. This is

only a small amount of error over design specification and with minor adjustments the code could

become within specifications.

Guest Recognition

The validation for the Guest Recognition code was done by performing 15 trials at varying distances. A

distance of 20cm was chosen at the start and 5 trials run. Each set saw an increase of 10cm of distance

with 5 more trials run. For each trial, a note was made if the QR code was scanned at the distance as

well as the time it took for the code to be scanned. Complete trial data for Guest Recognition can be

seen in in Appendix C.

63

Table 8 - Overall Guest Recognition Data

Time to Scan (s)

Average 2.108

Min. 1.56

Max. 2.91

The trials of this code showed that the QR code was scanned every time run and that the maximum wait

time was 2.91 seconds. This is well within our design specification of 20 seconds for guest recognition.

Elevator

The validation for the elevator code is currently a work in progress as time constrains would not allow

for data to be collected before this report was completed. An addendum to this report will be submitted

when data has been collected. The data for the elevator code will encompass a binary yes and no for

two different needed processes of the code, being able to recognize the correct floor and the robot

exiting the elevator at the correct floor. Trials will be run with the robot traveling from the 4th floor

down to the 1st floor as well as the converse, 1st floor to the 4th floor.

Obstacle Avoidance

The validation for the Obstacle Avoidance code was done by performing 10 trials of the code. The robot

was at an initial measured starting position and was to drive to a goal a measured distance away. An

obstacle was placed in the ideal path of the robot so that the robot would have to successfully calculate

a new path and navigate around. The final position of the robot was measured once it had reached

where it believe the goal to be. The error of where the robot was compared to where the actual goal

was located was then calculated in terms of X and Y displacement. Complete trial data for Obstacle

Avoidance can be seen in Table 26 in Appendix C.

Table 9 - Overall Object Avoidance Data

X Coord. Error (cm) Y Coord. Error (cm)

Average 24.1 50.4

64

Min. 1.0 23.0

Max. 38.0 63.5

The trials of this code showed a maximum error of 24.1cm in the X-direction and 50.4cm in the Y-

direction. The minimum error shown was 1cm in the X-direction and 23cm in the Y-direction. It is good

to note that though the error is great from the end position, there is no localization happening in

between the start and the goal location. While the obstacle avoidance code does avoid the obstacle in

the path of the robot on every instance, the distance from the goal is too great to be considered within

specifications.

65

Context and Impact

Economic Analysis

The materials being used for the robotic guide were chosen using a decision matrix, with the lowest cost

and availability having utmost importance. This has shaped the design by allowing for very simple

production, aside from the creation of the program. The programming will inherently be the most

complex piece due to the fact that multiple codes are being compiled together. The majority of the

components used for the design are existing materials from the SAS laboratory; therefore the cost of

materials for the proof of concept will be relatively low. The projected costs of all materials can be seen

in Table 11 found in Appendix A. Since the MATLAB software and all the hardware are provided, the

actual cost for those materials is essentially zero. All other software is available for free. The only

potential cost will be for the support materials to support and house the Xtion Pro and laptop. Those

parts will be fabricated in the machine shop with little to no cost of production.

Writing, testing, and validation of the programming will be the most time consuming aspect of the

design. Since this is being completed as a senior design project, that programming cost is zero.

Otherwise, the labor cost of the programming would be a main component of the budget. If this robotic

guide went to production the only costs would be the development cost of the program, and the

additional pieces of hardware. Since the majority of hardware is off-the-shelf, those costs will remain

relatively constant.

The market for a robotic guide is small with this specific application in mind, although, the robot is being

designed with expandability to other markets. Ease of swapping components and changing the

programming allow for this. Other robots have been created in this market, and were mostly used for

research projects. One example was created in 1998 called “MINERVA: A Second-Generation Museum

Tour-Guide Robot”. MINERVA was online for two weeks and was used as a tour-guide in the

Smithsonian’s National Museum of American History. MINERVA was the second generation following

RHINO, which was used in the German Museum Bonn in 1997. Honda also tried to use its ASIMO robot

as a guide in the Miraikan Science Museum in Tokyo in June 2013. Unfortunately, it was shown to have

very limited capabilities when it came to human robot interaction. Currently, there is not an off-the-

shelf robot that can be purchased for this specific application. All of the examples mentioned earlier are

not available, and are used for research purposes.

66

Environmental Analysis

The potential environmental impact of this project is very minimal. The majority of the parts being used

are made from metals and plastics that can be recycled depending on what recycling services are

available in different areas. The only part that could have an environmental impact is the rechargeable

3000 mAh NiMH Battery. Nickel-metal-hydride batteries contain nickel and electrolyte and are

considered non-hazardous waste. These batteries are considered environmentally friendly due to the

fact that they contain only mild toxins. These batteries can be recycled using programs such as

Call2Recycle or a local recycling facility.

Social Impact Analysis

Regarding the specific application of this robot guide in the SAS Lab, there is no direct impact to

available jobs. Currently there is no one employed to specifically perform this job task. When someone

has a guest it is his or her responsibility to greet the guest in the lobby. The job is actually a nuisance to

whoever is expecting a guest in the building. It takes time for someone to go down to the lobby and

guide their guest up to the lab, therefore, having a robot perform this task allows the main user to

continue working and not lose any productivity. There is no reason to pay someone to do this job on a

daily basis due to the limited number of guests in the building. Using a robot guide improves the quality

of life for the user and allows them to work more efficiently. This has shaped the design to make the

robot as autonomous as possible. With the end goal of having the main user just push a button to start

the robot, there is little needed interaction from the main user.

According to A Roadmap for U.S. Robotics 2013, “An effective socially assistive robot must understand

and interact with its environment, exhibit social behavior, focus its attention and communication on the

user, sustain engagement with the user, and achieve specific assistive goals”. This is the basis for the

social impact of the design of this robot guide. Without effective communication to the user, this robot

is not very useful. The guests must be comfortable while interacting with the robot guide and continued

engagement with the robot is necessary. Otherwise the robot might lose the guest or the guest would

seek help from a human.

67

Ethical Analysis

There are numerous ethical implications involved with human-robot interaction. The robot must always

act in a safe and ethical manner, while effectively completing the main tasks at hand. The users and

guests are considered potentially vulnerable with respect to physical interactions with the robot. The

robotic guide completes its tasks through social rather than physical interaction. Any sort of physical

interaction would be due to error with obstacle avoidance and other unforeseen fast changes in the

environment. To have an ethical and safe robot, many safety features are being implemented to

minimize the potential for human injury. The guests must also be able to trust the robot. Most people

will most likely be very unfamiliar with a robot guide, and since it is a new technology they may have a

hard time trusting the robot. Since the building has many other departments not involved with the SAS

Laboratory, there are a lot of other guests that will be around the robot. This can also have some

implications as to what the other guests think of the robot. It will be designed to be as least threatening

as possible to avoid any issues with a sense of security in the guests.

Aside from physical interactions, there are also emotional interactions possible with the robot guide.

“While socially assistive robotics do not typically involve any physical contact between the robot and the

user, the interaction may result in unwanted emotions such as strong attachment or aversion” [1]. This

is important because the users and guests must be dependent on the robot guide to complete its tasks,

but nothing more, unless future upgrades provide that. It is also necessary for guests to not have an

aversion toward the robot. If this occurs, the guest might not want to listen to the robot, and try to seek

help from a human instead, which completely defeats the purpose of using the robot guide. This will be

considered in the design process so problems such as these do not arise.

While there are many concerns with the robot’s interaction with humans, security is also a major

concern. The building has secure entrances where anyone who enters either needs to swipe a keycard

or they must show I.D. to a security guard at the main desk. In order to maintain the security of the

building and the people inside, the robot guide must be designed to mitigate any security risks. The

keycard attached to the robot must be secured, as well as the laptop running the robot. The guest

recognition software will allow the robot guide to determine if the guest being greeted is the expected

person, so the robot can determine if that person should be allowed into the building. The security

guards in the building will also have to be briefed on the motivation and job of the robot so they are not

surprised by its function.

68

Discussion/Future Work

Care was taken to introduce elements of modularity within the overall design of the system. The initially

proposed system is designed to serve as scaffolding for design improvements later down the line. This

promotes the overall robustness of the design and enables basic functionality of the robotic

guide/usher. For example, the current design proposes utilizing a QR code scanner to identify the guest

before the “ushering” process can begin.

The overall goal of senior design is to create a case study with broad sweeping possibilities and

opportunities. The field of autonomous mobile robotics is on the precipitous of broad changes to society

and general life. The vision of a robotic assistant for common activities is hampered by the limited

development within the field of human robot interaction. One exciting case is Honda’s ASIMO. “Honda's

famed humanoid robot ASIMO has many skills, including running, serving drinks, hopping on one foot,

and kicking a soccer ball. And it absolutely loves to dance” [23]. However, ASIMO’s capabilities are less

indicative of automation, but rather of puppeteering. Scientists and engineers carefully work behind the

scene to manipulate and control ASIMO’s actions. Although strides have been made toward introducing

more autonomy to ASIMO, this example serves as a clear indicator that fully automated robots which

efficiently and accurately interact with humans are a ways away. With this realization in mind, the senior

design project acts as a case study in which human robot interaction can be studied.

Conclusions

The project proved to be a successful attempt at creating a proof of concept system that can be used to

further robot human interaction. The robot was able to perform the tasks laid out before it, including

greeting and verifying a guest, traversing an elevator and navigating a hallway safely. While there is

more work needed to be done before a human can easily interact with the system, the ground work has

been laid and the current system does allow for some minor interaction. The data collected, shown in

the Validation Data section, proved that the system is robust and can function when ran. The modular

structure was maintained throughout the project to allow for further upgrading at the conclusion of the

project. This further upgrading can include both hardware and software upgrades.

69

Future Work

Hardware Upgrades

The codes written for this project have been made to be modular and portable to more advanced

hardware with little to no changes to the code. This could be demonstrated with the Guest Recognition

script working for both the Asus Xtion Pro as well as a HD Webcam added to the robot later in the

design. With that, some hardware upgrades could accompany continued work on this project to better

improve functionality.

The vision system of the robot used both an Xtion Pro as well as an HD Webcam to observe its

environment and capture images. Upgrades could include adding a mechanical servo to change the

height of a single camera for use at multiple heights. Additional depth cameras or sensors could be

added to the robot to create code to determine which of the four elevators of the building opens for

travel. If these two options are not viable, a depth camera that has better resolution than the Xtion

could be swapped out for better obstacle detection and depth measurements.

The robot base, iRobot Create, gets the job done for a proof of concept but does have its errors with it.

The maximum movement speed of the Create compares to a slow shuffle for average humans. The

Create also saw an error in the motors not starting up at the same time correctly. This error mostly

occurred while turning, which lead to an offset in position and angle, especially over long distances. It

would be suggested to have a robot with more accurate movement as well as a better top speed and

stability.

Facial Recognition

Guest recognition is a crucial aspect in the design of the robot guide. The robot must be able to

recognize the guest or guests that are visiting the SAS Lab. In order to do this several different methods

of guest recognition are possible. For the early stages of the design, QR codes will be used to accomplish

this task. QR codes are free and easy to generate, and specific codes can be generated for different

guests. The guest will have the QR code on their smartphone, emailed from their host. The Xtion Pro

camera will be used to scan the code, and the specific codes will be programmed into MATLAB for the

robot to recognize. For this to work, the robot will have to wait in the lobby for the guest to approach it

and scan the code. If the code is not recognized, the robot will continue to search for the correct code

before proceeding to guide the guest to the desired floor.

An additional method of guest identification is facial recognition. This will have to be developed and

tested throughout the design process. It is much more complex than QR code recognition, since many

factors are involved such as lighting, facial expressions, and facial orientation. It also requires a database

70

of pictures of all the possible guests that will be entering the building. This type of software is available

from numerous different sources, but a software package that can be used in MATLAB is preferred. In

order to use facial recognition in MATLAB a program such as Eigenfaces can be used. The simplified

steps of the recognition software are as follows: first the face needs to be detected, second the depth

data and key points on the face are determined, and lastly the data is compared to the stored facial

data. An outline of a typical face recognition system can be seen in Figure 50 below.

Figure 50 - Outline of facial recognition system [24]

The source code for the Eigenfaces program in MATLAB is readily available from Santiago Serrano at the

Drexel University Robotics Lab4 . Compiling this code into MATLAB will allow for the use and

manipulation of the Eigenface program to be used with the robot guide. The success rate for facial

detection using this program will be validated and tested. Another issue using this type of facial

recognition is the fact that every possible guest needs to have his or her picture programmed into the

database. If there are any new guests, a picture will need to be obtained and the program will have to

be edited to allow for their face to be recognized. For this proof of concept, a limited number of guests

will be tested to verify the software.

4 http://www.pages.drexel.edu/~sis26/Eigenface%20Tutorial.htm

http://www.pages.drexel.edu/~sis26/Eigenface%20Tutorial.htm

71

Speech Recognition

Speech recognition is technology that allows for a person to use spoken word as input into a computer

system. It allows for people to replace keyboards and mice with a microphone, where all they need is

their voice. Not only does this often allow for faster easier input, it also grants those with certain

physical disabilities a chance to experience technology that they normally cannot. Typical uses of speech

recognition are automatic phone systems, virtual personal assistants and electronic transcribing systems

[25].

Speech recognition has evolved throughout the years, but the most common practice remains Speech to

Text (STT). A user says something and the computer produces a written transcription. Unfortunately this

process is very complex and often leads to errors, however when executed properly can significantly

enhance a product, take for an example a cellphone that can write texts from spoken word.

The process works by using an analog-to-digital convertor to translate the analog wave (collected from a

microphone) into digital data that the computer can analyze. The software then applies filters to remove

unwanted noise and adjust volume levels. After processing, the software breaks the data down into

smaller segments and matches them against a database of phonemes, which are all the different sounds

that make up a language. Using the matched phonemes, the software compares the analyzed data

against its own library and determines the best match for the word [26].

With regards to the robot, speech recognition could be used to confirm or question commands given by

the robot. The robot could ask the user to follow it and confirm that they are able to before leaving the

lobby.

The entire method is very similar to facial recognition, however speech recognition uses a general vocal

database and does not need audio samples from guests, making it a very feasible option. Unfortunately,

the system does normally require someone to speak clearly and could run into issues if a user has a thick

accent or has trouble speaking.

72

Speaker Recognition

Speaker recognition is a process that is used to identify someone based on different elements of their

vocal patterns. The system is normally divided into two different elements, identifying a person and

verifying a person. Identification is used to determine whether or not someone could be with a group of

people. Verification on the other hand is used with just a single active voice to determine if someone is

who he or she claims to be. Speaker recognition technologies are still in their infancy, but there are well

known companies, including AT&T and TI that are currently trying to further the field [27].

This project deals with verifying that a guest is who they claim to be, therefore will focus on the

verification element of recognition. An individual approaches the system and speaks a predetermined

phrase or word. This individual has previously said this same phrase or word to the system before, which

has compiled a database of different possible speakers. The individual’s speech is analyzed and different

features extracted, and compared against the database. If it is found that the patterns in the speaker’s

voice match those from the database, the speech is accepted and the person verified.

While there are many uses for this type of system, particularly with phone systems, its application in this

system will be difficult. There are a lot of errors that can arise when using this system with respect to

both human interaction and the system itself. With human interaction, there are a lot of different things

that can affect speech. Someone who is under a lot of stress or duress will not sound the same as they

would in a happier more relaxed state of mind. Someone who may enunciate their words clearly

throughout the day may have an issue mumbling early in the morning. If a person comes down with a

head cold the day of a meeting, their voice will likely sound different than had they been healthy. There

are also issues with hardware. A different microphone used for initial database screening and for

verification could create different patterns that the system may not recognize. Poor acoustics in a room

can create a lot of interference, also disturbing the perceived speech patterns [28]. Due to all the

uncontrollable variables, speaker recognition will be a backup plan if there are issues with image

recognition.

73

Elevator

Service Panel

The service elevator uses a different panel than the normal passenger elevators. Instead of illuminating

a bright white light, the service panel consists of small dots that light up orange when pressed. In order

to deal with this, a color extraction can be performed. First an image of the elevator panel must be

captured, similar to the earlier Elevator section. The image is then saturated to increase the intensity of

the orange color and make it more prevalent. Using this new image, a hue value range is selected that

encompasses the orange of the button [29]. Anything that does not fall into that range is turned to

black, while the orange is then turned to white, allowing for now for a similar process to the current

elevator methodology.

Elevator Door Detection

While the robot is successfully able to exit the elevator, it cannot enter it on its own. The ability for the

robot to independently be able to enter the elevator would help greatly increase the successfulness of

the project. The robot needs to be able to determine which elevator is its goal elevator, verify that the

doors are open, and navigate to its position behind the control panel. The two most feasible ways of

achieving this, given the current hardware, are via speech recognition and template matching.

Elevator Speech Recognition

One method of attempting to access the elevator utilizes the visitor that the robot is guiding. Because

the project is a study on robot human interaction, this method could help make the entire system more

interactive. Using the pre drawn map, there could exist as many goals as there are elevators. Once the

robot and visitor reach the elevator bank, the robot can prompt the visitor to tell it what number

elevator has arrived. Using the numbers labeled next to the elevator, the visitor can identify which

elevator arrives. Using the methodology in the Speech Recognition section above, the robot can

translate that number into a goal location and proceed to the specified location. Once at the location in

front of the desired elevator, it can scan the depth to verify that the doors are open. If the depth is too

short, it would mean that the doors are currently shut, and the system would wait until they opened, i.e.

depth distance increasing, before moving.

Template Matching

The other most feasible method of detecting which elevator arrived would be through template

matching. The robot could navigate to the center of the elevator bank and proceed to rotate between

viewing each different elevator. At each elevator view, shown by the numbers one through four in

Figure 51 below, the system would run a template match. It would compare current view to a template

image, of that view with the doors open.

74

Figure 51 - Rotating Elevator Bank Views

If the system found a match, it would know that that elevator had arrived. However this method is very

time consuming and may not work with such a low resolution camera. There is also likely to be issues

with other guests walking in front of the system, obstructing it while it is trying to capture an image

which could cause it to miss an elevator.

75

Project Management

Multiple tools are being used to aid in the organization of the project. Google Drive5 is used to keep all

of the miscellaneous files in a central location. Thanks to desktop integration, it allows for simultaneous

file editing without the need for an intermediary step. Storing the documents online also helps reduce

possible losses due to computer crashing or accidental deletion. This also allows for editing and viewing

of all files from any location. Freedcamp6 is used as a central discussion hub, allowing for easy

communication between team members. It also serves as a shared calendar full of due dates for

different tasks and milestones. The wall also allows important links and messages to be posted for the

entire group to see. GanttProject7 software was used to create a clear and easy to read Gantt chart. The

Gantt chart being used for this project can be found at Figure 55 in Appendix A. The Gantt chart shows

an overall picture of when the main tasks will be worked on and completed. Many deliverables from the

fall and winter quarters that have been completed can be seen as such on the Gantt chart.

The project is currently on budget and details can be seen in the budget table at Table 11 in Appendix A.

All components have been provided thus far from the SAS Lab. The materials that are not readily

available in the SAS Lab will be ordered from McMaster-Carr. The parts that need to be ordered make

up the main support materials for the robot. Any additional costs incurred will be out of pocket or

provided by the SAS Lab, and are currently planned to be zero to none.

SVN Repository

Atlassin SourceTree is a free Git and Mercurial client for both Windows and Mac. It is being used as the

desktop client to create, clone, commit, push, pull, and merge all of the code that is being created for

the robot. This is all being done through an online Bitbucket repository, which has multiple branches for

the different members of the team. This repository allows for convenient sharing of all the code being

worked on as well as keeping all the different versions of the code. The different commits to the

repository clearly show any changes made to each part of the code for easy tracking and debugging. The

different repositories created and used in this project can be found at

https://bitbucket.org/drexelSR10/navigation and https://bitbucket.org/drexelSR10/qr-code-integration.

Both repositories are open source and available.

5 http://www.google.com 6 https://freedcamp.com/ 7 http://www.ganttproject.biz/

https://bitbucket.org/drexelSR10/navigation

https://bitbucket.org/drexelSR10/qr-code-integration

http://www.google.com/

https://freedcamp.com/

http://www.ganttproject.biz/

76

References

[1] Robotics VO, "A Roadmap for U.S. Robotics: From Internet to Robotics," in Robotics in the United

States of America, 2013.

[2] S. Thrun, M. Bennewitz, W. Burgard, A. B. Cremers, F. Dellaert, D. Fox, D. Hahnel, C. Rosenberg, N.

Roy, J. Schulte and D. Schulz, "MINERVA: A Second-Generation Museum Tour-Guide Robot".

[3] OpenNI, "Open Source SDK for 3D sensors - OpenNI," [Online]. Available: http://www.openni.org.

[Accessed 28 November 2013].

[4] D.-J. Kroon, "Kinect Matlab SDK," 06 June 2013. [Online]. Available:

http://www.mathworks.com/matlabcentral/fileexchange/30242-kinect-matlab.

[5] iRobot Corporation, "iRobot Create Open Interface".

[6] J. M. Esposito, O. Barton and J. Kohler, "Matlab Toolbox for the iRobot Create," 2011. [Online].

Available: http://www.usna.edu/Users/weapsys/esposito/roomba.matlab/.

[7] S. Thrun, W. Burgard and D. Fox, Probabalistic Robotics Textbook, 2000.

[8] B. Mader, "Localization and Mapping with Autonomous Robots," Radboud University Nijmegen,

Nijmegen, 2011.

[9] S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, 2000.

[10] H. Bay, A. Ess, T. Tuytelaars and L. Van Gool, "Speeded-Up Robust Features (SURF)," Elsevier, Zurich,

2008.

[11] A. Dutta, A. Kar and B. Chatterji, "Adaptive Corner Detection Algorithm and its Extension using

Window-based Approach for Gray-scale Images," IETE Journal of Research, 2011.

[12] Microsoft Corporation, "Kinect for Windows Sensor Components and Specifications," [Online].

Available: http://msdn.microsoft.com/en-us/library/jj131033.aspx.

[13] OpenNI, "Reference Guide," 2014. [Online].

[14] Mathworks, "extractFeatures," Mathworks, 2014. [Online]. Available:

77

http://www.mathworks.com/help/vision/ref/extractfeatures.html. [Accessed January 2014].

[15] Mathworks, "matchFeatures," Mathworks, 2014. [Online]. Available:

http://www.mathworks.com/help/vision/ref/matchfeatures.html. [Accessed January 2014].

[16] "Wiki Image," 6 February 2014. [Online]. Available:

http://en.wikipedia.org/wiki/File:Motion_planning_configuration_space_curved_valid_path.svg.

[Accessed 12 March 2014].

[17] H. Safadi, "McGill Hill College - Comp 765 Presentation," 18 April 2007. [Online]. Available:

http://www.cs.mcgill.ca/~hsafad/robotics/index.html . [Accessed 12 March 2014].

[18] Mathworks, "Find Image Rotation and Scale Using Automated Feature Matching," Mathworks,

2014. [Online]. Available: http://www.mathworks.com/help/vision/examples/find-image-rotation-

and-scale-using-automated-feature-matching.html. [Accessed April 2014].

[19] Mathworks, "nnz," Mathworks, 2014. [Online]. Available:

http://www.mathworks.com/help/matlab/ref/nnz.html. [Accessed April 2014].

[20] S. Deng, "Matlab Central File Exchange," 26 December 2007. [Online]. Available:

http://www.mathworks.com/matlabcentral/fileexchange/18091-text-to-speech. [Accessed 2014].

[21] ZXing, "Official ZXing ("Zebra Crossing") project home," Github Inc., 2014. [Online]. Available:

https://github.com/zxing/zxing. [Accessed January 2014].

[22] L. Shapira, "QR Code encode and decode," Mathworks File Exchange, 2 November 2010. [Online].

Available: http://www.mathworks.com/matlabcentral/fileexchange/29239-qr-code-encode-and-

decode. [Accessed January 2014].

[23] J. Falconer, "Honda's ASIMO Gets New Job at Museum," 8 July 2013. [Online]. Available:

http://spectrum.ieee.org/automaton/robotics/humanoids/honda-asimo-humanoid-robot-gets-

new-job-at-museum.

[24] I. Atalay, "Face Recognition Using Eigenfaces," M. Sc. THESIS, Istanbul, 1996.

[25] N. Unuth, "What is Speech Recognition?," About, [Online]. Available:

http://voip.about.com/od/voipbasics/a/What-Is-Speech-Recognition.htm. [Accessed January 2014].

[26] E. Grabianowski, "How Speech Recognition Works," HowStuffWorks, 10 November 2006. [Online].

Available: http://electronics.howstuffworks.com/gadgets/high-tech-gadgets/speech-

78

recognition.htm. [Accessed February 2014].

[27] S. Furui, "Speaker Recognition," in Survey of the State of the Art , 1996.

[28] J. P. C. JR, "Speaker Recognition: A Tutorial," in Proceedings of the IEEE, VOL. 85, 1997.

[29] gnovice, "Isolating Hues in Matlab," stackoverflow, 21 October 2010. [Online]. Available:

http://stackoverflow.com/questions/4063965/how-can-i-convert-an-rgb-image-to-grayscale-but-

keep-one-color. [Accessed April 2014].

[30] A. K. Lam, "ARCHITECTURE AND APPLICATION OF AN AUTONOMOUS ROBOTIC SOFTWARE

ENGINEERING TECHNOLOGY TESTBED (SETT)," University of Southern California, 2007.

[31] J. Borenstein and Y. Koren, "The Vector Field Histogram - Fast Obstacle Avoidance for Mobile

Robots," IEEE Transactions on Robotics and Automation, pp. 278-288, June 1991.

[32] G. N. DeSouza and A. C. Kak, "Vision for Mobile Robot Navigation: A Survey," IEEE Transactions on

Pattern Analysis and Machine Intelligence, pp. 237-267, February 2002.

[33] J. M. Holland, Designing Mobile Autonomous Robots, 2004.

[34] S. Lee, "Depth camera image processing and applications," in IEEE International Conference,

Orlando, FL, 2012.

[35] V. Pappalardo, G. Interdonato, M. P. Russo and A. Vitanza, "An efficient motion system for

autonomous robots," Eurobot, 2007.

[36] F. Xu, V. B. Hendrik, M. Nuttin and R. Moreas, "Concepts for dynamic obstacle avoidance and their

extended application in underground navigation," Robotics and Autonomous Systems, 2003.

[37] "How Rhino Does Its Job," [Online]. Available: http://www.iai.uni-

bonn.de/~rhino/tourguide/html/how.html.

[38] TranSafety, Inc., "Study Compares Older and Younger Pedestrian Walking Speeds," Road

Engineering Journal, 1997.

[39] J. Ballester and C. Pheatt, "Using the Xbox Kinect Sensor for Positional Data Acquisition," Kansas,

2011.

80

Appendices

Appendix A

Table 10 - Parts List for Assembly

81

Table 11 - Budget Table

Description Qty. Unit Price ($) Cost ($)

iRobot Create 1 219.99 219.99

Microsoft Kinect 1 179.99 179.99

Laptop (Acer Ferrari) 1 599.00 599.00

Matlab 1 500.00 500.00

Optically Clear Cast Acrylic Sheet 2 16.36 32.72

Optically Clear Cast Acrylic Sheet 1 5.49 5.49

Aluminum Inch T-Slotted Framing System

- Four-Slot Single 1 26.90 26.90

Aluminum Inch T-Slotted Framing System

- 90 Degree Bracket 3 4.06 12.18

Zinc-Plated Steel End-Feed Fastener 4 2.71 10.84

Aluminum Female Threaded Round

Standoff 6 1.36 8.16

Aluminum Male-Female Threaded Hex

Standoff 4 0.56 2.24

Zinc-Plated Steel Pan Head Phillips

Machine Screw 1 2.50 2.50

Zinc-Plated Steel Pan Head Phillips

Machine Screw 1 3.44 3.44

Creative Live Webcam 1 22.20 22.20

Total Cost 1603.45

82

Table 12 - Decision Matrix

Cost Availability Research Mounting

Ability Feasibility

Multiple

Sensors Modularity Total

Robot Hardware

iRobot Create 4 5 4 4 4 4 3 28

Custom Design 3 3 1 5 3 3 4 22

Parallax Eddie Robot 2 4 2 4 2 3 2 19

Sensors

Asus Xtion Pro 3 4 0 5 5 4 4 25

Acoustic Sensor 4 2 0 5 4 1 3 19

Camera Sensor 2 2 0 5 4 1 3 17

Laser 1 1 0 5 3 1 3 14

Recognition Methods

QR Code 5 5 4 0 4 0 3 21

Auditory 5 3 3 0 1 0 1 13

Facial Recognition 5 3 4 0 2 0 2 16

Obstacle Avoidance

Vector Field Histogram 5 0 2 0 3 0 5 15

Virtual Force Fields 5 0 4 0 4 0 5 18

Table 13 - Project Needs (1 of 2)

83

Design Category Need Specs Associated

A1 Aesthetics Is not visually alarming to guests 35

B1 Business Design cost within given budget 17

D1 Documentation General documentation of programming 29

D2 Documentation General documentation for usage 29

Du1 Durability Resistant to structural damage due to impact 20,21,22

Du2 Durability Secure components 33

Du3 Durability Stability - will not topple over 14

Du4 Durability Operates normally with minimal maintenance 39

Du5 Durability Failed parts can be repairable or replaced easily 44

Env1 Environmental Recyclable Components 45

Eth1 Ethics Maintain privacy of guests 46

Eth2 Ethics Guests trust robot to guide them to correct location 35

Op1 Operation Minimize need for human input 42,15

Op10 Operation Optimal/quick response time to input 2,16

Op11 Operation Hardware optimally mounted 25,26

Op2 Operation Minimize impact on duration for elevator ride 7,27

Op3 Operation Sudden object detection 6

Op4 Operation Weight appropriate for maneuvering 9

Op5 Operation Easy to reset if errors occur 15,16

Op6 Operation Must work consistently 42

Op7 Operation Must use available power supply 19

Op8 Operation Operates under normal atmospheric conditions 47

Op9 Operation Optimize travel speed for timely completion of tasks 28,16

Table 14 - Project Needs (2 of 2)

84

Design Category Need Specs Associated

P1 Programming Robust Guest Detection 4,5,38

P10 Programming Error handling 41

P2 Programming Modular Software Components 40,41

P3 Programming Robust obstacle avoidance 6,34,36,41

P4 Programming Code is easy to follow 32,48,49

P5 Programming Proper thread start, interrupt, and stop processes 4,15,16

P6 Programming Modular code structure 32

S1 Safety Potential for injuries during normal operation mitigated 43,31,1

S2 Safety Appropriate form factor for quick visual detection 13

S3 Safety Product must pass all standards 50,51

S4 Safety Noise output 8

Sec1 Security Theft resistant cage for laptop and Xtion Pro 37

Sec2 Security Stealing Robot 37

Sec3 Security Locking keycard 37

Sec4 Security Utilization of Computer GPS 37

St1 Storage Ease of storage 18

St2 Storage Minimize footprint 18

Su1 Set-up Minimal set-up required 16

Su2 Set-up Reliable power source 10,11,12

85

Table 15 - Specifications and Metrics (1 of 2)

Spec # Priority Need Metric

1 Max Impact Velocity S1 <.01 m/s

2 Travel Speed Op10 0<x<.25 m/s

3 Distance to Recognize Visitors P1 2 < x < 4 ft.

4 Time to recognize guest P1, P5 <20s

5 Number of visitors can recognize at once P1 1 guest

6 Distance to stop when obstacle detected Op3, P3 x < 2 in.

7 Time to enter elevator Op2 > 25 sec.

8 Noise output S4 < 40 dB

9 Total Weight Op4 < 25 lbs.

10 Robot Battery Life Su2 2 hours

11 Computer Battery Life Su2 3 hours

12 Trips before having to charge Su2 5

13 Visible from S2 20

14 Can withstand a toppling force Du3 15 lbf.

15 Number of inputs from user Op1, Op5, P5 1

16 Time to initiate sequence Op10, Op5, Op9, P5 5 sec.

17 Amount of $ Spent B1 $0 < x < $100

18 Footprint St1,St2 Area < 1 foot x 1 foot

19 Battery Inputs Op7 120 v

23 Response time to external disturbances P3 0.5 sec.

24 Level of sound required for proper voice commands P1 60 dB

25 Field of view for Xtion Pro camera Op11 43° vertical by 57° horizontal

field of view 26 Height of Xtion Pro for optimal range of view Op11 30<x<50in

86

Table 16 - Specifications and Metrics (2 of 2)

Spec # Priority Need Metric

27 Time to Exit elevator Op2 < 6 sec.

28 Total time to guide 1 guest Op9 x < 5 min.

29 Number of documents/manuals D1, D2 > 2

30 # of organizational compartments or ties St2 < 5

31 Distance of protrusion for any component from main body S1 < 2 in.

32 Annotated coding P4, P6 0.8

33 Displacement of components during operation do not exceed Du2 < 0.2 in.

34 Number of objects obstacle avoidance functions able to handle P3 < 2

35 Number of human like features A1 > 2

36 Stopping Time P3 <.5s

37 Different types of security features Sec1,Sec2,Sec3 3

38 Amount of Visitors can recognize at once P1 1

39 Operating time before component failure Du4 300 hrs.

40 Potential guest recognition methods P2 x>2

41 Potential methods of obstacle avoidance P2, P10, P3 x>1

42 Autonomous reliability Op1, Op6 0.7

43 Success rate of obstacle avoidance S1 0.7

44 Off-the-shelf parts for replacement/repair Du5 0.85

45 Percent of recyclable components Env1 0.8

46 Amount of stored guest information Eth1 0.2

47 Office temperature Op8 60 < x < 85 °F

48 Minimize redundant functions P4 yes

49 Clear and logical name schemes P4 yes

50 Safety requirements for personal care robots S3 ISO/FDIS 13482

51 Performance criteria and related test methods for service robot S3 ISO/AWI 18646-1

Table 17 - Event Breakdown

88

Figure 52 - Flow Chart – Elevator to Lobby and Back8

8 Project functions on the basis that robot will begin in elevator, and will be placed back upon

completion

89

Figure 53 - Flow Chart - Elevator to Room

90

Figure 54 - Fall Term Gantt Chart

91

Figure 55 – Winter Term Gantt Chart

92

Appendix B

Figure 56 - Detail Model Drawing

93

Appendix C

Table 18 - Complete Z Depth Localization Trial Data

Offset Distance - Z Actual Offset Correction Result Error

(cm input to roomba) (cm measured) (cm measured) (cm difference)

90 90 82 2

95 95 82 2

100 100 82 2

85 85.5 81 1

105 105.5 80 0

100 100 82 2

90 90 81 1

95 95 82 2

85 85 81 1

105 105.5 80 0

110 110 80 0

110 110 80 0

115 115 80 0

120 120 80 0

75 75 80 0

70 70 80 0

60 60 79.5 -0.5

65 64.5 80 0

55 55 82 2

120 119.5 80 0

60 60 79 -1

55 55 82 2

65 65 80 0

75 75 80 0

70 70 80 0

94

Table 19 - Complete X Displacement Trial Data (1/2)

Run # Offset Distance - X Actual Offset from center Correction Result Error

(cm input to

roomba)

(cm measured) (cm output from code) (cm difference)

1 0 0 0.4219 -0.4219

2 0 0 0.7017 -0.7017

3 0 0 0.7017 -0.7017

4 0 0 0.9815 -0.9815

5 0 0 0.2791 -0.2791

6 0 0 0.142 -0.142

7 0 0 0.2819 -0.2819

8 0 0 0.5618 -0.5618

9 0 0 0.2819 -0.2819

10 0 0 1.2666 -1.2666

11 10 9.5 9.2145 0.2855

12 10 9.5 9.2145 0.2855

13 10 9.5 9.0709 0.4291

14 10 9.5 9.383 0.117

15 10 9.5 9.0709 0.4291

16 10 9.5 9.2145 0.2855

17 10 9.5 9.2145 0.2855

18 10 9.5 9.3581 0.1419

19 10 9.5 8.9273 0.5727

20 10 9.5 9.2145 0.2855

21 20 20.5 20.6164 -0.1164

22 20 20.5 19.4115 1.0885

23 20 20.5 19.8334 0.6666

24 20 20.5 20.3958 0.1042

25 20 20.5 19.8334 0.6666

26 20 20.5 19.5521 0.9479

27 20 20.5 19.5521 0.9479

28 20 20.5 19.5521 0.9479

29 20 20.5 19.8334 0.6666

30 20 20.5 20.3958 0.1042

31 -20 -20 -17.0477 -2.9523

32 -20 -20 -17.21 -2.79

33 -20 -20 -17.3512 -2.6488

34 -20 -20 -17.5337 -2.4663

35 -20 -20 -17.5337 -2.4663

36 -20 -20 -17.3922 -2.6078

Table 20: Complete X Displacement Trial Data (2/2)

95

Table 21 - Complete X Displacement Trial Data (2/2)

Run # Offset Distance - X Actual Offset from center Correction Result Error

(cm input to

roomba)

(cm measured) (cm output from code) (cm difference)

37 -20 -20 -17.2507 -2.7493

38 -20 -20 -17.2507 -2.7493

39 -20 -20 -17.2507 -2.7493

40 -20 -20 -17.2507 -2.7493

41 -10 -10 -8.3227 -1.6773

42 -10 -10 -7.3151 -2.6849

43 -10 -10 -7.3151 -2.6849

44 -10 -10 -7.3151 -2.6849

45 -10 -10 -6.8832 -3.1168

46 -10 -10 -7.1711 -2.8289

47 -10 -10 -6.8832 -3.1168

48 -10 -10 -6.898 -3.102

49 -10 -10 -7.0272 -2.9728

50 -10 -10 -7.1711 -2.8289

96

Table 22 - Complete Rotational Localization Trial Data (1/4)

Run

#

Degree

Offset

Degree output Final Degree Location

(To Roomba) (Calculated

Correction)

(Error/Difference)

1 5 4.1 0.9

2 5 4.9 0.1

3 5 4.8 0.2

4 5 4.9 0.1

5 5 4.6 0.4

6 5 4.5 0.5

7 5 4.5 0.5

8 5 4.9 0.1

9 5 4.5 0.5

10 5 5.0 0.0

11 10 9.8 0.2

12 10 9.6 0.4

13 10 9.5 0.5

14 10 10.5 -0.5

15 10 9.9 0.1

16 10 11.1 -1.1

17 10 8.6 1.4

18 10 10.5 -0.5

19 10 9.1 0.9

20 10 10.5 -0.5

21 15 17.2 -2.2

22 15 13.8 1.2

23 15 15.0 0.0

24 15 14.2 0.8

25 15 14.8 0.2

26 15 15.8 -0.8

27 15 12.5 2.5

28 15 13.8 1.2

29 15 16.8 -1.8

30 15 11.0 4.0

31 20 22.6 -2.6

32 20 18.3 1.7

33 20 17.8 2.2

34 20 22.6 -2.6

35 20 17.5 2.5

97


Run

#

Degree

Offset



Correction)

(Error/Difference)

36 20 20.6 -0.6

37 20 18.9 1.1

38 20 18.8 1.2

39 20 19.7 0.3

40 20 21.9 -1.9

41 25 25.6 -0.6

42 25 24.7 0.3

43 25 22.7 2.3

44 25 23.1 1.9

45 25 26.5 -1.5

46 25 24.7 0.3

47 25 23.0 2.0

48 25 25.0 0.0

49 25 23.8 1.2

50 25 22.9 2.1

51 0 0.0 0.0

52 0 0.0 0.0

53 0 0.0 0.0

54 0 0.0 0.0

55 0 0.0 0.0

56 0 0.0 0.0

57 0 0.0 0.0

58 0 0.0 0.0

59 0 0.0 0.0

60 0 0.0 0.0

61 -5 -3.8 1.2

62 -5 -5.3 -0.3

63 -5 -6.9 -1.9

64 -5 -3.0 2.0

65 -5 -6.1 -1.1

66 -5 -4.6 0.4

67 -5 -6.3 -1.3

68 -5 -4.6 0.4

69 -5 -6.1 -1.1

70 -5 -5.1 -0.1

98


Run

#

Degree

Offset



Correction)

(Error/Difference)

71 -10 -9.7 0.3

72 -10 -9.4 0.6

73 -10 -8.6 1.4

74 -10 -11.9 -1.9

75 -10 -10.4 -0.4

76 -10 -10.2 -0.2

77 -10 -9.9 0.1

78 -10 -9.4 0.6

79 -10 -9.7 0.3

80 -10 -11.8 -1.8

81 -15 -14.1 0.9

82 -15 -15.3 -0.3

83 -15 -15.1 -0.1

84 -15 -14.3 0.7

85 -15 -15.8 -0.8

86 -15 -14.4 0.6

87 -15 -13.8 1.2

88 -15 -15.8 -0.8

89 -15 -14.1 0.9

90 -15 -15.1 -0.1

91 -20 -19.8 0.2

92 -20 -18.6 1.4

93 -20 -19.4 0.6

94 -20 -19.7 0.3

95 -20 -20.4 -0.4

96 -20 -21.7 -1.7

97 -20 -18.2 1.8

98 -20 -21.8 -1.8

99 -20 -18.9 1.1

100 -20 -21.0 -1.0

101 -25 -22.9 2.1

102 -25 -23.7 1.3

103 -25 -24.4 0.6

104 -25 -23.6 1.4

105 -25 -25.2 -0.2

99


Run

#

Degree

Offset



Correction)

(Error/Difference)

106 -25 -24.7 0.3

107 -25 -24.6 0.4

108 -25 -22.8 2.2

109 -25 -25.9 -0.9

110 -25 -24.2 0.8

Table 26 - Complete Obstacle Avoidance Trial Data

Run #

X Coord. Y Coord. X

Coord. Y

Coord. Hit

Obstacle X Coord. Y Coord.

Measured Measured MATLAB MATLAB Yes/No Difference Difference

1 240 239 208 298 No -32 59

2 229 253 208 298 No -21 45

3 246 234.5 208 298 No -38 63.5

4 234 253 208 298 No -26 45

5 233 242 208 298 No -25 56

6 228 248 208 298 No -20 50

7 240 237 208 298 No -32 61

8 226 243 208 298 No -18 55

9 236 252 208 298 No -28 46

10 207 275 208 298 No 1 23

100

Table 27 - Complete QR Code Reader Trial Data

Run # Distance to QR Code Time to Scan

(Measured in cm) (Seconds)

1 20 2.11

2 20 2.67

3 20 2.91

4 20 2.67

5 20 1.92

6 30 1.78

7 30 1.98

8 30 1.85

9 30 1.77

10 30 1.86

11 40 1.77

12 40 2.87

13 40 1.56

14 40 1.63

15 40 2.27

101

Appendix – QRCodeDetection.m

addpath('Mex')

addpath('Text To Speech');

addpath('QR Processing');

javaaddpath('QR Processing/core-1.7.jar');

javaaddpath('QR Processing/javase-1.7.jar');

SAMPLE_XML_PATH='Config/SamplesConfig.xml';

clc;close all

[num_data text_data] = xlsread('Codes.xls');

n=numel(num_data);

Success=0; % Failure condition

% Start the Xtion Pro Process

Xtion ProHandles=mxNiCreateContext(SAMPLE_XML_PATH);

% Initialize the different cameras

figure;

I=mxNiPhoto(Xtion ProHandles);

I=permute(I,[3 2 1]);

for i=1:360

I=mxNiPhoto(Xtion ProHandles); I=permute(I,[3 2 1]);

I_qr = flipdim(I, 2);

if decode_qr(I_qr) == 0

else

break

end

set(h1,'CDATA',I);

drawnow;

end

message = str2double(decode_qr(I_qr));

% Compare to see if any of the codes from the worksheet match the qr

for i=1:n

if message == num_data(i,1)

102

tts(text_data{i,1})

Success=1;

else

end

end

% If the program isn't successful, give visitor a heads up

if Success==0

tts('Sorry, scan failed. Please try again.')

else

end

close all

mxNiDeleteContext(Xtion ProHandles);

103

Appendix – CreateTemplate.m

addpath('Mex')


% Start the Xtion Pro Process


I=mxNiPhoto(Xtion ProHandles); I=permute(I,[3 2 1]);

RwDT=mxNiDepthRealWorld(Xtion ProHandles); RwDT=permute(RwDT,[2 1 3]);

%D=mxNiDepth(Xtion ProHandles); D=permute(D,[2 1]);

h=figure(1);

hz=imshow(I);

imwrite(I,'Template.jpg','jpg')

104

Appendix – RunComparison.m

close all

addpath('CreateToolbox')

addpath('Mex')


try

%% Initialize System

%[serialObject] = RoombaInit(8); %Initialize Robot %Change port (#)

depending on computer

%Initialize camera

avgDiff=640; %set an initial difference between points, 640 chosen

because of resolution

for i=1:10

if abs(avgDiff)<=20

disp('Lined Up Boss')

break

else

% create template


I2=mxNiPhoto(Xtion ProHandles);

I2=permute(I2,[3 2 1]);

%imwrite(I2,'Compare.jpg','jpg')

% make images gray so harris can be used

I1 = rgb2gray(imread('Template.jpg')); %Template

I2 = rgb2gray(I2); %Comparison

%I2 = rgb2gray(imread('Compare.jpg')); %Comparison

%Find the corners.

points1 = detectHarrisFeatures(I1);

points2 = detectHarrisFeatures(I2);

%Extract the neighborhood features.

[features1, valid_points1] = extractFeatures(I1, points1);

[features2, valid_points2] = extractFeatures(I2, points2);

%Match the features.

indexPairs = matchFeatures(features1, features2);

105

%Retrieve the locations of corresponding points for each image.

matched_points1 = valid_points1(indexPairs(:, 1), :);

matched_points2 = valid_points2(indexPairs(:, 2), :);

%%Visualize corresponding points. You can see the effect of

translation between the two images despite several erroneous matches.

% figure(1);

% showMatchedFeatures(I1, I2, matched_points1, matched_points2);

% Getting Template Data

TempData=parsePoints(matched_points1,1);

CompData=parsePoints(matched_points2,1);

%may have to switch depending on what is x

differenceMat=zeros(length(TempData),1);

%turning

for i=1:length(TempData)

differenceMat(i,1)=CompData(i,1)-TempData(i,1);

end

avgDiff=mean(differenceMat); %taking the average of the difference

in points for more consistent values

if abs(avgDiff)<=20

avgDiff

disp('Lined Up Boss')

break

else

theta=(avgDiff/640)*58 %ratio of distance to resolution (640) * fov

(57 deg)

turnAngle(serialObject,.2,theta)

avgDiff

end

end

end


catch exception


getReport(exception)

end

106

Appendix – XYZTemp_Angle.m

%% XYZ Template Creation

% clear all

% clc

% close all

addpath('Mex_OpenNI1')


try

% Create context to OpenNI and Kinect sensor

KinectHandles=mxNiCreateContext(SAMPLE_XML_PATH);

% Detects the SURF Points and Features from the marker object

T = rgb2gray(imread('chair.jpg'));

T_points = detectSURFFeatures(T);

[T_features, T_valid_points] = extractFeatures(T, T_points);

% Takes a picture and turns it into grayscale

XYZT=mxNiPhoto(KinectHandles);

XYZT=flipdim(XYZT,2);

XYZT=rgb2gray(permute(XYZT,[3 2 1]));

% Takes a depth real world data collection

XYZ=mxNiDepthRealWorld(KinectHandles);

XYZ=permute(XYZ,[2 1 3]);

Z=XYZ(:,:,3);

maxZ=3000; Z(Z>maxZ)=maxZ; Z(Z==0)=nan;

% Detects the SURF Points and Features from the picture

XYZT_points = detectSURFFeatures(XYZT);

[XYZT_features, XYZT_valid_points] = extractFeatures(XYZT, XYZT_points);

% Matched pairs

T_indexPairs = matchFeatures(XYZT_features, T_features);

% Geometric Transform

matchedXYZTPoints = XYZT_points(T_indexPairs(:, 1), :);

matchedTPoints = T_points(T_indexPairs(:, 2), :);

[tform, inlierTPoints, inlierXYZTPoints] =

estimateGeometricTransform(matchedTPoints, matchedXYZTPoints, 'similarity');

107

figure(1)

showMatchedFeatures(T, XYZT, inlierTPoints, inlierXYZTPoints, 'montage');

% Valid points are all the similar points, mpoints are the average

% placement of all of the points.

XYZT_valid_points = parsePoints(inlierXYZTPoints ,1);

XYZT_mpoints = mean(parsePoints(inlierXYZTPoints ,1));

figure(2)

imshow(XYZT)

hold on

plot(XYZT_mpoints(1,1),XYZT_mpoints(1,2),'ro')

% Calculates the angle, X, and Z

XYZT_theta = (XYZT_mpoints(1,1)/640)*58;

XYZT_Z = Z(uint16(XYZT_mpoints(1,2)),uint16(XYZT_mpoints(1,1)));

XYZT_X = XYZT_Z / tand(XYZT_theta);

save('XYZT_Angle.mat','T_points','T_features','XYZT_theta','XYZT_Z',

'XYZT_X')

mxNiDeleteContext(KinectHandles);

% clear all

% clc

% close all

catch exception



end

108

Appendix – Xlocalization_Angle.m

%% Localization (XYZ)

% Assuming that we have an XYZ marker for a given location.

% clear all

% clc

% close all

addpath('CreateToolbox')

addpath('Mex_OpenNI1')


%% Main Code

try

load('XYZT_Angle.mat')

% Initialize the camera

KinectHandles=mxNiCreateContext(SAMPLE_XML_PATH);

% Takes an image and converts it to grayscale

I=mxNiPhoto(KinectHandles);

I=flipdim(I,2);

I=rgb2gray(permute(I,[3 2 1]));

% Takes a depth real world data collection

XYZ=mxNiDepthRealWorld(KinectHandles);

XYZ=permute(XYZ,[2 1 3]);

Z=XYZ(:,:,3);

maxZ=3000; Z(Z>maxZ)=maxZ; Z(Z==0)=nan;

% Detects the SURF Points and Features from the picture

points = detectSURFFeatures(I);

[features, valid_points] = extractFeatures(I, points);

%Match the features.

X_indexPairs = matchFeatures(T_features, features);

%Retrieve the locations of corresponding points for each imge.

matchedTPoints = T_points(X_indexPairs(:, 1), :);

matchedPoints = points(X_indexPairs(:, 2), :);

figure(1)

109

[tform, inlierTPoints, inlierPoints] =

estimateGeometricTransform(matchedTPoints, matchedPoints, 'similarity');

showMatchedFeatures(T, I, inlierTPoints, inlierPoints, 'montage');

% Point positions, and average point positions.

points = parsePoints(inlierPoints ,1);

mpoints = mean(parsePoints(inlierPoints ,1));

figure(2)

imshow(I)

hold on

plot(mpoints(1,1),mpoints(1,2),'ro')

% Calculates the angle, X, and Z

i_theta = (mpoints(1,1)/640)*58;

i_Z = Z(uint16(mpoints(1,2)),uint16(mpoints(1,1)));

i_X = i_Z / tand(i_theta);

% Calculates the change in position

Position = [(XYZT_X - i_X) (XYZT_Z - i_Z)];

% Commands the roomba to move in the Z direction by the change in

% position.

% travelDist(serialObject,0.1,-(Position(1,2)/1000));


catch exception



end

110

Appendix – ObstacleDetection.m

%% Obstacle Detection & Avoidance % Code to implement obstacle detection and translate to a working map file % for a iRobot Create. The robot will be moving down hallways in a variable % enviroment so obstacles may present themselves within the path of the % robot.

%% Clear

clear all close all clc

%% Initialize toolboxes

addpath('Mex'); addpath('CreateToolbox'); addpath('QR Processing'); addpath('Data'); javaaddpath('QR Processing/core-1.7.jar'); javaaddpath('QR Processing/javase-1.7.jar'); SAMPLE_XML_PATH = 'Config/SamplesConfig.xml';

%% Depth Map - Ground Plane

load('Data\XYZDepth_HallObs5.mat') %load('Data\XYZDepth_Obst.mat')

pastZ = XYZ(480,1,3); GNDedge = zeros(306560,2); k=0;

FrontSight=zeros(100,640);

for j = 1:640 x = j; for i = 0:50 k = k+1; y = 480-i; newZ = XYZ(y,x,3); deltaZ = abs(newZ-pastZ); if deltaZ < 40 && deltaZ > 7 GNDedge(k,1) = x; GNDedge(k,2) = y; end pastZ = XYZ(y,x,3); end for i = 50:115 k = k+1; y = 480-i; newZ = XYZ(y,x,3); deltaZ = abs(newZ-pastZ);

111

if deltaZ < 60 && deltaZ > 10 GNDedge(k,1) = x; GNDedge(k,2) = y;

end

pastZ = XYZ(y,x,3); end end

%% Data processing % Gets rid of all zeros in arrays to making data processing faster GNDx = GNDedge(:,1); GNDy = GNDedge(:,2); %GNDx = nonzeros(GNDx); %GNDy = nonzeros(GNDy); GNDy = 480-GNDy;

%GNDx = smooth(GNDx(:)); %GNDy = smooth(GNDy(:)); GNDx = round(GNDx(:)); GNDy = round(GNDy(:));

GND =[GNDx GNDy]; TF1=GND(:,2)==0;

GND(TF1,:) = [];

TF1=GND(:,1)==0; GND(TF1,:) = [];

%TF2=zeros(length(GND),1); for u=1:1:640 % trying to remove entries with a single point

if sum(GND(:,1) == u)==1; Index = find(GND(:,1)==u,1); GND(Index,2)=0; end

end

TF1=GND(:,2)==0;

GND(TF1,:) = [];

GND(:,1) = smooth(GND(:,1)); GND(:,2) = smooth(GND(:,2)); GND(:,1) = round(GND(:,1)); GND(:,2) = round(GND(:,2));

112

for u=1:1:640 % trying to remove entries with a single point

if sum(GND(:,1) == u)==1; Index = find(GND(:,1)==u,1); GND(Index,2)=0; end

end

TF1=GND(:,2)==0;

GND(TF1,:) = [];

BinaryMatrix=ones(115,640);

for u=1:640 Index = find(GND(:,1)==u); LB=min(GND(Index,2)); UB=max(GND(Index,2)); for p=LB:1:UB BinaryMatrix(p,u)=0; end

end

for u=1:640 Index = find(GND(:,1)==u); LB=min(GND(Index,2)); if LB<=35 for p=1:1:LB BinaryMatrix(p,u)=0; end end end

for u=1:640 Index = find(GND(:,1)==u); UB=max(GND(Index,2)); if UB>=85 for p=UB:1:115 BinaryMatrix(p,u)=0; end end end

BinaryMatrix=flipud(fliplr(BinaryMatrix)); TopBinary=zeros(365,640); BinaryMatrix=[TopBinary;BinaryMatrix];

for u=2:640 if sum(BinaryMatrix(:,u))>=1 && sum(BinaryMatrix(:,u-1))==0 &&

sum(BinaryMatrix(:,u+1))==0 BinaryMatrix(:,u)=0;

113

end end

[row,col] = find(BinaryMatrix==1); row=row; OneLocations=[row,col];

TF1=OneLocations(:,1)==0; OneLocations(TF1,:) = [];

% for i=1:length(row) % ObstaclePoints(=[XYZ(row,col,3),XYZ(row,col,2)]; % end

figure('Name', 'Ground Points') imshow(((I))) set(gca,'YDir','reverse') set(gca,'XDir','normal') hold on rectangle('Position',[1,1,640,140]) plot(col,row,'G.')

%% Translate to map

Map; m(1,100) = 2; [loc_y, loc_x] = find(m==2);

for r = 1:length(OneLocations(:,1)) %OneLocations(r,2) = 480 - GNDy(r); obj_X(r,1) = XYZ(round(OneLocations(r,1)),round(OneLocations(r,2)),1)/10; %

map X converted to cm obj_Z(r,1) = XYZ(round(OneLocations(r,1)),round(OneLocations(r,2)),3)/10; %

map Y converted to cm end

obj_X = round(obj_X); obj_Z = round(obj_Z);

for r = 1:length(obj_X) m(loc_y + obj_Z(r), loc_x + obj_X(r)) = 1; end

[MapObsLocations(:,1) MapObsLocations(:,2)]=find(m==1); % [X Y]

114

Appendix – PotFieldDrive.m

%% Potential Field Vector & Speed Calculation % 5/16/2014

clear all clc close all

addpath('Mex'); addpath('CreateToolbox'); addpath('QR Processing'); addpath('Data'); javaaddpath('QR Processing/core-1.7.jar'); javaaddpath('QR Processing/javase-1.7.jar');

%% Loading sample data space = uint8(zeros(300,300)); % Room walls = uint8(ones(300,10)); m = [walls space walls]; m = SetLoc(m,25,50); % Sets the current location of the robot m = SetGoal(m,250,250); % Sets the goal of the robot m(105,100) = 1; m(150,200) = 1; m(225,200) = 1; m(100,150) = 1; deltaX_o = 0; deltaY_o = 0;

%% Initialization % % Initialize the Roomba % [serialObject]=RoombaInit(8); % % % Initialize the Kinect % SAMPLE_XML_PATH='Config/SamplesConfig.xml'; % KinectHandles=mxNiCreateContext(SAMPLE_XML_PATH);

%% Main Drive Speed toward Goal [goal_y, goal_x] = find(m==3); [loc_y, loc_x] = find(m==2); [obs_y, obs_x] = find(m==1);

alpha = .15; s_goal = 50; % Attractive force (maximum velocity vector allowed) r_Robo = 17; % 15 cm diameter theta_f = 90; beta = .15; s_obs = 5;

figure(1); hold on axis([0 320 0 300]) plot(loc_x, loc_y,'bo') plot(goal_x, goal_y,'ro')

115

plot(obs_x, obs_y,'go') grid on

for i = 1:300 [loc_y, loc_x] = find(m==2); d_goal = sqrt((goal_x - loc_x)^2 + (goal_y - loc_y)^2); theta_goal = atan2d((goal_y - loc_y),(goal_x - loc_x)); for j = 1:length(obs_x) d_obs(j) = sqrt((obs_x(j) - loc_x)^2 + (obs_y(j) - loc_y)^2); theta_obs(j) = atan2d((obs_y(j) - loc_y),(obs_x(j) - loc_x)); end

% Calculate the deltaX & deltaY to the goal if d_goal < r_Robo deltaX_g = 0; deltaY_g = 0; vel = 0; theta_goal = 0; break elseif r_Robo <= d_goal && d_goal <= s_goal + r_Robo deltaX_g = alpha*(d_goal - r_Robo)*cosd(theta_goal); deltaY_g = alpha*(d_goal - r_Robo)*sind(theta_goal); elseif d_goal > s_goal + r_Robo deltaX_g = alpha*s_goal*cosd(theta_goal); deltaY_g = alpha*s_goal*sind(theta_goal); end

for k = 1:length(obs_x) % Calculate the deltaX & deltaY to the obstacle if d_obs(k) < r_Robo deltaX_o(k) = -sign(cosd(theta_obs(k)))*10; deltaY_o(k) = -sign(sind(theta_obs(k)))*10; elseif r_Robo <= d_obs(k) && d_obs(k) <= s_obs + r_Robo deltaX_o(k) = (-beta*(s_obs + r_Robo - d_obs(k))*cosd(theta_obs(k))); deltaY_o(k) = (-beta*(s_obs + r_Robo - d_obs(k))*sind(theta_obs(k))); elseif d_obs(k) > s_obs + r_Robo deltaX_o(k) = 0; deltaY_o(k) = 0; end end

deltaX = sum(deltaX_o) + deltaX_g; deltaY = sum(deltaY_o) + deltaY_g;

vel = sqrt(sum(deltaX)^2 + sum(deltaY)^2)/100 % /100 to get from cm/s to

m/s theta = atan2d(sum(deltaY),sum(deltaX));

if theta == 90 m = AdjustLoc(m,5,0); else m = AdjustLoc(m,sind(theta)*5,cosd(theta)*5); end if theta_goal == 0 theta_f = 0 else

116

theta_f = theta - theta_f end

info(i,1) = loc_x; info(i,2) = loc_y; info(i,3) = vel; info(i,4) = theta_f;

plot(loc_x, loc_y, 'b*')

theta_f = theta; %turnAngle(serialObject, vel, theta_f) %travelDist(serialObject, vel, 0.05) end

117

Appendix – ButtonDetection.m

addpath('CreateToolbox'); addpath('QR Processing'); addpath('Data'); addpath('Elevator/actual'); addpath('otherScripts/TTS'); javaaddpath('QR Processing/core-1.7.jar'); javaaddpath('QR Processing/javase-1.7.jar');

%% Initialization % Initialize the Roomba [serialObject]=RoombaInit(8);

floor=[0 300 227 100;227.5 308.5 201 106;17.5 199.5 207 106;227.5 201.5 207

106;19.5 95.5 207 106;226.5 103.5 207 106;31.5 0.5 207 106;228.5 0.5 207

106];

desiredFloor=1; CurrentFloor=0; % Floor light turned off ButtonPressed=0; % Button Not Pressed loopcount=0; %original = rgb2gray(imread('numpad.jpg')); load('Numpad.mat');

vid = videoinput('winvideo', 1, 'MJPG_1280x720'); %change to usb webcam src = getselectedsource(vid); vid.FramesPerTrigger = 1; vid.ReturnedColorspace = 'grayscale';

%% Loop while ButtonPressed==0 && CurrentFloor==0

% loopcount=loopcount+1 %figure;imshow(original);

start(vid); distorted = getdata(vid);

%distorted = rgb2gray(imread('numpad.jpg')); %figure, imshow(distorted)

ptsOriginal = detectSURFFeatures(original); ptsDistorted = detectSURFFeatures(distorted);

[featuresOriginal, validPtsOriginal] = extractFeatures(original,

ptsOriginal); [featuresDistorted, validPtsDistorted] = extractFeatures(distorted,

ptsDistorted);

118

indexPairs = matchFeatures(featuresOriginal, featuresDistorted);

matchedOriginal = validPtsOriginal(indexPairs(:,1)); matchedDistorted = validPtsDistorted(indexPairs(:,2));

% figure; %

showMatchedFeatures(original,distorted,matchedOriginal,matchedDistorted); % title('Putatively matched points (including outliers)');

[tform, inlierDistorted, inlierOriginal] =

estimateGeometricTransform(matchedDistorted, matchedOriginal, 'similarity');

% figure; % showMatchedFeatures(original,distorted,inlierOriginal,inlierDistorted); % title('Matching points (inliers only)'); % legend('ptsOriginal','ptsDistorted');

Tinv = tform.invert.T;

ss = Tinv(2,1); sc = Tinv(1,1); scaleRecovered = sqrt(ss*ss + sc*sc); thetaRecovered = atan2(ss,sc)*180/pi;

outputView = imref2d(size(original)); recovered = imwarp(distorted,tform,'OutputView',outputView); % figure; imshow(recovered) recoveredclip = imcrop(recovered,floor(desiredFloor,:)); recoveredclip = im2bw(recoveredclip,.8); %figure;imshow(recoveredclip) %figure, imshowpair(original,recoveredclip,'montage')

CompareButton= nnz(recoveredclip); % count amount of white

if CompareButton>350; ButtonPressed=1 else ButtonPressed=0 end

if ButtonPressed==0 disp('Please Press Button') else while CurrentFloor==0

start(vid); distorted2 = getdata(vid);

%ptsOriginal = detectSURFFeatures(original); ptsDistorted = detectSURFFeatures(distorted2);

%[featuresOriginal, validPtsOriginal] = extractFeatures(original,

ptsOriginal);

119

[featuresDistorted, validPtsDistorted] = extractFeatures(distorted2,

ptsDistorted);

indexPairs = matchFeatures(featuresOriginal, featuresDistorted);

matchedOriginal = validPtsOriginal(indexPairs(:,1)); matchedDistorted = validPtsDistorted(indexPairs(:,2));

[tform, inlierDistorted, inlierOriginal] =

estimateGeometricTransform(matchedDistorted, matchedOriginal, 'similarity');

Tinv = tform.invert.T;

ss = Tinv(2,1); sc = Tinv(1,1); scaleRecovered = sqrt(ss*ss + sc*sc); thetaRecovered = atan2(ss,sc)*180/pi;

outputView = imref2d(size(original)); recovered = imwarp(distorted2,tform,'OutputView',outputView);

recoveredclip = imcrop(recovered,floor(desiredFloor,:)); recoveredclip = im2bw(recoveredclip,.8);

CompareButton= nnz(recoveredclip); % count amount of white

if CompareButton>350; CurrentFloor=0 else CurrentFloor=1 end end end end

tts('Correct Floor') ExitElevator(serialObject,.2,.3)

120

Appendix – ExitElevator.m

function [ status ] = ExitElevator( serPort, turnSpeed, travelSpeed )

% Turn to face far wall turnAngle(serPort,turnSpeed,90);

travelDist(serPort,travelSpeed, .8); turnAngle(serPort,turnSpeed,-90);

% Can take depth map to reposition but doubtful it will work at that % distance

travelDist(serPort,travelSpeed, 1);

status=1; end

121

Appendix – Demo.m

% Hallway Drive Demo Looped % 4/14/2014

clear all clc close all

addpath('Mex'); addpath('CreateToolbox'); addpath('QR Processing'); addpath('Data'); javaaddpath('QR Processing/core-1.7.jar'); javaaddpath('QR Processing/javase-1.7.jar');

%% Initialization % Initialize the Kinect SAMPLE_XML_PATH='Config/SamplesConfig.xml'; KinectHandles=mxNiCreateContext(SAMPLE_XML_PATH);

pause(5)

% Initialize the Roomba [serialObject]=RoombaInit(8);

% Load the map Map;

% Initialize parameters QR_OC = 0; % OC = Origin Code (facing Origin QR Code) RT_OC = 0; % OC = Origin Code (facing Origin QR Code) QR_1C = 0; % 1C = 1st Localization Point Code (Facing QR Code) RT_1C = 0; % 1C = 1st Localization Point Code (Facing QR Code) QR_2C = 0; % 2C = 2nd Localization Point Code (Facing QR Code) RT_2C = 0; % 2C = 2nd Localization Point Code (Facing QR Code) QR_3C = 0; % 3C = 3rd Localization Point Code (Facing QR Code) RT_3C = 0; % 3C = 3rd Localization Point Code (Facing QR Code) QR_EC = 0; % EC = End Code (facing End QR Code) RT_EC = 0; % EC = End Code (facing End QR Code) r_Robo = 15; % Radius of the iRobot (15 cm) midHall = 80; pxDiff = 50; turnSpeed = 0.1; travSpeed = 0.5; QRSpeed = 0.15; m = SetLoc(m,101,midHall); % Sets the current location of the robot m = SetGoal(m,301,midHall); % Sets the goal of the robot

%% Origin (OC)

% Start point is facing a QR code at the initialization point. while RT_OC == 0 RT_OC = RLocal(KinectHandles,serialObject,'RT_OC',pxDiff, turnSpeed);

122

end if RT_OC == 1 disp('RLocal[OC]: Done!') end while QR_OC == 0 [QR_OC, delY] = QRLocal(KinectHandles,serialObject,'QR_OC',midHall,

QRSpeed); end if QR_OC == 1 disp('QRLocal[OC]: Done!') end

% turnAngle down hallway turnAngle(serialObject,turnSpeed,88);

% Updates location based on error taken from QR localization function m = AdjustLoc(m,delY,0);

%Calculate displacement & drive [dist_y, dist_x] = FindDel(m); travelDist(serialObject,travSpeed, abs(dist_y));

%% 1st Checkpoint (1C)

% turnAngle toward the QR Code turnAngle(serialObject,turnSpeed,-90);

% Start point is facing a QR code at the initialization point. while RT_1C == 0 RT_1C = RLocal(KinectHandles,serialObject,'RT_OC',pxDiff, turnSpeed); end if RT_1C == 1 disp('RLocal[1C]: Done!') end while QR_1C == 0 [QR_1C, delY] = QRLocal(KinectHandles,serialObject,'QR_1C',midHall,

QRSpeed); end if QR_1C == 1 disp('QRLocal[1C]: Done!') end


% Updates location based on error taken from QR localization function m = AdjustLoc(m,(dist_y*100)+delY,0); m = SetGoal(m,634,midHall);

% Calculate displacement & drive [dist_y, dist_x] = FindDel(m); travelDist(serialObject,travSpeed, abs(dist_y));

%% 2nd Checkpoint (2C)

123

% turnAngle toward the QR Code turnAngle(serialObject,turnSpeed,-90);






%% 3rd Checkpoint (3C)

% turnAngle toward the QR Code turnAngle(serialObject,turnSpeed,90);



% turnAngle down hallway turnAngle(serialObject,turnSpeed,-92);


124


%% Endpoint Check % turnAngle toward wall turnAngle(serialObject,turnSpeed,-90);

while RT_EC == 0 RT_EC = RLocal(KinectHandles,serialObject,'RT_EC',pxDiff, turnSpeed); end if RT_EC == 1 disp('RLocal[EC]: Done!') end while QR_EC == 0 [QR_EC, delY] = QRLocal(KinectHandles,serialObject,'QR_EC',midHall,

QRSpeed); end if QR_EC == 1 disp('QRLocal[EC]: Done!') end

125

Appendix – QRLocal.m

function [ status, x ] = QRLocal( KH, serPort, code_str, ddist, speed ) %QRLocal - XYZ Localization using QR Codes % OUTPUTS % status - State variable of the function [1 = success / 0 = fail] % x - Calculated x translation due to different positioning of QR % code % % INPUTS % KH - KinectHandles, the connection to the Microsoft Kinect % serPort - serialObject, the connection to the iRobot Create % code_str - String signifying location that the QRlocal is performed % ddist - Desired distance to maintain to QRCode % speed - Travel speed in m/s

try if strcmp(code_str,'QR_OC') == 1 code = 'OriginCode'; load('QR_OC.mat') elseif strcmp(code_str,'QR_1C') == 1 code = 'Point1'; load('QR_1C.mat') elseif strcmp(code_str,'QR_2C') == 1 code = 'Point2'; load('QR_2C.mat') elseif strcmp(code_str,'QR_3C') == 1 code = 'Point3'; load('QR_3C.mat') elseif strcmp(code_str,'QR_EC') == 1 code = 'EndPoint'; load('QR_EC.mat') end I_msg = 0; ddist = ddist*10; for i = 1:30 if i == 30 % If the loop doesn't find a usable QR code within x number of % frames. This can be implemented as a while loop once fully % debugged. status = 0; % 0 signifies failure of the function to localize via QR

Code. break else if strcmp(I_msg,code) == 0 % Scan image for QR Code mxNiUpdateContext(KH); I=mxNiPhoto(KH); I=permute(I,[3 2 1]); I=flipdim(I, 2); I_msg = decode_qr(I); % Decode the image for a QR code elseif strcmp(I_msg,code) == 1 mxNiUpdateContext(KH); XYZ=mxNiDepthRealWorld(KH); XYZ=permute(XYZ,[2 1 3]);

126

% Takes an image and converts it to grayscale I=mxNiPhoto(KH); I=flipdim(I,2); I=rgb2gray(permute(I,[3 2 1]));

Z=XYZ((480/2),(640/2),3); % Assuming y by x Znew = Z - ddist; if (abs(Znew) > 25) && (abs(Znew) < 1000) travelDist(serPort, speed, Znew/1000); % CHECK the VALUE of Z. elseif (abs(Znew) > 1500) disp('Znew calculated to be greater than 1.5m') else % Detects the SURF Points and Features from the picture points = detectSURFFeatures(I); [features, valid_points] = extractFeatures(I, points);

%Match the features. X_indexPairs = matchFeatures(XYZT_features, features);

%Retrieve the locations of corresponding points for each imge. matchedXYZTPoints = XYZT_points(X_indexPairs(:, 1), :); matchedPoints = points(X_indexPairs(:, 2), :); %figure(1) [tform, inlierTPoints, inlierPoints] =

estimateGeometricTransform(matchedXYZTPoints, matchedPoints, 'similarity'); showMatchedFeatures(XYZT, I, inlierTPoints, inlierPoints, 'blend');

% Point positions, and average point positions. valid_points = parsePoints(inlierPoints ,1); mpoints = mean(parsePoints(inlierPoints ,1));

% Calculates the angle, X, and Z i_X = XYZ(uint16(mpoints(1,2)),uint16(mpoints(1,1)),1);

% Calculates the change in position x = -(XYZT_X-i_X); x = x/10; % Convert to cm

status = 1; break end end end end catch exception status = 0; x = NaN; getReport(exception) end end

127

Appendix – Rlocal.m

function [ status, theta_f, ctheta_f] = RLocal( KH, serPort, location, diff,

speed ) %RLocal - Rotational Localization % OUTPUTS % status - State variable of the function [1 = success / 0 = fail] % theta_f - Total theta rotated by the template matching algorithm % ctheta_f - Total theta rotated by the 2Z wall algorithm % INPUTS % KH - KinectHandles, the connection to the Microsoft Kinect % serPort - serialObject, the connection to the iRobot Create % location - String signifying location that the Rlocal is performed % diff - Number of minimum pixel difference desired in template match % speed - Turn speed in m/s

try % if conditional that checks the location string and imports the % appropriate template corresponding to the location specified. if strcmp(location,'RT_OH') == 1 load('RT_OH.mat') loc = 'Hall'; elseif strcmp(location,'RT_OC') == 1 loc = 'Code'; elseif strcmp(location,'RT_1C') == 1 loc = 'Code'; elseif strcmp(location,'RT_1H') == 1 load('RT_1H.mat') loc = 'Hall'; elseif strcmp(location,'RT_2C') == 1 loc = 'Code'; elseif strcmp(location,'RT_2H') == 1 load('RT_2H.mat') loc = 'Hall'; elseif strcmp(location,'RT_3C') == 1 loc = 'Code'; elseif strcmp(location,'RT_3H') == 1 load('RT_3H.mat') loc = 'Hall'; elseif strcmp(location,'RT_EC') == 1 loc = 'Code'; elseif strcmp(location,'RT_EH') == 1 load('RT_EH.mat') loc = 'Hall'; end

% Initialize the parameters avgDiff=640; A_tot = 0; theta_f = 0; ctheta_f = 0; theta = 0;

for i=1:10 if abs(avgDiff)<=diff

128

status = 1; break elseif i == 10 disp('Unable to RLocalize') status = 0; else if strcmp(loc,'Hall') == 1 % Takes an image and converts it to grayscale. mxNiUpdateContext(KH); I=mxNiPhoto(KH); I=flipdim(I,2); I=rgb2gray(permute(I,[3 2 1]));

% Detects and extracts the features from the grayscale image. points = detectSURFFeatures(I); [features, valid_points] = extractFeatures(I, points);

% Match the features with the features from the template. R_indexPairs = matchFeatures(RT_features, features);

% Retrieve the locations of corresponding points for each image. RT_Data = parsePoints(RT_valid_points(R_indexPairs(:, 1), :), 1); R_Data = parsePoints(valid_points(R_indexPairs(:, 2), :), 1); [tform, inlierTPoints, inlierPoints] =

estimateGeometricTransform(RT_Data, R_Data, 'similarity'); showMatchedFeatures(RT_I, I, inlierTPoints, inlierPoints, 'montage');

% Calculates the difference between the matched points. differenceMat=zeros(length(inlierPoints),1); differenceMat(:,1)=inlierPoints(:,1)-inlierTPoints(:,1);

% if conditional that determines the course of action to % correct the error, Kp = 1 loop. if length(differenceMat) <= 3 if A_tot < 4 turnAngle(serPort,speed,-10); A_tot = A_tot + 1; disp('Nothing seen [Hall]') else break end else avgDiff=mean(differenceMat); theta=(avgDiff/640)*58; if abs(avgDiff)<=diff disp('Found it') status = 1; break else turnAngle(serPort,speed,-theta); end end elseif strcmp(loc,'Code') == 1 mxNiUpdateContext(KH) XYZ=mxNiDepthRealWorld(KH); XYZ=permute(XYZ,[2 1 3]);

129

Z1=XYZ((480/2),(100),3); % Assuming y by x Z2=XYZ((480/2),(600),3); delZ = Z2 - Z1; disp('Calc delZ [Code]') if abs(delZ) > 20 X1=XYZ((480/2),(100),1); % Assuming y by x X2=XYZ((480/2),(600),1); Xtot = abs(X2) + abs(X1); ctheta = atand(delZ/Xtot); turnAngle(serPort,speed,-ctheta) disp('delZ > 25! [Code]') ctheta_f = ctheta_f + ctheta; else status = 1; break end end end end catch exception getReport(exception)

end

130

Appendix – Init.m

%% Initialization Script % Clear clear all clc close all

%% Main % Add paths addpath('Mex'); addpath('CreateToolbox'); addpath('QR Processing'); addpath('Data'); addpath('TTS'); javaaddpath('QR Processing/core-1.7.jar'); javaaddpath('QR Processing/javase-1.7.jar');

% Roomba Init and Display Charge Percentage [serialObject] = RoombaInit(8); [Charge, Capacity, Percent] = BatteryChargeReaderRoomba(serialObject); disp(Percent)

% Connect to camera % Initialize the Kinect SAMPLE_XML_PATH='Config/SamplesConfig.xml'; KinectHandles=mxNiCreateContext(SAMPLE_XML_PATH);

131

Appendix – DepthTemp.m

%% Depth Template Creation

addpath('Mex_OpenNI1') SAMPLE_XML_PATH='Config/SamplesConfig.xml';

try % Create context to OpenNI and Kinect sensor KinectHandles=mxNiCreateContext(SAMPLE_XML_PATH);

% Takes a picture and turns it into grayscale I=mxNiPhoto(KinectHandles); I=flipdim(I,2); I_rgb = permute(I,[3 2 1]); I_gray = rgb2gray(permute(I,[3 2 1])); I=rgb2gray(permute(I,[3 2 1]));

XYZ=mxNiDepthRealWorld(KinectHandles); XYZ=permute(XYZ,[2 1 3]);

% Detects the valid SURF points, extracts the local features, and exports

the RT_features & RT_valid_points variable to a .mat file. save('Data\XYZDepth_HallObs5.mat','I','I_rgb','I_gray','XYZ')


% clear all % clc % close all % catch exception mxNiDeleteContext(KinectHandles); getReport(exception) end

132

Appendix – parsepoints.m

function points=parsePoints(points, ptsInputNumber)

fcnInputVarNumber = 2 + ptsInputNumber; varName = ['MATCHED_POINTS', num2str(ptsInputNumber)];

if ~isa(points, 'vision.internal.FeaturePoints') && ~isa(points,

'MSERRegions') validateattributes(points,{'int16', 'uint16', 'int32', 'uint32', ... 'single', 'double'}, {'2d', 'nonsparse', 'real', 'size', [NaN 2]},... mfilename, varName, fcnInputVarNumber); else points = points.Location; end

10-Report-Spring

Documents

Transcript of 10-Report-Spring