Method for Autonomous Picking of Paper Reels

8/4/2019 Method for Autonomous Picking of Paper Reels

http://slidepdf.com/reader/full/method-for-autonomous-picking-of-paper-reels 1/89

Technical report, IDE1144 , September 13, 2011

Master’s Thesis in Intelligent Systems

By:- Hasan, Meqdad Hamdan and Kali, Rahul Raj.

School of Information Science, Computer and Electrical Engineering

Halmstad University

Method for AutonomousPicking of Paper Reels





Method for Autonomous Picking of Paper Reels

By:- Hasan, Meqdad Hamdan and Kali, Rahul Raj.

Halmstad University

Project Report IDE1144

Master’s thesis in Intelligent Systems, 15 ECTS credits

Supervisor: Ph.D. Bjorn Astrand

Examiner: Prof. Antanas Verikas

September 13, 2011

Department of Intelligent systemsSchool of Information Science, Computer and Electrical Engineering

Halmstad University





Preface

Here in a short words were words never could describe the feeling of thankfulfor peoples who encourage us in this work and keep us on the track when ourenergy drops to zero. To the people who illuminate the road for us when ourideas interned a situation of chaos. Thanks to our parents, grand parents andour brothers who were with us over seas by their hearts and their praying.Thanks to Dr. Bjorn Astrand who guide us during this thesis and track uson every turn of it. Thanks to the help that is provided by Dr. Walid Tahafrom Information and computer department and by Nadya Karginova fromIntelligent systems department. Finally thanks a lot for the people who helpus with little words and they are in other lands nowadays.

i



ii



Abstract

Autonomous forklift handling systems is one of the most interestingresearch in the last decades. While research fields such as path planningand map building are taking the most significant work for other type of autonomous vehicles, detecting objects that need to move and picking

it up becomes one of the most important research fields in autonomousforklifts field.We in this research had provided an algorithm for detecting paper reelsaccurate position in paper reels warehouses giving a map of the ware-house itself. Another algorithm is provided for giving the priority of papers that want to be picked up. Finally two algorithms for choos-ing the most appropriate direction for picking the target reel and forchoosing the safest path to reach the target reel without damage it areprovided.While working on the last two algorithms shows very nice results, build-ing map for unknown stake of papers by accumulating maps over time

still tricky. In the following pages we will go in detail by the stepsthat we followed to provide these algorithms started from giving an overview to the problem background and moving through the method thatwe used or we developed and ending by result and the conclusion thatwe got from this work.

iii



iv



Contents

1 Introduction 11.1 Malta . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2 Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.3 Goals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.4 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.5 Outlines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2 Background 7

2.1 Problem Description . . . . . . . . . . . . . . . . . . . . . . . 7

2.2 State of the Art . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.3 Closely Related Works . . . . . . . . . . . . . . . . . . . . . . 112.3.1 Method of autonomous material handling . . . . . . . 12

2.3.2 Perception and detection . . . . . . . . . . . . . . . . 12

2.3.3 Map building . . . . . . . . . . . . . . . . . . . . . . . 13

3 Methods 15

3.1 Path Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1.1 Kinematic model . . . . . . . . . . . . . . . . . . . . . 15

3.1.2 Path selection . . . . . . . . . . . . . . . . . . . . . . . 17

3.2 Perception . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.2.1 LRF simulation . . . . . . . . . . . . . . . . . . . . . . 203.2.2 Error simulation . . . . . . . . . . . . . . . . . . . . . 20

3.2.3 Segmentation and Feature Extraction . . . . . . . . . . 21

3.3 Map Building . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.3.1 Matching algorithm . . . . . . . . . . . . . . . . . . . . 26

3.3.2 Map updating and map generation . . . . . . . . . . . 30

3.4 Paper Reel Selection . . . . . . . . . . . . . . . . . . . . . . . 31

3.4.1 Paper availability and reel priority . . . . . . . . . . . 31

3.4.2 What is the grasping direction? . . . . . . . . . . . . . 36

v



4 Results 374.1 Segmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . 404.2 Extracting Map . . . . . . . . . . . . . . . . . . . . . . . . . . 424.3 Sum Maps into one . . . . . . . . . . . . . . . . . . . . . . . . 464.4 Map analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5 Discussion 63

6 Conclusions 65

Appendix 69

vi



List of Figures

1.1 Oil drum picking is one of the application examples . . . . . . . . 1

1.2 Paper reels are a second application example . . . . . . . . . . . . 11.3 Truck Used in MALTA Project . . . . . . . . . . . . . . . . . . . 3

1.4 Wooden pallets have specific direction for picking even some times

its a unique. The picking direction is perpendicular to the top

wooden plates, the opposite direction of the red row. . . . . . . . 31.5 Loading Unknown Position Paper Reel. . . . . . . . . . . . . . . 4

2.1 Illustration to the problem, (Prescribed environment contain stack

of paper reel needs to transport. 1- Letters show the truck in several

locations (A,B and C). 2- Numbers show different paper reel in

different location. 3- Arrow refer to several direction to pick. 4-

Bold arrow show the best praxis exist now. 5- Black dots refer to

available directions to pick. 6- Cross sign refer to the reflected laser

beams. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.2 Estimation of paper reel locations from uncertain laser range finder

reading. Black circles are estimated by using circle fitting algo-

rithms and red ones are the true paper reels location. See the dif-

ference between estimated and true ones. Picture is taken from [ 1] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.1 Simplification of MALTA robot kinematic model used to compute

the position of the vehicle from wheel encoder information (bicyclemodel). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.2 The arrangement of points that chosen to generate the path. Six

points are chosen. P 1 is the original point of the robot frame. P 2is a point in the middle of the forks. P 3 is a point in a distance

of 1.5d on the robot X-axis. P 6 is the center of the paper reel. P 5is a point on the surface of the paper reel at the same direction of

picking direction. P 4 is a point on the same direction of P 5 but

with 1.5d distance of the paper reel surface. ”d” refers to the robot

total length. The path is generated by using B-spline cubic function. 17

vii



3.3 The path selection to transport reels from unloading area to theloading area. The path is described by 9 points. P 1, P 2 and P 3 as

points (P 4,P 5 and P 6) in Figure 3.2 . 3 points (P 4,P 5 and P 6) to

guide the vehicle through the container entrance. Other 3 points

(P 7,P 8 and P 9) to guide the vehicle to the farthest unoccupied cell

in the container. . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.4 Line fitting problem. . . . . . . . . . . . . . . . . . . . . . . . . 24

3.5 Circle fitting results. σρ (LRF beam error variance) here is 0.05. . 25

3.6 Weighted linear regression. Note there is high error in calculating

start and end points using equation 3.30 . σρ (LRF beam error

variance) here is 0.05. . . . . . . . . . . . . . . . . . . . . . . . 253.7 example of matching created map with predefined one . . . . . . . 30

3.8 Each reel has 360 accessibility angles. Instead, It is considered as

interval. Thus each reel has 72 accessibility angels. . . . . . . . . 32

3.9 Each object is considered as an obstacle if the distance between that

object and the reel more than 1.7 of the length of the truck until

the end of the gripers . . . . . . . . . . . . . . . . . . . . . . . . 32

3.10 If the obstacle is some thing like wall, i.e the reel location is close

to wall. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.11 Description of use of built problem to calculate the accessibility

angle. The dots refer to the possible picking direction. The X’srefer to inaccessible directions. . . . . . . . . . . . . . . . . . . . 35

3.12 Change the presentation of accessibility angles to linear presenta-

tion then remove the inaccessible angles and flag them. . . . . . . 36

4.1 Testing environment . . . . . . . . . . . . . . . . . . . . . . . . 37

4.2 The truck shape and the arrangement that is chosen to the LRF

fitting on the truck. Both are fitted on the same hight. . . . . . . . 37

4.3 Scanning image from simulated LRF. The image shows that the

image is relative to the scanner position . . . . . . . . . . . . . . 39

4.4Simulated errors in scans

. . . . . . . . . . . . . . . . . . . . . . 404.5 Scanning points after preprocessing. All point are rotated and

translated to the global map. . . . . . . . . . . . . . . . . . . . . 40

4.6 Extracting features and building local map. σρ here is 0.2. Good

estimation for line segment in the bottom. . . . . . . . . . . . . . 41

4.7 Extracting features and building local map. σρ here is 0.2. Note

the bad estimation of line in the left . . . . . . . . . . . . . . . . 42

4.8 The result of building local map from circles and lines. . . . . . . . 43

4.9 Extracting features and building local map. The best estimation for

close paper reels. . . . . . . . . . . . . . . . . . . . . . . . . . . 44

viii



4.10 The summation of two maps into bigger one. The biggest circlesrepresent the extraction of paper reels in first map.The smallest

ones represent the extraction of them in the second map. The mid-

dle one show the summation of both by using Kalman filter. . . . . 464.11 The result of summation two line segments together.The algorithm

sums the first two left segments but it does not add to them the

third line. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 474.12 The adding of two segments when they are verticals. Note that the

result not as expected, where one can expect the result should lay

somewhere between the two smallest lines but what happen, that is

the new longer line segment lay out of both original segments. . . . 484.13 Other example of summation of two circle columns or paper reels

from two different map using Kalman filter. The black one is the

result of the summation. . . . . . . . . . . . . . . . . . . . . . . 484.14 Summation of two local maps into one again. The bottom line

segments are summed correctly together while one segment appeared

in the left. This segment simply transferred to the new map without

change. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494.15 In this figure two local map are summed together the map from

Figure 4.16 and the map from Figure 4.17 . Note the difference

between them. . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

4.16 The local map that is extracted from first LRF at scan image num-ber 71. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.17 The local map that is extracted from second LRF at scan image

number 71. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 534.18 The result of building the global map of combining 112 local map,

56 local map from each LRF are built from 56 scan image over 56

position of the truck. . . . . . . . . . . . . . . . . . . . . . . . . 534.19 The simulation environment and the initial position of the truck.

Light circles are concrete pillars. Dark circles are paper reels.

Lines are walls and container. . . . . . . . . . . . . . . . . . . . 54

4.20 The paper reels arrangements. . . . . . . . . . . . . . . . . . . . 554.21 All papers are processed then the access directions for each paper

reel are determined (the magenta points). . . . . . . . . . . . . . 554.22 The paper reel #13 is removed first then the rest paper reel are

processed again. . . . . . . . . . . . . . . . . . . . . . . . . . . 564.23 Paper reel number 12 is removed this time, then the map processed

again. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 564.24 Now paper reel number 26 is removed. Note two different and sep-

arate intervals of accessibility direction on paper reel # 11 are ap-

peared . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

ix



4.25 The same for paper reel number 11. . . . . . . . . . . . . . . . . 574.26 And so on for all other reels until all papers are processed. . . . . 574.27 The picture shows picking intervals and access direction. . . . . . 584.28 The path is generated by using b-spline function and by giving 10

different points to the function as input. The path calculated to

each paper real from each possible picking direction then the short-

est one is chosen. . . . . . . . . . . . . . . . . . . . . . . . . . . 584.29 All possible paths to the paper reel. . . . . . . . . . . . . . . . . . 594.30 The chosen path is the shortest one and the easiest from paths in

figure 4.29. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 594.31 Selected paths and picking directions for each real. . . . . . . . . . 594.32 The chosen picking direction provide safe moving for truck to not

be in conflict with any other obstacle . . . . . . . . . . . . . . . . 614.33 Also the selected path provide safe reaching to the paper reel. . . . 614.34 However, we could not simulate the return back movement where

the rotation of truck direction happen instantaneously not exactly

as in real life. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

6.1 Ackerman Steering Principle. . . . . . . . . . . . . . . . . . . . 696.2 The paper reel in the middle is located between a wall a column. . 716.3 Adding two availability together after converting the angles to linear

representation to get the total available directions. . . . . . . . . . 73

x



List of Tables

4.1 A comparison between segmentation results; ”number of effective

segments and length of segments” when three values of noise re-duction constant are used. . . . . . . . . . . . . . . . . . . . . . 41

4.2 Features extraction from one LRF scan, and building local map

as in fig.4.6 . Li refers to line feature. C i refers to circle feature.

Empty place refers to no reading available for this measure because

its not available for that feature. error refers to bad estimation. . . 424.3 The uncertainty and location of features in fig.4.7 . Li refers to

line feature. C i refers to circle feature. Empty place refers to no

reading available for this measure because its not available for that

feature. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434.4 The uncertainty and location of features in fig .4.8 . . . . . . . . . 444.5 The uncertainty and location of features in fig .4.9 . . . . . . . . . 454.6 Summation of two lines into global map,fig.4.11 a case spot light . 474.7 Summation of two Vertical lines. Objects in figure 4.12. A case

spot light . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 484.8 result of Summation two pillars as in fig .4.13 . . . . . . . . . . . 494.9 The result of fuse two local map together using Kalman filter.M ap1

represents some features from Figure 4.16.M ap2 represents the

same of them from Figure 4.17 . The last part of the table show

the result of summing them together as appears in Figure 4.15 . . . 514.10 The position, uncertainty and error of the same paper reels and

column from table 4.9 after accumulating several local map. These feature are represented in a right place as appears in Figure 4.18 . . 51

4.11 The shape of the table that generated from the algorithms described

in this research from analysing a given map for paper locations.

This is only a sample from the simulation environment above. . . . 60

xi





Chapter 1

Introduction

In industry and other fields like mining, material handling and material trans-portation is one of the most costly activity. The cost of this activity increasesif the material have to pick and transport in some kind of order or arrange-ment. The cost of order picking is estimated by 55% from warehouse op-eration expense [7]. However this cost increases in case of damages due toerrors of picking. To avoid such high cost and to avoid other type of prob-lems, demands for efficient material handling with efficient picking regime

solutions are growing rapidly. Examples for such a kind are, steel wire reelsmanufactures, Oil drums handling Figure 1.1, paper mills Figure 1.2 andother industries. The autonomous mobile robotic solution has a great poten-tial to offer improvements in terms of costs, safety, efficiency, flexibility andavailability [1].

Figure 1.1: Oil drum picking is oneof the application examples

Figure 1.2: Paper reels are a second application example

1



2

1.1 MaltaMALTA project (Multiple Autonomous forklift for Loading and Transp-ortation Applications) [1] is a project developed in Orebro University andHalmstad University in Sweden, together with Danaher Motion Saro, LindeMaterial Handling, and Stora Enso Logistics. The research in this project isto develop a fully autonomous AGV (Automated Guided Vehicle) (forklift)for loading, unloading and transporting materials in highly dynamic envi-ronments. The forklift in this project was modified by replacing the twoforks with clamps Figure 1.3. The system was equipped with two laser rangefinders for detecting obstacles and paper reels. The system also equippedwith AGV control system comprises a set of hardware and software compo-nents (PC, IO modules, field bus controller, rotating laser ranger, etc.). Thecontrol system interfaces the actuators and sensors of the truck through thealready built-in local CAN network [1]. The task of the AGV controller isto provide tradition functionality (navigate the truck between two location(initial and goal locations)). A system of spinning laser range finder fittedon the top of the truck canopy and reflective markers fitted around the ware-house used to determine the exact location of the truck. The path planningis done by external PC to determine the path between the current locationand the target location. In the case of picking paper reels the target location

is the paper reel itself. The paper reel can be detected by two laser rangefinder fitted in front of the truck. The truck has been operated and tested forseveral times. As a result of testing the truck able to pick paper reels whicharranges in special way, see Figure 1.5. One can observe from the picturethat truck can pick paper from only one direction. One other thing one canobserve from this paper that truck can not avoid picking paper if there is twopaper reels closed to each other. This cause damage in paper reels. Althoughthe percentage of damage is small its still huge if it is calculated by money.

1.2 ProblemThe problem of material handling is studied and researched in different ar-ticles. [1], [6], [5] are some examples where this topic have been studied.Any way non of them answer the question; ” The material have to be pickedin what order?” Detection the target (in this research and in [ 1] is a paperreel while in both [6] and [5] it is a wooden pallet) is one of the problems.To detect the target its required to estimate its position very accurate toavoid hitting it instead of picking it. For wooden pallets which studies in[5] the order of picking them was according to the top one of them how-



Method for Autonomous Picking of Paper Reels 3

Figure 1.3: Truck Used in MALTAProject

Figure 1.4: Wooden pallets have spe-cific direction for picking even sometimes its a unique. The picking di-rection is perpendicular to the topwooden plates, the opposite direction of the red row.

ever his work does not provide the operator by any kind of tables about ”Inwhat order should pallet be picked?” the same in [6]. Both of them did notmeet any problem in defining the direction of picking where wooden palletshave a special and sometimes a unique direction of picking (Figure 1.4). [1]who provide MALTA truck to us studies the case of detecting paper reels(cylindrical object) in 2D environment. However, they did not answer theprevious question. The main problem in cylindrical object handling is thatcylindrical object (the reel) has 360 picking direction. So ”what is the bestdirection of piking”. These two questions: For a stack of paper reelsthat are needed to be moved from unloading to loading area (sayfrom warehouse to a container) by using autonomous guided vehi-cle, what is the order of moving paper reels? What is the graspingdirection for each one of them? were our research interests.

1.3 Goals

The goal of the thesis is to provide MALTA project and such other applica-tions by algorithm to help the autonomous trucks which handle cylindricalmaterials like paper reels and oil drums to decide what is the best pickingdirection. Also this thesis aims to provide these applications by algorithm todecide - for a kind of material arrangements - which piece should pick first.However, these two objectives not only what we expect from these thesis.



4

Figure 1.5: Loading Unknown Position Paper Reel.

While we looking forward to solve the problems discussed above, we alsolooking forward to provide good algorithm to build and update the truckmap from the sensor data.

We aim in this thesis to reduce number of damages in paper reels byprovide a safe picking algorithm. Also we aim to reduce storage area bygive forklift a good filling algorithm. We also aim to save fuel consumptionand increase truck maintenance period by using good ordering algorithmfor piking case and container loading scenario. For these aims the expectedresults are lists of paper reels locations and picking order. These lists shouldalso contain the grasping direction of each paper real and the optimal locationfor each (where it should put?). These results are expected to generate frompredefined map, fused data from sensory data and predefined map.

1.4 Approach

For these objectives, and to study problem case, a simulation environment onmatlab application is to be built. Firstly, the two objectives are to be studied,developed and to be tested on a predefined map in a static environment. Thena simulation of laser range finder and environment are to be built. Then thecase of building map from sensor data are to be studied. Actually to makethe problem easy to be studied, the case of dynamic environment where thereare a lot of variables (such as other moving truck in the environment or the




change in number of material pieces (paper reels in this case) by increasingor decreasing due to other truck activities) is not considered. Hopefully, thisthesis can also provide a good background to other researchers in future todevelop picking function to cover dynamic environment.

To build and test this simulation environment different algorithms have tobe used such as segmentation algorithm, line and circle detection and fittingalgorithms. Kalman filter is to use to fuse data together, other necessarymethod are descried in detail later on in this thesis.

1.5 Outlines

This research is coming in six different chapters. Chapter 1 is the introduc-tion part. In chapter 2 we give a deeper view to the problem studied in thisthesis, the best implementation for it so far and previous work related to thisproblem.

Then in chapter 3, we describe the method that we used here. We startwith describing kinematic model of our truck. Then we move to describethe mathematical models and equations that are used to build the simulator.Then we describe in some details the algorithms that used in segmentationand detection problems especially form laser scanner data .Then we describe

how we build a global map using data from different laser scanners and howwe update it by time. Then we move to discuss the algorithm that we developto analyse the constructed map to find the available paper reel in the mapand the best picking angle for it. After that we discuss the filling problemand the algorithm used in loading (filling) scenario.

In chapter 4 we discuss the results. Then in chapter 5 we return back toour work and try to high light its break points and analyse it. Then finallyin chapter 6 we give our conclusion of what we did and what we did not inthis thesis.



6



Chapter 2

Background

In reality, working inside warehouses or outside, to transport cylindrical ma-terials like drums and paper reels usually done by forklifts that driven byhumans. In the case of autonomous forklift, transporting materials of anykind is a very challenging work. Machines (forklifts) are not as cleaver as hu-man. In such case problems start from machine localization, dead reckoning,edge detecting, map building, path planning and decision making includingcontrol systems, sensor data understanding and etc. Our work here will con-sider one of these problems in detail and some of other problems where we

cannot finish our work without considering them. In this chapter we willstart by discussing our problem in more detail then we will move to mentionthe best available praxis and we will end by mention the previous relatedwork to our work in this thesis.

2.1 Problem Description

Let us return back to Figure 1.2 and think a while how could an autonomousforklift handle this huge number of paper reels. How it can decide which one

of them it has to pick up first? Also at what elevation?To give deeper view to the problem and understand it from its all sides

let us take a look to the Figure 2.1. The figure illustrates paper reels stackin some storage area and an autonomous forklift wants to transport themone by one to inside the container. As in MALTA forklift the forklift inthe figure has an LRF (Laser Range Finder) to detect paper reels and todetermine their locations. The first problem appears from the figure, thatthe truck can detect different papers in different locations when it moves.Note how the truck can detect papers (1,2,3,4 and 6) when it is in location(A). While it can detect papers (1,2,3,4,5 and 6) when it is in location (B).

7



8 Chapter 2. Background

Figure 2.1: Illustration to the problem, (Prescribed environment contain stack of paper reel needs to transport. 1- Letters show the truck in several locations (A,B and C). 2- Numbers show different paper reel in different location. 3- Arrow refer

to several direction to pick. 4- Bold arrow show the best praxis exist now. 5- Black dots refer to available directions to pick. 6- Cross sign refer to the reflected laser beams.

Note, that paper 5 is a plus. If the truck in location (C), it can detect onlypapers (8,7,6,4 and 3). Note, that papers 7 and 8 appeared for the first timehere while papers 1 and 2 disappeared in this situation. Note, in all locationspapers in the back cannot be detected. As the truck can not figure wholestack using only the LRF sensor. So how it can decide which paper reelshould be picked up in the first? let us assume that truck does not move

in several free paths. Let us assume, like in Figure 1.5, that truck move inpredefined and fix paths. Even in this situation the question still arises ,what paper reel should pick in the first? let us assume the worst situationwhere the forklift in location B and it will move directly to pick reel number5. This will cause damage in the two other reels (4 and 5). Let us alsodescribe other dangerous situation. Assume that there is no space betweenreels, and truck moved to choose one of reels number 3 or 4. In this casetruck will damage 2 and 4 or 3 respectively by its gripers. Of course if thedriver is a human, then reel number 2 will be chosen because off removingit is easier and will make grasping other easier, also because it is the nearest




one to the container.Again, assume we have the same dangerous situation as above, where

all reels close to each other, and the decision was to pick reel number 2 forthe same reasons mentioned before. If the driver for this truck is a humanhe will come to this reel from a safe direction then he will grasp it. Inautonomous forklift if there is no algorithm to give the forklift a decisionfrom which direction it will grasp paper reel, exactly like what happen inMALTA forklift, see Figure 1.5, the truck will move toward reel number 2in Figure 2.1 with the direction of the bold arrow, and again that is a reallydangerous situation which should be avoid. Note, such situation will damage

reel number 3. The question is if there are a lot of available direction, truckcan come to reel and grasp it from, (Note, the black dots on reels and thethin arrows.) which one of them is the safer? If they all are safe as eachother, which one the truck should come and grasp reel from?

Figure 2.2: Estimation of paper reel locations from uncertain laser range finder

reading. Black circles are estimated by using circle fitting algorithms and red onesare the true paper reels location. See the difference between estimated and trueones. Picture is taken from [ 1 ]

Other problem appears when using sensory data. That is sometimeserrors in sensory data, see Figure 2.2, cause the same problem above even if there is a space between reels. So how could truck know the exact locationof the paper reels? Finally, because off using data from LRF to detect andfigure reels, and that give limited information as we mentioned above, could




it possible there is other paper reel which is more suitable to pick-up first?In short, this is a typical example for what could happen and the expected

problem in situation of loading reels. Other kind of problems appear whenwe move to think in filling container scenario. The main question appearhere where forklift should put the second paper reel inside the container?Should it put it behind the first one or next to it? Other question also comesto the mind in this scenario when forklift start fill the second line in thecontainer (where the first reel of this line should be? Is it exactly next to thefirst one of the first line or between two reels form the first line to save thearea of container? How the situation will be when paper reels from different

diameters?These are the problems what we try to make a solution for them in thisresearch.

2.2 State of the Art

The most recent and the best praxis can found in some short details in [1].As mentioned above, A. Bouguerra et. al. (2007) created a forklift usedin special case; which is to transport cylindrical materials with several basediameter from storage area to load in a train wagons, containers or any other

transportation means. This forklift developed to work inside warehouse in ahigh dynamic area.

A. Bouguerra et. al. used a modified environment by pre-installed infras-tructure to solve the problem of localization. As mentioned in chapter onethey used spinning LRF with reflectors that is fitted around the environmentto determine the exact location of the truck. Due to the high stack of paperreel in the environment as in 1.2 they found this solution is not so sufficient.Later they suggested two solutions. The first one was by fitting reflectorsunder bottom of the ceiling and pointing the LRF or a camera to ceiling.The second solution, they suggested, was by using paper reels themselves as

landmarks in a SLAM process. Also they suggest to address each paper reelby a unique bar -code.

For the navigation function they used a predefined map uploaded to thetruck by operator. This map contains a defined possible drivable paths as acollection of lines and B-splines. It also contains location of different thingslike dangerous places (floor drop-down elevation), storage area, loading andunloading areas and doors of the warehouse. They use an external PC togenerate run time trajectory using cubic B-spline. This function allows themto provide machine with safe motion and obstacle avoidance functions. How-ever using the predefined map made some limitation (as they said). These




limitations were, the truck not allowed to change its path at runtime. Thatmeans the truck have to stop before generate its new path. This appears inloading scenario as in Figure 1.5 where the truck has to move until point Abefore generate its new path to inside container or toward paper reels.

For perception functions, as said before, they used two LRF to detectpaper reels and other obstacles. Later, after they publish their works, theyadd a third LRF in back of truck to detect and track obstacles and reelsin backward motion. However the main function of perception process is todetect and track paper reels, which means, in 2D, finding circles center anddiameters. They segments LRF data to several segments. They use distance

between points to segment the scan image. Then the circle fitting algorithmis applied and the position and radius are estimated. If the diameter of circleis outside predefined interval the circle is rejected. Others are considered aspaper reels. To estimate location of reels in global coordinate the relativeposition provided by previous step is combined with global position of thetruck provided in a previous paragraph. Finally tracker keep a global mapof detected paper reels. So the global position of each paper reel is updatedusing Kalman filter.

Since the truck work in a dynamic environment, its so important to keepa generated map in the previous step updated all the time. So they used

Euclidean distance to associate closest reel in the map to the sensing data. If there is no reel in the map, a new one is added to that location in the globalmap and vise versa.

In loading scenario they used the radial distance to pick the nearest paperreel to predefined point (point of generating path), point A in Figure 1.5. Butthey did not consider of picking direction of paper reel.

Finally, we have to say, in there published papers [1] and [2], which weconsider them as our first references the researchers did not give a deeperview to their works. These views can achieve by understanding the works intheir references.

2.3 Closely Related Works

Autonomous forklift handling systems is to our opinion one of the most inter-esting research in the last decades. The robots’ intelligence is demonstratedby navigation and exploring area. A little number of researches demonstrateon the decision of picking and grasping areas. However for the purpose of this research we divide the previous works to three fields; whole system,perception and detection and map building and picking problem researches.




2.3.1 Method of autonomous material handlingAs we described in section 2.2 above researchers in MALTA project were thefirst who developed a solution to handle cylindrical objects by autonomousvehicle. However, they are not the first who deal with forklifts that handlingmaterials. Garibotto in [5]was one of the earlier researchers in this filed.In his research he, and his partners developed a forklift to handle palletsautonomously with availability to operate forklift manually. Forklift task,drivable paths and location of loading and unloading areas was feed to therobot manually through a portable keypad. Robot localization is solved byfusion sensory data from three dimension vision with odometry data. They

used H shape landmark fixed on the ground to be a suitable reference tomake error correction for robot location. Same solution is used to determinebeginning and ending of pallets loading area. Predefined map for workingarea or warehouse also is used. For detecting pallets and picking it up theyused 3D vision provided by a video camera but not the same one used tonavigation task.

2.3.2 Perception and detection

Most previous work used mainly two type of perceptions, the wide spread-

ing one is the vision using video camera [5] or omni-directional one. Lastdecade and due to the decreasing in cost and computational demands almostresearches and researchers head to use laser range finder (LRF) as the mainenvironment preceptor for robots [1]- [6] and [14]. However there are severalpreceptors used instead of the previous two but the used of them decreaseby the time due to technical problems such as the high noise (Sonar andinfra red sensor) or needing to use more sensors with it to give direction anddistance to the target (infrared sensors). The advance of using the LRF notonly lower cost and low demands in computational power but also becauseits dependence on the environment is lower than others, for example the

brightness, humidity and temperature not affect its reading. Beside any oneof these or some time collection of them like in [5]they used other sensors tomeasure other robot variables such as speed (encoder) internal temperature,direction (Odometer) and other variables. While almost of researchers useddata from real laser range finder (LRF) or they do not mention how theysimulate this device.

The data collected from sensors (LRF) in [1],[6], [14] and [16] are clusteredto groups according to the distance between two consecutive points. If thedistance between these two points is more than a predefined threshold theline fitting algorithm is applied [6] and [14], the circle fitting algorithm applies




is in [1] or one of them is applied in [15], according to the result of comparingangles between four consecutive points as will be explained in next paragraph.

2.3.3 Map building

Map building, localization and object detection are wide research area andthey studied a lot last decades. The process of map building start fromobject detection and object fitting. For indoor environment as our systemhere, objects usually are described by line segments, arc and circles. JodoXavier [15] one of the researchers who study map building technique for the

indoor environment. In his research he try to cluster data collected fromLRF to groups according to distance between two consecutive points. If thedistance is more than the threshold a group is created. Then he ignored anygroup contain less than four point. After that he calculate the angle betweentwo limits point and any point in the middle. If the angle was between thevalue of (90 ◦ and 135 ◦) then there are some circle inside these points. Hesaid that he tuned these values to detect the maximum number of circles.When the circle detected he apply his algorithm to fit circle. To fit circlehe drew a line between each two consecutive points, from the middle of thisline he construct a perpendicular line. The intersection point of the twoperpendicular lines is the center of the circle and the distance between thispoint and any of the two points is the radius of the circle. If there is morethan one intersection point he consider the mean point as the center thenhe calculate the radius. More details about the application return to [15].However, Xavier used his method to detect moving persons and peoples. Sohe said he tuned an inscribed angle between 90 ◦ and 135 ◦. From a theoreticalpoint of view there is a week point in this algorithm. That is the algorithm issensitive to the error deviation. For example for large error in data detectionthe algorithm will fail. However he presented a method to avoid this weaknessby ignoring from calculation points that lay out of threshold tolerance.

The wide and most common method which is used to fit circles from

laser data is the least square method, well known by curve regression. P.Nuez [10] and his team worked on this algorithm. They proposed a circlefitting algorithm in the Cartesian coordinate space. However, his algorithmdeal with the case when the error in both coordinates X and Y are notindependent. Also he provide a variance matrix associated to the estimatedcircle parameter. Indeed, a lot of algorithms used Cartesian coordinate tofit circles using least square method. However, Julian Ryde [12] used leastsquare method to fit data directly from laser scanner reading based on polarcoordinate system because of using that is faster and more accurate. Formore details about his method return back to [12].




For purpose of map buildings Zezhong [16] developed an algorithm dependon three steps to build a complete map. At the first step he took the data fromLRF then he segment it according to algorithm named it (adaptive clusteringalgorithm). Then he compute line segment correspondence to each clusterby calculate center point, length and orientation from stochastic variables.In the second step he built a local map from the segments by arranginglines counter clockwise. Finally, he built a global map from the differentlocal maps. To update the global map at each laser scan he compared localmap and global map, if some line segments appears in the local map butnot in the global one he insert them to the global map. The final global

map is the merge of all local maps together. To localize the robot he usedsome features in the room as land marks and referred to them as a completeline segments in the environment map. Then he try to find the possiblecomplete line segments in each local map by comparing the length betweenlandmarks and each line of local map. The maximum likelihood was chose asthe best possible matching. Then the current position is computed accordingto this match. After several matching steps in relative position he get exactrelative position for the robot. The heading of the robot was calculated asthe difference between two vectors. First vector is a vector between centerpoints of two landmarks in the environment map. The second vector isthe vector between the center points of the correspondence line segmentsin the local map. Then the absolute global coordination are computed bytransfer relative position for any of the correspondence line segments to theglobal coordination knowing the calculated heading and the coordinates of the center point of the landmark.



Chapter 3

Methods

In this section we will describe in some details methods that are used to solveproblems in this thesis and hypothesis supposed to deal with it. This sectionis divided into several parts as we move in the problem solving. First we willdescribe the approach used in path planning and trajectory generator. Thenwe will move to explain perception algorithm. After that we will describealgorithm used for generating a map from the perception data. The final twoparts will be about the core of this research. They are, firstly, about reelselection then the problem of loading scenario.

3.1 Path Planning

We start from only one hypothesis. That is, we have predefined map for allunmovable facilities and object in the warehouse. This hypothesis is trueand it is satisfactory to start from. As we mention in section 2.3.1, almostresearches in the field of indoor autonomous vehicles are assumed that theyhave such kind of map. So we said this hypothesis are true. The reason makesthis hypothesis satisfactory is the simplicity of creating this map. Usually

architecture plans for warehouses are found in archive. So it is easy to createan accurate map from them contains location of main facilities such as walls,doors, columns, drop-down floors and drivable paths. In this case not sonecessary to provide exact location for each paper reel.

3.1.1 Kinematic model

As we described in chapter 1 and as in Figure 1.5 the MALTA robot is anordinary industrial forklift with two modified forks. The wheels (the mainpart of the driving system according to this topic) are a stander wheels with

15



16 Chapter 3. Methods

Figure 3.1:Simplification of MALTArobot kinematic model

used to compute the po-sition of the vehicle from wheel encoder information (bicycle model).

two degree of freedom, one rotation around the wheel axis and the otherone around an offset steering point. The two front wheels are only with onedegree of freedom without any ability to steering but they have the power totraction. The two rear wheels have the ability to steering. They are followAckermann steering principle1. For simplicity and with assuming that there

is no slip in motion we can simplify the robot model to be as a robot withonly two wheels one steering wheel in the back and one in the front (bicyclemodel as in figure 3.1).

Let φ is the steering angle of the rear wheel in the simplification model3.1. Let v is the linear velocity of the robot and let L is is the distance betweenthe rear and front wheels. Then let X and Y be the x and y position of therobot in the global coordinate and let θ be the heading of the robot in thesame coordinate and let dT is the sampling period. Then the position of theglobal coordinate is

P =

X Y θ

(3.1)

Then the change in position as a function of v and φ (kinematic modelof our robot) can be described as follows:

1For more information about Ackermann steering principle and the method of cal-culating steering angle for each wheel look to Desmond King-Hele, F.R.S., ErasmusDarwin’s Improved Design For Steering Carriages - and Cars. Notes and records of theRoyal Society of London, Vol. 56, No.1 (Jan., 2002) pp (41- 61)




Figure 3.2: The arrangement of points that chosen to gener-ate the path. Six points are cho-sen. P 1 is the original point of the robot frame. P 2 is a point in the middle of the forks. P 3is a point in a distance of 1.5d

on the robot X-axis. P 6 is thecenter of the paper reel. P 5 is a point on the surface of the pa-per reel at the same direction of picking direction. P 4 is a point on the same direction of P 5 but with 1.5d distance of the paper reel surface. ”d” refers to therobot total length. The path isgenerated by using B-spline cu-bic function.

δθ =

v

2L ∗ dT ∗ sin(2φ)

δX =v ∗ dT

2

cos(θ +

v

L∗ dT ∗ sin(2φ)) + cos(θ)

δY =

v ∗ dT

2

sin(θ +

v

L∗ dT ∗ sin(2φ)) + sin(θ)

(3.2)

For more details about the derivation of the kinematic model see theappendix.

3.1.2 Path selection

The mission of choosing the robot path here not to plan the path inside thewarehouse and not to avoid obstacles. The main idea about this topic is toprovide the truck with the safest path to reach the paper reel place from thesuitable direction and to put is inside the loading area by a suitable way.The suitable direction will be explained in section 3.4.2.

To simulate path for this process, a cubic B-spline function is used togenerate the path. The function is done as in [8]. To make the function workprobably, six different points should be provided to the function at least.

These points should be chosen to insure soft starting and soft moving forthe truck, while the truck control system have to be designed to follow these




Figure 3.3: The path se-lection to transport reels

from unloading area to theloading area. The path is described by 9 points.P 1, P 2 and P 3 as points(P 4,P 5 and P 6) in Fig-ure 3.2. 3 points (P 4,P 5and P 6) to guide the ve-hicle through the container entrance. Other 3 points(P 7,P 8 and P 9) to guidethe vehicle to the far-thest unoccupied cell in thecontainer.

points. These points as in Figure 3.2 first three points was the origin pointof the truck relative frame, the middle point between the two forks and apoint in front of the truck in the same heading direction at a distance of truck equal to 1.5 times of the truck length. The last three points are chosento be the center of the target paper reel, a point on the circumstance of thereel at the same piking direction, and a point in the same picking directionbut at 1.5 truck length from the circumstance of the reel. This arrangementof points insure a straight movement for truck during picking process.

The mission of transporting reels is defined by four main variables, the

reel location, reel picking direction, the loading area coordinates in globalframe and the access of the loading area (container gate). Again the B-spline function is used to generate path from paper reel to the loading area.The loading area in simulation environment is described by a matrix containthe coordinates of the container, (Container here is the loading area), andthe occupied locations inside these coordinates. Each cell of this matrix is asquare cell with side dimension equal to the diameter of the paper reel plusa safe distance for truck grippers. See Figure 3.3.

The mission is to fill the farthest row starting from the first empty loca-tion. The mission above is correct if the orientation of the container as in




Figure 3.3 and Figure 4.1 or the location was in the north side of the map.The path to the first empty location of the farthest row of the container gridis described by 9 points as follows (see Figure 3.3):

Pathloading =

P 1 = (C x, C y)t

P 2 = (C x, C y)t − rteiαt

P 3 = (C x, C y)t − (r + 1.5d)eiα

P 4 = P 5 − 1.5deiγ

P 5P 6 = P 5 + 1.5deiγ

P 7 = E cell − (rt + 1.5d)eiγ

P 8 = E cell − rteiγ

P 9 = E cell + rteiγ

(3.3)

Where (C x, C y)t is the center of paper reel number t, t = 1, 2, 3...n and n isthe total number of paper reels in the stack. P 5 is a random point on thecontainer gate where

Glc +w

2ei(γ −π

2 ) ≤ P 4 ≤ Grc −w

2ei(γ −π

2 ) (3.4)

Glc is the gate left coordinate. Grc is the gate right coordinate. γ is thecontainer orientation. w is the truck total width. d is the truck total length.E cell is the nearest edge of the farthest unoccupied cell in the container. rt

is the paper reel radius. αt is the picking direction of the target reels.

The return back path is described by the same way but in reverse order,and here α, r and (C x, C y) are the parameters of the next paper reel in thestack.

Pathreturning back = [P 9, P 8, P 7, P 6, P 5, P 4, P 3, P 2, P 1]T (3.5)

The only difference here is that points P 1, P 2 and P 3 are calculated accordingto the location and piking direction of the new paper reel target t + 1.

3.2 Perception

In the simulation environment we used LRF as the main perception device.The following parts describe the method we used to simulate this device andthe reading of this device. In the third part of this section we will describethe method we used to cluster and fit the data are collected from the LRF.




3.2.1 LRF simulationThe simulated LRF is SICK S300 which is send and receive 360 beam withprecession of 0.5 ◦ and cover 180 ◦. The maximum range of the beam is 80m.The principle of LRF sensor is simple. By calculating the time of flight of each beam, it can detect the distance of the object while the object insidethe LRF range. We simulate each beam as a line and the reflection pointas the intersection point between the beam line and the nearest object. Theinput of the simulator is the location of the LRF in the environment [x,y, θ]’and the environment map itself. The output of the function is the reflectionof each beam with its correspondence direction. The reflection is described

by the exact distance between the LRF and the target (object).Inside the simulator the following steps are made;

• The environment map is transferred and rotated clockwise around theposition of the LRF.

• All object of the map which are located behind the LRF are removed.

• In iterative loop, the intersection between each object ( walls, and otherobstacles as line segments, columns and reels as circles) and each beamare tested then calculated if there?

• For each beam, the distances for all intersections are collected in onearray then the minimum distance are considered as the reflection pointof the beam.

• Finally, a normal distributed error is added as in the following section.

3.2.2 Error simulation

To make the simulator near to reality two kind of error are added to thesimulator readings. The first one is the error in the location of robot. We

1Readings from LRF are taken every sampling periods T which means, sometimes, if the speed of robot motion is not high enough, one or more readings could be taken inthe same location. Form another hand, if the robot motion are high the gape in robotpositions between any two consecutive reeding will be big. However to solve this probleman a special algorithm is necessary to make synchronization between Odometer readingsand LRF or other sensors reading. As a simple example is an algorithm to extract readingfrom these sensors in a specific time δt. In our work here we ignored the simulation of thesampling time for the LRF. Instead we arranged to extract one reading at each location of the robot in the environment. We found this more than enough to simulate the samplingtime of LRF because in real applications we not expect to get a one reading in eachlocation.




assumed it could be stimulate the error in the odometry readings. To makethe calculation easier the error between X and Y positions is assumed to beuncorrelated. The error in location is simulated as follows :

δP noisy = δP +δP

P t−1N (0, σ p) (3.6)

where P is X, Y or θ. Finally different σ is chosen for each of them tomake the error uncorrelated.

The second type of error is the error in LRF reading. Also this kind of error is simulated by the same way as the previous one and as the following

equation illustrate.

dnoisy(i) = d(i) +d(i)

dmax

N (0, σd) (3.7)

Where dmax is the maximum range of the LRF. The main difference be-tween the two kind of errors is that the error in equation 3.6 are accumulatedby time while it is not in equation 3.7.

3.2.3 Segmentation and Feature Extraction

Data taken from LRF is in the polar form as P i = (ri, θi) where θi is the

direction of the point according to the LRF location. Converting these datato Cartesian form as P i = (xi, yi) will generate an unnecessary error due toapproximation process. So we decide to work with all data in the polar formonly for the segmentation purpose. However several methods of segmenta-tion algorithm are studied and tested,(the all methods in [11]). Then themost popular one as shown in follows is chosen because it gives suitable re-sults without needing for complicated calculations. This algorithm known as(PDBS) (Point-Distance-Based-Segmentation Method ). This algorithm re-turns each segment as a flag for the location of start and end points. Thedetailed work in [11].

Consider a full scan image of size N points P s. The scan image is describedas S i = {(θi, ri) | i = k : n} where 1 ≤ k < n ≤ N .

The segmentation condition is, if D(ri, ri+1) > Dthd then segments areseparated otherwise do nothing. D is the Euclidean distance as in equation3.8.

D(P i, P i+1) =

r2i + r2

i+1 − 2riri+1cos(θi − θi+1) (3.8)

and Dthd is defined as follows.

Dthd = C 0 + C 1 min{ri, ri+1} (3.9)




C 1 =

2(1 − cos(θi − θi+1)) = D(P i, P i+1)ri

(3.10)

C 0 is the constant parameter that used for noise reduction.The results of segmentation using the above two algorithms are as in

figures ?? to ??.

Circle fitting

Fitting circle means find the most suitable center and radius describeall points in the segment. Since the data which is extracted from LRF noisy

and not accurate We used two combined methods to find the most suitablecircle. The work depended totally on the works of Chernov and Lesort [3]and [4]. The circle equation is

(x − a)2 + (y − b)2 = R2 (3.11)

where, (x,y) the coordination of any point on the circle in Cartesian coor-dinate, (a,b) the coordinates of the center point and r is the radius of thecircle. To fit our segment points, which they are not less than four points, toa circle we started from algebraic fitting method. Then we used the resultas an initial guess to the geometric fit. We used the two methods that aresuggested in [4] Newton-based, Taubin algebraic fit because its an optimizefor speed. Then we moved to use the geometric method that suggested byLevenberg-Marquardt to fit circle in full (a,b) and R space.

The noisy reading of the Laser Range Finder (LRF) generate a wide rangeof expected circle even when we used the most suitable circle fitting. A deepexplanation about that is explained in [10]. However the uncertainty in thethree parameters are calculated as follows.

C = σ2(W W )−1 (3.12)

Where () refers to transpose, ( )−1 refers to inverse and C is the covari-ance matrix in the three parameters, (X center, Y center, R), σ is the unbiasedstranded deviation of the radius R from estimated center to each observedpoint and it calculated as follows:

σ2 =Σ(e2

Ri)

n − 3(3.13)

Where n is the number of points in the observed segment, and eRi isthe difference between the estimated radius and the distance from estimatedcenter to each point in the segment d(C a,b; P x,y).

eRi = R − (a − xi)2 + (b − yi)2 (3.14)




The matrix W is a (nx3) matrix described by the three column uvand 1

W =

u1 v1 1u2 v2 1

. . .. . .. . .

un vn 1

ui =xi − a

R(3.15)

vi = yi − bR

i = 1, 2, 3...n;

The covariance matrix is used- as we mentioned in previous subsection-to recognize the most suitable circle and to reject others. The way of usingthat was by testing the summation of the diagonal elements of the matrixand compare it with a predefined threshold. In our case we used a fixedvalue (0.2, the error uncertainty of the LRF that provided from manufacturerspecification). So the test condition was

(3 × 0.22) > (σ2a + σ2

b + σ2R) (3.16)

Once the three parameter of circle a, b and R and the covariance ma-trix are calculated the result is exported to a local map as a circle with itsuncertainty in location and dimension.

Line fitting

Fitting data points to a line is more easier than fitting them to a circle. Itcould be done directly by using Weighted linear regression as mentioned in[13] . So by considering Figure 3.4 and by considering the line equation

ρ cos θ cos α + ρ sin θ sin α − r = 0 (3.17)

ρ cos(θ − α) − r = 0 (3.18)

where the following symbol R i, Theta i, alfa, and d in figure is ρi, θi, αand di in equations. The idea is to minimize the sum of perpendicular dis-tance di, the error to be zero. Because of error existence the above equationcan be written

ρi cos(θi − α) − r = di (3.19)




Figure 3.4: Line fitting problem.

The error that is wanted to minimize is

S = Σd2i = Σ(ρi cos(θi − α) − r)2 (3.20)

Considering the weight of each point in the scan image w to be the inverseof that point variance w = 1

σ2

i

the scan points can regress to a line that

is described by its norm length r and norm angle α by the two followingequations.

α =1

2tan−1

Σwiρ2

i sin2θi − 2Σwi

ΣΣwiρi cos(θi)w jρ j sin(θ j)

Σwiρ2i cos2θi − 1

ΣwiΣΣwiw j ρiρ j cos(θi + θ j)

(3.21)

r =Σwiρi cos(θi − α)

Σwi

(3.22)

Fortunately the weight w for points are same so we do not want to worryabout this value. This is because we are doing segmentation for scan image

from one LRF in each time. The value of the weight w becomes importantwhen one sum the scan points from different sensors then try to segmentthem. The above two equations are written as follows

α =1

2tan−1

Σρ2

i sin2θi − 2n

ΣΣρi cos(θi)ρ j sin(θ j)

Σρ2i cos2θi − 1

nΣΣρiρ j cos(θi + θ j)

(3.23)

r =Σρi cos(θi − α)

n(3.24)

Where, n is the total number of points in the segment, i and j are thesum counters i = 1, 2, . . . , n and j = 1, 2, . . . , n.




Figure 3.5: Circle fitting results. σρ

(LRF beam error variance) here is0.05.

Figure 3.6: Weighted linear regression.Note there is high error in calculating start and end points using equation 3.30.σρ (LRF beam error variance) here is0.05.

The propagation of uncertainty for these two parameters during extrac-tion is done according to the error propagation law 3.34 but by the ignoring

the first term. So the uncertainty becomes.

C r,α = F ρ,θC X F T ρ,θ (3.25)

where F ρ,θ is the Jacobian matrix of r and α according to each ρ and θin the segment. So the size of F ρ,θ is 2 × n. ’T’ in the equation refers tothe transpose. C X is a diagonal matrix contain the variance of each point inboth dimensions ρ and θ, which mean the size of C X is (n × n).

C r,α =

σ2

α σr,α

σr,α σ2r

(3.26)

C x =

σ2ρ1 0 · · · · · · · · · 0

0. . .

......

......

...... σ2

ρn0 · · · 0

...... 0 σ2

θ1· · · 0

......

... 0. . .

...0 0 · · · · · · 0 σ2

θn

(3.27)

F ρ,θ =

∂α∂ρ1

· · · ∂α∂ρn

∂α∂θ1

· · · ∂α∂θn

∂r∂ρ1

· · · ∂r∂ρn

∂r∂θ1

· · · ∂r∂θn

(3.28)




For the fitted line one can calculate the start and end point of the segmentby calculating Y as

Y = −cos α

sin αX +

r

sin α(3.29)

However, when the extracted line is vertical, which means the value of α isnear to 0 or π using equation 3.29 becomes useless because the value of thedominator in the equation goes to zero. So the equation is converted to findthe value of X instead of Y .

X = −sin α

cos αY +

r

cos α(3.30)

Finally to minimize the error of the feature extractor, remove from themap all lines with the following condition.

(2 × 0.22) > (σ2r + σ2

α) (3.31)

where 0.2 is the LRF beam error variance provided by manufacturer as inequation 3.16

3.3 Map Building

Once all feature from the laser scanner readings are taken the process off map building starts. The procedure of map building consists from threesteps. The first step is to build local map from each laser range scanning.The second one is to match local maps to other map from previous readingsand from other Laser scanners then to sum them together generating a globalmap. The final step is to match our global map with the predefine map thatwe have to discriminate each object and give it its appropriate description.Here we are going to describe each of the last two steps, knowing that, thefirst step is already described above. Where in the end of the process of feature extraction above, the local map have been already generated. So

here we are going to speak about matching and summation algorithm.

3.3.1 Matching algorithm

From section 3.2 one can see that features at each map are described intwo ways, lines and circles. Lines describes walls and circles describes bothpaper reels and concrete columns. By the way, at each scanning there arethree different maps expected. The previous map from previous scanning,the predicted map of the previous one and the current map from currentscanning. If there are more than one LRF available more current maps can




be considered, one from each laser scanner. So matching algorithm betweenthese maps are necessary to find the corresponding features between eachmaps with the rest. The matching algorithm which is described here consistsfrom two levels. The first one to match all expected maps to each other. Thesecond one is to match resulted map to the predefined one and discriminatepaper reels from the concrete columns, and factory walls from other wallsand other obstacles.

The local maps that are created from extracted features in section 3.2 arerelative maps. This means that each map is described in the sensor frame.So the first step in the matcher is to rotate each map to the global frame.

Because of extraction features return three parameters in the case of circle(Radius, two coordinates for the center) and in the case of the line returnsfour parameters (two points, start and end of the line, (two coordinates for each), the slop of the line and its intersection with Y axis. So the rotation andtransformation is direct if both the location of the sensor and the location of the robot are known.

For circles the most important thing is the center. So the new locationof circles are

C = [C x C y 1]T (3.32)

C n = T RC (3.33)

Where T is the homogeneous translation rotation matrix according torobot location. R is the homogeneous translation rotation matrix accord-ing to sensor location. Fortunately, the third parameter of the circle is notaffected by the rotation translation. The uncertainty of the location of thecircle is affected by this rotation translation. The main important effect isto the uncertainty of the location of the robot, because this uncertainty ischanged by time, and may increase too. The uncertainty of this location isreflected to the location of the features according to the law of error propa-gation

Σn = F C ΣF T C + F inΣinF T in (3.34)

where Σ refers to the uncertainty in the coordinate of the center of eachcircle and Σ refers to the uncertainty of the location of the robot. F in

refers to the Jacobian matrix of the location parameters [∆x ∆y θ]. F C

is the Jacobian matrix of the rotation-translation function according to theparameters of the center in 2D [X c Y c] as follows.

F C =

cos(θ) −sin(θ)sin(θ) cos(θ)

0 0




The translation of the lines are done by the same way as in the circle.The only difference is considering the norm length and norm direction of thelines.

The second step of the matcher is to find the corresponding featuresbetween each two maps. This is done by the following steps.

• For each circle, calculate the geometric distance between its center andthe other centres in the map. The correspondence circle in the othermap is the circle with the center to center distance less than threshold,( the best thing is to choose the radius of any of them as a threshold).

• For each line, calculate the geometric distance between its (norm r, andnorm direction α). The correspondence line, is the line with a differenceless than specific threshold too. However, this is not the only conditionfor lines.

• If two lines have the same slope and intersection, then check if thereis overlapping between them. If there is then the two lines are corre-sponding. Otherwise, if the distance between these two lines less thanspecific threshold consider them as one, else keep each one of themalone.

• Calculate the perpendicular distance between each circle and each linein the map. If the distance less than the radius of the circle, thencalculate the intersection between the line and the circle. If there isany intersection or overlapping then remove the object with less sumof uncertainty matrix diagonal.

After finding the corresponding features from the two different maps applythe Kalman filter in static mode [13] (update stage) to calculate the newfeatures and new features uncertainty.

P n = P m1 + K ∗ (P m2 − P m1) (3.35)

1Σn

= 1Σm1

+ 1Σm2

(3.36)

K =Σm1

Σm1 + Σm2

Kalman filter gain (3.37)

Where

P n is the new estimated parameter of any feature.P m1 is the parameters estimated for the feature from map number 1.Σm1 is the associated estimated uncertainty for the feature from map

number 1.




P m2 is the parameters estimated for the feature from map number 2.Σm2 is the associated estimated uncertainty for the feature from map

number 2.Σn is the new estimated uncertainty.

The second stage (level) of the matcher is to discriminate all features. Thisis done by matching the current, new map with the predefined map usingthe same procedure as above. Here the matcher discriminate circles to paperreels or columns and discriminate lines to factory walls, container walls orobstacles.

The assumption is the map does not contain the location of paper reel.

The matcher tries to find the correspondence circle to the correspondencecolumn. If this is the case, the circle will flagged as column. If there is nocorrespondence column in the predefined the circle is considered as a paperreel. In the case of lines, the matcher tries to find the correspondence factorywall, or container wall to the line feature. If this correspondence is exist, thenthe line is considered as one of them, according to the nearest. If the case isnot exist, simply the line consider as an matcher error and its removed fromthe map. By this way the matcher give us an accurate map. In the end of thisstage the matcher calculate the uncertainty of the location of the robot and of the feature extractor. This is done by considering the columns as landmark

for the position coordinates and walls as land mark for direction coordinate.The main assumption here is the rotation and translation between the builtmap and the predefined one, according to previous process, is small enoughto find the exact correspondence between features, which guarantee that wedo not have error in the matcher results.

To understand what is happening here take a look to the Figure 3.7. Thetotal rotation of the built map can express only by the difference in the angleof lines segments. So only what is necessary to do here is to find the angleof each line segment θ and compare it with the angel of corresponding linein the predefined one.

The rotation in angel ∆θ becomes (∆θ = θlineSegment − θwall). Therefore,the uncertainty in the estimated slop of any line segment reflects directlyto the difference in the direction. So one can calculate the difference in thedirection directly as follows,

∆θ =Σn

i=1(θline Segmenti − θwallj)

n(3.38)

And the uncertainty of this difference in direction as follows

σ2θ =

Σni ((∆θi − ∆θ)σ2

Segmenti slope)

n(3.39)




Figure 3.7: example of matching created map with predefined one

The translation is calculated by the same way but by using the center of columns as references instead of walls. Note that, calculation of translationhave to be done after applying rotation to the whale map.

3.3.2 Map updating and map generation

Once the matching process finished, the current global map is generated. Thenext step is to keep track of all occurred and disappeared paper reels in theenvironment. After discriminating all features in matcher, the paper reels areextracted to a special data structure such as array. In each localization step,and to make using of Kalman filter in its dynamic mode, the new locationof robot itself and the new location of each paper real in the data structure

is predicted according to the velocity and direction of the robot. Then byusing Kalman filter as described in equations 3.35 to 3.37 each update stepadd a new paper reel to the map. If the predicted step does not predict afeature (paper reel), i.e. if a paper real is detected by the scanners but it notfound in the data structure, a new paper reel is added to the data structurehence to the map. If a paper real is found in the data structure, and itsnew location is predicted, but there is no update stage for it, i.e. it doesnot appear in the scanners images, this paper reel directly removed from thedata structure hence from the map. Here, because while the robot movingdifferent paper reels go out of the range of the scanner, the main effort spent




to keep only track of those reels that locate in the range of LRF scanner orscanners.

3.4 Paper Reel Selection

The main topic of this research is to know the priority of paper reels selectionand the direction of grasping the paper reel. First of all, the location of paper reel is calculated and estimated accurately from above procedures.The importance of this point appears when one know that a small differencein distance, say 5 cm, in the estimation of the paper reel may destroy the reel

totally. In this section we are going to discuss the algorithm that allows tothe vehicle to choose the most suitable reel and decide from what directionit should be picked up.

3.4.1 Paper availability and reel priority

As described in section 3.3.2 above, once paper reel is detected, it is extractedto a special data structure. The process of extraction is first appear firstextract no mater where is the reel or how much it is far from the truck.The logical assumption that one can consider to answer the question which

reel is better to select first? is to select the nearest most accessible reel . Sothe selection algorithm has three stages to find the proper reel and take adecision.

1. Find the most accessible reel.

2. Find the nearest one of the accessible reels.

3. Update the path of the robot on line if first appears paper reels all arenot accessible.

Before calculating the accessibility, each target reels, assumes to be acces-sible from the 360 degree. However, because it wants a lot of computationalpower and space, the assumption is to check accessibility in intervals, whichis sufficient enough to give us a clear evidence about what direction is ac-cessible. So first of all each paper reel has been given 72 accessible directionby step of 5 degrees. This is start from 0 direction with the horizon to 359degrees see Fig. 3.8.

Accessibility direction of paper reels means that there is no obstacle inthe truck way to grasp it from that direction, i.e. the truck has to be ableto manoeuvre if it move toward the reel from that direction. The truck is




Figure 3.8: Each reel has 360 accessibility angles. Instead, It is considered as interval. Thuseach reel has 72 accessibility angels.

Figure 3.9: Each object is considered as an obsta-cle if the distance between that object and the reel more than 1.7 of the length of the truck until theend of the gripers

able to manoeuvre freely if the shortest distance between the surface of thereel and any obstacle surface more than 1.5 times of the total truck length2. Obstacles could be walls, columns or other paper reel. If the distancebetween any obstacle and the reel less than 1.5 times the total length of thetruck, the angle of accessibility is calculated as in the following procedure.

• Make the following definitions;

– Accessibility angle (γ ) is the angle between the center of reeland the horizon where there is not any obstacle.

– Picking angle (θ) is the robot heading angle (between the robotand the horizon). In the moment of grasping action . By this twodefinition and as its appear from Figure 3.11 the relation betweenthese two angles is given by

θ = γ + π

2This value is chosen by experiment in the simulated environment. We believe thatthere is better way to choose this value more accurately. However, for the sake of thisresearch this assumption is sufficient




Figure 3.10: If the obsta-cle is some thing like wall,i.e the reel location is closeto wall.

– Centre to center distance is the distance between the centerof the target paper reel and the center of the target obstacle inthe case of circular obstacle and to the surface of the obstacleotherwise, the shortest one, P .

– Centre to Centre angle is the angle between the center line andthe horizon, in the case of circular obstacle.

– is the angle between the picking direction and the center linedirection.

• The obstacle objects are divided into two groups according to the sizeof its surface and the diameter of the reel. The very huge surface,(Lines; walls) and limited surface size as (Circles; columns and other reels).

• In the case of very huge surface (walls) the shortest distance between




the center of the reel and the obstacle is the perpendicular distance tothat obstacle. The direction of robot movement (picking direction), thereel and the obstacle creates a triangle as its appear in figure 3.10. Inthe figure below shows that the inaccessible directions are located be-tween obstacle, reel and two picking angles, say ˆthetamin and ˆthetamax.If the minimum required distance, that allow to the robot manoeuvres1.5d, where d is the total length of the truck, is considered. Then thesetwo angles are calculated as follows, see Figure 3.10

ˆthetamin = floor(mod(λ − π

2+ sin−1( P −r−g

d+g), 2π)

180a/π) ∗ ( 180a

π) (3.40)

ˆthetamax = ceil(mod(λ + π

2− sin−1( P −r−g

d+g), 2π)

180a/π) ∗ (

180a

π) (3.41)

where P is the distance from the center of the reel to the obstacle. ris the radius of the reel and a is the interval length (in our work herea = 5o). λ is the obstacle norm direction. ’g’ is a safty distance toinsure safe calculations. In this case we assumed it to be two timesthe gripper width. Then the possible accessible directions is the rest

of interval from ˆthetamax to ˆthetamin moving in the positive direction(counter clockwise).

• In the case of circular obstacle. The robot calculate the maximum inac-cessible direction and the minimum one according to the belt problem[9]. See Fig. 3.11. By using the definition above. The maximuminaccessible angel is given by

ˆthetamin = floor(mod((π + φ − ), 2π)

180aπ

) ∗ (180a

π) (3.42)

the maximum inaccessible angel is given by

ˆthetamax = ceil(mod((π + φ + ), 2π)

180aπ

) ∗ (180a

π) (3.43)

The possible accessible directions are direction intervals that are layingbetween the maximum angle ˆthetamax and the minimum one ˆthetamin

moving in the positive directions(counter clock wise). The main factorin these two equations is the angle . This angle is calculated by theequation




Figure 3.11: Description of use of built problem to calculate the accessibility an-gle. The dots refer to the possible picking direction. The X’s refer to inaccessibledirections.

= sin−1( r1 + r2 + gP

) (3.44)

where r1 and r2 are the radius of the target reel and the obstacle re-spectively. ’g’ is a safety distance as above.

• Convert the circular representation of the accessible intervals to a linearrepresentation. I.e from figure 3.8 to figure 3.12. As it appears in figure3.12 inaccessible direction are removed.

• The process above is repeated for all obstacles appears around the

target reel. At each time the inaccessible angles are removed.

Once the accessibility of the reel is calculated the paper reel exports to theanother data structures ( say stack or array). The new data structure shouldcontain the position of the reel center, the radius, accessibility directions,uncertainty of the location and diameter and the distance to the robot. Nowthe picking priority is to the nearest available (accessible) paper reel. If thereis more than one paper reel has the same distance to the robot. The priorityis to reel that appears first in the data structure. This data structure shouldbe valid until the robot move the reel from its position. In the moment that




Figure 3.12: Change the presentation of accessibility angles to linear presentation then remove the inaccessible angles and flag them.

this reel has been moved this data structure has to be deleted then createdagain according to the new situations for the rest of reels.

3.4.2 What is the grasping direction?

Once the most accessible nearest paper reel is chosen the algorithm ignoresall other reels. If this reel has more than one access direction the algorithmchooses the middle one as picking direction. If the reel has only one, thenthere no chance to chose other direction. Just use it. If this reel has two

the algorithm chose any of them, the first appears has the first priority. If there are more than one but they are in different sides of the reel say northand south, calculate the distance to the reel using the middle one for each of them then take the shortest one.

Calculation of this distance means that the robot should be able to gen-erate its path to the reel from that direction on-line, section 3.1.2. Then thelength of that path should calculated according to the shape of that path.



Chapter 4

Results

The working environment had been modelled as a map of line segmentsand circles. Walls and other type of obstacles are represented as lines whilecolumns and paper reels are represented as circles. Such representation couldbe accepted in different environments. Lines are represented by two pointfor each line P start and P end. Circles are represented by only one point P center

and radius r. Objects are classified by labels, each object type has one label.For example 1 for walls, 3 for columns and so on.

Over this environment we done all our works. Some-times and at some

stages we divide this environment for serving some purpose as when we dealtwith map building. While we considered the whole map as an input for theLRF, we only used the environment without paper reels to robot navigation.Then we leave algorithms to discover the rest of the environment ( paperreels).

Figure 4.1: Testing environment Figure 4.2: The truck shape and the arrangement that is chosen to theLRF fitting on the truck. Both are

fitted on the same hight.

37



38 Chapter 4. Results

This environment have been simulated based on Matlab. The implemen-tation of the methods above was in the same procedure as that methods werementioned in the method chapter. A global map has been built from tworange finders. The map is analysed to find the best arrangement of selectingpaper reels. Then the proper reel has chosen and the proper direction hasdefined.

The testing environment consist of 4 warehouse walls, one of them isdivided into two segments around an entrance for a container, a 3 walls of container. 42 concrete columns with diameter of 1.5m and 48 paper reels withthe same diameter as the concrete columns. The environment is illustrated

in Figure 4.1. The black lines represent the warehouse walls. The blue linesrepresent the container. The black circles are the concrete columns. The redones are the paper reels. The environment size is 80 × 60m2. The distancebetween each column and the other one is 12m in X axis and 7m in Y axis. The size of the container is 8 × 22m2 1. As appears in the figure someof columns are located among the paper reel stacks which was one of thechallenges.

The truck is simulated to look like an ordinary one. The size of truck isarbitrary. The length from the end point to the fork face is 2500mm. Thetotal width is 1600mm. The fork size is 1200×50mm2. The other dimensions

is not necessary here because we are working with 2D environment. This isillustrated in Figure 4.2.

The laser scanners are simulated by calculating the intersection betweeneach laser beam and each feature of the map. A normal distributed erroris added to the scanning image to give some noise to the image to be morerealistic. A variance of 20mm per each meter and 0.1 degree per each degreeare chosen to be the error deviation. The resolution of the scanner is 0.5degree, which means two beams in each degree. The maximum range of thelaser beam have been chosen to be 35m. Two laser scanner are added in thesimulation environment. The first one is fixed in the middle of the forklift,and directed straight forward. This laser scanner covers 180o of directions90o degree from each size. The other one is fixed beside the first one butwith a little bit rotation. See figure 4.2 for LRF’s arrangements. This onecovers area of 270 degrees, 135 degrees from each direction.

The implementation of LRF simulation was by inspecting and calculatingintersection between each LASER beam and each object in the map. Thisfunction order becomes O(n×m) where n is the number of LASER beams and

1The size of each component in this environment is built to be near to the actual sizein the reel world. However, because the high number of choices in the practice, the chosensize is not accurate, especially for truck simulation.




m is the number of objects in the map. The first run for the LASER simulatorfunction takes totally 3 minutes to convert and it was only for one scan. Tominimise the time of implementation we removed from the map all objectsthat located outside the range of the LRF. Also in the first implementationthe length of each laser beam was 80m but later on to minimise the timeas much as possible the length of beams is minimized to be as mentionedabove. The current implementation takes up to 1 minute in the worst case.Figure 4.3 shows the resulted scans from the two LRF’s. The blue pointsis the scans from the first one. The magenta points are the scans from thesecond one. The figure shows the scans relative to the LRF position. So the

scans are rotated and translated to the origin. The difference in scan pointsbetween scan images is the difference in rotation and location of the two LRFon the forklift body. Figure 4.4 illustrates the error in scan points.

Figure 4.3: Scanning image from simulated LRF. The image shows that the imageis relative to the scanner position

All scan points are extracted in a polar form P s = (α, r). So first allpoints are converted to Cartesian coordinate then rotated to global frame.Then the returns to the polar form again. After that they are passed toprocess in segmentation algorithm. Figure 4.5 shows the scans after rotatingthem and translating them to the global frame.




Figure 4.4: Simulated errorsin scans Figure 4.5: Scanning points after preprocessing.

All point are rotated and translated to the global map.

4.1 Segmentation

Five different type of segmentation algorithms have been tested. The onethat mentioned in section 3.2.3 and the other four algorithms that describedin [11]. We were interesting in the adaptive point algorithms, especially theone that we describe in section 3.2.3 and in kalman filter algorithms, espe-

cially the second one. The two Kalman filter algorithms that are describedin [11] are predicts the next point in the scan image according to previouspoints behaviour. However, both of them are very good algorithms if the scanimages present the environment features as lines only where the behaviour of next point is expectable. So in our case they are useless because we have twokind of features; circles and lines. In a circle such as the one that is illustratedin figure 4.4 the algorithm segments points to two or more groups accordingto the direction of gradient. So the algorithm was inapplicable in our case.The poor and the few numbers of features that can detects by these two algo-rithms (which based on Kalman filter) were the main reason to reject both of

them. The three of adaptive point to point distance that represented in [11]also have been tested. The main thing about these algorithms is that thesealgorithms depends on one or two constant parameters as a noise reductions.According to choice of these parameters the segmentation process could berich or poor. However, there is a limit to how much this process could be rich.Finally we chose the (Point-Distance-Based-Segmentation Method, (PDBS)[11]).

In table 4.1 above, the effective segments means the segments that itpossible to interpret to meaningful feature. The correct segment means thesegments that cover a real features. However, the incorrect segments are the




C 00

.5

σr

σr 1

.5

σrTotal number of segments 12 15 17

Number of effective segments 6 8 9Number of correct segments 6 8 9Number of incorrect segments 4 1 1Average length of a segment 10.9 12.2 13.6Average length of an effective segment 10.8 13 15.3Number of unconsidered scan points(of 360) 229 177 129

Table 4.1: A comparison between segmentation results; ”number of effective seg-ments and length of segments” when three values of noise reduction constant areused.

Figure 4.6: Extracting features and building local map. σρ here is 0.2. Good estimation for line segment in the bottom.

segments who considered only part of points that reflected from a real object.

Its obvious that the number of features that are detected are more whenC 0 is higher. However, the error in the fringes (the limit range of the LRF)

are also more. The best choice according to these figures and the table is tochoose the value of C 0 to the be the highest. However, when the distancefrom point to its neighbour point distance is not really big choosing of C 0to be high shows a bad result and high error in feature detection see figure4.8. In this figure the feature in location (20, 50) is totally wrong. Instead of segment the two circles to two different segments the procedure returns onesegment which fitted on a circle. From another hand the line on the top leftof the figure are segmented to several segments where each of them can fit acircle with a smaller radius than a threshold which leads to reject all lines inthat place. So in the result choosing the value of C 0 to be equal to σr was




Figure 4.7: Extracting features and building local map. σρ here is 0.2. Note thebad estimation of line in the left

the best choice for us.

4.2 Extracting Map

f Uncertainty Parameters Error# UCx UCy UR Cx Cy R ex ey eRL1 0.1222

*10−30.0574*10−3

1.557 0.173 0.014 0.173

C2 0.2131*10−3

0.3245*10−3

0.3611*10−3

11.998 6.000 0.751 0.001 0.000 0.001

C3 0.0052 0.0008 0.0038 23.955 6.008 0.701 0.045 0.008 0.051C4 0.0968 0.1460 0.0810 36.084 5.830 0.877 0.084 0.170 0.127C5 0.0084 0.0014 0.0059 23.880 13.933 0.672 0.120 0.067 0.075C6 0.2186 0.3546 0.3805 12.002 14.008 0.755 0.002 0.008 0.005

C7 0.0039 0.0173 0.0163 12.100 22.157 0.896 0.100 0.157 0.146L8 1.8148 0.0060 1.120 43.1028 error error

Table 4.2: Features extraction from one LRF scan, and building local map as in fig.4.6 . Li refers to line feature. C i refers to circle feature. Empty place refersto no reading available for this measure because its not available for that feature.error refers to bad estimation.

The features are fitted after the segmentation procedure directly. The cir-cle fitting procedure flowed the algorithm that is mentioned in section 3.2.3.The circle fitting algorithm was start by fitting a circle to its correspondence




points algebraically then forwarding the result from this fit to geometric fit-ting algorithm to find an accurate fit to the circle. We have used the matlabscripts from [4] to do this step. We have used the Algebraic circle fit byTaubin, based on Newton’s method. Then we forward the result to Geomet-ric circle fit algorithm which minimise the orthogonal distances based on thestandard Levenberg-Marquardt scheme in the full (a,b,R) parameter space.

f Uncertainty Parameters Error# UCx UCy UR Cx Cy R ex ey eR

C1 0.0059 0.0086 0.0105 24.016 13.981 0.753 0.016 0.019 0.003C2 0.0090 0.0021 0.0058 35.803 21.930 0.611 0.197 0.070 0.139C3 0.0042 0.0012 0.0030 23.892 21.934 0.669 0.039 0.066 0.081

C4 0.0029 0.0057 0.0046 23.961 29.960 0.709 0.039 0.040 0.041C5 0.0010 0.0071 0.0054 11.971 30.013 0.751 0.013 0.029 0.001C6 0.0474

*10−30.1713*10−3

0.1485*10−3

12.005 21.998 0.745 0.005 0.002 0.005

L7 0.3644 0.0001 0.037 0.959 0.037 0.959 xxxx

Table 4.3: The uncertainty and location of features in fig .4.7. Li refers to line feature. C i refers to circle feature. Empty place refers to no reading available for this measure because its not available for that feature.

Figure 4.8: The result of building local map from circles and lines.

The line fitting algorithm also follows the method in section 3.2.3. How-ever, in implementation and because there is an error in estimation the




value of α, we have chosen an interval to apply equation 3.30. That is if −5o ≥ α ≤ 5o or 175o ≥ α ≤ 185o apply the equation 3.30 and apply equa-tion 3.29 elsewhere. This process gave us a flexibility to estimate accuratestart and end point of a line segment and by it we have escaped from theproblem of the constant X on the vertical lines.

f Uncertainty Parameters Error# UCx UCy UR Cx Cy R ex ey eRC1 0.0085 0.0011 0.0073 36.072 38.009 0 .816 0.072 0.009 0.066C2 3.532

*10−51.284*10−5

4.105*10−5

24.001 37.999 0.751 0.001 0.001 0.001

C3 0.0104 0.0266 0.0260 31.013 50.811 0 .720 0.013 0.039 0.030C4 0.0081 0.0054 0.0101 27.744 50.682 0 .555 0.136 0.168 0.195

C5 0.0005 0.0026 0.0020 26.292 50.778 0 .714 0.038 0.072 0.046C6 0.0006 0.0058 0.0044 24.014 46.085 0 .804 0.014 0.085 0.054C7 0.0554 0.1069 0.1413 19.481 49.611 0 .852 0.699 1.239 0.102C8 0.0022 0.0049 0.0031 12.157 53.757 0 .620 0.157 0.243 0.130C9 0.0021 0.0014 0.0019 12.025 45.958 0 .728 0.025 0.042 0.022L10 3.9193 0.0003 3.067 3.830 0.0746 3.830

Table 4.4: The uncertainty and location of features in fig.4.8

Figure 4.9: Extracting features and building local map. The best estimation for close paper reels.

Figures 3.6 shows the results from fitting line using equation 3.29 insteadof equation 3.30. Its clear the error in estimating the start and end point of the line segment. Figure 3.5 shows the result of fitting a circle to its scanpoints. In this figure its clear that there a segment where the algorithm of extracting features reject its interpretation because of its high uncertainty inboth cases of line and circle fitting (The segment in the bottom).




f Uncertainty Parameters Error# UCx UCy UR Cx Cy R ex ey eRC1 0.0011 0.0002 0.0008 36.017 45.991 0.753 0.017 0.009 0.003C2 0.0024 0.0050 0.0047 37.115 50.585 0.523 0.065 0.265 0.227C3 0.0201 0.0697 0.0724 35.609 50.903 0.792 0.029 0.053 0.042C4 0.0029 0.0087 0.0078 34.085 50.749 0.673 0.005 0.101 0.077C5 0.0013 0.0051 0.0046 32.527 50.851 0.739 0.103 0.001 0.015C6 0.0009 0.0036 0.0031 30.974 50.877 0.775 0.006 0.027 0.025C7 0.0003 0.0013 0.0011 29.425 50.833 0.739 0.005 0.017 0.011C8 0.0996

*10−30.6666*10−3

0.4763*10−3

27.874 50.827 0.736 0.006 0.023 0.014

C9 0.0795*10−3

0.5340*10−3

0.3700*10−3

26.334 50.825 0.731 0.004 0.025 0.019

C10 0.0105 0.0085 0.0167 24.753 50.834 0.757 0.027 0.016 0.007C11 0.0212 0.0256 0.0426 23.236 50.839 0.724 0.006 0.011 0.026C12 0.2934

*10−30.1069*10−3

0.3173*10−3

24.014 45.990 0.733 0.014 0.01 0.017

Table 4.5: The uncertainty and location of features in fig.4.9

Figures 4.6 to 4.9 show the results from extracting one local map from oneLRF device by fitting the line and circles. Table 4.2 is the correspondencetable to figure 4.6. Its contain the value of the uncertainty for each featureand its parameter. The first columns from the table is a classifier columnwhere C and L refers to circle and line feature respectively. In case of line xbecomes α and y becomes r.

In Figure 4.6 one can note that there is 7 features are fitted correctly,one features is ignored (24, 22), this happen because the high uncertaintyappears during fitting this feature to a circle and because the radius of theresult circle from the fitting procedure is less than the validation interval.Where the validation interval is an interval of radius’s where if the estimatedcircle radius lay inside this interval the circle is acceptable other wise itsrejected. In this case the fitting was a little bit less than 0.5 meter so thecircle is rejected. The line L8 in table 4.2 which is the linear feature in thetop of figure 4.6 is totally wrong. Its obvious from first column in the tablewhich shows the uncertainty of the angle direction how much is high. Other

features estimation is acceptable except C 7 which shows high error in centerposition estimation up to (10, 16)cm (columns 8, and 9) respectively. Thereason of this high error because its far from the range finder where the errorbecomes higher.

Figure 4.7 and it correspondence table 4.3 shows the result of extractinganother map. The resulted features are acceptable again except the line L7

which show high uncertainty for estimating the angel of the norm α whichlead to bad estimation of the norm length almost 1 meter . Circle C 2 whichis the farthest column of the scanner, (LRF location is at (15,17)), also showhigh error (14cm) in diameter estimation and (20, 7)cm in center estimation.




Figure 4.10: The sum-mation of two maps intobigger one. The biggest circles represent the ex-traction of paper reelsin first map.The smallest ones represent the extrac-tion of them in the sec-

ond map. The middleone show the summation of both by using Kalman

filter.

The situation in Figure 4.8 and its corresponding table 4.4 is difference.Here we chose the noise reduction constant to be = 1.5σ2. Although there isone paper reel that is estimated totally wrong due to the segmentation whichconsidered scan points from x = 18 to x = 20 as one segment, the result fromextracting other features are acceptable. As same as the situation in figures

4.6 and 4.7 the farthest features C 4,C 8 andL10 are bad estimated due to itslocation from the scanner. The scanner here was in (18,38). C 4 which is aspecial case the bad estimation for it because the few number of points thatused to fit it (5 points).

Figure 4.9 is another example of extraction map. Here its clear thatalmost paper reels are estimated very well. Expect circle C 2 in table 4.5.This circle which estimated small and uncertain in position is estimated fromfitting 5 points too and these points are the farthest from the scanner. Whatto say here is that the best estimation for paper reels is for reels which areclose to the scanner.

4.3 Sum Maps into one

As mentioned in methods chapter summing map into one done by usingKalman Filter. Figures 4.10 until 4.18 show the effect of combining mapstogether using Kalman filter. Figures 4.10 just an example of the result forestimation of pillars location. In the figure its obvious the enhancement thathappens in the new map (black circle) which give us a hand over uncertainestimation from any single map.




Figure 4.11: The result of summation two line segments together.The algorithm sums the first two left segments but it does not add to them the third line.

f Uncertainty Parameters Points# Uα Ur α r x1 y1 x2 y2

L1M 1 0.0025 0.0000 4.7349 0.3961 17.7556 0.0037 18.7644 0.0265L1M 2 0.6526

*10−50.2631*10−5

4.7138 0.2650 8.2751 -0.0149 11.9015 -0.0098

L2M 2 0.1447*10−3

0.0299*10−3

4.7138 0.6381 17.5606 -0.0312 19.3631 0.0311

L1result 0.6491*10−5

0.2484*10−5

4.7066 0.0472 8.2751 -0.0951 18.7644 -0.1558

L2result 0.1447*10−3

0.0299*10−3

4.7138 0.6381 17.5606 -0.0312 19.3631 0.0311

Table 4.6: Summation of two lines into global map,fig.4.11 a case spot light

Table 4.6 gives a numerical results for the case in figure 4.11. In this casethe algorithm applies Kalman Filter very well. However, only two of threesegments line are summed together to the new map while the other one istransferred as it. Such cases happen because in the algorithm we supposed

that there is only one suitable feature object for each object from first mapin the second map or there is nothing. So whenever two or more objectsappear from second map are corresponding to one object from the first one,the first object is summed and the rest are transferred and the same thinghas been done if two objects from first map are corresponding to one objectin the second map.

Table 4.7 and figure 4.12 are giving another example of another case thathappens when adding vertical lines together to a global map. The resultsometimes becomes unexpected as in this case here. The result out of bothtwo inputs. Our explination to such situation is, if we look to the uncertainty




Figure 4.12: The adding of two segments when they are verticals. Note that the result not as expected,where one can expect theresult should lay some-where between the twosmallest lines but what happen, that is the new longer line segment lay out of both original segments.

f Uncertainty Parameters Points# Uα Ur α r x1 y1 x2 y2

LM 1 0.003 0.0000 0.0075 0.128 0.285 20.969 0.256 17.109LM 2 0.246

*10−50.163*10−5

0.0008 0.011 0.028 22.013 0.017 7.787

Lresult 0.206*10−5

0.122*10−5

6.2532 0.041 0.701 22.013 0.275 7.787

Table 4.7: Summation of two Vertical lines. Objects in figure 4.12 . A case spot light

of the length of the norm estimation (LM 1,U r) in the table 4.7 we can notethat the uncertainty is zero which lead to singular matrix or matrix badestimation one when we use it in Kalman filter. The same thing happens infigure 4.13 and table 4.8.

Combining circles in figure 4.13 and table 4.8 is worse than both initialobjects. However, comparing with exact location of this pillar (24, 6) andR = 0.75 we can note from table the result tries to minimise the error inY-direction, look to the table 4.8.

Figure 4.14 shows one of the best results of summing two maps together

Figure 4.13:Other example of summation of twocircle columns

or paper reels

from two different map using Kalman

filter. The black one is the result of the summation.




f Uncertainty Parameters# UCx UCy UR Cx Cy R

CM 1 0.1196 0.0057 0.1097 24.2112 6.2693 0.8928CM 2 0.0766 0.0069 0.0653 24.0375 6.1140 0.7986

Cresult 0.0404 0.0029 0.0359 24.2729 6.1916 0.9767

Table 4.8: result of Summation two pillars as in fig.4.13

Figure 4.14:Summation of two local maps intoone again. Thebottom line seg-ments are summed correctly together

while one segment appeared in the left.This segment simply transferred to thenew map without change.




Figure 4.15: In this figure two local mapare summed together the map from Fig-ure 4.16 and the map from Figure 4.17 .Note the difference between them.

where we do not have a lot of special cases as figures 4.13 and 4.11 above.

Figure 4.15 gives another example of summing tow local map from twodifferent LRF. These two maps are represented in Figures 4.16 and 4.17. Notethat the number of extracted features in second map 4.17 is more than thenumber of them in the first one 4.16. This is because second LRF is simulatedto has a wider range comparing with the first one (270 angles range, while thefirst one only 180 angles range). So that the extra objects are transferred tothe global map(Figure 4.15) as they while other objects (paper reels here) aresummed using kalman filter. Note also the bad estimation of line. This bad

estimation happens because of error in segmentation algorithm. Such errorscan be avoided or minimized by decreasing the value of the noise reductionconstant C 0 in segmentation algorithm. Table 4.9 shows a numerical resultsof summation the two maps in Figures 4.16 and 4.17. The last part of thetable (sum) shows the error of paper reels position estimation comparing withits true location. Its obvious how the position estimation in the final resultenhanced. Figure 4.18 shows the result of building global map with noprior knowledge about the environment. The map in this figure are built from112 scan image, 56 scan image from each laser range finder while the robotwas moving from location (10, 10) to (30, 50) in almost straight line. Some




Parameters Uncertainty ErrorCx Cy R UCx UCy UR ex ey eR

Map1

36.0092 46.0093 0.771 0.0038 0.0005 0.0028 0.0092 0.0093 0.02135.6629 50.7907 0.7177 0.0079 0.0167 0.0202 0.0329 0.0593 0.032332.5118 50.8821 0.7735 0.0014 0.0056 0.0053 0.0182 0.0321 0.023523.265 50.8134 0.6972 0.0118 0.0291 0.0359 0.035 0.0366 0.0528

Map2

35.9972 45.9978 0.7501 0.0013 0.0002 0.0009 0.0028 0.0022 0.000135.6202 50.9086 0.791 0.0057 0.0225 0.0232 0.0098 0.0586 0.04132.5657 50.8704 0.7849 0.0017 0.0045 0.0049 0.0357 0.0204 0.034923.2416 50.787 0.6869 0.0049 0.0125 0.0147 0.0116 0.063 0.0631

Sum

35.997 46.0009 0.754 0.00094 0.00016 0.00066 0.003 0.0009 0.00435.6164 50.7885 0.6908 0.0032 0.0086 0.0096 0.0136 0.0615 0.059232.5319 50.8566 0.7593 0.0008 0.0024 0.0024 0.0019 0.0066 0.009323.2476 50.7942 0.6898 0.0034 0.0085 0.0102 0.0176 0.0558 0.0602

Table 4.9: The result of fuse two local map together using Kalman filter.M ap1

represents some features from Figure 4.16.M ap2 represents the same of them from Figure 4.17 . The last part of the table show the result of summing them together as appears in Figure 4.15 .

Table 4.10: The position, uncertainty and error of the same paper reels and column from table 4.9 after accumulating several local map. These feature are represented in a right place as appears in Figure 4.18.

Parameters Uncertainty ErrorCx Cy R UCx UCy UR ex ey eR

35.9925 46.0008 0.7446 10−04×0.15

10−04×0.039

10−04×0.11

0.0075 0.0008 0.0054

35.5979 50.8141 0.7058 10−03×0.14

10−03×0.48

10−03×0.5

0.0321 0.0359 0.0442

32.5265s 50.8454 0.7443 10−04×0.0509

10−04×0.0915

10−04×0.1168

0.0035 0.0046 0.0057

23.2171 50.7173 0.6564 10−03×0.077

10−03×0.4467

10−03×0.2763

0.0129 0.1327 0.0936

features in the map are estimated in a acceptable way and detected. However,some features like the concrete pillars in (24, 36) has not been detected. Thishappens some times because some other features can take higher priorityin matching algorithm or because the feature uncertainty is bigger than the

threshold. The other type of bad estimation is lines that are appeared in themiddle of the map and in the top and the circle in point (29, 49.5). Such badestimation happens because of segmentation algorithm error. This kind of error is avoidable by decreasing the noise reduction constant in segmentationalgorithm C 0.

The lines in the bottom of the map and those are in the left are the linesrepresent two of the walls in the environment. Its obvious that some of themare summed together and other of them are transferred as they are. Thishappen because of the threshold that used in matching algorithm. Wherethese threshold calculate the euclidean distance between two matching lines.




Figure 4.16: The local map that is extracted from first LRF at scan image number 71.

If the line norm angles of two line segments are located in the oppositedirection like 5o and 175o the result becomes as in the figure above. To solvethis problem its important to calculate the threshold of each norm lengthand norm direction alone instead of calculating them together.

The bad line estimation in the top of the map is happen when the scanpoints’ segment contain only 5 points; 2 from the end of one column and3 from the beginning of the next columns scan or line scan. In such casethe estimation of the line will return a very low uncertainty. This causeremoving of some features from the map because the algorithm takes thesum of diagonal of the uncertainty matrix to decide which feature have toconsider and which one have to remove where ever there is an intersection

between two features has appeared. One suggestion is to use prior knowledgeof the environment to tell the robot that this feature is high priority thanthe other one (matching algorithm) before using the uncertainty. However,such suggestion had been not tested.

Table 4.10 just gives a numerical results of paper reel estimation in fig-ure 4.18 and there uncertainty and the error of estimation in meter accordingto the real position in the environment. These paper reels are the same paperreels appear in table 4.9. Comparing the results in the two table togethershows better uncertainty but also difference in position error.




Figure 4.17: The local map that is ex-tracted from second LRF at scan im-age number 71.

Figure 4.18: The result of building the global map of combining 112 local map,56 local map from each LRF are built from 56 scan image over 56 position of thetruck.




4.4 Map analysisThe predefined map is processed to fined the most suitable arrangement forpicking and the best direction of the picking. The main procedure followedthe procedure that mentioned in section 3.4. Then according to the locationof the robot the predefined map is processed. The first paper reel is thenearest available one to the robot.

Figure 4.19: The simulation environment and the initial position of the truck.Light circles are concrete pillars. Dark circles are paper reels. Lines are walls and container.

The simulation environment is same as the environment above. The onlydifferent is the assumption of the initial location of the robot. Figure 4.19shows a map of the environment and the initial position of the truck. Theassumption of the truck location is built upon the believe that the truckshould set somewhere near the container location (door) because its moreconvenient and more money consuming. The paper reels arrangement isgiven as in figure 4.20.

Calculating the availability of any paper reel has been done as in sameas section 3.4 above. First, consider any object that locates on a distanceless than 1.5 times the total length of the truck from the target paper reel




Figure 4.20: The paper

reels arrangements.

Figure 4.21: All papers are pro-cessed then theaccess directions

for each paper reel

are determined (the magenta points).

as obstacle. Then apply the equations 3.42 and 3.43 for circular obstacleand equations 3.40 and 3.41 for linear obstacles. The paper reel that has theless number of inaccessible directions is the most available paper reels. Forcalculating the nearest paper reel we considered the straight distance fromforklift initial position to the paper reel. We assumed that the container isthe destination that paper reels have to be moved to. Its more convenient

to call it the loading area. The new distance is the straight distance fromthe paper reel to the center of the loading area. Practically we think thatthis assumption is acceptable because the mission of the truck will be totransfer this stack of paper reels from its current location (unloading are)to the new location (loading area). If the loading area is empty (as itsexpectable) then the center point of the loading area will be the mean of thetotal travelling distance for truck mission inside the loading area (container).The nearest reel then is the reel with the minimum summation of distances(Truck-reel/reel-loading area). Figures 4.21 to 4.26 show the procedure howis done.




Figure 4.22: Thepaper reel #13 is re-moved first then therest paper reel areprocessed again.

Figure 4.23: Paper reel number 12 isremoved this time,then the map pro-cessed again.

First, in figure 4.21 the availability (accessibility directions) are calcu-

lated. The magenta dots on each paper reels show the accessibility directionfor each one. Then the nearest one extracted to external matrix taking thehighest priority to be moved. Then its place becomes empty which give ac-cessibility toward the next paper reel ”figure 4.22. Again the accessibilityare calculated then the nearest reel is chosen then moved and so on until thewhole stake is processed. However, when we use the word moved, or removedin this level we do not mean moving the reel physically. Its only extractingthe reel from its original matrix to another one, where papers here arrangedaccording to there priority. This matrix is the result of this algorithm whereits return the priority, paper reel number from figure 4.20, the position of

the paper reel center, the paper reel diameter and all accessibility directionsfor the reel2.Note in figures 4.21 to 4.26 the procedure goes well. In figure 4.21 the

reels that have the highest number of accessibility directions is the reels inthe corners. See the reel number 26 in Figure (4.21) has a few number of accessibility directions. While after removing paper reel number 13 (Figure4.22) the number of accessibility directions increase by a remarkable way.From another hand the paper reels in the lowest row (except the corners

2We believe now that extracting this matrix in this level is not necessary, where thenext step can be done directly after each paper reel then the reel could be moved physically.




Figure 4.24: Now paper reel number 26 is removed. Note twodifferent and separate intervalsof accessibility direction on paper

reel # 11 are appeared

Figure 4.25: The same for pa-

per reel number 11.

Figure 4.26: And so on for all other reels until all papersare processed.




Figure 4.27: Thepicture shows picking intervals and accessdirection.

Figure 4.28: The path isgenerated by using b-spline

function and by giving 10 different points to the function as input. The path calcu-lated to each paper real from each possible picking direc-

tion then the shortest one ischosen.

and reel number 12) have only one accessibility direction because they allare surrounded by each other. Paper reel number 12 which is surrounded byother reels from 3 directions and a column from south direction -figure 4.19(Column number 20 locates in a distance less than 1.5 times the robot length)has no accessibility direction at first (figure 4.21). Then after removing paperreel number 13, the reel number 12 got more a suitable number of accessdirections (figure 4.22) which gives it the priority to be removed (figure 4.23).

The access directions of paper reel number 12 are not in conflict with thecolumn location or other paper reels locations. Other paper reels are treatsby the same way. The rest figures until 4.26 show the first 10th paper reelsremoving priority. Table 4.11 shows the result of this procedure.

After finding the best arrangement for moving paper reels (Column 2in table 4.11), the step was to find the most suitable direction among theavailable to pick up the reel, i.e to find the best interval among intervals inthe fifth column of table 4.11. The best interval is shown in the sixth columnof the same table.

This step had been done by choosing the direction which insure the short-




Figure 4.29: All possible

paths to the paper reel.

Figure 4.30: The chosen path is theshortest one and the easiest from pathsin figure 4.29 .

Figure 4.31: Selected paths and pick-

ing directions for each real.




Prio-rity P.No. Center(x,y) Radius Access directions Bestaccess1 13 (37.2,50.9) 0.75 {95o:5o:180o} 100o

2 12 (35.6,50.9) 0.75 {115:5:175} 1203 26 (37.2,52.4) 0.75 {90:5:235} 904 11 (34.1,50.9) 0.75 90 & {130:5:175} 905 25 (35.6,52,4) 0.75 {110:5:175} 1106 10 (32.5,50.9) 0.75 {90:5:105} & {145:5:175} 907 24 (34.1,52.4) 0.75 {90:5:180} 908 9 (31.0,50.9) 0.75 {90:5:175} 909 23 (32.5,52.4) 0.75 {90:5:160} 9010 39 (37.2,55.3) 0.75 {115:5:220} 12011 8 (29.4,50.9) 0.75 {90:5:175} 9012 22 (31.0,52.4) 0.75 {90:5:160} 9013 48 (33.3,53.9) 0.75 {90:5:145} 90

14 38 (35.6,55.6) 0.75 {195:5:220} 20015 7 (27.9,50.9) 0.75 {90:5:175} 10016 47 (31.8,53.9) 0.75 {90:5:155} 9017 21 (29.4,52.4) 0.75 {90:5:160} 9018 37 (34.1,55.3) 0.75 {90:5:100} & {190:5:220} 9019 6 (26.3,50.9) 0.75 {90:5:175} 10020 46 (30.2,53.9) 0.75 {90:5:160} 9021 20 (27.9,52.4) 0.75 {90:5:160} 10022 36 (32.5,55.3) 0.75 {90:5:130} & {190:5:220} 9023 5 (24.8,50.9) 0.75 {100:5:175} 10024 45 (28.7,53.9) 0.75 {90:5:165} 9025 35 (31.0,55.3) 0.75 {90:5:145} & {185:5:220} 9026 19 (26.3,52.4) 0.75 {120:5:160} 10027 4 (23.2,50.9) 0.75 {120:5:175} 12028 34 (29.4,55.3) 0.75 {90:5:220} 90

29 44 (27.1,53.9) 0.75 {90:5:165} 100

Table 4.11: The shape of the table that generated from the algorithms described in this research from analysing a given map for paper locations. This is only a sample from the simulation environment above.

est distance from the robot location to the paper reel coming from that di-rection. Figure 4.28 shows an example of the implementation.

The main problem that we can address here that we could not updatethe path on-line, i.e. the path should generated before any movement that

could be done by the truck. Figure 4.29 shows all possible paths that reachthe paper reels from all access directions. Its obvious that the lowest path isthe shortest one. Its the path with heading direction (100o). So this is theselected direction as in figure 4.30. After determining the picking directionall other picking directions are removed and the chosen one is extracted toordering table as in table 4.11. Then the procedure continue for the rest paperreels in the stack. Figure 4.31 shows the result of applying this algorithm forall paper reel and according to the path that chosen in figure 4.28.

Figure 4.29 shows the generated path to each picking available pickingdirection. This figure is a zoom to Figure 4.28. It is obvious that each path




Figure 4.32: The chosen picking direc-tion provide safe moving for truck to not be in conflict with any other obstacle

Figure 4.33: Also the selected path pro-vide safe reaching to the paper reel.

has its own length. The shortest path of them is chosen to be a guide to thesuitable piking direction as in Figure 4.30. From Figure 4.30 we can note howthe picking direction and the path to that direction are chosen to insure asafe manoeuvring for the truck (Figure 4.32). Figure 4.31 shows all suitable

Figure 4.34: However, we could not simulate the return back movement where therotation of truck direction happen instantaneously not exactly as in real life.

picking direction for all paper reels in stack. One should note that the pikingdirection and the path are done according to the sequential transferring forpaper reels. This means if there is any intersection between the path and anyother paper reel, which happen a lot in the figure, it can not be considered asproblem because the intersected paper reel will be removed in the time of thetime of path use. Figure 4.33 shows how the robot handling the paper reels.From the picture note how the robot can reach its target paper reel from




the best picking direction by a safe way. Figure 4.34 shows a small problemappears due to the lake of simulation controller behaviour in the robot. Inthis picture and because the heading of the robot should changed by 180 o

the robot rotates locally. We did not solve such problem but we believe if the controller has been simulated we can avoid such problems.



Chapter 5

Discussion

In the result chapter we have described our results and we have addressedthe main problems that we have faced. Our work focus on two main prob-lems, building a map from a laser range finder scan image and analysingthat map or any provided map with any arrangement of paper reels to findthe suitable arrangement for picking and the best direction to pick up eachpaper reel. Although, we have tested our algorithms and evaluate it withonly one arrangement of paper reels we believe its able to work on severalarrangements.

The building map algorithm is an algorithm of order O(N 2) where N is the number of features in the map. We are lucky because the number of features are small in our environment. However, this is not the case in reality.The algorithm itself is accumulative algorithm, which means the number of features in the map increases with every new scan. So later on after severalscans the order of algorithms becomes O(N.M) where N is the number of the features in the map which becomes in the knowledge base and M is thenumber of the features from the new scan, compare figures 4.18 with 4.14.

While we believe that our algorithm can give an accurate estimation forlocations of paper reels and other feature due to its accumulative behaviour

we find it also add a lot of unwanted features to the map figure 4.18 for thesame reason. During feature extraction some features are extracted totallymistaken (which mean they do not exist).

This happens once each extraction process or once each two extractionprocess. However, they remain in the final map see figure 4.7. They stayin the final map because in the summation algorithm we add each nearestfeatures together. We believed that the rest will be only the paper reels sothe algorithm transferred them as they are.

One solution could be that at each update step the matcher should beable to save three previous sequential maps then by comparing current map

63



64 Chapter 5. Discussion

with its three previous maps, knowledge base map and the predefined ware-house map that is provided to the robot. If the feature is not one of thewarehouse structures and its not appear in three sequential scan then thematcher algorithm should remove it.

In figure 4.18, which is an example of the built map, almost of warehousewalls are ignored and replaced with circles instead. The circle in the placeof walls appear once in ten extracting step as an bad fitting for some scanpoints, where in some cases the segment length is not long enough to reflectthe real normal distribution of the error. For example if the length of thesegment is only five or seven points and these points could shape a circle

then the circle appears while the reality is that points are a part of a line.In the first we have believed that such problem will not appear. So wemade the algorithm to omit the line wherever a line and a circle intersects.However, after that we tried to over come this problem by comparing thesome of the diagonal of the uncertainty matrix. The surprising part waseven in such solution the diagonal of circles’ uncertainty was less than thediagonal of lines’ uncertainty. The solution of this problem in our believenow is to fix the lines that fit walls in matching step in the knowledge baseand when ever a new bad estimation happen and a circle appear the circleshould be removed.

The algorithm of analysing the map to find the best arrangement of thepaper reel picking is from the order of O(N 2.M ) where is N is the numberof paper reel in the map and M is the number of the features in the map.Again in computer science such algorithm could be not preferred. However,we are dealing all time with limited number of paper reels and not a veryhuge number of features. So its speed is acceptable.

We assumed that the truck able to build the map or the map that isthe input of this algorithm is giving a full view of the paper reels situations.Which means the map is reflect the surrounding environment of the paperreel. To built such map the truck should rotate around whale the stack andbuild it. This is not the required case in reality where only the truck able to

see the reel from one direction. We suggest to use only 180o from availabilityinstead of the whale 360o that we have used here. This is the face of thepaper reel which is faced the truck during its motion.

Finally, we can say, there is different step able to done to update thisresearch. However, according to our assumptions the algorithm of evaluatingpaper reels to find the best arrangement is robust.



Chapter 6

Conclusions

In this research we made a simulation environment to test several methodsand evaluate them on arrangements of paper reels. We used two differentlaser scanner to generate a local and then global map. A method of com-bining several local maps in one big global map is proposed and have beentested. Another method for finding the best paper reel to pick is proposedand a method to select the picking direction too. The result was sufficientin this aspect. However, the algorithm had been not tested in a real-timeenvironment. We hope to test it in future.

We aimed in this research to discriminate between reels and concretecolumns if they have the same shape and radius and the concrete columnwas inside the paper reels stack. We had not get good results yet, but wehope we will do that soon. We did that by combining two maps, the map thatbuilt by our algorithm and our a predefined one. Our algorithm of generatingglobal map from accumulating several local map has a lot of errors and badestimations. However, the columns that are detected and built are accuratein a sufficient way.

We generate paths depend on b-spline function. At some levels we wantto update the path to follow new updates to the map. We are doing that

off-line not on line, where the robot have to stop then generate a new pathto continue.

We came up with algorithm to generate a table of predefined map con-taining the order of reels, the direction of picking each reel, where the mapof the paper reels are built by our algorithm.

We test several segmentation algorithms. Most of segmentation algo-rithms tries to solve the problem of detecting linear segments. This causedifferent drawbacks when using them in an environment containing severalkind of feature shapes.

Finally the Matlab script that we are built is easy to use. All constants

65



66 Chapter 6. Conclusions

and variables are arranged by a way to be easy to reach and modify. The mainthing in the script is providing it with a text file describe the environmentand the arrangements that needed.



Bibliography

[1] Bouguerra Abdelbaki, Andreasson Henrik, Lilienthal Achim J., Astrand

Bjorn, and Rognvaldsson Thorsteinn. Malta : a system of multipleautonomous trucks for load transportation. In Proceedings of the 4th European conference on mobile robots (ECMR).

[2] A. Bouguerra, H. Andreasson, A.J. Lilienthal, B. Astrand, and T. Rogn-valdsson. An autonomous robotic system for load transportation. InEmerging Technologies Factory Automation, 2009. ETFA 2009. IEEE Conference on , pages 1 –4, sept. 2009.

[3] N. Chernov and C. Lesort. Least squares fitting of circles. Journal of Mathematical Imaging and Vision , 23:239–252, 2005. 10.1007/s10851-

005-0482-8.

[4] N. Chernov and C. Lesort. Fitting ellipses, circles, and lines by leastsquares. www.math.uab.edu/ chernov/cl/MATLABcircle.html, 2008.Department of Mathematics University of Alabama at BirminghamBirmingham AL 35294 USA.

[5] G. Garibott, S. Masciangelo, M. Ilic, and P. Bassino. Robolift: a visionguided autonomous fork-lift for pallet handling. In Intelligent Robots and Systems ’96, IROS 96, Proceedings of the 1996 IEEE/RSJ International

Conference on , volume 2, pages 656 –663 vol.2, nov 1996.[6] D. Lecking, O. Wulf, and B. Wagner. Variable pallet pick-up for au-

tomatic guided vehicles in industrial environments. In Emerging Tech-nologies and Factory Automation, 2006. ETFA ’06. IEEE Conferenceon , pages 1169 –1174, sept. 2006.

[7] J.P. Liu. Case study of picking method selection for cosmetic broken-case picking operation. In Industrial Engineering and Engineering Man-agement, 2007 IEEE International Conference on , pages 357 –361, dec.2007.

67



68 Bibliography

[8] B-spline. http://en.wikipedia.org/wiki/B-spline, 2011.

[9] Belt problem. http://en.wikipedia.org/wiki/Belt problem, 2011.

[10] P. Nuez, R. Vazquez-Martin, A. Bandera, and F. Sandoval. An algorithmfor fitting 2-d data on the circle: Applications to mobile robotics. Signal Processing Letters, IEEE , 15:127 –130, 2008.

[11] Cristiano Premebida and Urbano Nunes. Segmentation and geometricprimitives extraction from 2d laser range data for mobile robot applica-tions. Science And Technology , pages 17–25, 2005.

[12] J. Ryde and Huosheng Hu. Laser based simultaneous mutual localisationfor multiple mobile robots. In Mechatronics and Automation, 2005 IEEE International Conference, volume 1, pages 404 – 409 Vol. 1, july-1 aug.2005.

[13] Roland Siegwart and Illah R. Nourbakhsh. Introduction to AutonomousMobile Robots. A Bradford Book, 1 edition, 2004.

[14] Luka Tesli, Gregor Klanar, and Igor krjanc. Simulation of a mobilerobot with an lrf in a 2d environment and map building. In Krzysztof Kozlowski, editor, Robot Motion and Control 2007 , volume 360 of Lec-ture Notes in Control and Information Sciences, pages 239–246. SpringerBerlin / Heidelberg, 2007. 10.1007/978-1-84628-974-3 21.

[15] J. Xavier, M. Pacheco, D. Castro, A. Ruano, and U. Nunes. Fast line,arc/circle and leg detection from laser scan data in a player driver. InRobotics and Automation, 2005. ICRA 2005. Proceedings of the 2005 IEEE International Conference on , pages 3930 – 3935, april 2005.

[16] Xu Zezhong, Liu Jilin, and Xiang Zhiyu. Map building and localizationusing 2d range scanner. In Computational Intelligence in Robotics and

Automation, 2003. Proceedings. 2003 IEEE International Symposium on , volume 2, pages 848 – 853 vol.2, july 2003.



Appendix

Kinematic example

Figure 6.1: Ackerman Steering Principle.

The bicycle kinematic model that appear in Figure 3.1 is a simplicity of Figure 6.1. with assumption that there are no slip in wheels motion.

Let α be the steering angle of the right rear wheel and β is the steeringangle of the left rear wheel. The simplification model of these two wheelsas in Figure 3.1 is the wheel in the center point of the line between the twowheels. The steering angle of this wheel is φ. For simplicity we assumed thatthe new steering angle is the mean of the two Oregonian angles.

φ ≈(α + β)

2(6.1)

Let X and Y be the x and y position of the robot in the global coordinate andlet θ be the heading of the robot in the same coordinate. Then the position

69



70 Bibliography

of the global coordinate is

P =

X , Y , θ

(6.2)

When the robot rotate around its center of rotation ICR by a constant speedthe change in location δP will be

δP = Pcosδθ (6.3)

δP = v ∗ dT ∗ cosδθ (6.4)

For the rear wheel the moved distance in the the same time dT will be

δS = vt ∗ dT (6.5)

where vt is the tangential velocity at the center of the rear wheel as in theleft top corner in Figure 3.1. Also we know that

δθ = ϕ ∗ dT (6.6)

where ϕ is the angular velocity of the robot around its inertial center of rotation. From the relation between the angular velocity and the linear

velocity we can say:ϕ =

vt

d(6.7)

where d is the radius of the circular motion of the rear wheel to the ICR.From Figure 3.1we can say that

d =L

sinφ(6.8)

where L is the distance between the rear and front wheels. Then by substitute6.8 and 6.7 in equation 6.6 we will get the following equation

δθ = vt ∗sinφ

L∗ dT (6.9)

From our knowledge we know that the tangential velocity vt is

vt = v ∗ cosφ (6.10)

then the result will be the change in the heading θ as follows

δθ =v

L∗ cosφ ∗ sinφ ∗ dT =

v

2L∗ dT ∗ sin(2φ) (6.11)




From equation 6.4we can derive the change in robot position as follows

δX = v ∗ cosδθ ∗ dT ∗ cos(θ + δθ)

δY = v ∗ cosδθ ∗ dT ∗ sin(θ + δθ) (6.12)

If the sampling period dT is small enough -say 0.1 sec. for example- the termcos(δθ) ≈ 1, which means simply, we can neglect it from our calculations.Then the simple form from the equation 6.12could be in the form

δX = v ∗ dT ∗ cos(θ + δθ)

δY = v ∗ dT ∗ sin(θ + δθ) (6.13)

δX = v ∗ dT ∗ cos(θ +v

2L∗ dT ∗ sin(2φ))

δY = v ∗ dT ∗ sin(θ +v

2L∗ dT ∗ sin(2φ)) (6.14)

However, neglecting the cosine term from calculations would produce sys-tematic error so its not recommended to neglect it. The simple form fromequation 6.12which could be easier to use in calculations is as in equations3.2.

Availability calculation example

Figure 6.2: The paper reel in the middle is located between a wall a column.



72 Bibliography

This example is to understand the method used in calculating the avail-ability angles. Let the paper reel in Figure 6.2 locates in a distance of p w d = 208cm of a wall and on a distance of p c d = 348cm from an-other paper reel or concrete pillar. Let the truck length is 370cm and thegrabber width is 5cm. Let the angle of the center line between the paperreel and the circle obstacle is φ = 210o and the angle of the wall norm isλ = −90o. Let the radius of the paper reel is r1 = 90cm and the radius of the column is r2 = 95cm. Finally let the picking interval is a = 5o

Its obvious that the both of the wall and the column is considered asobstacle because the distance from any of them to the paper reel is less

than one and half time the truck total length. So its necessary to calculatethe availability from them. Start by calculating picking direction from eachobstacle alone. By using equations 3.40 and 3.41 we calculate the availabilityfrom wall. So from equation 3.40 the minimum picking angle is

theat1 min = floor(mod(270 − 90 + sin−1( 208−90−10

370+10), 360)

5) ∗ 5 = 195o

(6.15)and the maximum piking direction from equation 3.41 is

theat1 max = ceil(mod(270 + 90 − sin−1( 208−90−10

370+10

), 360)

5 ) ∗ 5 = 345o (6.16)

So availability are all directions

{195 : −5 : 345}

which means{0 : 5 : 195} and {345 : 5 : 360} (6.17)

No calculate availability from the column by using equations 3.42, 3.43and 3.44. The angle is calculated as in equation 3.44.

= sin−1( 95 + 90 + 10348

) = 34o (6.18)

The minimum picking direction is as equation 3.42

theat2 min = floor(mod(180 + φ − , 360)

5) ∗ 5 = 355o (6.19)

and the maximum picking angle from the column is calculated by equation3.43 as follows:

theat2 max = ceil(mod(180 + 210 + 34, 360)

5) ∗ 5 = 65o (6.20)




and the availability are direction from theat2 min to theat2 max clockwisewhich mean all directions

{355 : −5 : 65} (6.21)

Then after converting the two availability to linear representation thenby adding both as in figure 6.3 we will get the final true availability to be asin the third line in the figure which is

{65 : 5 : 195} and {355 : 5 : 360} (6.22)

Figure 6.3: Adding two availability together after converting the angles to linear representation to get the total available directions.

Documents

Method for Autonomous Picking of Paper Reels