Title: Camera Controlled Robot. Theme: Real-time Systems with Feedback. Project period: 5th semester 2006

Save this PDF as:
 WORD  PNG  TXT  JPG

Size: px
Start display at page:

Download "Title: Camera Controlled Robot. Theme: Real-time Systems with Feedback. Project period: 5th semester 2006"

Transcription

1

2

3 Faculty of Engineering, Science and Medicine Department of Electronic Systems Fredrik Bajers Vej 7 A Aalborg Ø Denmark Title: Camera Controlled Robot Theme: Real-time Systems with Feedback Project period: 5th semester 2006 Project group: 06gr508 Members of the project group: Henrik Wøldike Dalsager Thomas Esbensen Brian Thorarins Jensen Morten Egelund Jensen Michael Odgaard Niss Christoffer Sloth Supervisor: Henning Nielsen Number printed: 9 Number of pages: Report: 92 Total: 124 Ended: December 21th 2006 Abstract: A problem is set up that requires feedback controlled movements of a robot. The robot is constructed with LEGO bricks and is based on the LEGO Mindstorms NXT. A web camera is used to determine the position and direction of the robot. The task of the robot is to reach multiple targets within a given field. The field contains obstacles that must be avoided. The targets are plastic cups that can be stacked by the robot when it collects them. The behaviors of LEGO Mindstorms NXT motors are modeled and, by loading these with properties of the robot, the movements performed are simulated in the regulation software. For positioning of items on the field, a patch is made for the EyesWeb software, which is used to analyze the images from the web camera. The coordinates of the detected items are then converted from image to world coordinates. The control software is developed in Java and controls the robot by planning a route to the nearest target, and then steers the robot to follow the route. By using a simulator for the movements of the robot and the data from image processing, the correct location and direction of the robot is estimated. The software then decides how the robot should be regulated by driving in circular motions of different ratios. The system has proven to be able to pick up all cups and avoid all obstacles at the set-ups tested.

4

5 Det Ingeniør-, Natur- og Sundhedsvidenskabelige Fakultet Institut for Elektroniske Systemer Fredrik Bajers Vej 7 A Aalborg Ø Danmark Titel: Kamerastyret robot Tema: Tilbagekoblede realtidssystmer Projektperiode: 5. semester 2006 Projektgruppe: 06gr508 Gruppemedlemmer: Henrik Wøldike Dalsager Thomas Esbensen Brian Thorarins Jensen Morten Egelund Jensen Michael Odgaard Niss Christoffer Sloth Vejleder: Henning Nielsen Oplag: 9 Sideantal: Rapport: 92 Samlet: 124 Afsluttet: 21. december 2006 Synopsis: Der er opstillet en problemstilling, der inddrager tilbagekobling og skal være i stand til at styre en robot. Robotten er konstrueret i LEGO og er baseret på LEGO Mindstorms NXT. Et webkamera benyttes som sensor til bestemmelse af robottens position og vinkel. Robottens opgave er at køre til forskellige mål på en prædefineret bane. Banen indeholder forhindringer, der skal undgås, samt et antal plastikkopper, der skal indsamles af robotten. I forbindelse med indsamling af kopper stables disse af robotten. De anvendte LEGO Mindstorms NXT motorer er modelleret og indgår sammen med belastningen fra robotten i en model. Efter denne model simuleres robottens bevægelser i software. For at positionsbestemme emner på banen udfærdiges en patch til programmet EyesWeb, der analyserer billeder fra webkameraet. De herfra fundne billed-koordinater konverteres til koordinater på gulvet. Programmet til regulering af robotten er skrevet i Java og fungerer ved først at planlægge en rute til et mål, hvorefter robotten styres efter den planlagte rute. Ved at kombinere data fra billedbehandling og fra en simulator estimeres robottens reelle position og retning. Herudfra styres robotten mod målet ved at benytte cirkulære bevægelser med forskellige radier. Systemet har i en række test vist sig at være i stand til at indsamle alle kopper og undgå samtlige forhindringer.

6 Preface This report is created by group 508 on 5th semester 2006 under the Department of Electronic Systems at Aalborg University. The theme of the semester is Real time systems with feedback. In this report it is documented how a feedback controlled system can be used for controlling a robot autonomously. The robot is constructed with LEGO bricks, is based on the LEGO Mindstorms NXT and is monitored by a web camera. The goals of this report are to: Set up the necessary requirements to describe the task for the system to control. Provide a basic understanding of image analysis through the development of a patch for analyzing images in the EyesWeb software. Set up a model of a camera to be able to use it as a sensor device for locating elements on the floor. Describe a method to plan a path in a two dimensional system. Set up a simplified mathematical model of the actuators of the robot. Set up a kinematic model of the robot. Define a strategy to select the movements of the robot necessary to make it follow the planned path. Determine a feedback loop and describe how a regulator can be inserted to control the system. Give an insight into the problems that occur when using a delayed and distorted sensor system for controlling a system, and provide a control strategy to handle it. Evaluate the system through testing in relation to the tasks that are set up. The report is written in American English and a title page with an abstract in Danish is included. A word abbreviation is found following the preface, giving an overview of the technical terms used. It is recommended for the reader to skip the word abbreviation and use it as reference when an unfamiliar expression is encountered. Appendices are found after the conclusion, and the index is found last in the report. On page 124 a CD is found containing additional documentation. This also includes the used EyesWeb patches, the source code for the developed software, an executable version of the developed software, and the code documentation for the developed software. Page 6 of 124

7 References in the Report Source material references are identified by a name enclosed in brackets. For instance in [Gonzalez and Woods, 2002], the title refers to the bibliography, and the number refers to the year of publication. Figures and tables are referred to by the number of the object. A reference like Figure 3.1 indicates that it is figure number 3.1, where the first number refers to the chapter and the second number refers to the consecutive figure number for the chapter. References to other parts of the report are done by referring to the number of the chapter or section where the content is located. Equations are referred to much like figures and tables, however, with the number enclosed by brackets; i.e. Equation (1.1). References to the CD are made in the following way, [Test/] where the text inside the brackets is the path on the CD where the information is located. Henrik Wøldike Dalsager Thomas Esbensen Brian Thorarins Jensen Morten Egelund Jensen Michael Odgaard Niss Christoffer Sloth Page 7 of 124

8 Word Abbreviation This section should be regarded as a work of reference. API - Application Programming Interface API is a set of calling conventions defining how a service is invoked through a software package. Base The base is the area which the robot drives to when it must unload cups that have been picked up. The location of the base is specified by the user in the software. Bounding box A bounding box encloses the items of interest, i.e. it is the smallest rectangle, parallel to the coordinate system which can enclose the item. EyesWeb EyesWeb is a Windows based tool for creating interactive digital multimedia applications. It is a non-profit open software platform developed by the InfoMus Lab - Laboratorio di Informatica Musicale at the University of Genoa, Italy. EyesWeb is designed to form digital sound and images for real-time purposes. It contains a number of libraries that are organized as an index of the different blocks that the program offers [Pedersen and Andersen, 2004, p. 7]. The software functions as a drag and drop program and needed functionality can be made from a structured network of blocks connected in a graphical user interface. Field The area in the cameras range of vision makes up the field where the robot operates. Image coordinates Image coordinates defines the position of a pixel in an image. In this project the coordinate system is left-handed and has its origin in the top left corner of the image. Items Items denote all elements on the field including the robot. LEGO Mindstorms NXT The NXT is the brain of a MINDSTORMS R robot. It s an intelligent, computer-controlled LEGO R brick that lets a MINDSTORMS robot come alive and perform different operations. [LEGO, 2006b] Objects Objects denote elements on the field excluding the robot, i.e. cups and obstacles. Obstacles Obstacles are red rectangles placed on the field. The robot must avoid these. Power set point Power set point is a part of the LEGO command which is used to control the motors. Power set point defines the duty cycle of the voltage applied on a motor. Robot Control The two computers used in the system have very distinct tasks. One is dedicated to running the EyesWeb software. The other runs the last parts of Image Processing, Control Center and the Graphical User Interface combined in the Robot Control software developed by the project group in Java. Target Target denotes the current objective the robot drives towards; this can be a cup or the base. Trajectory planning Trajectory planning usually refers to the problem of taking the solution from a robot motion planning algorithm and determining how to move along the solution in a way that respects the mechanical limitations of the robot. [LaValle, 2006, p. 3] Page 8 of 124

9 Turn Ratio Turn Ratio is a part of the LEGO command which is used to control the motors. Turn Ratio defines the velocity ratio between two motors. World coordinates World coordinates are used to define the placement of items on the field. This is obtained by converting the image coordinates onto a x-y-plane. Page 9 of 124

10 Contents 1 Introduction System Concept The LEGO Robot Determination of Tolerance Requirement Specification Requirements to the Environment Image Processing Requirements Interface between Image Analysis and Robot Control Control Center Requirements Interface between Robot Control and Robot User Interface Requirements System Description System Overview Feedback in the System Image Processing Image Analysis Coordinate Conversion Positioning of Items Route Planner Representation of Items on the Field Strategic Planner Path Planner Robot Model Modeling the Motors Kinematic Model of the Robot Robot Movement Robot Location Estimator Robot Movement Decider Actuator Translator Robot Movement Simulator Robot Control Software Robot Control GUI Interface to the Robot Flow of the Control Center Software Test Determination of Frame Rate Supplemental Tests Acceptance Tests System Test Conclusion 89 Page 10 of 124

11 Contents Bibliography 91 A Positioning based on a Camera 93 A.1 Camera Calibration A.2 Coordinate Conversion B Parameters for Modeling the Robot 102 B.1 Parameters for LEGO Mindstorms NXT Motor B.2 Determination of Back Supporter Friction B.3 LEGO Mindstorms NXT Controller C Kalman Filtering 112 C.1 Normal Distribution C.2 Weighted Inputs D Network 114 D.1 OSI Reference Model D.2 Interface between Image Analysis and Robot Control D.3 Interface between Robot Control and the Robot i Direct Commands for LEGO Mindstorms NXT 120 Index 123 CD 124 Page 11 of 124

12 Introduction 1 Introduction In this chapter the concept of the system is introduced. When the concept is known, the specifications of the robot, controlled in this project, are set up. Based on the specifications of, and the job of, the robot, the precision required when detecting the robot is determined. 1.1 System Concept In this section the concept of the system is described, i.e. the overall mode of operation is introduced and the system is divided into functional parts running at two different computers. The task of the system is to let a remote-controlled robot collect plastic cups in a predefined area. This collection must happen automatically and without any user involvement required; thus it is an autonomous system. The robot must be able to avoid obstacles located on the field. Furthermore, the robot must be able to return the collected cups to an unloading base, for which the location is specified in software. The concept of the system is shown in Figure 1.1. Web Camera Image Analysis Robot Control Bluetooth Antenna Cup Obstacle Robot Field Figure 1.1: Concept of the autonomous system. To determine where the plastic cups and obstacles are located, a web camera is used to monitor the field. Data from the camera is analyzed and information about locations of cups and obstacles provides the background for maneuvering the robot on the field, collecting cups, and returning these to the unloading base. Page 12 of 124

13 1.2 The LEGO Robot Camera The camera provided is a web camera installed in the ceiling, pointing at a floor area. The area in range of vision makes up the field where the robot operates. Through a USB connection the camera is connected to a computer handling image analysis. Image Analysis The multimedia analysis software EyesWeb is handling image analyzing. The patch for this must be designed to recognize the robot, cups, and obstacles located on the field and specify the coordinates of these items in image coordinates. Robot Control The coordinates found by EyesWeb are in the form of pixel coordinates, thus a model must be set up to transform the coordinates into the coordinates on the field. Based on the position of the robot markers, Robot Control is capable of determining the location and direction of the robot. From this data, and information about cups and obstacles, it is possible to calculate a route for the robot to follow. The task of Robot Control is to transmit commands to the robot to automatically make it follow this route. Hence, it can pick up cups, one by one, and secondly return to the base for unloading these cups. If the robot does not react as expected this must be revealed by the coordinates received from Image Analysis. To control the robot, a Bluetooth connection must be established between Robot Control and the robot. Robot The robot is made with LEGO bricks and contains the LEGO Mindstorms NXT, which provides the onboard computer. The robot must continuously receive instructions and send status information to Robot Control. Hereby, the robot functions as a remote-controlled unit and is not by itself able to pick up cups. The robot must carry visible identification markers to enable it to be recognized in Image Analysis. Having the concept described, the next section deals with introducing the robot used in this project. 1.2 The LEGO Robot The robot used is based on the LEGO Mindstorms NXT, and is shown in Figure 1.2. It has two wheels that can be individually controlled and a cup collecting mechanism that enables the robot to collect numerous cups. Instructions on how to build the robot can be found in [LEGO_Robot/] on the attached CD. The design has some unfortunate properties due to the cup collecting mechanism, where the elevation of the motor that powers the pick up sequence moves the center of gravity upwards. The robot is therefore expected to be somewhat unstable at high speeds, sharp turns, and during rapid speed changes. Also the tires, axles, and the suspension can cause some directional instability. To recognize the robot in Image Processing, the robot has two green spheres as markers mounted on the line of traveling direction. The markers are placed on the robot so that the largest marker is placed in the front end of the robot. The smaller marker is placed on line with the larger marker but in the rear end of the robot, making it possible to determine the direction of the robot. The identification markers for Image Processing are placed on the top of the robot to ensure that the markers are visible from the camera, independently of the cup load and the direction of the robot. Furthermore a peace of black cardboard is put below the markers to ease recognition of the markers and both markers are placed at the same height, i.e. the center of markers is at the same height. The dimensions of the robot are found in Table 1.2. Page 13 of 124

14 Introduction d m1 d m1,p d m2,p d m2 w h center d wheel d wb d front d rear pivot point d aw Figure 1.2: Illustration of dimensions of the robot used in this project. The robot, which must be controlled is set up and its dimensions are given. In the next section, the precision required of Image Processing is determined, to be able to pick up a cup. Page 14 of 124

15 1.3 Determination of Tolerance Index in Figure 1.2 Label Value w Width 155 mm h center Height to center of markers 276 mm d front Distance from pivot point to front 183 mm d rear Distance from pivot point to rear 170 mm d m1,p Marker one distance to pivot 82 mm d m1 Marker one diameter 58 mm d m2,p Marker two distance to pivot 142 mm d m2 Marker two diameter 39 mm d wb Wheel base 110 mm d wheel Wheel diameter 55 mm d aw Arm width 127 mm Total weight 1137 g Weight at rear support point 381 g Maximum speed 0.45 m/s Table 1.1: Technical data of the robot. 1.3 Determination of Tolerance The purpose of this section is to find the required precision of Image Processing. The tolerance is calculated from the precision needed to ensure that a cup can be located within the arms of the robot. The tolerance is determined from the worst-case situation where the position of the markers of the robot and the cup, to be collected, is determined most inaccurate. To be able to pick up a cup, the cup must be located in between the arms of the robot, where the front point is located. Figure 1.3 illustrates how the front point is determined when the detected location of the front and back marker of the robot varies at a specific tolerance. d fc f p d tol d tol p c p b p f r valid Valid region d fc Figure 1.3: Illustrates how the front point in between the arms of the robot varies due to a tolerance in the determination of items in Image Processing. The dotted circle containing the ellipse illustrates the valid region for the front point f p. p b is the center of the back marker, p f is the center of the front marker, and p c is the center of the cup to be collected. The distance between the front marker and the front point is d fc. The cup to be collected also varies with the tolerance. Hence, the valid location of the front point is illustrated by the dotted circle with center in p c, named valid region. The radius of this circle can be calculated using Equation (1.1). r valid = d aw 2 r cup d tol [m] (1.1) where: r valid is the radius of the valid region [m]. d aw is the arm width of the robot [m]. r cup is the radius of a cup at the same height as the arms of the robot [m]. d tol is the tolerance for each point [m]. Page 15 of 124

16 Introduction The centers of the front and back marker can both be determined within a circle with radius similar to the tolerance. The center of the back marker, the front marker, and the front point always lie on a straight line. Therefore, the location of the front point depends on the location of the markers. The distance between the center of the front marker and the front point is d fc. When the centers of the front and back markers vary, the location of the front point is within the ellipse drawn in Figure 1.3. If the tolerance should be acceptable, the circle representing the valid region must contain the entire ellipse. This means that a cup can be contained within the arms of the robot. The precision of the front point can be improved by increasing the distance between the two markers of the robot. Figure 1.4 shows a situation where the maximum tolerance is obtained using Matlab. The circle intersects the ellipse, meaning that the tolerance has the maximum value accepted. The tolerance used is 13.2 mm. Figure 1.4: Shows a plot where the maximum tolerance allowed is used. The entire ellipse is contained within the dotted circle, representing the valid range of the front point. The Matlab file used for calculating can be found in [Determination_of_Tolerance/] on the attached CD. The tolerance of 13.2 mm will not be required globally, since only the objects located close to the robot will have high demands to precision. The tolerance must be applicable for a distance similar to the distance from the back marker of the robot to a cup located in between the arms of the robot. This distance is shown in Equation (1.2). d locale = d m2,p + r m2 + d front + r cup d locale = 380 mm (1.2) where: d locale is the distance specifying the locale interval [m]. d m2,p is the distance from the center of the back marker to the pivot point [m]. r m2 is the radius of the back marker [m]. d front is the distance from the pivot point of the robot to the front point [m]. Notice that Image Processing must recognize the entire element to be able to determine its position. Therefore, d locale is the distance between the farthest edges of the cup and the back marker. The task of the system is to let a remote-controlled robot collect plastic cups in a predefined area. Data from a camera is analyzed and information about locations of cups and obstacles provides the background for maneuvering the robot on the field. The robot is constructed with LEGO bricks and is based on LEGO Mindstorms NXT. Within a distance of 380 mm the tolerance of each element detected by Image Processing must be ±13.2 mm. In the next chapter requirements to the system is set up based on the concept described in this chapter. Page 16 of 124

17 2 Requirement Specification In this chapter requirements to the system are presented in different categories. Some of the requirements are set only to define the design. These requirements are not explicitly tested, as they must be fulfilled before anything else is developed. Requirements that are defined for the design are marked with a (*) and will not be tested. 2.1 Requirements to the Environment The environmental requirements define the conditions where the system is designed to operate. Requirements to the environment are set to specify the test set-up for the system. In this matter, it differs from the other categories as these derive from the concept of the system. ENV1 - (*) The angle between the floor and the optical axis of the camera must be 59. This requirement comes from the physical shape of the test set-up. ENV2 - (*) The vertical distance from the camera to the floor must be 3241 mm. This requirement comes from the physical shape of the test set-up. ENV3 - (*) The light intensity on the field must be similar to the condition which can be found in an office environment. However, there must be no direct sunlight. The light intensity range in office environments is by Danish Working Environment Authority stated to be about 200 to 500 lux [Arbejdstilsynet, 1996]. Direct sunlight is not allowed due to its introduction of undesirable reflections from items and the field itself. ENV4 - (*) The operational temperature range must be in the region 10 to 40 C. Only the robot and the web camera are placed in the working area. Therefore, the temperature range is determined on the basis of their operational temperature ranges. The operating temperature range of the web camera is 0 to 45 C [Philips, 2001, p. 25]. According to LEGO Technical Support line: The NXT is specified to function correctly within a temperature range of but we have tested the NXT long beyond these ranges without problems. But we do not give any guarantees outside these values. [LEGO, 2006]. Joining requirements from these two products, the operational temperature range is specified. ENV5 - (*) Obstacles must be stationary, red, have a size of at least 50 mm 50 mm, and have a maximum height of 1 mm. The red color is chosen by the project group because it is one of the primary colors. The height is limited to 1 mm so it can be assumed that obstacles are located in floor level. The minimum size is set to ensure that the obstacles can be detected. ENV6 - (*) Obstacles must be parallel to the top edge of the field. The project group has put up this demand to simplify the detection of obstacles. ENV7 - (*) The objects that the robot must pick up must be empty white 210 ml plastic cups standing with the opening upwards. A plastic cup is an object easy to handle, stack, and transport, when facing upwards. Therefore, the project group has chosen that plastic cups must be the objects to pick up. ENV8 - (*) No more than 9 cups and 5 obstacles must be placed on the field simultaneously. Not to overload the field, the project group has set a maximum number of objects to be placed on Page 17 of 124

18 Requirement Specification the field simultaneously. ENV9 - (*) The distance between obstacles must be at least 50 mm. This distance is defined by the project group to ensure that EyesWeb can detect each obstacle as an individual object. ENV10 - (*) The distance between cups must be at least 150 mm. This distance is defined by the project group to ensure that EyesWeb can detect each cup as an individual object. This corresponds to that the camera can see minimum 50 mm of the floor in between two cups. 2.2 Image Processing Requirements Image Processing covers the processing of images in both Image Analysis and Robot Control. As Image Processing handles translation between the items placed in the real world and the coordinates used to control the robot, it is defined how the images of items should be perceived by the system. IMG1 - Cups, obstacles, and the robot must be recognized individually on the field. The recognition of individual objects is necessary due to the operational principle of the system. IMG2 - Within a distance of 380 mm the location of each element must have a precision of ±13.2 mm. The precision for the locale distance is obtained from Section 1.3, page 15, where the precision of Image Processing has been calculated to be able to collect a cup. IMG3 - (*) The frame rate of the camera must be chosen to obtain the smallest delay in Image Processing. The frame rate should be determined due to test of Image Processing. The chosen frame rate must be the one with the smallest delay and the smallest variation of the delay. IMG4 - (*) The resolution of the camera must be set to This has been determined by the project group. IMG5 - When containing zero to ten cups the robot must be recognized. When the robot holds up to ten cups, the number of cups must have no influence on the ability to recognize the robot. 2.3 Interface between Image Analysis and Robot Control The coordinates for the items are passed to Robot Control from Image Analysis. As so, it must be defined how the two parts handle coordinates. INTF1 - (*) The coordinates in the string, containing positions of items, must be of a corner followed by the height and the width of the box. It has been chosen by the project group to send the coordinates in a string. By sending the coordinates of a corner, the height, and the width of the bounding box, containing an item, a standard function in EyesWeb can be used. INTF2 - (*) The coordinates of the robot, cups and obstacles must be separated with the first letter of their individually words; i.e. r, c, o. Each element in the string must be separated with a comma. The string must be started by a 1 and terminated by a #. This is chosen to ensure that the different items can be separated and recognized by their type. Page 18 of 124

19 2.4 Control Center Requirements INTF3 - (*) The IP protocol must be used between the computers running Image Analysis and Robot Control. This has been chosen by the project group to follow a well-known standard. 2.4 Control Center Requirements Control Center is the controlling part of Robot Control. Control Center plans and controls the actions of the robot. This is done with respect to the input from Image Processing and a reference path that the robot must follow. Route Planner uses cups and the base as possible targets for the robot. CTRL1 - (*) Route planning must occur whenever a target is reached. The project group has decided that the next target should not be selected before a job is finished. Hereby, the robot must drive to the selected target even if a better solution emerges during execution of a job. CTRL2 - When several cups are possible targets, the path to be calculated must be the one for which an estimate of the traveled distance is shortest. The distance must be specified as the distance between the pivot point of the robot and the center of the cup. By using this approach the aim is to avoid detours around obstacles. CTRL3 - (*) Control Center must control the robot. In line with the concept of the system and the project proposal, Control Center must handle control of the robot. CTRL4 - (*) Control Center must operate with a fixed sampling frequency of 25 Hz. For Control Center to handle regulation of the robot, a fixed sampling frequency is chosen by the project group. This is done to obtain a fixed and well-defined interval between regulations. 2.5 Interface between Robot Control and Robot This interface is defined to be able to design a proper communication between the robot and Robot Control. INTF4 - Bluetooth connection must be established between Robot Control and the robot. A Bluetooth interface is implemented in the LEGO Mindstorms NXT. Therefore, the project group has chosen that this interface must be used. INTF5 - (*) Commands to the robot must follow instructions defined for LEGO MINDSTORMS NXT. In Enclosure i, page 120, an overview of direct commands for the LEGO MINDSTORMS NXT is shown. 2.6 User Interface Requirements A graphical user interface in Robot Control enables the user to interact with the system. GUI1 - (*) The user must be able to start and stop the operation of the system. If the system automatically starts on power up there is no possibility to ensure that all objects and Page 19 of 124

20 Requirement Specification the robot is in place. Therefore, a start function is needed. A stop function is also required. GUI2 - (*) The location of the robot and objects must be graphically displayed. The user at the Control Center might not be located in a position where he is able to get a view of the field. Therefore, the field must be sketched in the GUI to enable the user to actually follow the process. GUI3 - (*) The battery voltage level of the LEGO Mindstorms NXT must be displayed. This is used to monitor when to replace the batteries. GUI4 - (*) It must be possible to set the position of the base in the user interface. This has been determined by the project group. Having set up the requirements to the system, an overview of the system is given in the next chapter. Page 20 of 124

21 3 System Description The purpose of this chapter is to describe the system as a whole. In the first section, a block diagram of the system is shown to illustrate the functional structure of the system. In the section followed, the regulation technique used to control the robot is described. 3.1 System Overview In Section 1.1, page 12, the concept of the system is introduced based on Figure 1.1, where functional boxes are hidden. The purpose of this section is to reveal the content of the functional boxes, thus specifying the functionalities of the system, instead of focusing on the implementation on two different computers. Therefore, Figure 3.1 is set up followed by a description of functionalities. Web Camera Robot Recognizer Cups Recognizer Obstacles Recognizer Image Processing Data Assembler Coordinate Converter Control Center Route Planner Strategic Planner Path Planner Robot Location Estimator Robot Movement Decider Actuator Translator GUI User Input Handler Robot Movement Simulator User User Information Display Robot Motor Controller Battery Voltage Level Measure Unit Figure 3.1: Block diagram describing the system. Image Processing Image Processing has the function to recognize items on the field. The input to Image Processing is images of the field from a web camera mounted in a fixed position. The output of Image Processing is world coordinates for cups, obstacles, and the robot. The content of Image Processing, as shown in Figure 3.1, is described in the following text. Robot Recognizer The robot is identified through two green markers placed on the center line of the robot. Based on these markers it is possible to identify the back and front of the robot, and hereby, the direction of the robot. Each green marker is recognized and marked with a bounding box. The output is the Page 21 of 124

22 System Description image coordinates of a corner and the width and height of each of the two boxes as explained in INTF1. Cups Recognizer Each cup is recognized and marked with a bounding box. The output is the image coordinates of a corner and the width and height of each of these boxes. Obstacles Recognizer Red areas with dimensions of at least mm on the field are identified as obstacles that the robot have to avoid. Each obstacle is recognized and marked with a bounding box. The output is the image coordinates of a corner and the width and height of each of these boxes. Data Assembler Identification data from the robot, cups, and obstacles is assembled in a string in the stated order. The data for the robot starts with the letter r in the data string. In the same way, a c and an o is inserted in the string in front of cup and obstacle data respectively, as stated in INTF2. In this way, data for different types of items are separated and it is possible to handle each item individually. Coordinate Converter The data from the Data Assembler is image coordinates with dimensions specified in pixels. These data is converted into world coordinates by Coordinate Converter. Control Center Control Center calculates which path the robot is supposed to drive and sends the motor control signals to the robot, necessary for driving along this path. Furthermore, Control Center steers the robot back to the planned path if it, departs from the path. To be able to compare the actual route to the planned route, Control Center gets the location and direction of the robot from Image Processing. Route Planner Route Planner is a function only used when the system must select a target and plan a path to this target. Therefore, the box surrounding Route Planner is drawn with dotted lines in Figure 3.1. Strategic Planner Strategic Route Planner decides which cup the robot is going to pick up next. This planning can be accomplished in many different ways of varies complexity. The project group has decided to implement this by choosing the cup, for which an estimate of the traveled distance is shortest. The base is selected as target if no cups are reachable to the robot or the robot must unload the cups collected. Path Planner Path Planner calculates the path to the target selected by Strategic Planner. Robot Location Estimator Robot Location Estimator estimates the coordinates of the robot by evaluating both the coordinates available from Image Processing and the simulated location and direction of the robot from Robot Movement Simulator. This ensures that missing or erroneous frames from Image Processing does not affect the performance of the system. Page 22 of 124

23 3.1 System Overview Robot Movement Decider Robot Movement Decider decides which way the robot must drive to follow the path planned by Path Planner. Actuator Translator Actuator Translator converts the output of Robot Movement Decider into commands for the two propulsion motors. Robot Movement Simulator To be able to calculate an estimation of the location and direction of the robot, Robot Movement Simulator calculates the position and direction of the robot for each sampling period. Robot The robot is not intelligent itself, thus its role exclusively is to follow instructions given by Control Center. This behavior is consistent with the concept of the system and the project proposal. Motor Controller Commands transmitted to the robot, containing the operations of the motors, are translated into rotation of the motors, by the LEGO Mindstorms NXT. The motor controller both handles the motors for propulsion and the motor for the operation of the claw. Battery Voltage Level Measure Unit The LEGO Mindstorms NXT measures its battery voltage level, which can be returned to the user when requested. Furthermore, the model of the motors is adjusted according to the battery voltage. GUI - Graphical User Interface The user interface enables the user to interact with the system. Even though the system is autonomous, and therefore does not require inputs from a user, the user interface still provides features, convenient for a user to have. User Input Handler User inputs are handled by User Input Handler. The user is able to start and stop the system. In this way the user can decide when the collection of cups should begin. The GUI also provides the possibility to stop the system and thereby any movements of the robot. The user is also able to specify the location of the base in the software. This enables the user to specify the location where the collected cups must be unloaded. Information Display System information is shown in the user interface to enable the user to keep track of the system. This allows remote monitoring of the system. A graphical readout is provided to give an overview of the location of objects and the robot to a given time. This is done by drawing cups, obstacles, the base, and the location and direction of the robot in a frame corresponding to the field where the robot operates. Furthermore, the calculated route to the current target is shown. The ongoing activity of the system is also available together with the battery voltage of the LEGO Mindstorms NXT. Except for the robot, the functionalities described are software related and are implemented on the two computers. The computer running Image Analysis is dedicated to running the EyesWeb software, which is used for analyzing the image data and construct bounding boxes around the Page 23 of 124

24 System Description detected items. Image coordinates of these are transmitted to the computer running Robot Control where the last parts of Image Processing are run, i.e. conversion of item locations to floor coordinates. Also, Control Center, and the Graphical User Interface related with it, are run from here. The next section describes the regulating concept of the system. 3.2 Feedback in the System The movements performed by the robot can be considered as the result of the system. Feedback is introduced in the system by monitoring the robot with a web camera. This emerges from Figure 3.1 on page 21. The concept of feedback is described in this section. When a cup is to be collected, the starting point is a route calculated by Route Planner. Route Planner, described in Chapter 5, page 42, returns a fixed route which the robot must be controlled to follow. However, the robot may diverge from the route, so corrections must be made to lead it back on the route. These corrections must be carried out by a controller. Dependent on the deviation from the route, the robot must compensate accordingly. Figure 3.2 gives an overview of the regulation system with feedback. Figure 3.2: Overview of the regulation system with feedback. The robot has an initial location and direction denoted (x 0, y 0, θ 0 ). To be able to get the robot moving, commands are sent to operate the actuators, which influence the position and direction of the robot. The commands are defined by LEGO and is Power set point, concerning the speed of the wheel set to rotate at the highest speed, and Turn Ratio, dealing with the velocity relation between the two wheels. In the LEGO Mindstorms NXT, the commands are translated into pulse-width modulated voltages for the two propulsion motors. In Appendix B.3, page 111, measurements are found to determine the actual behavior of the controller in relation to the commands sent. Determination of Position and Direction Based on the commands sent, the robot moves and obtains a new location and direction (x, y, θ). This is filmed by the camera and processed in Image Processing, as described in Chapter 4, page 27. Page 24 of 124

25 3.2 Feedback in the System Concurrently, a new position and direction of the robot is calculated in Robot Movement Simulator based on the commands sent to the robot. This part is described in Section 7.4, page 64. The estimation of the new position and direction (x est, y est, θ est ) is based on the output from the simulator (x sim, y sim, θ sim ) and data from Image Processing (x img, y img, θ img ). This is described in Section 7.1 on page 56. The simulator is reset to the estimation to avoid an error building up. Controller The controller can fetch the estimated position, and combined with the route calculated, it determines which commands to be send to the robot for it to follow the route. An advantage of having feedback in the system is that smaller errors can be tolerated. The model of the robot is allowed to be less accurate. Thus, the directional stability of the robot is not as important as for the case without feedback. This is due to the principle of feedback, where the output of the system is subtracted from a reference to determine the correction required. In proportion to the defined route, information perceived by the camera is continuously used to rectify the position and direction of the robot. The simulation is used to validate the data from Image Processing as described in Section 7.1 on page 56. Regulation Technique The robot will drive in a circular motion if the wheels turn with constant angular velocities. The radius of the circle will depend on the velocity ratio of the left and right wheel. Since considerable time passes between new commands are sent to the robot, the robot will move in a series of sections of circles. This is described in the kinematic model of the robot in Section 6.2, page 53. To follow the route planned by Route Planner, the approach chosen is to let the robot drive towards a point at a certain distance ahead of it. This is described in Robot Movement Decider in Section 7.2, page 58. The regulation technique obtained by this approach exploits that the more the robot deviates from the route, the more intensive the correction will be. Scope of Corrections In Figure 3.3 to Figure 3.5, the robot drives from waypoint i on a route towards waypoint i + 1. Waypoints are corner points on a route and are described in Section 5.1 on page 44. It appears from Figure 3.3 and Figure 3.4 that the scope of correction depends directly on the deviation from the route, d dev. This applies if the robot looks ahead for a point on the route at a fixed distance away. Figure 3.3: A small correction is necessary to follow the route. The deviation from the route, d dev, is small. Thereby, only a small correction is needed to drive towards the intersection point a specific distance ahead of the robot. Figure 3.4: A large correction is necessary to follow the route. The deviation from the route, d dev, is large. Thereby, a large correction is needed to drive towards the intersection point a specific distance ahead of the robot. From Figure 3.3 and Figure 3.4 the robot seems always to converge towards the route. This is a favorable characteristic that guarantee stability. However, Figure 3.5 shows a situation where the Page 25 of 124

26 System Description Figure 3.5: The deviation from the route, d dev, is larger than the radius of view against a point on the route. Hereby, no intersection is possible and the operation must be aborted. deviation from the route is larger than the radius of view towards a point on the route. Therefore, no intersection is possible and the operation of the system must be halted. It follows that the robot will converge towards the route if the deviation, d dev, is less than the radius. For this reason, both data from Image Processing and Robot Movement Simulator is taken into account, so that operation of the system can continue, even if Image Processing in a few bad frames erroneously recognized the robot to be too far away from the route. In this chapter a block diagram of the system is introduced. The structure reveals a system with feedback so an overview of the regulation in the system is described. It is shown how the robot converges towards a planned route when the approach chosen is to let the robot drive towards a point on the route, a given distance away from the robot. The corrections made are based on data from a simulator and from Image Processing. Image Processing is described in the next chapter. Page 26 of 124

27 4 Image Processing This chapter describes Image Processing starting with a description of the image analysis handled in EyesWeb. Afterwards, the conversion of image coordinates to world coordinates is described. Finally, positioning of items is described. 4.1 Image Analysis From IMG1 it is given that Image Processing must be able to detect items on the field. This implies that the analysis finds the image coordinates of red obstacles, green markers of the robot, and white cups in each image. According to IMG4 this must be done with a resolution of pixels. The frame rate is chosen to 25 fps by testing the final EyesWeb patch, as shown in Section 9.1, page 75. A block diagram of the image analysis is shown in Figure 4.1. Live image Extraction of colors Detection of items Coordinate determination of items Assembling of output string Output string Figure 4.1: Block diagram of the image analysis. The input to the image analysis is live image data from the web camera, and the output is a string containing image coordinates of items formatted as described in INTF1 and INTF2. In the analysis the first task is to extract the specific colors from the image. Then, from the knowledge of these regions, the different types of items are located, their coordinates are found, and an output string is created. The output string is then sent to the computer running Robot Control. On this computer, the image coordinates of the items are converted into world coordinates. Extraction of Colors This section describes color systems and how color extraction can be performed. It is described how to detect the red color for the obstacles, the green color for the markers of the robot, and the color white of cups. Two color systems exist; the subtractive color system and the additive color system. The characteristics of the subtractive color system are that when colors are added, the outcome ultimately ends up as black. The subtractive primary colors are cyan, magenta, and yellow. The secondary colors are red, green, and blue. The additive color system is different in the way that if colors are added, the outcome ultimately end up as white; instead of black. The additive primary colors are red, green, and blue. The secondary colors are cyan, magenta, and yellow [Johnson, 2004, p. 135]. In Figure 4.2 the primary and secondary colors of the additive color system can be seen. R Y G M W C B Figure 4.2: The primary and secondary colors of the additive color system. The initial letter of each color is shown in the figure. Page 27 of 124

28 Image Processing The additive color system is also called RGB and is a standard system used in many cameras. The camera used in this project is an ordinary web camera; Philips PCVC740K ToUcam Pro. The camera uses a RGB system called RGB24, which implies the use of 24 bit to represent the color of each pixel, which gives the possibility of 2 24 = 16 M different colors in each pixel [Philips, 2001, p. 24]. Digitally, the colors are represented by three matrixes; one for each primary color; red, green and blue. For example the color of the pixel in the top left corner is consisting of contributions from the first row and first column of each matrix. The color extraction should extract the matrixes holding the information needed. If yellow is to be extracted, the blue color channel is subtracted from the mean value of the red and green color channels. The outcome is a one-dimensional matrix. Hereby, the output image will be in gray-scale with 8 bit data per pixel. The result of this extraction is that a red object and a yellow object, with half the intensity in both the red and green channels, will be identically recognized. Therefore, unwanted color components should be eliminated in the item detection block. Extracting Primary colors In this project extraction of primary colors is used to extract red obstacles and the green markers of the robot. Five red obstacles are shown in an example in Figure 4.3. In Figure 4.4 the intensity of the red channel is shown, using the method of extracting primary colors. The approach used is to find the mean value of the two excessive channels and subtract this from the channel desired; in this situation the red channel. Thereby, the output value can vary between 0 and 255 which is similar to representing each pixel with 8 bits. Figure 4.3: Image with five obstacles and nine cups before color extraction. Figure 4.4: Image showing intensity of red using the method of primary color extraction. Extracting White To extract the cups on the field, white must be extracted from the input image. The procedure for extracting the white color is to find the mean value of the three channels. A threshold must be applied to the extracted values to evaluate whether the color is red, green, or white. This is explained in the next section. Detection of Items From color extraction it is known that thresholding is needed. Thresholding can be made by comparing intensities in an extracted image with a threshold level. Thus every pixel can be represented with only one bit. A bit is set only in pixels with higher intensities than the threshold specified. A problem with applying a threshold level is that certain spots in the background image may contain a high value of the extracted color, and thereby errors can occur in these areas. Therefore, the use of background subtraction is applied before thresholding. The background subtraction uses an image of the background in the color channel of interest; this image should hold no items and should be extracted from the input image. With this approach, the errors obtained from high intensity areas of the background are minimized. To ensure that there are no changes in the Page 28 of 124

29 4.1 Image Analysis background image, the gain of the camera must be fixed and there should be no changes in the light conditions, as this will cause variations from the background subtracted. The background subtraction process is illustrated in Figure 4.5 to Figure 4.7. Figure 4.5: One line of the background image after color extraction. Figure 4.6: One line of the image, after color extraction, with an item placed on the background. Figure 4.7: The result of background subtraction after the color extraction. The dotted line indicates the intensities before background subtraction. When the background subtraction is made it will affect the intensity of the items in the image. This can be seen in Figure 4.7 where the dotted line indicates the original intensities, which are higher than the intensities on the resulting image. This can cause problems if the difference is small between the intensity of the background and of the item. This implies that the smaller the difference between the background and the item is, the greater the error applied by background subtraction is. A graphical illustration of the threshold level applied to the background subtraction can be seen in Figure 4.8 and in Figure 4.9. Figure 4.8: Line of pixels with threshold level shown. Figure 4.9: Line of pixels showing the result of the thresholding. After the threshold level is applied, the pixels with high intensities of the color of interest are represented by ones, and the rests are represented by zeros. Morphology Noise in the background causes problems, because noise is recognized as small objects with high intensities located arbitrary in the image. To minimize the problems from this noise, morphology is introduced. Throughout the morphology description [Gonzalez and Woods, 2002, chapter 9] is used as a reference. There are two basic operators in morphology; dilation and erosion. Both need a structure element to function. An example of a structure element is shown in Figure Before using the structure element an origin must be defined. The origin is used to determine which part of the structure element is used to overlap the object being processed by either dilation or erosion. The origin is moved along the edges of the object. In erosion, the pixel holding the origin of the structure element is placed inside the object and the pixels hidden behind ones in the structure element are erased. In dilation, the pixel holding the origin of the structure element is placed outside the object, and all the pixels in the structure element are added. Page 29 of 124

30 Image Processing Figure 4.10: An example of a structure element of size 3 3. Erosion Erosion is used to contract the boundaries of an object, denoted A. The erosion of the object A by the structure element B is denoted A B. When the structure element is moved along the edges of the object, A, it removes pixels from the object. In Figure 4.11 an example of erosion is shown. Figure 4.11: Erosion of the object A by the structure element B. In Figure 4.11 it appears that the boundaries of the object are contracted by half of the structure element on all sides of the area. This is applicable if the structure element used is similar to the one in Figure Dilation Dilation is used to expand the boundaries of an object, denoted A. The dilation of A by B is denoted A B. When the structure element travels around the edges of the object it will add more pixels to the object. In Figure 4.12 an example of dilation is shown. Figure 4.12: Dilation of the object A by the structure element B. From the example of dilation in Figure 4.11 it appears that the boundaries of the object are expanded by half of the structure element on all sides of the area. Page 30 of 124

31 4.1 Image Analysis Open and Close When combining erosion and dilation it is possible to open or close objects in an image, meaning that pixels can be removed or added to objects. In this project, open is used to remove small object of noise when detecting the markers of the robot and close is used to fill the objects detected. Close is obtained as a dilatation followed by an erosion, and is denoted A B. This is expressed in Equation (4.1). A B = (A B) B (4.1) where: A is the object. B is the structure element. A closing can be used to fuse narrow breaks and eliminate small holes in objects. Related to the project, this is used to fill the detected objects if small holes or narrow breaks occur. An illustration of the closing of a noise element, N, is illustrated in Figure It should be noticed that the noise, N, equals the result, N B. Figure 4.13: An example of closing performed on a noise element N. The noise element remains unchanged. The gray color indicates the element seen, and the dotted line indicates the outline of the original noise element. An illustration of closing applied to an object with a hole inside is illustrated in Figure Notice that the hole in the middle of A disappears after closing. Figure 4.14: An example of closing performed on an object with a hole inside. The hole in the middle disappears. The gray color indicates the object seen, and the dotted line indicates the outline of the original object. Open is obtained as an erosion followed by a dilation, and is denoted A B. This is expressed in Equation (4.2). A B = (A B) B (4.2) An opening can be used to eliminate small islands and break narrow gabs. Related to the project, this is used to eliminate noise in the background when the markers of the robot are being detected. The opening is applied since the background contains a considerable amount of noise in the blue color channel, which may appears as small noise elements. An illustration of opening a noise element, N, is illustrated in Figure It appears on the figure that due to the size of the noise element and the structure element, the noise element disappears from the output. Page 31 of 124

32 Image Processing Figure 4.15: An example of opening performed on a noise element N. The noise element disappears. The gray color indicates the element seen, and the dotted line indicates the outline of the original noise element. Figure 4.16: An example of open performed on an object with a hole inside. The object remains unchanged. The gray color indicates the object seen, and the dotted line indicates the outline of the original object. An illustration of opening performed on an object is shown in Figure No changes can be observed. Determination of coordinates is described in the next section. It should be noticed that the size of the structure element should be chosen such that it is larger than the noise elements that should be removed and smaller than the objects. Coordinate Determination of Items To determine the coordinates of an item, a bounding box containing the area is inserted around the area of interest. First the matrix of the 1-bit quantized image is examined. For every pixel holding a one, the neighboring pixels are examined to determine how big the area of high intensity is. When the area is detected, a rectangular bounding box is defined from the maximum and minimum x- and y-values. This is returned in a numbered one dimensional matrix containing x- and y-coordinates of a corner together with the width and the height of the box. Since EyesWeb is not always returning the top left corner, the width and the height can be negative, meaning that the top left corner was not chosen. Figure 4.17 illustrates a bounding box with return values shown; assuming that coordinates of the top left corner are returned. (x,y) Δx Δy Figure 4.17: Rectangular bounding box with return values shown. Numbered one dimensional matrixes hold the information of all the items detected. They should be converted into a string and be send to the computer running Robot Control; this is done in the output string creation block. Page 32 of 124

33 4.1 Image Analysis Assembling of Output String To create the output string for Robot Control, each vector from the item coordinate determination block is applied with a letter, r (robot), c (cup), or o (obstacle). Furthermore, each value is separated by a comma and the string is terminated by #. The maximum number of items is nine cups, five obstacles, and two robot markers. To ensure that EyesWeb resets all slots not holding valid data, the initial letters for the objects are inserted in the extra slots; if fewer than the maximum number of objects is detected. Furthermore, zeros are inserted as coordinates, width, and height of the robot, if a robot marker is missing. A one is inserted in front of the string to indicate the start of image data. The structure of the string is also defined in INTF1 and INTF2. In the next section it is described that two different patches are used. An example of the structure of a valid string, returned from detection of cups and obstacles, is shown in Equation (4.3) and a valid string, returned from the detection of the robot, is shown in Equation (4.4). 1, c, x, y, x, y,, o, x, y, x, y,, # (4.3) 1, r, x, y, x, y, x, y, x, y, # (4.4) When the string is completed it is send to the computer running Robot Control where the coordinates can be converted into world coordinates. This conversion is described in section 4.2 on page 35. The interface between the computer running Image Analysis and the computer running Robot Control is described in Appendix D.2, page 115. Optimization of the Program Due to the large data flow in the color extraction block it has been chosen to split the program into two separate patches; an object patch returning coordinates of objects and a robot patch returning coordinates for the markers of the robot. This separation is made to minimize the delay in the robot patch. Furthermore, it is decided to convert the coordinates from image coordinates to world coordinates at the computer running Robot Control. During tests it has been clarified that the blue color channel contains higher amounts of noise than the red and green channels. In Figure 4.18 to Figure 4.20 the intensities in each color channel is shown after background extraction, and with a threshold level of 10 applied. To improve the recognition of cups, a threshold level is applied to both the green and red channel, after which the results are ANDed together. To minimize the problem that obstacles might be detected as being white, the inverse of the obstacle output is ANDed together with the cup output. Figure 4.18: Red color channel after background extraction and thresholding. Figure 4.19: Green color channel after background extraction and thresholding. Figure 4.20: Blue color channel after background extraction and thresholding. A test using the robot patch is performed to determine the frame rate of the camera, and hereby the frequency of output data from EyesWeb. The test is described in Section 9.1 on page 75. The test concludes that a frame rate of 25 fps must be used since this gives the smallest variation in the delay and, similar to 30 fps, the smallest maximum delay. Page 33 of 124

34 Image Processing Structure of the Program The final program structure is based on the previous sections and is implemented as two patches in EyesWeb The structure can be seen in Figure 4.21 and Figure The implemented patches can be found in [Image_Processing/Patches] on the CD. Live image Extracting white Subtracting white background component Detecting cups Creating output string Output string Extracting red Subtracting red background component Detecting obstacles Background image Figure 4.21: Block diagram illustrating the structure of the patch for detection of objects. Live image Subtracting background Extracting green Detecting robot Creating output string Output string Background image Figure 4.22: Block diagram illustrating the structure of the patch for detection of the robot. Start-up Sequence The start-up steps for the program are listed below. 1. Start the patch for detecting cups and obstacles. 2. Capture background image. 3. Place cups and obstacles on the field. 4. Set Robot Control to receive coordinates of objects. 5. Terminate the objects patch, when Robot Control has received the cups and obstacles. 6. Start the patch for detecting the robot. 7. Place the robot on the field. 8. Set Robot Control to fetch the position of the two markers of the robot. 9. Start the system in autonomous control. Image analysis is performed in EyesWeb and is able to detect the locations of cups, obstacles, and the markers of the robot. An item is selected by inserting a bounding box around it. The output string from EyesWeb contains the image coordinates of the detected items. The output string is send to the computer running Robot Control where the coordinate conversion is performed. This is followed by a positioning of items, where the actual positions of items are determined in the floor level. The following section describes the coordinate conversion, where image coordinates are translated into world coordinates. Page 34 of 124

35 4.2 Coordinate Conversion 4.2 Coordinate Conversion To determine the location of items on the field, it is necessary to convert image coordinates into world coordinates. For the conversion to be possible, a camera calibration is needed to give an applicable determination of the location anywhere on the field. The calibration is described in Appendix A.1 on page 93. When calibration data is available, it is possible to make a conversion between image coordinates and world coordinates. The essential equations, making the conversion possible, are summarized in this section, while details about the coordinate conversion are described in Appendix A.2 on page 96. Compensation must be made to the image coordinates to prevent influence from the lens distortion of the camera. The distortion consists of a radial and a tangential distortion. The radial distortion is illustrated in Figure The tangential distortion is caused by inaccuracies in the manufacturing and is typical less significant; hereby, this will be omitted [Bouguet, 2006b]. Figure 4.23: Illustration of the radial distortion caused by non-linearities in the optics of the camera. The image coordinates are converted to normalized camera coordinates, and then compensated for distortion, using Equation (4.5). x cn 1 = y cn 1 + k c [1] r 2 + k c [2] r 4 + k c [5] r (4.5) 6 x id c c[1] f c[1] y id c c[2] f c[2] where: x id and y id are the image coordinates with distortion [pixel]. x cn and y cn are the normalized camera coordinates compensated for distortion [ ]. k c contains the distortion coefficients [ ]. c c is the principal point [pixel]. f c is the focal length [pixel]. r is the radius to the normalized image point [ ]. The normalized, distortionless camera coordinates are the ones to be represented in world coordinates. These are shown in Equation (4.6). P img = x cn y cn (4.6) 1 where: P img is a normalized distortionless point in camera coordinates [ ]. The relationship between a point in the camera coordinate system and the same point in the world coordinate system can be described with the extrinsic parameters as shown in Equation (4.7). Page 35 of 124

36 Image Processing P world = R c 1 (P camera T c ) (4.7) where: R c and T c are the extrinsic parameters from the camera calibration. P camera is a point in the camera coordinate system. P world is the same point as P camera in the world coordinate system. To determine the correct world coordinates on the field, P w, the parametric representation of the line passing through the transformed focal point P fw, and the transformed camera point P imgw, is used. This is illustrated in Figure The parametric representation is used to find a point on the line in the world coordinate system, as a function of a z-value, expressing the height above floor level. This can be seen from Equation (4.8). Camera coordinates x P fw y z P imgw x y Image plane with image coordinates World coordinates z P w x y Figure 4.24: Sketch to determine P w based on P fw and P imgw. ( ) zw z fw P w (z w ) = P fw + (P imgw P fw ) (4.8) z imgw z fw where: P w is a point in the world coordinate system that lies on the line formed by P fw and P imgw [m]. z w is the z-value in the world coordinate system, equal to the vertical distance to the floor [m]. P fw is the focal point transformed to world coordinates [m]. P imgw is a point in camera coordinates transformed to world coordinates in the image plane [m]. z fw is the z-value of P fw [m]. z imgw is the z-value of P imgw [m]. It is then possible to convert an image point into a point in the world coordinate system. Page 36 of 124

37 4.2 Coordinate Conversion Test of Coordinate Conversion A requirement is set to the locale precision of the coordinates of items. IMG2 applies for the entire Image Processing so the precision of the coordinate conversion cannot verify whether the requirement is met; however, at least the coordinate conversion must meet this requirement. A local accuracy test is performed in Section A.2. The size of the local error is important for the operation of the system; for example when an obstacle must be avoided or a cup must be picked up. The local accuracy test shows that between two points located with a distance of 408 mm, which is greater than the local distance of 380 mm. The largest difference, in the actual distance and the distance based on the points converted, is 6 mm. Hereby, the conversion of coordinates meets IMG2 of a local precision of ±13.2 mm. When two points in the local area both have a tolerance, it means that the distance between these may deviate two times the tolerance. The global error is not that decisive, because the control of the robot is made relative to points in a small area. However, a global accuracy test is performed to show how accurate the coordinate conversion is with reference to the origin in the world coordinate system. The details of the tests are described in Section A.2 on page 100. The results from the global accuracy test show that the largest distance between the measured and converted point is 11.8 mm. For another point this difference is 11.3 mm, but for all other points the deviations are less than 10 mm. The global accuracy test and the local accuracy test show that the largest deviation is found in the upper-left area of the field. This can be caused by the tangential distortion as it has been omitted, and has shown to be largest in the upper-left area of the field. Remarks about Local Area and Height The local accuracy test of the coordinate conversion uses boards in the floor level. However, it should be noticed that the height of the item to be positioned influences the accuracy of the point in the floor level. The local accuracy test of the coordinate conversion, illustrated in Figure 4.25, uses line 1 and line 2 to determine the position of two points in the floor level. When the location of the robot should be determined, the height of the robot implies that line 3 is used to determine P 2. Line 3 and line 2 will give a different error in the point P 2, because two different points in the image plane are used. With the distortion in the camera, two different points on the image will give two different errors on the floor. Figure 4.25: Illustration of points in the local area at difference heights. The test described in Section 9.3 on page 83 verifies that the robot can be located with the desired precision in a local area. It should also be noted that the results obtained in this test are applicable to the entire Image Processing. Having image coordinates translated into world coordinates, the next section describes the positioning of items, where the actual positions of items are determined in the floor level. Page 37 of 124

38 Image Processing 4.3 Positioning of Items This section explains how positions of obstacles, cups, and the robot are determined by using the identification from the image analysis and the conversion of coordinates. Since the identification in EyesWeb is based directly on data from the distorted image, special precautions have to be taken when positions of items are found. The effect from this is that a square item in the real world is shown as a skewed quadrangle in the image. This effect is shown in Figure The squared boards are rectangles in the real world, and the red markers are a rectangles in image coordinates. Positioning of Obstacles The obstacles are rectangles and needs to be recognized as such. For illustrative purposes, images of squared boards will in this section be used instead of red obstacles. As known from Section 4.1, page 27, an item is identified by a position, a width, and a height, forming a rectangle; this is shown in Figure The labels in the figure refer to the position in the field, e.g. the board with the label top-left corner is placed in the top-left corner of the field. Figure 4.26: Squares shown as they would be recognized in image analysis with red bounding boxes around them. The labels refer to the position in the field. The four corners of the red bounding boxes are converted into world coordinates using the coordinate conversion described in Section 4.2. This gives four points per rectangle as illustrated with blue crosses in Figure It is clear from the figure that the four points do not form a rectangle. Figure 4.27: The identified coordinates of the bounding boxes converted into world coordinates. The item in the top-left corner of the field is used as an example. For this item, point 2 and 4 are the correct corners, because these corners of the red bounding box matches two corners of the actual item, as shown in Figure A variation can be seen in the bottom-right corner of the field. The situation changes a little because no corners are matching exactly. Here the x-coordinates of point 1 and 3 are used, as the actual item touches the bounding box nearest to those corners. Moreover, the y-coordinates of point 2 and 4 are used, because these are also the closest match. The procedure can be expressed in a more general way. The values used are always the x- coordinates of the points closest to each other in the x-direction and the y-coordinates of the points closest to each other in the y-direction. In Figure 4.28, this is illustrated for the four items. The Page 38 of 124

39 4.3 Positioning of Items procedure can be used because image analysis always makes a bounding box so that the entire item is surrounded, which means that in some places too large an area is surrounded. Figure 4.28: The converted coordinates used to find outlines of the boards. As written earlier, obstacles are rectangles, thus the positions of these can be determined using this method. Requirement ENV6 is set up because this method described assumes that the rectangles on the floor are parallel to the world coordinate system. If the objects are not parallel to the coordinate system the resulting positions will not be accurate. Because there are no real-time demands to the positioning of obstacles, the position is calculated by averaging over 100 frames. The effect is a filtration of the position that removes errors that may occur in one image. Positioning of Cups There are several difficulties involved with determining the correct position of a cup. Cups are not flat like obstacles, and they are not rectangular shaped. Hereby, a cup looks very different dependent on the viewing angle. This is illustrated in Figure Figure 4.29: Cups identified as it would happen in the image analysis; here marked with a red rectangle. To ensure a correct recognition of the center of the cup it is important to choose the right points to calculate the center from. Each corner of a red bounding box is converted into world coordinates as it is done when positioning an obstacle. This gives the points marked with blue crosses in Figure Only point 3 and 4 can be used as is, because only the bottom corners of the red bounding box are located in the floor plane. To be able to use the top points also, these are converted from the identified rectangle into world coordinates using a z-value corresponding to the cup height. This conversion gives the points 1b and 2b, shown in Figure 4.30, which would be the two points in top, if the cup was seen directly from above with a square drawn around it. As previously shown, the points are not aligned, so the correct points must be chosen. First the original blue points in Figure 4.30 are considered. The same approach as for the obstacles-case can be used, because the blue points all lies in the floor plane. This means that Page 39 of 124

40 Image Processing Figure 4.30: The identified coordinates converted into world coordinates. The two top corners are each shown in a situation where they have been calculated with a z-value of 0 (blue point) and with a z-value equal to the cup height (red point). for the cup in the top-left corner of the field, the points 2a and 4 would be selected, if the same procedure as used for obstacles is utilized. As experienced already, point 2a cannot be directly used; instead the corresponding point in the floor level, 2b, is used. The important thing to notice is that the points selected are those points with the greatest mutual distance in the x-direction, which is opposite to the obstacle-case. Though, it still is the points with the smallest mutual distance in the y-direction that are selected and used as y-coordinates for finding the location of the cup. Again, the cup in the top-left corner of the field is used as an example of determining the position of a cup center. Now, four points have been identified. For the two points to use in the x-direction, i.e. point 2b and 4, the mean value of the x-values is used as the x-coordinate for the cup center. In Figure 4.31 this is illustrated by the two lines of equal lengths; marked a. The y-coordinate of the cup center is found by first taking the y-value of point 2b, move it the distance c: A distance corresponding to the top radius of a cup. Then taking the y-value of point 4, move it the distance b up, a distance corresponding to the bottom radius of a cup. Secondly, the mean value of the two y-values is used as the y-coordinate of the cup center. Figure 4.31: The converted coordinates of the cups used to find the cup center. The x- and y-coordinates found corresponds to the center of the cup. To ensure a better positioning of cups, the averaging method, similar to the one used for positioning of obstacles, is applied. Positioning of the Robot The method used for positioning the robot is different from the previous mentioned. This is due to the fact that the identification markers of the robot are spherical. No matter which direction a sphere is seen from, it looks like a circle. Hereby, the center point of the bounding box obtained from the image analysis will be the center point of the circle seen. In Figure 4.32 this point is marked with a blue cross. If the point is converted into world coordinates using a z-value corresponding to Page 40 of 124

41 4.3 Positioning of Items the height of the center of the marker, the obtained point in world coordinates will be the center of the sphere. The applies, because a line from the center pixel of the circle in the image plane through the blue point on the sphere always goes through the center of the sphere. Figure 4.32: The position of one of the robot identification markers in two situations. From the position of the markers, Robot Control is able to calculate the position and direction of the robot. In this chapter Image Processing has been described in three parts. Through image analysis, coordinate conversion and positioning of items, the location of an item on the field can be determined. The precision of Image Processing is tested in Section 9.3 on page 80. The test shows that Image Processing is able to recognize cups, obstacles, and the robot with the required tolerance of 13.2 mm defined in IMG2. In the next chapter the planner is described to be able to determine a target and plan a route to it. Page 41 of 124

42 Route Planner 5 Route Planner The purpose of this chapter is to described the route planner and introduce the representation of items on the field. Route Planner is able to determine a target and plan a route to it. The calculations needed are based on the positions of the robot, the target, and the obstacles. Furthermore, the battery voltage level and cup load of the robot are taken into consideration. The target can either be a cup or the base, depending on whether a cup is to be picked up or cups are to be unloaded. When a cup is selected as target all other cups are considered to be obstacles, but the base will set no restrictions and the robot is authorized to drive through this. 5.1 Representation of Items on the Field To be able to plan a route and maneuver the robot in between obstacles, waypoints are inserted at all corners of obstacles, which is allowed for the robot to pass through. To ease calculations, calc margins and safety margins are inserted around obstacles. Hereby, the representation of the robot can be reduced to a single point, which is not allowed to intersect with the safety margins. An overview of the intention of waypoints, safety margins, and calc margins are given in the following list. Safety margins are used when moving the robot on the field. The single-point representing the robot is not allowed to intersect safety margins. The location of safety margins only takes the dimensions of the robot and the precision of Image Processing into account. Calc margins extend safety margins and are used when planning a path for the robot to move. Calc margins are inserted almost on top of waypoints and must prevent planning a path in between a waypoint and a safety margin of an obstacle. The location of calc margins takes the precision of both Image Processing and the movement of the robot into account which includes the system delay. A waypoint is located with sufficient distance to an obstacle such that the robot can pass through the waypoint without colliding with the obstacle. The location of waypoints takes the precision of both Image Processing and the movement of the robot into account. A path for the robot might be as drawn in Figure 5.1. For the two obstacles, respectively, waypoints A, B, C, D and E, F, G, H are inserted. It should be noticed that a safety margin and a calc margin are inserted around each obstacle. In the situation in Figure 5.1, waypoints A and D are invalid due to their location within the calc margin of the field; notice that only calc margins and waypoints are considered in Route Planner. The calc and safety margins of the field exist so that it can be guaranteed that the entire robot stays within the field edges, when the robot is represented as a single point. Also, C and E are invalid since they are located within the calc margin of an obstacle. Calc margins and waypoints are introduced because a path should not be planned upon the edges of safety margins. It is not possible for the robot to travel straight on the edge of a safety margin, because it cannot be guaranteed that the robot will not exceed the safety margin due to uncertainty in Image Processing. Therefore, a gap is introduced, taking uncertainty in the controller into account; namely the calc margin. Safety Margins As previously mentioned, safety margins are inserted to be able to represent the robot as a single point, which can ease calculations in Robot Movement Decider, described in Section 7.2, page 58. As described in section 1.2, page 13, a pivot point is located in the point of rotation between the wheels. For this reason, the position of the pivot point is used for the single-point representation of Page 42 of 124

43 5.1 Representation of Items on the Field A D Robot Safety margin Calc margin Safety margin Calc margin B E Invalid waypoint C Obstacle Waypoint H F G Cup Figure 5.1: Example of a planned path to a cup. Two obstacles are placed on the field and their valid and invalid waypoints are included in the figure along with safety margins and calc margins. r max + Pivot point Figure 5.2: Robot top view. The distance r max is the distance from the pivot point to the remotest point of the robot. the robot. As shown in Figure 5.2, r max is the distance from the pivot point to the remotest point of the robot. According to requirement IMG2, items on the field must be determined with a precision of at least ±13.2 mm. Hereby, the distance a safety margin must extend an obstacle can be calculated. The result is shown in Equation (5.1). d safe = r max + d precision d safe = 190 mm + 13 mm d safe = 203 mm (5.1) where: d safe is the distance a safety margin must extend an obstacle [m]. r max is the radius from the pivot point of the robot to the remotest point of the robot [m]. d precision is the precision of the Image Processing [m]. Extending an obstacle with a safety margin is shown in Figure 5.1 and in Figure 5.3. Page 43 of 124

44 Route Planner Waypoints and Calc Margins In Figure 5.3 waypoints and calc margins are inserted. It is shown how a safety margin extends the obstacle and how a calc margin extends the safety margin. Outmost, waypoints are located. d calc d safe Obstacle Figure 5.3: Shows how a safety margin is inserted around an obstacle. A calc margin extends the safety margin, and waypoints are inserted at each corner of the obstacle; just outside the calc margin. If a path between two waypoints intersects a calc margin, this path is considered invalid by the planner. Therefore, waypoints are not placed on the top of calc margins, since a path must be realizable between two consecutively waypoints of an obstacle. For instance, in Figure 5.1, both F and H are visible to G. When implementing Route Planner in software, waypoints are located as close to calc margins as possible, as shown in Figure 5.3. Calc margin is based on the system delay and the speed of the robot. From Section 1.2, page 13, the maximum speed of the robot is determined to be 0.45 m/s. However, it has been chosen by the project group only to apply an operating speed of 0.20 m/s. Section 9.2 on page 78 shows that the system delay is typically 340 ms. Hereby, d calc can be determined. The result is shown in Equation (5.2). d calc = d precision + v op t delay d calc = 13 mm m/s 340 ms d calc = 81 mm (5.2) where: d calc is the distance a calc margin must extend a safety margin [m]. v op is the operational velocity of the robot [m/s]. t delay is the system delay [s]. The margin specified in Equation (5.2) applies for a worst-case situation for the typical system delay found in Section 9.2. Taking the maximum system delay into consideration, d calc must be increased to guarantee the operation of the system in worst-case scenario. However, during all tests performed d calc was set to 47 mm; an even smaller distance than specified in Equation (5.2). The distance was calculated based only on the delay of Image Processing; even though, the distance turned out to be sufficient in the situations tested. In the tests performed in Chapter 9, page 75, the robot did never collide with obstacles. The next section describes which conditions are used for determining a target. 5.2 Strategic Planner As specified in requirement CTRL2, page 19, when several cups are possible targets the path to be calculated must be the one for which an estimate of the traveled distance is shortest. A situation is shown in Figure 5.4. Page 44 of 124

45 5.3 Path Planner Cup 2 Robot Cup 1 Figure 5.4: Two possible targets are shown. The target to be selected is the one for which an estimate of the traveled distance is shortest. Cup 2 is nearest the robot as the crow flies, but since an estimate of the traveled distance is shortest to Cup 1, this cup is to be selected as target. The distance in straight lines through waypoints is used as estimate for the traveled distance. No guarantee can be given about traveling the possible shortest path due to the fact that Robot Movement Decider described in Section 7.2, page 58, creates the trajectory of the robot, accordingly to the curve obtained from Path Planner. Hereby, a different distance traveled might be obtained. The exact traveled distance can never be obtained in simulation, because movements of the robot are not only based on results from Robot Simulator but also on camera inputs. Though, the distance in straight lines through waypoints is a good estimate. The implementation of Strategic Planner is based on iterative calls of Path Planner. If Strategic Planner detects that a cup is located within a calc margin, permanently, it is redefined as being an obstacle. So far it has been described how Strategic Planner selects a cup as target. Beyond that, there can be three reasons to select the base as target instead. These are as follows. The battery voltage level of the robot is critically low. The robot is holding the maximum number of cups permitted. No cups are reachable to the robot. Having selected a target from the cups, the next section describes how a path is planned to the target. 5.3 Path Planner The purpose of Path Planner is to plan a route from the initial robot position to the location of the target. The target has been chosen by Strategic Planner; the route must go through valid waypoints. The shortest path from the robot to the target can be found by Dijkstra s Algorithm. The path must pass through waypoints linked by straight lines. Dijkstra s algorithm solves the problem of finding the shortest path from a source to a destination. The algorithm can find the shortest paths from a given source (robot) to all waypoints (one of these is the target) [Morris, 1998]. The algorithm is able to find the shortest paths to all waypoints, but only the path to the cup selected as target is useful, since all other cups are considered to be obstacles and these cups are not possible for the robot to reach. Page 45 of 124

46 Route Planner Dijkstra s Algorithm The initialization of Dijkstra s algorithm is presented before a pseudo-code description is set up. The algorithm makes use of the following lists; containing waypoints. slist: Contains the waypoints for which the shortest path from the source is known. This list is empty at the beginning. elist: Contains the waypoints for which the shortest path to the source is not yet known. At the beginning, all waypoints belong to this list. To be able to understand the algorithm presented in pseudo-code the following must be assumed. Array D contains the minimum distance from the source to all other waypoints. Array L contains the previous waypoint along the shortest path from source to all other waypoints. distance(u,v) is the distance from waypoint u to waypoint v. At the beginning, the distances in D from the source (robot) to each waypoint are set to infinity to ensure that a path shorter to the robot can be found. To indicate that the robot is the first waypoint on the route, the distance to the robot is set to zero to ensure that this waypoint is the first to be moved to slist. Then Dijkstra s Algorithm can be set up as seen below [Detmer, 2006]: while(elist is not empty) { choose a waypoint, u, from elist such that D[u] is minimum; if(d[u] == ) { break; //No remaining waypoints are reachable. } add u to slist and delete it from elist; for(each waypoint, v, in elist visible to u) { c = D[u] + distance(u,v); //The distance to waypoint v through waypoint u. if(c < D[v]) { L[v] = u; //u is the previous waypoint along the shortest path to waypoint v. D[v] = c; //The distance to waypoint v is updated. } } } Dijkstra s Algorithm Example To substantiate the explanation of Dijkstra s Algorithm an example is given. The starting point is taken in Figure 5.1, page 43. From here, valid waypoints are drawn together with distances between waypoints visible to each other. This is shown in Figure 5.5. In the following description, [] denotes an array. In the notation W(d,P), W is the waypoint for which d is the distance to source and P is the previous waypoint along the shortest path from source to the waypoint. 1. At the beginning: slist = [] elist = [Robot(0, -), B(,?), F(,?), G(,?), H(,?), Cup(,?)] Robot is the first waypoint, so this has the distance zero to itself. The distances to Robot for all other waypoints are set to to ensure that Robot is chosen. As this is the case, Robot is moved to slist and the distances from Robot to the waypoints visible to Robot are updated. 2. Then: slist = [Robot(0, -)] elist = [B(100, Robot), F(,?), G(,?), H(,?), Cup(,?)] Now B is the waypoint in elist that possess the smallest distance to Robot. Therefore, B is moved to slist and waypoints visible to B are updated. Page 46 of 124

47 5.3 Path Planner Robot Out of scale 100 B H 40 F G 20 Cup Figure 5.5: Follow-up of the example in Figure 5.1. The figure is simplified and only valid waypoints are shown. The distances between waypoints visible to each other are added. 3. Then: slist = [Robot(0, -), B(100, Robot)] elist = [F(140, B), G(,?), H(,?), Cup(,?)] F is moved to slist and G and Cup are updated since they are visible to F. 4. Then: slist = [Robot(0, -), B(100, Robot), F(140, B)] elist = [G(220, F), H(,?), Cup(210, F)] Cup is moved to slist. In this situation G is not updated since the distance to Robot through Cup is not shorter than Then: slist = [Robot(0, -), B(100, Robot), F(140, B), Cup(210, F)] elist = [G(220, F), H(,?)] G is moved to slist and H is updated since it is visible to G. 6. Then: slist = [Robot(0, -), B(100, Robot), F(140, B), Cup(210, F), G(220, F)] elist = [H(260, G)] H is moved to slist. 7. Finally: slist = [Robot(0, -), B(100, Robot), F(140, B), Cup(210, F), G(220, F), H(260, G)] elist = [] elist is empty and slist contains all waypoints sorted with regard to minimum distance to Robot. It appears that Cup is found in slist and hereby it is reachable for the robot. It also emerges that the previous waypoint along the shortest path from Robot to Cup is F. For waypoint F it appears that the previous waypoint along the shortest path from Robot to F is B, and from B the previous waypoint is Robot. Hereby, the shortest path to the target through waypoints linked by straight lines is found by Dijkstra s Algorithm. This path is shown in Figure 5.5 and passes through Robot, B, F, and Cup. In this chapter it is given that the robot is represented by a single-point representation. To be able to represent the robot only by a single point, margins are inserted around cups and obstacles. At the exterior of these rectangular margins, waypoints are inserted. Path Planner rests on Dijkstra s Algorithm and is able to calculate a route, through waypoints, from the robot to the nearest target candidate. Strategic Planner uses Path Planner, and is able to select the nearest cup as target. However, the base is selected as target if no cups are reachable to the robot or if the robot holds the maximum number of cups allowed or the battery voltage is below a certain level. Page 47 of 124

48 Route Planner A test of Route Planner is performed in Section 9.3 on page 85. The test shows that when several cups are possible targets, the path selected is the one for which an estimate of the traveled distance is shortest. To be able to maneuver the robot according to the route planned, a model of the robot must be set up. This is done in the next chapter. Page 48 of 124

49 6 Robot Model The purpose of this chapter is to set up a model of the motors, a model of the transmission from the motors to the wheels, and the kinematic model of the robot. These models have to be set up to make it possible for Actuator Translator and Robot Movement Simulator to model the robot. Firstly, the motors, and the robot applied as a load on these, is modeled. Secondly, a kinematic model is set up. 6.1 Modeling the Motors The objective of this section is to set up a motor model of the motors used in this project. The motors used in this project are the motors, which are included in the 8527 Mindstorms NXT Kit. Furthermore, a model of the load applied on to the motors has to be made. When the two models are put together it is possible to determine the velocity of the robot when different voltages are applied to the motors. Electric Model of the Motor The motors used in this project are DC motors with permanent magnets. An equivalent circuit of the electronics in a DC motor with a permanent magnet can be set up as shown in Figure 6.1 [Close et al., 2002, p. 348]. Figure 6.1: Equivalent circuit of a DC motor with permanent magnet. An equation describing the circuit shown in Figure 6.1 is set up in Equation (6.1). v a = i a R a + L a di a dt + 2V brush + e m [V] (6.1) where: v a is the voltage applied to the terminals of the motor [V]. i a is the current running through the motor [A]. di a dt is the first derivative of i a [A/s]. R a is the internal resistance of the motor [Ω]. L a is the internal inductance of the motor [H]. V brush is the voltage drop over a brush [V]. e m is the induced voltage of the motor [V]. The brush voltage, V brush, has been measured as shown in Appendix B, page 103. The test shows that V brush equals mv. Therefore, the brush voltage is negligible and will be disregarded in the motor model. Page 49 of 124

50 Robot Model The LEGO NXT DC motor has a permanent magnet, which means that it has a constant magnetic field. This implies that the induced voltage of the motor, e m, can be expressed as shown in Equation (6.2) [Close et al., 2002, p. 349]. where: k e is the electric motor constant [V s]. ω rotor is the angular velocity of the motor shaft [rad/s]. e m = k e ω rotor [V] (6.2) By inserting Equation (6.2) into Equation (6.1) a new expression of v a, dependent of the angular velocity of the rotor, can be set up. The expression is shown in Equation (6.3). Mechanical Model of the Motor v a = i a R a + L a di a dt + k eω rotor [V] (6.3) The mechanical part of the DC motor can be modeled as shown in Figure 6.2 [Close et al., 2002, p. 348]. The figure shows the rotor of the DC motor, the friction, and the moment of inertia, which influences the rotation of the rotor. This gives some torques pointing in the opposite direction of the torque, T e, produced by the motor. The equations are set up for positive angular velocities. An equation describing the mechanical part of the DC motor can be expressed as shown in Equation (6.4). Figure 6.2: Diagram of the mechanics of a DC motor with permanent magnet. T e = T friction + T inertia + T load [Nm] (6.4) where: T e is the electric torque produced by the motor [Nm]. T friction is the torque caused by friction [Nm]. T inertia is the torque caused by the moment of inertia [Nm]. T load is the torque caused by the load [Nm]. Because the magnetic field is constant, the electrical torque can be written as shown in Equation (6.5). where: k T is the mechanical motor constant [V s]. T e = k T i a [Nm] (6.5) From Equation (6.4) and Equation (6.5) a new equation describing the mechanics of the motor can be set up as shown in Equation (6.6). The motor is a permanent magnet DC motor, which Page 50 of 124

51 6.1 Modeling the Motors implies that k T equals k e and is constant [Ritchie, 2006, p. 18]. dω rotor k T i a = (B rotor ω rotor + A rotor ) + J rotor + T load dt dω rotor 1 = (k T i a B rotor ω rotor A rotor T load ) dt J rotor [rad/s 2 ] (6.6) where: J rotor is the moment of inertia of the rotor [kg m 2 ]. B rotor is the viscous friction of the rotor [Nm s]. A rotor is the dry friction of the rotor [Nm]. dω rotor dt is the angular acceleration of the rotor [rad/s 2 ]. Model of the Load It is assumed that the shaft connecting the motor to the load is totally stiff, which means that the angular velocity of the motor equals the angular velocity of the load. The load applied to the two motors consists of the moment of inertia of the two wheels, the torque caused by the mass of the robot, and the friction of the back supporter of the robot. The load applied, by the wheels, to the two motors can be expressed as shown in Equation (6.7), when both motors drive at the same angular velocity. T load,total = T mass + T inertia + T friction T load,total = m robot rwheel 2 dω rotor dω rotor + 2J wheel + (B supporter ω rotor + A supporter ) [Nm] (6.7) dt dt where: T load,total is the total torque caused by the load [Nm]. T mass is the torque caused by the mass of the robot [Nm]. T inertia is the torque caused by the moment of inertia of the two wheels of the robot [Nm]. T friction is the torque caused by the friction of the back supporter [Nm]. J wheel is the moment of inertia of one wheel [kg m 2 ]. r wheel is the radius of the wheel [m]. m robot is the mass of the robot [kg]. B supporter is the viscous friction of the back supporter [Nm s]. A supporter is the dry friction of the back supporter [Nm]. The moment of inertia of a wheel is equal for both the right and left wheel. Each wheel is assumed to have a uniform distribution of mass and to be shaped as a disk. Then the moment of inertia for a disk can be expressed as shown in Equation (6.8) [Close et al., 2002, p. 97]. The mass of each wheel is measured to be 17.5 g and the diameter of a wheel is 55 mm, which gives the moment of inertia shown in Equation (6.9). J cyl = 1 2 mr2 [kg m 2 ] (6.8) where: m wheel is the mass of the wheel [kg]. J wheel = 1 2 m wheelr 2 wheel J wheel = kg m 2 (6.9) Since the mass of the robot equals kg, as shown in Table 1.1, page 15, the torque caused by the mass can be calculated from Newton s 2nd law as shown in Equation (6.10). T mass = m robot rwheel 2 dω rotor dt T mass = kg m 2 dω rotor dt [Nm] (6.10) Page 51 of 124

52 Robot Model The torque caused by the friction of the back supporter of the robot is found as shown in Appendix B.2, page 109, and consists of a dry friction and a viscous friction. The distribution of the friction on the two wheels is not equal at all times. The distribution is dependent on the velocity difference between the two wheels. Two equations describing the distribution of T friction to each wheel can be set up as shown in Equation (6.11) and Equation (6.12). The two equations determines how much of the total friction of the back supporter that is applied to each of the two wheels, when the wheels turn at a certain speed. v right T friction,right = v right + v left T friction 1 T friction,right = T 1 + v friction [Nm] (6.11) left v right v left T friction,left = v right + v left T friction 1 T friction,left = T 1 + v friction [Nm] (6.12) right v left where: T friction,right is the torque caused by friction of the back supporter on the right wheel [Nm]. T friction,left is the torque caused by friction of the back supporter on the left wheel [Nm]. By inserting Equation (6.7) and Equation (6.12) into Equation (6.6), and assuming that the left wheel must accelerate half the mass of the robot, a new equation describing dωrotor dt for the left wheel can be set up as shown in Equation (6.13). Note that 2J wheel is reduced to J wheel, because only one wheel is included in this equation. dω rotor dt dω rotor dt = (k T i a B rotor ω rotor A rotor T load,left ) ( k T i a A rotor Asupporter ω 1+ v right rotor v left = J rotor m robotr 2 wheel + J wheel 1 J rotor B rotor + Bsupporter 1+ v right v left ) [rad/s 2 ] (6.13) where: T load,left is the torque applied to the left motor by the load [Nm]. To simplify the expression, the constant J rotor m robotr 2 wheel + J wheel is denoted c. State Space Representation of Loaded Motor Model By using the equations set up in the modeling of the motor, a state space formulation for each wheel can be set up. The result is shown in Equation (6.14) and Equation (6.15). [ ] dia dt dω rotor = dt R a L a k Tc 1 c ( k e L a B rotor + Bsupporter 1+ v left v right [ ) i a ω rotor ] + 1 c ( v a,right L a A supporter 1+ v left v right ) (6.14) + A rotor [ dia ] dt dω rotor = dt R a L a k Tc 1 c ( k e L a B rotor + Bsupporter 1+ v right v left ) ] + ω rotor [ ia 1 c ( v a,left L a A supporter 1+ v right v left ) (6.15) + A rotor With the robot applied as a load, a model of the motors has been set up describing the relation between the velocities of the two wheels and the currents and voltages applied to the two motors. A Simulink model of the robot can be found in [Robot_Model/]. The next section describes the kinematics of the robot, necessary to describe the movements of the robot. Page 52 of 124

53 6.2 Kinematic Model of the Robot 6.2 Kinematic Model of the Robot The purpose of this section is to set up a kinematic model of the robot to be able to determine the change in position and angle of the robot, when the wheels of the robot turn with constant but different angular velocities. From this point, it is possible to calculate the new position and angle of the robot after a given time. The kinematic model of the robot is used in Robot Movement Simulator. Thus, a model is set up that determines the kinematics of the robot from its recent position, the sample time, the velocity ratio between the two wheels, and the speed of the right wheel. It is assumed that the wheels of the robot do not slip. The robot is a two-driving wheel robot. Therefore, the pivot point and the angle between the center line of the robot and the x-axis are used to represent the position and direction of the robot as described in Section 5.1, page 43. This is shown in Figure 6.3. Note that the angle of the robot is Figure 6.3: The robot placed in a coordinate system. The angle of the robot and the point between the two wheels on the center axis is also shown in the figure. measured from the back to the front of the robot. The wheel base is marked in the figure, because it is a factor, when determining the kinematic model of the robot. The wheels of the robot are considered being infinitely thin. The wheel base equals the distance from the middle of the right wheel to the middle of the left wheel. When the wheels turn with constant angular velocities, the robot moves in a circular motion. It is the case in this project, since the sampling frequency of the system is limited and considerable time passes between each sampling. The angular velocities of the motors are unchanged in these periods. Hereby, the robot moves in a series of sections of circles, when different velocity ratios are applied to the two wheels. For this reason, a model of the robot is developed, determining the circular movements of the robot, when the two wheels turn with constant angular velocities. The robot can either drive in a clockwise or counter-clockwise motion. The counter-clockwise movement of the robot is illustrated in Figure 6.4. Due to simplification, the robot is only represented by the two wheels and a line between these. The angular change of the robot, θ robot, is shown in Figure 6.4 and is calculated using a sector of a circle. The distance on a circle arc to a given angle can be calculated as shown in Equation (6.16). where: d sector is the length of the sector of the circle [m]. θ sector is the angle of the sector of the circle [rad]. r sector is the radius of the circle [m]. θ sector = d sector r sector [rad] (6.16) Equation (6.16) is used to calculate θ robot, where the length of the sector is v right t and the radius of the sector is the distance from (x center ; y center ) to the right wheel, which gives the angle Page 53 of 124

54 Robot Model Figure 6.4: The robot moving counter-clockwise with a circular movement of radius r. When determining the direction of the robot consider it as a vector pointing towards the front end. The angle, θ robot, is then the angle between this vector and the x-axis. θ robot. The result is obtained from Figure 6.4 and shown in Equation (6.17). θ robot = v right t r + d wb 2 [rad] (6.17) where: t is the time that the robot has moved [s]. θ robot is the angular change of the robot [rad]. r is the radius of the circle that the robot follows [m]. d wb is the wheel base of the robot [m]. From θ robot, the new angle of the robot can be calculated as shown in Equation (6.18). where: θ robot,new is the new angle of the robot [rad]. θ robot is the old angle of the robot [rad]. θ robot,new = θ robot + θ robot [rad] (6.18) The velocity ratio of the two wheels is defined in Equation (6.19). where: v ratio is the velocity ratio [ ]. v right is the velocity of the right wheel [m/s]. v left is the velocity of the left wheel [m/s]. v ratio = v right v left [ ] (6.19) Page 54 of 124

55 6.2 Kinematic Model of the Robot It is clear that an equation similar to Equation (6.17) can be set up using v left instead of v right. Having these two versions of Equation (6.17), v ratio can be calculated as shown in Equation (6.20). v right t r + d wb 2 = v left t r d wb 2 v ratio = r + d wb 2 r d wb 2 [ ] (6.20) The velocity ratio is used to calculate the radius of the circle which the robot follows. The result is shown in Equation (6.21). r = d wb 2 vratio + 1 v ratio 1 [m] (6.21) The robot can either drive clockwise or counter-clockwise. To ensure that the coordinates of the robot moves in the same direction as the robot drives, r is negative when the robot drives clockwise. Hereby, also θ robot is negative when the robot drives clockwise. To calculate the new coordinates of the robot, the coordinates of the previous position of the robot must be rotated around (x center ; y center ). This is done by moving (x center ; y center ) to the origin. Then, the robot is rotated θ robot around the robot (0;0). To obtain the correct coordinates, the robot is moved back again. The result using a rotational matrix is shown in Equation (6.22). robot new = R(robot center) + center [m] (6.22) where: robot new is the new position of the robot [m]. R is the rotational matrix [ ]. center is the center of the circle, which the robot follows [m]. robot is the previous position of the robot [m]. By rewriting Equation (6.22), each coordinate of the new position can be calculated as shown in Equation (6.23). [ ] [ ] [ ] [ ] xrobot,new cos( θrobot ) sin( θ = robot ) xrobot x center xcenter + (6.23) y robot,new sin( θ robot ) cos( θ robot ) y robot y center y center In this chapter a kinematic model has been set up for the two driving wheel robot. The robot is represented by a point and an angle and the model can determine a new position and angle of the robot, when the two wheels drive with constant velocities. By combining the kinematic model and the loaded motor model, a new position and angle of the robot can be calculated, when different voltages are applied to the motors of the robot. In the next chapter movements of the robot is set up so that the robot is able to follow the route planned by Route Planner. Page 55 of 124

56 Robot Movement 7 Robot Movement The objective of this chapter is to design the regulator for the feedback loop of Control Center which controls the movement of the robot. This implies making the robot follow the route planned by Route Planner, described in Chapter 5, page 42, by combing the data from Image Processing and data from a simulation based on the models set up in Chapter Robot Location Estimator Robot Location Estimator estimates the location and direction of the robot by evaluating coordinates from both Image Processing and from Robot Movement Simulator. By having two sources, more information is available and a better estimate can be obtained as concluded in Appendix C on page 112. Inputs from Image Processing and Robot Movement Simulator serve as the basis for the estimate of the location and direction of the robot. The estimate is obtained using Kalman filtering, which can be used to give an estimate of a correct value based on two measurements with known precisions. Kalman filtering is described in Appendix C on page 112. The robot is represented by its pivot point and its direction (x robot, y robot, θ robot ). An estimate for each of these values is found by Kalman filtering the two input signals. Note that these values, x est, y est, and θ est, are calculated separately. By using Equation (C.3) on page 113, Equation (7.1) can be set up. Equation (7.1) shows how an estimate for the x-value of the robot is calculated. where: x est is the estimated x-coordinate of the robot [m]. x img is the x-coordinate based on camera inputs [m]. x sim is the simulated x-coordinate of the robot [m]. G is the Kalman gain defined in Equation (7.2) [ ]. x est = x img + G (x sim x img ) [m] (7.1) σ 2 img G = σsim 2 + σ2 img [ ] (7.2) where: σ 2 img is the variance of data from Image Processing [m2 ]. σ 2 sim is the variance of data from the simulator [m2 ]. Similar to Equation (7.1), y est and θ est are calculated, also with the Kalman gain defined in Equation (7.2). Weighting of Data from Image Processing and Simulator Based on the behavior of some tests performed, the project group has chosen to weight data from Image Processing and Robot Movement Simulator equally as shown in Equation (7.3). σ 2 img = σ 2 sim [m 2 ] (7.3) Notice that since the unit of variance is the square of the unit of observation, the variances, used for estimating the direction of the robot, will have the unit rad 2. Neither data from Image Processing or Robot Movement Simulator matches exactly what the robot actually does. Inaccuracies in Image Processing exist due to errors in coordinate conversion Page 56 of 124

57 7.1 Robot Location Estimator and incorrect locations of bounding boxes. Besides, data is delayed in Image Processing and only reach Control Center after some time. The delay is found in Section 9.1, page 75, to be maximum 131 ms. However, the delay does not have a fixed size and thereby it is not included. This causes an error in the result because no delay is assumed by the method. The data obtained from Robot Movement Simulator is based on a model of the robot and will not fit perfectly to the behaviors of the robot in all situations. As an example, the robot may pull a little to one side even if it is set to drive straight forward, and may skid when turning. This can be the result of a not-perfect design and/or an irregular surface; the model does not take this into account. Therefore, errors may build up as time passes. To minimize the influence from these errors, the simulation is reset to the estimated position and direction after each sample period. Actually, this means that data from Image Processing is dominant in the estimation. Only to a given limit, data from the simulation can affect the estimated values. This limit depends on the variances, and of the changes in data from Image Processing and Robot Movement Simulator in between two samples. Synhronization between Data from Image Processing and Simulator For the system to be able to operate properly, the deviation between data from Image Processing and simulation must not be too large. Based on the behavior of some tests performed, the following constraints are set up: The difference in the location of the robot stated by Image Processing and Robot Movement Simulator must not be too large. The distance between the two points must not exceed 50 mm. The difference in the directions of the robot stated by Image Processing and Robot Movement Simulator must not be too large. The difference in the angles must not exceed 20. If the difference between data from Image Processing and data from simulation becomes too large, the robot is simply stopped. After the robot is stopped, Control Center waits for a while to be able to eliminate the importance of the delay in Image Processing. When the delay has elapsed, data from Image Processing holds information about the robot in its stationary location; i.e. where it was stopped. Since the location and direction of the robot is now known, a new sequence is started by planning a new route. Every time a new route is planned, the location and direction of the robot is only based on data from Image Processing. Invalid Data from Image Processing It might happen that certain frames from Image Processing hold incorrect information. These must be categorized as invalid and must be rejected. As an example, if the large front marker of the robot is suddenly recognized to be smaller than the smaller back marker, it will look like a 180 degree sudden rotation of the robot. Also, if some noise in the image is confused with the markers of the robot, it may look like the robot suddenly jumps to the opposite end of the field. This invalid information must be rejected. Therefore, new coordinates from Image Processing continuously is checked against the previous data received; expressed in the following pseudo formulation. img[i] img[i-1] + delta_simulation[i] where: img[i] is the new data from Image Processing. img[i-1] is the previous data from Image Processing. delta_simulation[i] is the change from the previous to the new point in time; calculated by the simulator. If new data from Image Processing claims that the robot has move further than 50 mm away from the position approximated in the pseudo formulation, the data is disregarded. The same happens if the new angle differs more than 20 from the approximated angle. Page 57 of 124

58 Robot Movement If invalid data arrives from Image Processing, the new estimate is calculated based on data from simulation and the approximation for the new data from Image Processing. If invalid data is received five successive times, the robot is immediately stopped; just as described in the previous section. Having estimated a position and direction of the robot, the next section describes which trajectory the robot has to drive to follow the route planned by Route Planner. 7.2 Robot Movement Decider The purpose of this section is to describe the functionality of Robot Movement Decider. Robot Movement Decider decides which trajectory the robot has to drive to follow the path planned by Route Planner; without colliding with any obstacles. It is desirable to let the robot drive in a trajectory which has no sudden changes in direction; otherwise the robot is forced to make large decelerations and accelerations. Hereby, it is necessary to follow another route than the one planned by Route Planner; since this route is just a set of straight lines connected. Moving in straight lines would give large decelerations and accelerations of the robot because of discontinuities in the route. As described in Chapter 6, page 49, the robot moves in circular motions, and hereby it is preferable to let the robot move in circles. To plan a trajectory, which ensures that the robot does not deviate too much from the path planned by Route Planner, the approach chosen is to let the robot drive towards a point at a certain distance from the robot. This point must lie on the line spanned by the waypoints. This regulating technique makes a comprehensive correction when the robot is deviating much from the line, while only a small correction is made when the robot is close to the line. This happens because of the changes in the radius of the circle, which the robot must follow. This circle must have the direction of the robot as tangent to ensure that no abrupt changes in the direction of the robot occur. The process of constructing the circle for which the robot must follow is divided into eight steps. These steps are explained separately in the following text. 1. Find the equation of line a, which is the line between the two waypoints. An equation of line a is found to be able to find points on the line, which the robot must follow. Line a together with a representation of the robot with an angle θ robot, and coordinates (x robot ; y robot ) are shown in Figure 7.1. Figure 7.1: Line between waypoints i and i + 1 called a. 2. Find the intersection point between line a and a circle with radius r with center in the single-point representation of the robot. The intersection point must be the one nearest to the end waypoint. The robot must drive towards the intersection point (x intersect ; y intersect ) shown in Figure 7.2. The circle is constructed using a fixed radius r, describing the distance that the robot looks ahead for line a. The circle says nothing about how the robot drives to the intersection point. Page 58 of 124

59 7.2 Robot Movement Decider Figure 7.2: Circle with radius r intersects line a. The robot must drive towards the intersection point closest to waypoint i Find the midpoint of the line b that passes through (x robot ; y robot ) and (x intersect ; y intersect ). The midpoint (x mid ; y mid ) shown in Figure 7.3 has the same distance to both the intersection point and to the robot. This is necessary if a circle has to intersect the two points. Figure 7.3: Line with midpoint (x mid ; y mid ) between intersection point and the point representing the robot. 4. Find the equation of line c, which is perpendicular to line b and intersects (x mid ; y mid ). A circle which intersects both (x robot ; y robot ) and (x intersect ; y intersect ) must have a center with the same distance to both points. Every point on line c has the same distance to (x robot ; y robot ) and (x intersect ; y intersect ). Therefore, the circle, which the robot must follow, has to have center on line c. Line c is shown in Figure 7.4. Figure 7.4: A circle with center on the line is able to intersect both (x intersect ; y intersect ) and (x robot ; y robot ). Page 59 of 124

60 Robot Movement 5. Find the equation of the line d perpendicular to θ robot, intersecting (x robot ; y robot ). To make the robot follow a circle, this circle has to have θ robot as tangent. Therefore, a line d is drawn, as shown in Figure 7.5. Figure 7.5: Line perpendicular to θ robot, insuring that the circle has θ robot as tangent. 6. Find center of the circle, which the robot must follow. The point where line c and line d intersects must be the center of the circle. The intersection point (x center ; y center ) is shown in Figure 7.6. Figure 7.6: Center of the circle which the robot must follow. 7. Calculate radius of the circle, which the robot must follow. The distance between (x robot ; y robot ) and (x center ; y center ) is the radius of the circle, which the robot must follow. When this radius is known, the circle can be drawn as shown in Figure 7.7. Figure 7.7: Circle which the robot must follow. 8. Calculate the velocity ratio between the two motors, which makes the robot follow the circle. The velocity ratio can be calculated by using Equation (6.21) on page 55. In this equation, r is positive when the robot drives counter-clockwise and negative when the robot drives clockwise. The Page 60 of 124

61 7.2 Robot Movement Decider expression to calculate the velocity ratio is shown in Equation (7.4). v ratio = r + d wb 2 r d wb 2 [ ] (7.4) where: v ratio is the velocity ratio of the two wheels of the robot [ ]. d wb is the wheel base of the robot [m]. To be able to determine the direction of the robot, Robot Movement Decider must return the velocity of one of the wheels. By the project group it is chosen to return the velocity of the right wheel. It is necessary to specify the velocity of the right wheel to make the robot drive at different speeds. Besides, the velocity of the wheel must be known to determine the direction of the robot; e.g. for v ratio = 1 the robot can either drive straight forwards or straight backwards, if nothing else is stated. There are a few special cases where Robot Movement Decider divagates from the equation explained above. These cases are described in the following list. When the angle of the robot almost equals the angle of line b, the radius of the circle followed would go towards infinity. In this situation it has been decided to make the robot follow a straight line, as shown in Figure 7.8. When the angle between line b and θ robot is less than 5, Robot Movement Decider must return the velocity ratio 1 and a velocity of the right wheel; no radius must be calculated. Figure 7.8: The robot must drive in a straight line, if the angle becomes less than 5. When the angle between θ robot and line a is greater than 115, it is decided by the project group that the robot must turn around its pivot point until it is pointing towards the end waypoint. The situation is shown in Figure 7.9. It means that Robot Movement Decider must return the velocity ratio -1, and a velocity of the right wheel. Figure 7.9: The robot must turn around its pivot point, if the angle becomes greater than 115. Page 61 of 124

62 Robot Movement In this section an equation for the velocity ratio between the two wheels of the robot is set up. In the next section, the velocity ratio and the velocity of the right wheel are translated into numbers for the LEGO commands, which can be send to the robot. 7.3 Actuator Translator The purpose of Actuator Translator is to convert the velocity ratio and the velocity of the right wheel into numbers for the LEGO command, which can be send to the robot. Providing this, the model of the robot must be taken into account. A model of the DC motors has been set up in Chapter 6. The model is describing the dynamics of the robot when different voltages are applied to the motors at a given initial velocity. The dynamics of the motors implies that two differential equations occur, which are difficult to implement in the used programming language. Therefore, some assumptions are made to simplify the model of the robot. To simplify the model it is assumed that each wheel of the robot drives with a constant velocity in every sampling period. To show that it is acceptable to assume a constant velocity in an entire sampling period, a simulation of the acceleration of the robot is performed. The result is shown in Figure The figure shows the speed of the robot during an acceleration from rest to 0.2 m/s, when the battery voltage is 8 V, and at a Power set point of 56. Figure 7.10: The black line rising from rest to 0.2 m/s shows the simulated speed of the robot. The red line shows the derivative of i a going towards 0 A/s. The simulation shows that the maximum time of acceleration is about 300 ms. The robot will only make an acceleration this big once per job. The error made by assuming constant velocity, and an acceleration time of 0 ms, is shown in Figure According to Figure 7.11, the error made is less than 10 mm. Therefore, it is assumed that the robot drives with a constant velocity in all sampling periods. This implies that dωrotor dt and dia dt equals zero in Equation (6.14) and Equation (6.15), page 52. Hence, the two state space formulations of Page 62 of 124

63 7.3 Actuator Translator Figure 7.11: Error made by simplifying the model of the robot by assuming constant velocity. the motors can be rewritten to the results shown in Equation (7.5) and Equation (7.6). [ ] [ Ra ] [ ] [ k 0 e v a,right ] L = a ( L a ) i a k 0 Tc 1 + ( L a ) c B rotor + Bsupporter 1 Asupporter 1+ v ratio ω 1 rotor c 1+ v ratio + A 1 rotor (( kt k e v a,right = + B rotor + B ) supporter R a 1 + v ratio 1 ω rotor + A ) supporter 1 + v ratio 1 + A Ra rotor [V] (7.5) k T [ ] [ Ra ] [ ] [ k 0 e v a,left ] L = a ( L a ) i a k 0 Tc 1 + ( L a ) c B rotor + Bsupporter 1 Asupporter 1+ v ratio ω rotor c 1+ v + A ratio rotor (( kt k e v a,left = + B rotor + B ) supporter ω rotor + A ) supporter 1 + v ratio 1 + v ratio + A Ra rotor [V] (7.6) k T R a To calculate v a, a relation between the Power set point, in the LEGO commands, and v a must be determined. The voltage applied to a motor of the robot is a square wave. The duty cycle of the signal applied to a motor has been measured for different Power set points as shown in Appendix B on page 111. From this it is clear that the duty cycle equals Power set point. Therefore, Equation (7.5) and Equation (7.6) can be rewritten to determine the Power set point when the battery voltage is inserted. The results are shown in Equation (7.7) and Equation (7.8). PSP right = (( kt k e + B rotor + B supporter R a 1 + v ratio 1 ) Ra ) vright r wheel + A supporter 1 + v ratio 1 + A 100 rotor [ ] (7.7) k T V bat (( kt k e PSP left = + B rotor + B ) supporter vleft 1 + v ratio R a + A supporter 1 + v ratio + A rotor ) Ra where: V bat is the battery voltage [V]. PSP right is the power set point for the right wheel [ ]. PSP left is the power set point for the left wheel [ ]. r wheel k T 100 V bat [ ] (7.8) Notice that the robot can have a positive or negative velocity. Therefore, the sign of the velocity is used during implementation to obtain a negative Power set point. This requires Equation (7.7) Page 63 of 124

64 Robot Movement and Equation (7.8) to be rewritten as shown in Equation (7.9) and Equation (7.10). (( kt k e PSP right = + B rotor + B ) supporter vright R a 1 + v ratio 1 ) Ra r wheel + A supporter 1 + v ratio 1 + A 100 v right rotor [ ] (7.9) k T V bat v right (( kt k e PSP left = + B rotor + B ) supporter vleft R a 1 + v ratio r wheel + A ) supporter 1 + v ratio + A Ra 100 v left rotor [ ] (7.10) k T V bat v left It is not always possible to make the robot drive at a desired velocity, because the batteries cannot always deliver the voltage required. Therefore, a Power set point has to be calculated and limited to 100, even if it is calculated to be higher. Thereafter, Turn Ratio has to be calculated. Turn Ratio can be calculated from the measurement results shown in Appendix B on page 111. When the left wheel is turning at a greater speed than the right wheel, Turn Ratio must be calculated as specified in Equation (7.11), else Turn Ratio must be calculated as specified in Equation (7.12). Turn Ratio = (v ratio 1) ( 50) [ ] (7.11) ( ) 1 Turn Ratio = 1 50 [ ] (7.12) v ratio Equations to determine the Power set point and Turn Ratio in the LEGO command for controlling the robot have been set up. This means that the velocity ratio and velocity of the right wheel calculated by Robot Movement Decider have been translated into numbers for the LEGO command, which can be send to the robot. To determine where the robot is located at each sample time, Robot Movement Simulator is needed, and will therefore be set up in the forthcoming section. 7.4 Robot Movement Simulator Robot Movement Simulator gets a Power set point, Turn Ratio, battery voltage, position, and angle of the robot as inputs. From this data a new position of the robot must be calculated given that a given time elapses between samplings. This means that Robot Movement Simulator must do the inverse of Actuator Translator; i.e. find velocities of the two wheels from the Power set point and Turn Ratio in the LEGO commands. The velocities found can be used to calculate a new position of the robot by applying the kinematic model of the robot. To be able to calculate the new position of the robot, Power set point and Turn Ratio must be converted into a velocity ratio and a velocity of the right wheel. Hereby, the kinematic model presented in Chapter 6, page 49 can be applied. Therefore, the velocity of the right wheel and the velocity ratio are calculated, using the inverse of Equation (7.7), Equation (7.8), Equation (7.11) and Equation (7.12), which are based on the model of the robot. Power set point represents the velocity of the wheel having the largest speed. When Turn Ratio is positive, Power set point represents the velocity of the left wheel, otherwise it represents the velocity of the right wheel. To calculate the velocity represented by Power set point, either Equation (7.13) or Equation (7.14) must be used, dependent on whether the robot turns left or right. ( k T V bat v right = PSP right R a 100 A ) supporter 1 + v ratio 1 A rotor ( k T V bat v left = PSP left R a 100 A ) supporter 1 + v ratio A rotor k Tk e R a k Tk e R a r wheel + B rotor + Bsupporter 1+ v ratio 1 [m/s] (7.13) r wheel + B rotor + Bsupporter 1+ v ratio [m/s] (7.14) The velocity ratio is defined as shown in Equation (6.19), page 54. This implies that two equations are necessary to calculate the velocity ratio from Turn Ratio, because Power set point Page 64 of 124

65 7.4 Robot Movement Simulator can either represent the velocity of the right wheel or the velocity of the left wheel. Turn Ratio is greater than zero when Power set point represents the speed of the left wheel, and for this situation it can be calculated as shown in Equation (7.15); otherwise Equation (7.16) must be used. Turn Ratio v ratio = + 1 [ ] (7.15) 50 ( 1 Turn Ratio v ratio = + 1) [ ] (7.16) 50 When the sampling frequency is known to be 25 Hz, the simulator is now able to calculate the position and angle of the robot by using the equations set up in the kinematic model of the robot explained in Chapter 6, page 53. The equations to use for the calculation are Equation (6.21), Equation (6.17), and Equation (6.23). The feedback loop of Control Center is designed through this chapter. First, data from Image Processing and simulation are used to estimate the position and direction of the robot. Then a correction to follow the route is calculated, the necessary commands understandable to the robot are calculated, and a new position and direction of the robot are simulated. The simulation of the robot has been tested separately, as explained in Section 9.2, page 77. The conclusion of this test is that the model deviates from the physical situation, because the wheels slip when the robot is turning. To decrease this deviation, the slip of the wheels must be reduced. The feedback loop has been tested through the system test described in Section 9.2, page 87. During this test the robot picked up every cup and avoided every obstacle. This means that the feedback loop works as expected. To be able to interface with the system, a GUI is part of the Robot Control software. This is described in the next chapter together with the software used to send commands to the robot through Bluetooth. Moreover, the overall flow of the software is described. Page 65 of 124

66 Robot Control Software 8 Robot Control Software This chapter introduces the user interface that enables the user to interact with the system. Furthermore, the implementation of the Bluetooth connection between the robot and Robot Control is described. Finally, a description follows of the overall flow in Control Center software during autonomous control. The Robot Control software can be found on the CD at the location [Robot_Control/Program/]. 8.1 Robot Control GUI The Robot Control software is equipped with a graphical user interface which makes it possible to monitor the system while it is running. A set of controls are also provided. The software can be run in three modes, which can be changed when the software is not moving the robot. The three modes are Manual Control, Autonomous Control and Simulation Control. Each mode provides a set of controls specific for the given mode. The user interface is shown in Figure 8.1. Figure 8.1: A screenshot of the user interface. The common controls are placed left of the field, and the mode specific controls are placed below the field. Page 66 of 124

67 8.1 Robot Control GUI Manual Control For manual control mode the controls are provided as speed and directional sliders, and a button for activating the pick up sequence. Dependent on their position, the sliders command the software to send a LEGO command with a combination of Power set point and Turn Ratio to the robot. This gives the user an opportunity to control the robot remotely. Both the sliders and the button for picking up cups are linked with the keyboard, so that the arrow keys affect the sliders and the space key, activates the pick up sequence. For rapid changes of the robot, the shift key can be pressed while using the arrow keys. This will change the value in steps of tens instead of ones. Furthermore, the escape key provides an emergency brake. Simulation Control For simulation control mode the controls provided allows for inserting virtual cups and virtual obstacles onto the field. Deletion of the inserted objects is possible either by clearing the field entirely or by deletion of a single object. These tools are mouse driven, so that it is possible to click and drop cups, or draw obstacles. After insertion on the field they can be moved around to fit the desired set-up. The robot and the base can also be moved around on the field before starting the simulation. This allows for testing the behaviors of the robot in specific cases. A simulation is started or stopped by the two buttons found along the field editing tools. To ensure that the cases can be recreated it is possible to save and load set-ups. Autonomous Control For autonomous control mode the controls provided are, except from the start and stop button, related to the network interface with EyesWeb. As so, the user can acquire new objects from Image Processing and then, when this is acquired, locate the robot before starting the operation. When all items have been detected, the autonomous operation of picking up cups can be started by pressing the start button. In autonomous control mode, the base can still be moved freely around on the field before pressing start. Pressing start in autonomous control will activate the system. This process can be aborted by pressing stop. Common Controls The current cup load and the maximum capacity can be altered before starting either simulation or autonomous control. This forces the robot to unload at the base when the load equals or have surpassed the capacity. The connect button will attempt to establish a connection with the robot through the selected communication port. At the bottom of the common controls a battery bar is shown, this bar is updated continuously with the battery voltage of the robot, when a connection is established. The remaining features of the GUI are related to the field shown. Here the objects and their margins, used for controlling the robot, can be shown. The same goes for the trace of the robot. Operating System Independence The software for controlling the robot is created in Java, making it independent of the operating system. It should be noted though that the Bluetooth pairing with the robot must be performed before the software can actually handle the robot. In a Windows environment the serial connection for the Bluetooth link must also be established prior to connecting with the robot from the software. In Linux however, the serial connection is established automatically, if RFCOMM in beforehand is informed which serial port to use for this Bluetooth connection. General information about RFCOMM and Bluetooth can be found in Appendix D.3. The implementation of the Bluetooth connection between Robot Control and the robot is described in the next section. Page 67 of 124

68 Robot Control Software 8.2 Interface to the Robot According to INTF4, a Bluetooth connection must be used between the robot and the Robot Control software. Hence, it is necessary to use a computer with a Bluetooth module for the Robot Control software, and also to implement a Bluetooth stub in Robot Control. It has been chosen to write the Robot Control software in Java, because the software can be used on almost all platforms; the only software requirement to the system is a Java Virtual Machine. The interface between the Bluetooth hardware and the user application is called the Bluetooth protocol stack [Hopkins, 2004]. This stack can be either software or firmware and is the stack that handles settings, communication parameters, power levels etc. The Bluetooth stack consists of various layers, but the layers used depends on the implementation. It is therefore not mandatory, which layers that are used. A short description of the Bluetooth structure is given in Appendix D.3, 117. To handle a Bluetooth connection within a Java program, several possibilities exist: 1. The whole Bluetooth protocol stack can be implemented in Java and everything handled directly in Java. The advantage of this approach is that the Java program has full control of the Bluetooth connection. The disadvantage is that it is hard to make it universal, because different Bluetooth units are handled in different ways. 2. The Bluetooth protocol stack can be implemented in C or some other language and then controlled using Java. The advantage is that the Java program does not have to handle the stack. Though, it must still handle the initialization and control of the communication and it is still hard to make universal, as the C part must be written for the specific Bluetooth unit. 3. All parts of the Bluetooth communication can be written in C, or some other language, and be handled from Java as an underlying layer. The Java program can use the communication in a simple manner with no concerns of the actual connection, but the part written in C still have to be fitted to the individual Bluetooth unit type. 4. The Bluetooth connection can be made using an external driver, and Java can use the established connection as a standard serial RS-232 connection using the standard Serial Port Profile, SPP. The advantage of this approach is that the Java program can handle the communication in a well defined manner, and at the same time, the program can be made universal as it does not depend on any specific Bluetooth unit, as long as the driver for the Bluetooth unit supports SPP. This makes the approach very useful as it can be used across various operating systems due to driver independency. Because of the advantage and flexibility of using an external driver for establishing the Bluetooth connection, it has been chosen to use this approach. To be able to use a RS-232 connection from Java, the javax.comm package or something similar is needed. The javax.comm package is available as a standard package from Sun [Sun, 2006]. After testing the javax.comm from Sun it turned out that it cannot send hex number 0x0A, which corresponds to a line feed. When a 0x0A is send, the package changes it into 0x0D 0x0A; i.e. Carriage Return and Line Feed. In cases with line by line commands send as lines of text this is not a problem, but since the 0x0A is needed in another context than normally, the javax.comm package is inappropriate to use for the communication with the LEGO Mindstorms NXT. As a consequence, it has been necessary to use the RXTX package as replacement for the javax.comm package [Jarvi, 2006]. The RXTX package is an independent package providing the same functionalities and the same API as javax.comm. This makes it appropriate to use, since the extensive documentation and tutorials from both Sun and the developers of RXTX can be used. The RXTX package is not necessary to install because it is provided with the software. Bluetooth support is implemented in the software, and communication can be established with the robot, using the interface specified by LEGO for the LEGO Mindstorms NXT. Page 68 of 124

69 8.2 Interface to the Robot Commands for the LEGO Mindstorms NXT The communication with the LEGO Mindstorms NXT must follow the protocol defined by LEGO [LEGO, 2006a, app. 1]. The protocol defines, among other things, a number of direct commands that can be sent to the LEGO Mindstorms NXT. It is a selection of these commands that are used to control the robot. Figure 8.2 shows how the protocol is build up, and the bytes are listed here: Byte Represent the length of the message without the two length bytes themselves. The length is in little endian format, i.e. the low-order byte of the number is transferred first. Byte 2 - States the command type. This can either indicate a reply command, a direct command or a system command, where both direct and system commands can be with or without a request for a reply. For the control of the robot, system commands are not used, only the direct commands and the reply commands are used. The direct command is sent from the computer to the LEGO Mindstorms NXT, and depending on the command a reply may be requested. When a direct command with a request for reply is sent, the LEGO Mindstorms NXT replies with a reply command. Byte 3 - The actual direct command. should do. This indicates what the LEGO Mindstorms NXT Byte 4-N - The remaining bytes in a command is data for the direct command. The length of the data depends on the actual command. The direct commands used are SetOutputState, SetInputMode, GetInputValues, ResetMotorPosition, and GetBatteryVoltage. The description of the direct commands used in this project can be found in Enclosure i, which is an extract of the original description of the direct command documentation from LEGO [LEGO, 2006a, app. 2]. Length LSB Length MSB Command type Command Data Byte 0 Byte 1 Byte 2 Byte 3 Byte 4 N Figure 8.2: Overview of the LEGO Mindstorms NXT protocol over Bluetooth. The N indicates that the command length can vary depending on the command. Direct Commands for Motor Controlling The protocol for direct commands is defined in [LEGO, 2006a, app. 2]. Here it is stated that, when controlling the speed and direction of the robot, the two motors for propulsion can be operated in a synchronized mode. The advantage of using the synchronized mode is that when the robot is set to move forward at a straight line, the LEGO Mindstorms NXT will automatically ensure that the motors run an equal number of rotations. This is done by measuring the tacho count of both motors, and ensuring that they always keep track of each other. The translation between the numbers indicating speed and direction, defined by LEGO, and the actual speed and direction of the robot is handled by the Actuator Translator described in Section 7.3 on page 62. Because of the motor synchronization it is necessary to reset the tacho count before changing both the speed and the direction. If the reset is not performed, the motors will return to their last zero position before starting to move as desired. Both speed and direction are changed in the same command, which leads to a procedure for changing direction or speed, which states: stop motors, reset tacho counts, and run motors in desired direction and at desired speed. Before a command is carried out, The LEGO Mindstorms NXT uses some time to process it after it is received. If commands are sent too often, the robot will not move at all as the LEGO Mindstorms NXT will not be able to finish processing the last command before a new command is received. Page 69 of 124

70 Robot Control Software As a consequence of the processing time, it has been determined to limit the number of transmitted changes in speed and direction to 3 times per second. This means that not all commands determined by Control Center are send. However as the commands for a stop or move a rotation around its pivot point is to be sent. The command is send totally independent of the elapsed time since the last transmission, as it chosen to have a higher priority than any other command possible. The motor related to handling the collection of cups is operated in a slightly different way. When the system is turned on the elevator cart is set to move upwards for infinity. Right after a request for the button status is sent, this request is repeated until the status of the button indicates that the button has been pressed. When the button is pressed the motor is stopped. The elevator cart is now in its zero position. The start up sequence can be seen in Figure 8.3. Check status of button no limit [not pushed] [pushed] Elevation cart is now in the zero position. Stop motor Check status of button [pushed] [not pushed] Figure 8.3: Flowchart for initialize motor related to handling pick up of cup. When a cup has to be picked up, the cup collector motor is set to run at full speed downwards with a predefined tacho limit that corresponds to moving down and opening the claws. Afterwards, the same procedure as for the reset is done, moving the elevator cart upwards until the button is pressed and then the motor is stopped. This approach means that there is no need for a sensor at the bottom position of the elevator cart. The movement upwards, which is stopped by a button push ensures that the zero position is always reached. This sequence is illustrated in Figure 8.4. limit of 12*360 degrees Check status of button Waiting for motor to finish running the amount specified by the limit. Delay no limit [not pushed] [pushed] Stop motor Elevation cart has returned to the zero position. Figure 8.4: Flowchart for motor related to handling pick up of cup. Getting Battery Information To obtain the battery voltage level, of the LEGO Mindstorms NXT, a request is send for the voltage and the LEGO Mindstorms NXT replies with the battery voltage. The battery voltage is updated every half second to ensure that Actuator Translator has the correct voltage, which it uses to determine the correct Power set point for a given speed. Because the request for the battery voltage requires the LEGO Mindstorms NXT to transmit data, it is determined not to do it more often than every half second. This is because that the change from receive to transmit on the LEGO Mindstorms NXT can lead to a 60 ms latency in the communication. As this introduces a longer delay before the robot can be controlled, it should be Page 70 of 124

71 8.3 Flow of the Control Center Software minimized during movement control [LEGO, 2006a, app. 2, p. 4, bottom]. Having the interface to the robot described, the next section describes the flow of the software, when operating in autonomous control. 8.3 Flow of the Control Center Software The purpose of this section is to give an overview of the flow of the Robot Control software during autonomous control. Documentation of the Java API concerning the developed Java packages for the Robot Control software can be found on the CD at the location [Robot_Control/API/] and the source code at [Robot_Control/Sourcecode]. Activity Diagram of the Software Flow The explanation of the autonomous control sequence takes starting point in the activity diagram in Figure 8.5. obstacles, and the robot Find possible target possible target [more possible targets] [no more possible targets] nearest target sequence for the next sample period to LEGO command command to robot movement Robot position and angle position and angle [target not reached] [target reached] Figure 8.5: Activity diagram showing the flow of the software during an autonomous control of the robot. Initially the position of the cups, obstacles and the robot is updated directly from data received from Image Processing. Then the correct target is selected by evaluating the distances to the possible targets and choosing the nearest candidate. As described in Section 5.2 on page 44 this is done by an iterative process, where each possible target is selected one by one and a temporary Page 71 of 124

72 Robot Control Software route is planned from the robot to the specific target. During this iterative process, the software keeps track of the target with the shortest path to the robot. When there are no more possible targets, a final route is made to the nearest target using the Path Planner described in Section 5.3 on page 45. After the route has been planned the actual control of the robot starts. Robot Movement Decider, described in Section 7.2 on page 58, plans the movement of the robot needed for the next sampling period. The movement is planned so that the robot will converge towards the original path planned, following circular movements. The movement determined by Robot Movement Decider is translated into a LEGO command, which the robot can understand. This task is handled by Actuator Translator described in Section 7.3 on page 62. The LEGO command is send to the robot that starts to move according to the instructions; the transmission to the robot is described in Section 8.2 on page 68. The LEGO commands are also used in the Robot Movement Simulator, see Section 7.4 on page 64, to predict what the robot does according to the commands send to it. It should be noticed that commands for changing the speed and changing the direction are only sent 3 times a second or less, to ensure that the robot gets the necessary processing time. The only exceptions from this behavior are when the Robot Movement Decider decides that the robot must either stop or move around its pivot point. These commands are considered to have a higher priority and therefore they are immediately sent to the robot. Next, Robot Location Estimator, described in Section 7.1 on page 56, estimates the position and direction of the robot based on the results from Robot Movement Simulator and the input from Image Processing described in Chapter 4 on page 27. If the robot, based on the estimated position, is determined to have reached the target, the control process is finished. If this is not the case, the loop is repeated until the target is reached. Sequence Diagram of the Software Flow Figure 8.6 gives another view of the process in the Robot Control software during autonomous control. However, the figure is related to the timing between objects in the sequence. A green box indicates an object, and the name in the box corresponds to the name of the object in the code. The sequence starts at the top and ends at the bottom of the figure. The horizontal lines indicate either a creation of an object or a communication between two objects. When a dotted line points from an object to the name of another object it indicates that the first object creates the second object, e.g. the dotted line labeled 1 means that RobotControl creates a StrategicPlanner object. The numbers on the lines indicate the order of execution, i.e. the numbers increase down the figure. The labels shown after some of the numbers indicate names of methods called from objects. The vertical dotted line for each object indicates the lifetime of a given object. A green column indicates when an object is running, either by itself or by using other objects. When the vertical line for an object is ended by a cross it symbolizes that the object has been destroyed. A blue rectangle indicates that the steps inside the rectangle is looped a number of times. The text in the square brackets indicates how many times the loop is run. Except from the events happening in the 4 objects; AAURobot, DataReceiver, Drop, and UDPThread, the figure shows the same process as the activity diagram in Figure 8.5, but at a higher level of detail. The RobotControl object is the main object in Robot Control and handles all superior tasks. MovementTask is the object that handles the execution sequence during a sampling period. The UDPThread object receives data from the computer running Image Analysis and transfers the data to DataReceiver using the synchronized transfer object Drop. DataReceiver is the object that interprets the data received, according to the methods described in Section 4.3 on page 38. The AAURobot object is used to transfer data from the DataReceiver to the rest of the program. Objects not described in this section that is shown on the figure corresponds to the section titles in the report. Page 72 of 124

73 8.3 Flow of the Control Center Software : RobotControl 1: : StrategicPlanner 2: 3: calculatetarget(batteryconsideration=) : PathPlanner loop 4: calculateplan() 5: [Repeated for all possible routes] 6: 7: calculateplan() 8: 9: : AAURobot 10: 11: 12: 13: : MovementTask loop 15: : RobotLocationEstimator 17: [Until termination] : RobotMovementDecider 19: : ActuatorTranslator 20: 21: : RobotMovementSimulator loop 22: 23: getestimate(robotcam=, robotsim=) [Until target is reached] 24: 25: getcommands(robotestimate=, pathplanner=) 26: 27: getlegocommands(velocityratio=, velocityright=, batteryvoltage=) 28: 29: getsimulation(robotestimate=, robotsim=, LEGOCommands=, batteryvoltage=) 30: 31: 32: 33: 34: 35: 36: 37: 38: 39: Figure 8.6: Sequence diagram showing the flow in the software during an autonomous control of the robot. : DataReceiver 14: take() 18: : Drop : UDPThread 16: put() Page 73 of 124

74 Robot Control Software This chapter finalize the description of the system by describing the developed Java software for Robot Control. The software is able to communicate with the robot using Bluetooth. A test described in Section 9.3 on page 86 shows that a bidirectional connection is successfully created. Robot Control is equipped with a graphical user interface and is provided with controls in three different modes. Hereby, it is possible to monitor the system, and the robot can be controlled either manually, in an autonomous mode, or simulated virtually on the computer running Robot Control. Having described the system, the following chapter contains descriptions and results of the tests performed. This includes the acceptance tests, the supplemental tests, and the system test, where the capabilities of the system is tested. Page 74 of 124

75 9Test This chapter contains descriptions and results of four different types of tests. The first test is performed to be able to determine the frame rate of the camera used. An acceptance test is included that tests unmarked requirements, set up in Chapter 2 on page 17, to be able to verify whether the requirements are met. As a supplement to the acceptance test, additional tests are performed to give a deeper knowledge of the behavior of the system. Finally, a system test is performed to verify the functionality of the entire system in a typical set-up. For the tests performed, set-ups, images, movies and data files are found on the CD at the location [Test/]. 9.1 Determination of Frame Rate The purpose of this test is to determine the frame rate of the camera used in Image Processing. The determination of frame rate is based on the response time of Image Processing, and the variation in the response time. The response time is important for the operation of the system, because Control Center only receives information about the previous location of the robot when a new sample is fetched from Image Processing. The response time is measured by using a test circuit, which through a serial connection can turn on and off a green LED using the RTS signal of the serial port. The test circuit is connected to the computer running Robot Control. A test program is made that turns on the green LED and starts a timer. When data from the computer running Image Analysis indicates that the LED is turned on, the timer is stopped. The time difference is then an expression for the response time of Image Processing. The test is only made on the part of Image Processing that is used to locate the robot, because only this part has real-time demands. The test software used and data from the test can be found on the CD at the location [Test/Determination_of_Frame_Rate]. The complete test procedure is as follows. The test circuit is placed on the field and connected to Control Center. Image Processing is started to recognize the robot; in this situation the green LED. One of the markers of the robot is also placed on the field to ensure that the Image Processing has the same amount of data to process as when the robot is to be recognized. The test program starts by turning on the LED by setting the RTS signal of the serial port. When the test program receives data from Image Processing indicating that the LED is turned on, the time is saved and the LED turned off by clearing the RTS signal. The test program saves the time and turns on the LED again when data from Image Processing indicates that the LED is turned off. The cycle of turning on and off the LED is done 100 times. The test is made with frame rates of 5, 10, 15, 20, 25, and 30 fps. The response time for every sample is illustrated in Figure 9.1 to Figure 9.6 for the different frame rates. Page 75 of 124

76 Test 450 Response time at 5 fps 450 Response time at 10 fps Response time [ms] Response time [ms] Sample Sample Figure 9.1: The response time of Image Processing at 5 fps. Response time at 15 fps Figure 9.2: The response time of Image Processing at 10 fps. Response time at 20 fps Response time [ms] Response time [ms] Sample Sample Figure 9.3: The response time of the Image Processing at 15 fps. Response time at 25 fps Figure 9.4: The response time of the Image Processing at 20 fps. Response time at 30 fps Response time [ms] Response time [ms] Sample Sample Figure 9.5: The response time of the Image Processing at 25 fps. Figure 9.6: The response time of the Image Processing at 30 fps. Page 76 of 124

77 9.2 Supplemental Tests The maximum response time and the maximum variation in response time for each test are shown in Table 9.1. Frame Rate Max response time Variation in response time [fps] [ms] [ms] Table 9.1: Test results for the response time of Image Processing and variation in response time, with different frame rates. Based on the test results, the project group has chosen to let Image Processing operate at a frame rate of 25 fps. At this frame rate, the smallest variation in response time is observed. Additionally, the maximum response time is similar to the maximum response time obtained at a frame rate of 30 fps. The maximum response time is 131 ms. One reason for the variation in response time can be caused by a difference between turn-on and turn-off time of the LED. From the results shown in Figure 9.1 it seems that the variation is constant at every second sample, which indicates difference between the turn-on and turn-off times. 9.2 Supplemental Tests In this section supplemental test procedures and results are described to give a deeper knowledge of the behavior of the system. Turning Abilities The purpose of this test is to evaluate how the model of the robot performs when turning. The test set-up is shown in Figure 9.7. A 90 turn is tested to be able to evaluate the accuracy of the model. During the test, the robot is set to run on simulation values only. To calculate how much the robot actually has turned, the location of the robot is found using Image Processing. To ensure a precise 90 turn, obstacles and a cup are placed in the simulator as shown in Figure 9.7. The test set-ups can be found on the CD at the location [Test/Supplemental_Tests/ Turning_Abilities/]. This creates a turning angle for the first corner that is defined by the position and angle of the robot. Therefore, the first turn cannot be used as a reference; instead the straight line after the first turn is used. The difference between the angle of the line driven before and after the second turn provides the turning angle. The path driven after the third turn is determined by the location of the cup and is also discarded. Approximated lines to the locations of the robot, delivered by Image Processing when driving on a straight line in simulation, are used as the direction of the robot. This will reduce the measurement inaccuracy of Image Processing data compared to only determining the angle of the robot based on two points before turn two and two points after turn two. The following procedure is used, and is repeated with a mirrored set-up to get both left and right turning abilities. In the simulator the robot, cup, and obstacles are placed as shown in Figure 9.7. During operation the path driven is determined by use of Image Processing. The angle between the two lines, θ right, is compared to the expected angle of 90. Page 77 of 124

78 Test Figure 9.7: Set-up to test turning abilities. Each obstacle has the size of an A4 paper. The test set-up is mirrored for testing the other direction of turning. Tests results of the turning abilities of three right turns and three left turns are shown in Table 9.2. Test θ right Difference θ left Difference [ ] [ ] [ ] [ ] Table 9.2: Test results of the turning abilities of the robot. During the test it was noticed that the wheels of the robot slipped during the turns, which makes the robot deviate from the model during these turns. To improve the turning abilities, the robot could be modified to get a better grip. During normal operation of the system the angular difference is not causing troubles, because it is canceled by the feedback from Image Processing. If the deviation between the robot and the model becomes too large during turns, Image Processing and simulation data might become out of synchronization as described in Section 7.1 on page 57. System Delay The purpose of this test is to determine the delay of the entire system. The test set-up is shown in Figure 9.8. The robot is located in the bottom of the field where the pixel resolution per floor area is highest. The idea is to measure the delay from a command is sent to the robot, to the response can be registered in Control Center. The operational speed of the robot is increased to the maximum value of 0.45 m/s and new batteries are inserted into the LEGO Mindstorms NXT. This is done to ease the registration of a change in position. The following procedure is used to perform the test. Place the robot on the bottom of the field with a cup located in front of it. Start an ordinary collection of cups procedure but save the system time just before a command is sent to the robot. Page 78 of 124

79 9.2 Supplemental Tests When the robot is registered to have moved, the system time is fetched. The system time must be saved at the same spot in the code in which the start time was registered; i.e. just before commands can be send. The system delay is calculated using the difference between the two system times. Figure 9.8: Set-up to determine system delay. The robot is located in the bottom of the field with a cup in front of it. The bottom of the field offers the highest pixel resolution per area. The test is performed four times. For each test the traveled distance as a function of time can be seen in Figure 9.9. Figure 9.9: For each of the four tests, the graph shows the traveled distance of the robot, given by Image Processing, as a function of time. At time zero the command is sent to the robot. The driven distance observed in the first point of movement and the registered delay in this point is shown in Table 9.3 for the four tests performed. It should be noticed that the robot have traveled some distance in the first point where a movement is registered. It is most likely that the robot initiates the movement between the first point of movement and the previous point in time. Hereby, the average system delay can be estimated to about 340 ms. The maximum delay in Image Processing is determined to be 131 ms in Section 9.1 on page 75. Comparing this delay to the delay of the entire system indicates that transmitting and processing a command takes in the region of 200 ms. Page 79 of 124

80 Test Test Registered delay Driven distance observed [ms] [mm] Table 9.3: The driven distance observed and the delay registered are shown for the four tests. 9.3 Acceptance Tests The purpose of this section is to test the unmarked requirements from Chapter 2 on page 17. Test conditions are specified separately for every requirement. To improve readability of this section, the requirement for each test is repeated before the test is specified. Image Processing Requirements IMG1 - Cups, obstacles, and the robot must be recognized individually on the field. Should be tested by placing one cup in the top left area of the field and one obstacle in the center of the field. When running Image Processing the output must be a string with two sets of coordinates; one set for the cup and one for the obstacle respectively. The coordinates must verify that the cup is located in the top-left corner and the obstacle is located near the middle of the field. No specific precision is required. The same test is continued by inserting the robot in the bottom-right corner of the field. By running Image Processing the output must be a string with two sets of coordinates; one set for the front marker and one set for the back marker of the robot. The coordinates must specify that the robot is located in the bottom-right area of the field. No specific precision is required. Figure 9.10: Image from the field in the set-up to test for individually recognition of items. Figure 9.11: Screenshot from the Robot Control, after recognizing the cup, obstacle, and the robot. An image of the test set-up is shown in Figure The test results show that Image Processing is able to recognized cups, obstacles and the robot individually. A screenshot from Robot Control is shown in Figure IMG2 - Within a distance of 380 mm the location of each element must have a precision of ±13.2 mm. The test consists of testing recognition of cups, obstacles, and the robot. This is described in the following sections. Page 80 of 124

81 9.3 Acceptance Tests Recognition of Cups The purpose of this test is to determine the local precision of cup recognition. A board containing four cups is used in the test set-up; a sketch of the board is shown in Figure The following procedure must be repeated with the board located in each corner of the field and performed once with the board located in the middle of the field. Before the procedure is followed, the distance between each cup must comply with Figure The board must be placed on the field and all cups must be visible. Coordinates of the cups must be found by using the averaging method described in Section 4.3, page 38. Based on the coordinates, the distances between the cups must be calculated. It must be controlled that every distance meets the required precision. As no reference point is used, each cup is allowed to deviate one time the tolerance. Figure 9.12: Set-up to test cup recognition. Four cups are installed on the board. The result of the test is shown in Table 9.4 to Table 9.8. Length Actual length Calculated length Difference [mm] [mm] [mm] a b c d e f Table 9.4: Test results from the testing of cup recognition in the top-left corner of the field. The lengths are specified in Figure Length Actual length Calculated length Difference [mm] [mm] [mm] a b c d e f Table 9.5: Test results from the testing of cup recognition in the top-right corner of the field. The lengths are specified in Figure Page 81 of 124

82 Test Length Actual length Calculated length Difference [mm] [mm] [mm] a b c d e f Table 9.6: Test results from the testing of cup recognition in the bottom-right corner of the field. The lengths are specified in Figure Length Actual length Calculated length Difference [mm] [mm] [mm] a b c d e f Table 9.7: Test results from the testing of cup recognition in the bottom-left corner of the field. The lengths are specified in Figure Length Actual length Calculated length Difference [mm] [mm] [mm] a b c d e f Table 9.8: Test results from the testing of cup recognition in the middle of the field. The lengths are specified in Figure The test results show that the largest deviation is 10.8 mm and is found in the bottom-right corner. The requirement is then fulfilled since the deviation is allowed to be 2 times the tolerance of 13.2 mm. It is hereby verified that the location of cups meets the precision required. Recognition of Obstacles The purpose of this test is to determine the precision of the size of an obstacle found by Image Processing. Five A4-sized (297 mm 210 mm) red cardboards must be located in each corner of the field, and one in the middle of the field. The cardboards must be placed on the field with the sides of the cardboards being parallel with the x- and y-axis of the world coordinate system. Coordinates of the obstacles must be found by using the averaging method described in Section 4.3 on page 38. Based on the coordinates, the height and width of the obstacles must be calculated. It must be controlled that the height and the width meets the precision. As no reference point is used, each side of the cardboards must deviate one time the tolerance; hereby, two times the tolerance in height and width. Page 82 of 124

83 9.3 Acceptance Tests The results of the test are shown in Table 9.9. Position Calculated Calculated Height Width of the Height Width Difference Difference obstacle [mm] [mm] [mm] [mm] Bottom-left Bottom-right Top-left Top-right Center Table 9.9: Test results for testing recognition of obstacles. The differences stated are between the calculated dimensions and the standard A4 dimensions. The test results show that the largest difference in a dimension of an obstacle is 14.2 mm; obtained for the height of the obstacle located in the bottom-right corner of the field. The requirement is then fulfilled since the deviation is allowed to be 2 times the tolerance of 13.2 mm. It is hereby verified that Image Processing is able to determine the dimensions of obstacles with the precision required. Recognition of Robot The precision of the ability to recognize the robot is tested in two cases. In the first test the robot must be at rest, and the determination of the location and direction of the robot is tested compared to two reference points. A test follows where the robot is in motion and the precision of the distance between the two markers is investigated. The Robot in Rest Positions The robot is located on a board also containing two drawing pins spanning a line parallel to the center line of the robot. This is illustrated in Figure The drawing pins are used as reference points. Figure 9.13: Set-up to test recognition of the robot. The robot and two drawing pins are located on the board. The center axis of the robot is parallel to the axis spanned by the two drawing pins. The following procedure must be repeated with the board located in each corner of the field and performed once with the board located in the middle of the field. Before the procedure is followed the location of the robot must be consistent with Figure The board must be located on the field. The robot and both drawing pins must be visible to the camera. Page 83 of 124

84 Test The location of the robot is determined by Control Center (x robot, y robot ). The locations of the drawing pins are manually found by pointing out the centers of the drawing pins in image coordinates and then convert these coordinates into world coordinates by using the coordinate conversion. The distance from the robot (x robot, y robot ) to the bottom drawing pin is compared to the actual distance of mm. It must be verified that the difference in the calculated and actual distance meets the locale precision. The distance is allowed to deviate with the tolerance of 13.2 mm. The angle of the robot is determined by Control Center. The angle of the line spanned by the two drawing pins is compared to the angle of the robot found. The difference of the two angles must be less than 6.8. This is calculated from the maximum deviations allowed for the two markers of the robot. The results of the test are shown in Table Position Calculated Difference Angle of Angle Difference of the distance drawing pins of robot board [mm] [mm] [ ] [ ] [ ] Top-left Top-right Bottom-right Bottom-left Center Table 9.10: Test results for testing recognition of the robot in a resting position. The results show that the largest deviation in positioning the robot is 5.3 mm, observed in the bottom-right corner. The largest difference between the angel of the robot and the angel of the two drawing pins is 1.8. The requirement is then fulfilled since the deviation is smaller than the allowed tolerances. The Robot in Movement The purpose of this test is to determine the precision of the recognition of the robot in motion. While the robot is driving on the field, the distance between the two markers of the robot is calculated for each frame and compared with the actual distance of 224 mm. The route traveled by the robot is shown in Figure 9.14 and the distribution of the results obtained is shown in Figure Figure 9.15 shows that the calculated distance lies between mm and mm. The variation from the 224 mm lies within two times the tolerance of 13.2 mm. In broad outline, the spreading follows the normal distribution with the mean value in 225 mm. The requirement is then fulfilled since the deviation is smaller than the allowed deviation of two times the tolerance. The results from the three sections above show that IMG2 is met. IMG5 - When containing zero to ten cups the robot must be recognized. The robot must be placed on the center of the field and pointing either towards the camera, away from the camera, to the left, and finally to the right side. This must be done with the robot containing zero, one, and ten cups. The number of cups must have no influence on the ability to determine the position of the robot. The positions determined are shown in Table The test results in Table 9.11 show that the change in the recognized position of the robot does vary no more than 8 mm, independent of the number of cups carried by the robot. It must be noticed that this deviation is only observed when the robot carried ten cups; otherwise, no differences are detected. It is concluded that the robot can be recognized when it contains zero to ten cups. Page 84 of 124

85 9.3 Acceptance Tests Figure 9.14: A screenshot from Robot Control showing the route of the robot for the test performed to determine the distance between the two markers. Figure 9.15: The distribution of the distance between the two markers of the robot. The robot was in motion during the test. Cups carried Pointing towards Pointing away Pointing Pointing the camera from the camera Left Right 0 (1304;963) (1327;900) (1293;913) (1425;939) 1 (1304;963) (1327;900) (1293;913) (1425;939) 10 (1302;971) (1327;900) (1293;913) (1432;939) Table 9.11: Test results for recognition of the robot carrying zero, one, and ten cups pointing in four different directions. Coordinates in each column can be individually compared. Control Center Requirements CTRL2 - When several cups are possible targets, the path to be calculated must be the one for which an estimate of the traveled distance is shortest. The distance must be specified as the distance between the pivot point of the robot and the center of the cup. Should be tested by placing the robot, three obstacles, and two cups on the field as specified in Page 85 of 124

86 Test Figure The cup to be collected first must be the one for which the traveled distance is estimated to be shortest. In the example, this is Cup 2. Figure 9.16: Test set-up for testing that the planner chooses the nearest cup in traveled distance to be target. The obstacles shown are A4 papers and the distances between these are 50 mm. All measurements are in mm. The test showed that Cup 2 was picked up first. Afterwards, the robot picked up Cup 1. This is as expected and hereby it verifies CTRL2. Interface between Robot Control and Robot INTF4 - Bluetooth connection must be established between Robot Control and the robot. To verify that a bidirectional connection is established between Robot Control and the robot, the battery voltage of the LEGO Mindstorms NXT is requested. If Control Center receives a battery voltage the Bluetooth connection works bidirectional. The test is done with different supply voltages and the received voltages can be seen in Table The received values for a specific battery voltage vary over time, so a minimum and a maximum value are stated in Table Supply voltage LEGO voltage min LEGO voltage max [V] [V] [V] Table 9.12: Received battery voltages over the bidirectional Bluetooth connection. The test results show that the received voltages are similar to the voltages supplied to the LEGO Mindstorms NXT. For this reason it can be concluded that a bidirectional Bluetooth connection is obtained. Page 86 of 124

87 9.4 System Test 9.4 System Test To test the implemented system, the entire system is enabled to control the robot. Two set-ups are created on the field to test if the system can pick up the cups placed in valid positions. The robot must avoid all obstacles and return to the base with the collected cups. Both tests have five obstacles and nine cups to make sure that Image Processing is tested to the allowed maximum. In both test set-ups the robot is required to follow straight lines, make soft turns, and to turn around itself at least once. Both tests where recorded, and can be seen on the CD in the location [Test/System_Test/Recorded_Camera_Data/]. First Test Set-up The first set-up on the field is shown in Figure This set-up has a cup placed within a safety margin, thus it is considered an obstacle, and only the 8 remaining cups are to be picked up. The obstacles are placed on the field so that the robot will have to navigate around them on the field to reach the cups. Figure 9.17: The first system test set-up on the field. Figure 9.18: Shows the path the robot followed in the first system test. The completed path of the robot is shown in Figure One cup was correctly declared invalid, and the robot was able to pick up the remaining cups without touching the obstacles. The cups were successfully unloaded on the base. Page 87 of 124

88 Test Second Test Set-up The second set-up on the field is shown in Figure This set-up has the obstacles placed a little to the right of the center of the field, thus forcing the robot to drive on the outer rims of the field where it did not travel in the first test. This test has no invalid cups, and the robot must be able to pick up all the cups on the field. To test that the interface behaves properly, the base is relocated before the test, to test that the robot will dump the cups at the repositioned base. Figure 9.19: The second system test set-up on the field. Figure 9.20: Shows the path the robot followed in the second system test. The completed path of the robot is shown in Figure The nine cups where all collected without touching the obstacles. The cups were successfully unloaded on the base. During both tests all cups were picked up. The system test verifies the operation of the entire system and underlines the expected behavior. The conclusion is found in the next chapter. Page 88 of 124

89 10 Conclusion It is shown that it is possible to control a robot autonomously by using feedback from a web camera. It is also shown that the software created is able to control a LEGO robot to pick up plastic cups while avoiding obstacles. Introduction A system is set up which can control the robot via Bluetooth by using a web camera to determine the position and direction of the robot. As Bluetooth is native in LEGO Mindstorms NXT, a robot is created with LEGO bricks based on the LEGO Mindstorms NXT. The dimensions of the created robot leads to a definition of a tolerance that must be satisfied if the system is to guarantee the collection of all the cups on the field. Combined with the test environment, this is used to specify the requirements that are set-up in Chapter 2 on page 17. System Description The system is able to handle missing or misleading frames from image processing based on data from the camera. This is accomplished by using a filter to estimate a position and direction of the robot by weighting data from a simulator and data based on camera outputs. Thus a reasonable estimate always exists concerning the actual location and direction of the robot. Knowing this, the regulation of the movements is done to follow the pre-calculated path. The path, which is followed, uses a set of straight lines connected and is calculated from the initial location of the robot to the nearest cup, avoiding all obstacles. When all cups are collected, the robot returns to the base for unloading. The robot follows the path in circular movements given by the kinematics of the robot. The feedback is defined as two feedback loops with the path planned as reference; the two loops are the simulation loop, and the camera data loop. Image Processing As the camera delivers images, an analysis of the images is implemented in the EyesWeb software. The patches for EyesWeb extract the colors in the images, and on this basis the location of items on the image plane can be found. A tool for Matlab is used to determine the properties of the camera, which provides distortion compensation and a transformation matrix for converting the image plane coordinates into world coordinates. This is done within the required tolerances, as it is shown in Section 9.3 on page 80. Route Planner Dijkstra s Algorithm is able to find the shortest path between a source and a target and is used for path planning. This defines that the robot drives towards waypoints that is located around the objects. It has been shown in the test in Section 9.3 on page 85 that the implemented algorithm is capable of finding the nearest cup when avoiding obstacles. Robot Model For simulation of the robot a kinematic model is set up. This is combined with a model of the actuators to give an estimate of the movement of the robot, when given voltages are applied to the two motors. The model is afterwards simplified with the assumptions that the system is in steady state, i.e. the acceleration time of the robot equals zero. This model is implemented in the software, and the performance is tested in Section 9.2 on page 77. The conclusion of this test is that the model deviates from the physical situation, because the wheels may slip when the robot is turning. Based on the kinematic model of the robot, a movement decider is implemented that define the movements that are required to follow the path planned. By combining the model of the robot with data from image processing, the data from simulation is capable of detecting bad frames in the image processing and reduce the effects caused by the delay. Page 89 of 124

90 Conclusion Control Center The system is continuously monitored by a graphical user interface which can be used to affect the software with a variety of actions. As an example, the simulation and the interface to the robot can be tested without enabling the autonomous part of the interface. In the software it is also possible to read the battery voltage of the robot and set the location of the base, where the robot should unload the cups collected. The system has been tested on two different types of field set-ups in Section 9.4 on page 87. In each set-up the robot was able to pick up all the valid cups, without intersecting any obstacles. Additionally, the robot succeeded to deliver the cups collected to the base for unloading. Thereby, it can be concluded that the system performs as expected. Page 90 of 124

91 Bibliography Bibliography [Arbejdstilsynet, 1996] Arbejdstilsynet (1996). Kunstig Belysning. 09/ [Betz, 1974] Betz, D. (1974). Relationship between Contact Resistance and Wear in Sliding Contacts. 12/ [Borch, 2006] Borch, O. (2006). Networks and Data Communication MM1. intranet/block%201/mm1.pdf 10/ [Bouguet, 2006a] Bouguet, J.-Y. (2006a). Camera Calibration Toolbox for Matlab. 10/ [Bouguet, 2006b] Bouguet, J.-Y. (2006b). Description of the calibration parameters. 10/ [Close et al., 2002] Close, C. M., Frederick, D. K., and Newell, J. C. (2002). Modeling and Analysis of Dynamic Systems. John Wiley & Sons, 3. edition. ISBN: [Daid, xxxx] Daid, C. M. (xxxx). Bluetooth Tutorial - Specifications. 11/ , year of publication unknown. [Detmer, 2006] Detmer, R. C. (2006). Dijkstra s Algorithm. 10/ [Gonzalez and Woods, 2002] Gonzalez, R. C. and Woods, R. E. (2002). Digital Image Processing. Prentice Hall, 2. edition. ISBN: [Hopkins, 2004] Hopkins, B. (2004). Getting Started with Java and Bluetooth. 10/ [Jarvi, 2006] Jarvi, K. (2006). RXTX: serial and parallel I/O libraries supporting Sun s CommAPI. 10/ [Johnson, 2004] Johnson, H. (2004). Mastering Digital Printing. Course Technology, 2. edition. ISBN: [LaValle, 2006] LaValle, S. M. (2006). Planning Algorithms. 10/ Page 91 of 124

92 Bibliography [LEGO, 2006a] LEGO (2006a). LEGO Mindstorms NXT Bluetooth Developer Kit. otherfiles/2057/lego%20mindstorms%20nxt%20bluetooth%20developer%20kit_ 58CE458E CB0-93D2-4BEC821C13C2.zip 10/ [LEGO, 2006b] LEGO (2006b). The NXT. 10/ [MathWorld, 2004] MathWorld (2004). Normal Distribution. 11/ [Maybeck, 1979] Maybeck, P. S. (1979). Stochastic models, estimation, and control. 11/ [Morris, 1998] Morris, J. (1998) Dijkstra s Algorithm. 10/ [Pedersen and Andersen, 2004] Pedersen, A. B. and Andersen, R. E. (2004). EyesWeb Compendium / [Philips, 2001] Philips (2001). PCVC740K Owner s manual. 11/ [Post and Ritchie, 2000] Post, R. and Ritchie, E. (2000). Dynamic Motor Modelling. pdf 12/ [Ritchie, 2006] Ritchie, E. (2006). E5-2: Modelling of Dynamic Systems III(X). E5-2ModellingOfDynamicSystems_3MM.pdf 11/ [Sun, 2006] Sun (2006). Java(tm) Communication API. 10/ [Tanenbaum, 2003] Tanenbaum, A. S. (2003). Computer Networks. Prentice Hall PTR, 4. edition. ISBN: [LEGO, 2006] LEGO (2006). Customer Number Technical Support: 10/ Page 92 of 124

93 A Positioning based on a Camera In this appendix calibration of the camera is described, and a calibration is performed. After the calibration it is described how to convert an image point into a point in world coordinates. A.1 Camera Calibration To be able to use a camera for locating items in the field of view, it is necessary to calibrate the camera to determine its parameters. This is done to take the distortion into account to be able to compensate for this. When the parameters are known, it is possible to convert an image coordinate into a world coordinate. The calibration is made in Matlab with the Camera Calibration Toolbox [Bouguet, 2006a] located in [Camera_Calibration/Calibration_Toolbox] on the CD, and the images used for calibration and testing is found in [Camera_Calibration/]. Distortion occurs due to the optics of the camera, and consists of a radial and tangential distortion. The radial distortion is caused by the curvature of the lens and is illustrated in Figure A.1. The tangential distortion is caused by manufacturing inaccuracies. For instance the lens may not be totally symmetrical, or it may be tilted so that it is in not parallel with the image sensor. The radial distortion is typical the most significant, while the tangential distortion typical is less significant and will be omitted in the calculations. To compensate for the distortion, it is necessary to calibrate the camera to determine the exact distortion, and hence, include it in calculations of image coordinates. Figure A.1: Illustration of the radial distortion caused by the non-linearity in the optics of the camera. The parameters that the calibration gives can be split up in intrinsic and extrinsic parameters. The intrinsic parameters are the internal parameters of the camera. The extrinsic parameters are used in the conversion between camera coordinates and world coordinates [Bouguet, 2006a]. Intrinsic Parameters Focal length (f c ) - is the length, given in pixels, from the Focal point to the Principal point; shown in Figure A.2 and Figure A.3. The pixel height and pixel width can be different, therefore the focal length is given as a 2 1 vector, where one value is given in pixel heights and the other in the pixel widths. Principal point (c c ) - is the center of the image, shown in Figure A.3, given in image coordinates as a 2 1 vector. Distortions (k c ) - is the image radial and tangential distortions coefficients given as a 5x1 vector. Skew coefficient (α c ) - is the skew coefficient of the angel between x and y pixel axis. Page 93 of 124

94 Positioning based on a Camera The intrinsic parameters are illustrated in Figure A.2 and Figure A.3. Camera image plane Image plane Focal point x y Focal length ( f c ) Focal length ( f c ) Figure A.2: Relationship between image plane and camera image plane. x y Camera coordinates z Focal point ( P f ) Focal length ( f c ) x Image plane with image coordinates y Principal point ( c c ) World coordinates z x y Figure A.3: Relationship between camera coordinates and world coordinates. Extrinsic Parameters T c - is a three dimensional vector with camera coordinates of the origin in the world coordinate system. R c - is a 3 3 matrix that transforms coordinates between the camera coordinate system and the world coordinate system. Page 94 of 124

95 A.1 Camera Calibration As input for the calibration, the Camera Calibration Toolbox requires images of a black and white square patterned board. A square patterned board is used because each corner of the board is found in two ways. First the corners are found, based on the dimensions of the squares when the outer corners are marked on the board. Then the Camera Calibration Toolbox finds each corner, based on the intensities in the pixels in an area around each corner, found from the dimensions of the squares. The difference between the locations of the two corners found is then used in calculation of the intrinsic parameters of the camera. To ensure an accurate calibration, 30 images of the square patterned board are taken; each in a different angle and position to the camera. To find the extrinsic parameters, an image is captured where the square patterned board makes up the plane which is the span of the x- and y-axis of the world coordinate system. Therefore, the image shown in Figure A.4 is used to find the extrinsic parameters. Here, the square patterned board is located on the floor, making the floor a reference for coordinates in the world coordinate system. Therefore, the actual floor level has a negative z-value equal to the height of the board. This is measured to be 3 mm. Figure A.4: The image captured from which the extrinsic parameters are generated. The calibration gives the following parameters: f c = [ ] ± [ ] c c = [ ] ± [ ] k c = [ ] ± [ ] α c = 0 T c = R c = The complete distortion found using the camera is illustrated in Figure A.5. The distortion of the camera is in accordance with the expected distortion shown in Figure A.1, where the distortion becomes larger further away from the center. Page 95 of 124

96 4 3 3 Positioning based on a Camera [pixel] [pixel] Figure A.5: The complete distortion calculated by the camera calibration. For each circle moving away from the principal point (O), the distortion increases with 1 pixel, in the direction of the arrow. A.2 Coordinate Conversion In order to use images for position determination on the floor, it is necessary to convert the image coordinates into world coordinates. The conversion is using the calibration results described in Section A.1. The calculations are performed in two overall steps. First, a compensation for distortion in the image coordinates is made, and next, the image coordinates are transformed into world coordinates. Compensation for Distortion In order to calculate the distortion, the image coordinates are converted to normalized camera coordinates, and the image origin point is moved to the principal point as shown in Figure A.6. The camera coordinates are normalized with f c. The distortion depends on the radius as shown in Figure A.1, page 93, and by moving the zero point to the center of the plane, it becomes more convenient to express the radius. x cnd y cnd 1 = KK 1 x id y id 1 (A.1) where: x cnd and y cnd are normalized camera coordinates with lens distortion [ ]. x id and y id are image coordinates with lens distortion [pixel]. KK is the camera matrix. The camera matrix, KK, is given as [Bouguet, 2006b]: Page 96 of 124

97 A.2 Coordinate Conversion Image plane with image coordinates x y Normalized camera coordinates x y r Figure A.6: Illustration of image coordinates and normalized camera coordinates. KK = KK 1 = f c[1] α c f c [1] c c [1] 0 f c [2] c c [2] f c[1] α c f c[2] 0 1 f c[2] α c c c[2] cc[1] f c[2] f c[1] c c[2] f c[2] (A.2) Combining Equation (A.1) and Equation (A.2) yields the result shown in Equation (A.3), when α c = 0. x cnd y cnd 1 = x id c c[1] f c[1] y id c c[2] f c[2] 1 (A.3) A set of new normalized camera coordinates, which are compensated for distortion, can be calculated as shown in Equation (A.4) [Bouguet, 2006b]. When calculating on the distortion, the z-value is omitted, because the distortion only affects the x- and y-values. [ ] [ ] xcn 1 = y cn 1 + k c [1] r 2 + k c [2] r 4 + k c [5] r xcnd (A.4) 6 y cnd where: x cn and y cn are normalized camera coordinates which are compensated for distortion [ ]. r is the radius as shown in Figure A.6 [ ]. The radius, r, is defined as r = x 2 cn + ycn. 2 Assuming a small distortion, the radius is calculated as r = x 2 cnd + y2 cnd. To calculate x cn and y cn more accurate, the radius is calculated iteratively for every new x n and y n until the difference between a new radius and the previous radius is sufficiently small. Finally, the distortionless, normalized camera coordinates can be expressed as shown in Equation (A.5). x cn y cn = k c [1] r 2 + k c [2] r 4 + k c [5] r 6 x id c c[1] f c[1] y id c c[2] f c[2] Then, the normalized camera coordinates can be expressed as shown in Equation (A.6). P img = x cn y cn 1 where: P img is a normalized distortionless point in camera coordinates [pixel]. (A.5) (A.6) Page 97 of 124

98 Positioning based on a Camera Coordinate Transformation The relationship between camera coordinates and world coordinates can be described with Equation (A.7) [Bouguet, 2006b]. P world = R 1 c (P camera T c ) (A.7) where: R c and T c are the extrinsic parameters from the camera calibration. P camera is a point in the camera coordinate system, where P world is the same point in the world coordinate system. In order to determine the point P w on the floor in the world coordinate system, as shown in Figure A.7, the points P f and P img in camera coordinates are transformed into P fw and P imgw in world coordinates using Equation (A.7). The w in the index of P fw and P imgw indicates that these points are expressed in world coordinates. P w is located on the straight line that passes through P fw and P imgw. By determining the parametric representation of the line, it is possible to determine any point on the line to a distinct z-value. The value z = 3 mm is in floor level, because the extrinsic parameters are found based on the image shown in Figure A.4 on page 95. Camera coordinates x P f y z P img x y Image plane with image coordinates P l World coordinates z P w x y Figure A.7: Sketch to determine P w based on P f and P img. The parametric representation of the line going through P fw and P imgw can be described using Equation (A.8). P l (t) = P fw + t(p imgw P fw ) (A.8) where: P l is any point on the line; refer to Figure A.7 [m]. t is the parametric variable, which will be found later [ ]. To determine a point on the line, as a function of a z-value, the equation of a plane is used for the plane parallel with the z-plane containing P l and a random point P Q. The equation of the plane can be described with Equation (A.9). N ( P l P Q ) = 0 (A.9) Page 98 of 124

99 A.2 Coordinate Conversion where: N is the normal vector of the floor plane chosen to N = [0, 0, 1]. P Q is a random point in the plane chosen to P Q = (1, 1, z l ). By inserting Equation (A.8) in Equation (A.9) it is possible to set up an expression for t in Equation (A.10). t = N ( P Q P fw ) N ( P imgw P fw ) t = z l z fw z imgw z fw [ ] (A.10) where: z w is the z-value in the world coordinate system, which is equal to the vertical distance to the floor [m]. z fw is the z-value of P fw [m]. z imgw is the z-value of P imgw [m]. Hence, the point P w can be determined as a function of the z-value, z w, as shown in Equation (A.11). P w = P l (t) (A.11) Test of Coordinate Conversion t= zw z fw z imgw z fw The test set-up is shown in Figure A.8. Five square patterned boards are placed on the field, and their mutual locations are measured. The measurement is done with a tape measure. As described in Appendix A.1, page 93, the extrinsic parameters are calculated based on the square patterned board that gives P 11 as origin in the world coordinate system. This means that the surface of the board constitutes the zero z-value. The actual floor level therefore has a negative z-value equal to the height of the board. An image is captured of each square patterned board and image coordinates are converted into world coordinates. P 52 P 53 P 22 P 23 P 12 P 13 P 51 P 42 P 43 P 54 P 21 y x P 11 P 14 P 32 P 33 P 24 P 41 P 44 P 31 P 34 Figure A.8: Illustration of the test set up for testing precision of the coordinate conversion. Page 99 of 124

100 Positioning based on a Camera Global Accuracy Test The results of the global accuracy test are shown in Table A.1, where the measured points, these points converted, and the deviations are shown. The deviation is between a measured point and the inter-related converted point. To have a reference point, the converted points are adjusted with an offset equal to the converted P 11. Point Measured point Converted point Deviation [mm] [mm] [mm] P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; P ; ; Table A.1: Coordinate test results for the global accuracy test. The deviation is between a measured point and the inter-related converted point. The converted points are adjusted with a offset equal to the converted P 11. The results from the global accuracy test shows that the largest distance between a measured and converted point is 11.8 mm; this applies for P 54. For the point P 53, the deviation is 11.3 mm. All other points have a deviation less than 10 mm. Local Accuracy Test The test set-up in Figure A.8 is used for the local accuracy test. However, in this situation, only points on the boards are used. Hereby, the knowledge to the actual locations of the boards is not decisive. The points on the board that are used are illustrated in Figure A.9. A local area is defined within 380 mm. The diagonal created by the two squares is 408 mm and therefore, this distance is used between two points such that the distance based on the converted points are compared with the actual distance. The test is made in the top-left, top-right, bottom-left and bottom-right corner of the field. The results of the local accuracy test are shown in Table A.2. The local accuracy test shows that the largest difference is 6 mm. The global accuracy test and the local accuracy test together show that the largest deviation is found in the upper-left area of the field. This can be caused by the tangential distortion, which has been omitted. The tangential distortion is illustrated in Figure A.10, and shows that the distortion is largest in the upper-left area. Page 100 of 124

101 A.2 Coordinate Conversion d 1 d 2 d 3 d 4 Figure A.9: Illustration of the points used to determine the local error. Distance Deviation [mm] d 1 6 d d d Table A.2: Result of the local accuracy test [pixel] [pixel] Figure A.10: The tangential distortion calculated by the camera calibration. For each circle moving away from the principal point (O), the distortion increases with 0.2 pixel in the direction of the arrow. Page 101 of 124

102 Parameters for Modeling the Robot B Parameters for Modeling the Robot In this chapter the constants for the LEGO Mindstorms NXT Motor are found. Since the back supporter of the robot is included in the model of the robot, the parameters characterizing this is also determined in this section. Parameters are found by measuring upon the actual motor and robot, using the model as reference for the physical system. To be able to implement the model of the robot, the behaviors of the LEGO Mindstorms NXT are investigated in the last part of this chapter. B.1 Parameters for LEGO Mindstorms NXT Motor Figure B.1: The motor model is based on this electronic equivalent. The model of the motor that was set up in Chapter 6 on page 49 is investigated further to help finding the described constants through experiments. The equation of the electronic equivalent of the DC motor that is set up in Chapter 6 is shown in Equation (B.1). The circuit diagram of the electronic equivalent is shown in Figure B.1. To the motor model a mechanical description of the motor also exists. This is needed to complete the expression for e m as it is dependent on the rotation speed of the rotor. The constants for the mechanics are also determined to complete the simulation model of the motor. The data from the measurements and the m-files draw the graphs can be found in [Motor_Parameters/]. v a = i a R a + L a di a dt + 2V brush + e m [V] (B.1) where: v a is the voltage applied to the terminals of the motor [V]. i a is the current running through the motor [A]. di a dt is the first derivative of i a [A/s]. R a is the internal resistance of the motor [Ω]. L a is the internal inductance of the motor [H]. V brush is the voltage drop over a brush [V]. e m is the induced voltage of the motor [V]. The equipment used for measurements are found in Table B.1. Page 102 of 124

103 B.1 Parameters for LEGO Mindstorms NXT Motor Type Equipment AAU NR Oscilloscope Tektronix TDS Inductive Probe Tektronix tcp Multimeter Fluke 189 true RMS Multimeter Fluke 189 true RMS Power supply GW GPC Signal generator Instek GFG Table B.1: Equipment used for tests. The Armature Resistance When fixating the armature on the motor and applying a DC voltage on the terminals, the coil will soon reach saturation, thus there will be no voltage drop over L a, because dia dt will equal zero. If there is no movement on the armature, no voltage is induced in the motor, thus there can be no voltage drop over the motor. Hereby, the result of Equation (B.1) will, after the saturation time of the coil has passed, simplify to Equation (B.2). V a = R a I a + 2V brush [V] (B.2) The set-up used for the test is illustrated in Figure B.2. This test was conducted four times, each time with a different angle on the rotor. The argument for this turn of the rotor is that some DC motors have an uneven set of coils connected, dependent on the rotation of the armature. The effects of this should, given the method used, be minimized. Figure B.2: Set-up of experiment to determine armature resistance. As Equation (B.2) is similar to the definition of the straight line, with R a as slope, the results are plotted with V a and I a on the axes, and the results are approximated with a linear regressing. This can be seen in Figure B.3. The brush voltage is estimated to be mv based on the intersection between the straight line and the voltage axis in Figure B.3. The armature resistance, R a, is estimated to 5.11 Ω. Brush Voltage According to the results obtained from Figure B.3, the brush voltage is estimated to mv. It should be noted that the actual brush voltage is nonlinear, and is smaller at low currents [Betz, 1974, p. 34]. This may have made the slope of the line in Figure B.3 greater than R a. However, as the voltage of mv is small compared to the minimum driving voltage of the motor, it is neglected. Hence, V brush is assumed to be 0 V during the rest of the appendix. The Armature Inductance Knowing the armature resistance, the inductance can be found by measuring the time constant, τ, for the charging or discharging of the coil. This is often done by fixating the armature and applying Page 103 of 124

104 Parameters for Modeling the Robot Figure B.3: Measured currents to different voltages. The best-fit first order approximation is shown. This allows for extraction of resistance and brush voltage. a step function on the terminals. However, on the LEGO Mindstorms NXT motor the design of the motor prevents direct interaction with the armature, as internal nylon gears are put between the actual motor and the rotor. For the resistance measurements this causes no problems as the gears are quickly wound up. However, the time constant of the gears are greater than the one found for the charge of the electrical system. As so, the proposed test would not measure the time constant of the electrical system, but instead the one found for the mechanical system. To circumvent the effect of the gears, a test is devised where the gears never interact with the motor. A square function is applied to the motor with a frequency high enough to cause the armature to change direction so fast that the mechanical movement is virtually zero. On the other hand, the frequency has to be kept low enough to allow the full time constant to be extracted. The value of the inductance can be calculated from the time constant for the electrical system as shown in Equation (B.3). L a = τr a (B.3) where: τ is the time constant for the electrical system [s]. The set-up for the inductance test can be seen in Figure B.4 where a suitable frequency for the motor was found; this was set to 100 Hz. A OSC R a i a L a + vac V OSC v a V brush V brush - Figure B.4: Set-up of experiment to determine armature inductance. Page 104 of 124

105 B.1 Parameters for LEGO Mindstorms NXT Motor Figure B.5: Measurements from an inductance test. Here the vertical red lines mark the measured tau. The measurements of the inductance test are illustrated in Figure B.5. Here, the time constant for the electrical system is found by measuring the time passed for a 63.2 % drop of the current. Inserted in Equation (B.3) combined with the known resistance value, gives an armature inductance, L a of 4.7 mh. Motor Constant The motor transforms electric energy into rotational energy. If it is assumed that a test with no friction is conceived, the constant described in Chapter 6, page 50, can be determined. If k e is the electrical motor constant, it can be determined by measuring the relation between the voltage and the rotational speed of the unloaded motor, as dictated by Equation (B.4). where: e m is the induced voltage of the motor [V]. k e is the electric motor constant [Vs]. ω rotor is the angular velocity of the motor shaft [rad/s]. e m = k e ω rotor [V] (B.4) To ensure this frictionless environment, a known speed can be applied to the axle with the help of another motor. Then the voltage generated on the armature is independent of the frictional forces. Therefore, the armature voltage can be measured to determine the constant. However, as it is virtually impossible to measure directly on the armature at this specific motor, the voltage generated on the terminals is used to determine the generated voltage at the armature. To be able to use this measurement, it should be remembered that the current drawn by the voltmeter through the armature resistance will cause a small voltage drop over this, so both a current probe and a voltage probe is used during the measurement of the motor constant. The measurements can then be adapted to e m using the equation for the electrical system seen in Equation (B.5). The test set-up is shown in Figure B.6. e m = V a R a I a 2V brush [V] (B.5) Twelve tests were conducted and the average value for the motor constant, k e, was found to V s. Page 105 of 124

106 Parameters for Modeling the Robot R a i a + A OSC v a V brush Motor ω rotor Ext. Motor V OSC - V brush Tacho V OSC Figure B.6: Set-up of the test to find k e. The electric motor constant k e that links voltage and angular velocity, is when the motor is based on a permanent magnet equal to the mechanical motor constant k T, as so, k e in this appendix replaces k T in all equations where this is normally found. Dry Friction and Viscous Friction The frictional forces of the motor is found using the motor constant, k e, and a current to estimate the torque delivered by the unloaded motor, defined in Equation (B.6). where: T e is the electric torque produced by the motor [Nm]. k e is the electric motor constant [V s]. i a is the current running through the motor [A]. T e = k e i a [Nm] (B.6) The calculated force is held up against the actual speed performed by the motor for the current applied. The relation between the rotor speed and the torque delivered will give the frictional forces of the motor, as the friction must be overcome before any movement occurs. In this relations there are three types of friction defined. The first one relates to the fact that the friction of an object is proportional to the velocity of it. This is known as the viscous friction, B. The second friction is known as stiction, C. However, as can be seen in Figure B.7 it is only important at very low speeds and relates to the fact that different surfaces glue together when standing still against each other. When this frictional force is overcome, the force needed to keep the reached speed drops to a minimum. As this friction gives a very small contribution, and as the motor is seldom run at this slow pace, it is neglected. The third friction force is what gives the mentioned minimum. It is determined as a simple static friction and is a force that is to be overcome no matter how low the velocity becomes; this is known as dry friction, A. Figure B.7: The frictions gives a torque/angular velocity relation as plotted. The two coefficients used are drawn as dotted lines [Post and Ritchie, 2000, p. 5]. Page 106 of 124

107 B.1 Parameters for LEGO Mindstorms NXT Motor The two used frictional forces are found using the plotted measurements of Figure B.8. Figure B.8: Results from test for determination of friction. The calculated torque and the measured speed are shown; a linearization is illustrated to give simple frictional forces. The slope of the line for the torque/angular velocity test provides the B friction, and the crossing with zero provides the A. Thus the friction parameters are found to be A rotor = Nm and B rotor = Nm s. Moment of Inertia The moment of inertia is found by running the motor at a high speed and then cutting of the power. This would, if the motor had no moment of inertia, cause the motor to stop instantaneously due to the friction forces. However, since a motor has a moment of inertia it will decelerate slowly before it reaches a complete stop. The moment of inertia can be found by examining Equation (B.7). T e = T friction + T inertia + T load [Nm] (B.7) where: T friction is the torque caused by friction [Nm]. T inertia is the torque caused by the moment of inertia [Nm]. T load is the torque caused by the load [Nm]. During the deceleration no current is applied to the motor, and it is assumed that T e equals T load equals 0 Nm. This implies that Equation (B.7) can be rewritten as shown in Equation (B.8). 0 = (B rotor ω rotor + A rotor ) + J rotor dω rotor dt J rotor = B rotorω rotor A rotor dω rotor dt [kg m 2 ] where: J rotor is the moment of inertia [kg m 2 ]. B rotor is the viscous friction of the motor [Nm s]. A rotor is the dry friction of the motor [Nm]. ω rotor is the angular velocity of the motor shaft [rad/s]. dω rotor dt is the angular acceleration of the motor shaft [rad/s 2 ]. (B.8) Page 107 of 124

108 Parameters for Modeling the Robot The test set-up is shown in Figure B.9. To calculate ω rotor, i a and v a are measured, and inserted in Equation (B.9). ω rotor = v a R a i a L a di a dt k e [rad/s] (B.9) Figure B.9: The set-up used for the test of the moment of inertia. When the power supply is disconnected, the current, i a, drops to about 0 A. This is seen in Figure B.10 as a dramatical fall in ω rotor, because dia di dt is large. After this point in time, a dt becomes much smaller and the voltage drop over the inductance is assumed to be zero. Therefore, Equation (B.9) is rewritten to Equation (B.10) and used to calculate ω rotor. ω rotor = v a k e [rad/s] (B.10) The deceleration test seen at Figure B.10 is used to derive the moment of inertia, by approximating the slop of the line, dωrotor dt, to three given angular velocities. From these values J rotor is derived from Equation (B.8), and the average value is set to be J rotor. Figure B.10: The deceleration of the motor is used for determining the moment of inertia. The red lines are the approximated dωrotor dt. The moment of inertia, J rotor, is calculated to be kg m 2. Page 108 of 124

109 B.2 Determination of Back Supporter Friction B.2 Determination of Back Supporter Friction The purpose of this section is to determine the dry and viscose friction of the back supporter, from a test where the robot drives straight forward at different speeds. The robot has been modeled in Chapter 6, page 49 and an expression describing the steady state conditions of the robot has been set up in Section 7.3. To use these models the unknown parameters, A supporter and B supporter have to be found. To determine these parameters the robot must be driven at different constant speeds by applying different voltages on the motors, at a velocity ratio of 1. From these measurements a straight line should appear, by inserting in Equation (B.11), which is found from Equation (7.7), page 63. ( v right B support + A support =(1 + v ratio 1 Vbat PSP right k T ) A rotor r wheel 100R a v ( )) right kt k e + B rotor [Nm] r wheel R a (B.11) where: B supporter is the viscous friction of the back supporter [Nm s]. A supporter is the dry friction of the back supporter [Nm]. v right is the velocity of the right wheel [m/s]. r wheel is the radius of the wheel [m]. V bat is the battery voltage [V]. v ratio is the velocity ratio [ ]. PSP right is the power set point for the right wheel [ ]. k T is the mechanical motor constant [V s]. k e is the electric motor constant [V s]. R a is the internal resistance of the motor [Ω]. B rotor is the viscous friction of the rotor [Nm s]. A rotor is the dry friction of the rotor [Nm]. The test is accomplished by measuring the time it takes for the robot to drive between two lines. As shown in Figure B.11, the actual distance driven of the robot depends on the offset, d off, from the straight line. An approximation for this distance is denoted d driven. The time driven is measured with a stop watch; AAU no Figure B.11: The test set-up for the forward run test. The test is used to specify that A supporter equals Nm and B supporter equals Nm s. The results obtained is shown in Table B.2. By inserting the Power set point and the battery voltage into Equation (7.13), page 64, the speed of the robot is simulated. In Table B.2 the simulated speeds are compared with the actual speeds. Page 109 of 124

110 Parameters for Modeling the Robot PSP Battery Voltage Simulated speed Actual speed Speed difference [V] [m/s] [m/s] [m/s] Table B.2: Test results for the robot model driving straight forward. Page 110 of 124

111 B.3 LEGO Mindstorms NXT Controller B.3 LEGO Mindstorms NXT Controller Measurements where done to determine the actual behavior of the controller in relation to the software interface. Firstly it is needed to determine how the controller translates the Power set point into the motor control signal. The measurements are summarized in Table B.3. Power set point Duty cycle [%] Table B.3: Power set point vs. duty cycle. It is concluded from these measurements that the controller translates the Power set point linear into the duty cycle. The other important aspect of the controller is the Turn Ratio. This is an interface which defines the velocity of the two motors in relation to each other, instead of setting the velocities individually. For the control of the robot this gives an advantage as it links the velocities of the motors to each other. This is done as shown in Table B.4. Another aspect of this is the mutual feedback control that makes the controller slow down a wheel if the other is somehow prevented in reaching full speed. Turn Ratio Left motor [rad/s] Right motor [rad/s] Table B.4: Turn ratio vs. angular velocity at a Power set point of 100 and with the motors unloaded. It is concluded that the turn ratio delivers a linearized system for turning the robot. Page 111 of 124

112 Kalman Filtering C Kalman Filtering This appendix describes how Kalman filtering can be used to give an estimate of a value based on two measurements with known precisions [Maybeck, 1979, p. 9]. C.1 Normal Distribution Before treating Kalman filtering an insight is given in the normal distribution. When measuring a value, measurements will deviate from the correct value. The distribution of these measurements shows their observed occurrence. The normal distribution in Equation (C.1) represents how likely each value of the arbitrary variable is [MathWorld, 2004]. Since a normal distribution can be illustrated for any measured variable, units are not used on observations in this section. P(x) = 1 σ (x xm) 2 2π e 2σ 2 (C.1) where: P(x) is the probability density function. x is the random variable. x m is the mean value. σ is the standard deviation which indicates the spread of the curve. The probability density function is plotted in Figure C.1. The unit of this function is the inverse of the unit of observation. Figure C.1: Shows the probability function of the normal distribution, P(x). The precision of a measurement can be expressed by the standard deviation σ or by the second order statistic σ 2, called the variance. The unit of variance is the square of the unit of observation. The probability to find the correct value between two x-values equals the integral of P(x) in this interval. Hence, the area below P(x) from to equals unity. For a Gaussian density, 68.3 % of the area is contained within the band σ units on each side of x m [Maybeck, 1979, p. 10]. The smaller the σ, the better the precision. A small σ results in a tall and narrow curve while a large σ results in a low and wide curve. C.2 Weighted Inputs Kalman filtering can be used to give an estimate of a value based on two measurements with known variances. Having two measurements from the same point in time, x 1 and x 2, the estimate x res can be calculated using Equation (C.2) [Maybeck, 1979, p. 12]. This equation can be rewritten into Page 112 of 124

113 C.2 Weighted Inputs Equation (C.3). The situation is shown in Figure C.2 as well. x res = σ2 2 σ1 2 + x 1 + σ2 1 σ2 2 σ1 2 + x 2 (C.2) σ2 2 x res = x 1 + σ2 1 σ1 2 + (x 2 x 1 ) σ2 2 x res = x 1 + G (x 2 x 1 ) (C.3) where: x 1 is a measurement to time t. σ 2 1 is the variance of x 1. x 2 is a measurement to time t. σ 2 2 is the variance of x 1. x res is the resulting output. G is the Kalman gain defined as σ 2 1 σ 2 1 +σ2 2 [ ]. It appears from Equation (C.2) that if σ 2 2 >> σ 2 1, i.e. the precision of the measurement x 1 is much higher than the precision of the measurement x 2, the resulting output will be very close to x 1. Moreover, it can be seen that if σ 2 1 = σ 2 2, the two measurements have the same precision and thereby the resulting output will be the mean value of the two measurements. Figure C.2: The measurements x 1 and x 2 have different variances. Since x 1 has the smallest variance, the resulting value from the Kalman filtering, x res, is closest to x 1. x res has the smallest variance and hereby the highest and most narrow curve. The variance of the resulting output can be calculated from Equation (C.4) [Maybeck, 1979, p. 12]. 1 σ 2 res = 1 σ σ 2 2 (C.4) where: σ 2 res is the variance of the resulting output, x res. It appears from Equation (C.4) that σ 2 res < Min{σ 2 1, σ 2 2}. Hereby, the precision of the resulting output will always have a higher precision than those of the inputs. This agrees with the condition that having an additional measurement will add more information to the knowledge already held. Page 113 of 124

114 Network DNetwork The purpose of this appendix is to determine the computer network architecture, which should be used between the computers running Image Analysis and Robot Control, and between Robot Control and the robot. A computer network architecture is a set of protocols and specifications that together is enough to implement the network [Borch, 2006, p. 13]. This is accomplished using the OSI Reference Model, which is described in the following section. D.1 OSI Reference Model The OSI Reference Model divides the network into seven layers as shown in Figure D.1 [Tanenbaum, 2003, p. 39]. The upper four layers of the OSI Reference Model are used to pass messages from or to the user, whereas the three lower layers are used to pass messages through the host computer. Only messages addressed to the host computer are passed to the upper layers. For each layer of the OSI Reference Model a short description is given, after which the model is used to form the network architecture used between the computers running Image Analysis and Robot Control. Figure D.1: The seven layers included in the OSI Reference Model. Physical Layer The physical layer is concerned with transmitting the physical bit stream over a communication channel, and therefore it defines e.g. the voltage levels, bit timing, and connectors used in the network. Data Link Layer The primary task of the data link layer is to break up the data into data frames, send back acknowledgement frames if the connection is reliable, and control the flow of data to avoid drowning slow receivers in data. Network Layer The network layer e.g. handles the routing of data from source to destination, and provides congestion control of the data transfer. Page 114 of 124

115 D.2 Interface between Image Analysis and Robot Control Transport Layer The transport layer is concerned with splitting the data from the upper layer into smaller units if needed. It may ensure an error-free point-to-point channel making sure all data has arrived at the destination in the right order. Session Layer The function of the session layer e.g. is to synchronize different machines by inserting checkpoints in transmissions, allowing them to continue transmission after a crash from the checkpoint inserted last. Presentation Layer The presentation layer is concerned with syntax and semantics of the transmitted data, making it possible for computers with different data representation to communicate with each other. Application Layer The function of the application layer is to make the user able to access information on the network through an application. D.2 Interface between Image Analysis and Robot Control The computer network architecture used between the computer running Image Analysis and Robot Control is specified using the OSI Reference Model. Physical Layer A fast physical layer is desired since the delay between image capturing and control of the robot has to be as low as possible. Twisted pair 100Base-T is a fast commonly used physical layer. This standard is used in many networks, which makes the implementation of 100Base-T easy. The data rate of 100Base-T is 100 Mbps, and will therefore be used in this network. Data Link Layer The data link layer is chosen to be fast Ethernet with the standard 802.3u, which is an update of increasing the data rate of the Ethernet from 10 Mbps to 100 Mbps. Ethernet uses a negative voltage as LOW and a positive voltage as HIGH. This makes it possible to determine the difference between idle and a period of consecutive LOW. Furthermore, Ethernet uses Manchester encoding, because this encoding makes it easy to synchronize the receiver and sender clocks. This is possible due to the fact that Manchester encoding divides each bit into two periods, meaning that a 1 is represented by sending a HIGH in the first period and LOW in the second period, and visa versa for representing a 0 [Tanenbaum, 2003, p. 274]. A Manchester encoded bit stream is shown in Figure D.2. Figure D.2: A Manchester encoded bit stream. An Ethernet data frame is composed as shown in Figure D.3. The frame starts with a preamble consisting of a bit pattern switching between LOW and HIGH, making a square wave allowing the receiver to synchronize its clock. The data field of an Ethernet frame is between 0 bytes and 1500 bytes, and because the minimum length of an Ethernet frame is 64 bytes and the Pad field is used to fill out the frame if the data field is less than 46 bytes. The Ethernet has a minimum data Page 115 of 124

116 Network frame length of 46 bytes because the sender must still be in the process of transmitting the data frame when a noise burst from the most distant station is received due to a collision [Tanenbaum, 2003, p. 277]. Figure D.3: An ethernet frame must have a minimum length of 64 bytes. Collisions on the Ethernet are handled using Carrier Sense Multiple Access with Collision Detection, CSMA/CD, protocol [Tanenbaum, 2003, p. 279]. This protocol allows all nodes to transmit when the channel is idle, but they have to abort immediately after a collision is detected. Ethernet uses a binary exponential backoff algorithm to determine the time from a collision until the transmitter should try to transmit again [Tanenbaum, 2003, p. 278]. This implies that after the first collision, the transmitters wait 0 or 1 worst-case round-trip propagation times before starting to transmit again. The random number can in general be between 0 and 2 i 1, where i is the collision number. The random number can maximum reach 1023, where it is frozen until 16 collisions have occurred, and exceeding this, a failure is send to the upper layer. Network Layer It has been chosen to use the IP protocol as Network Layer in INTF3. This protocol will therefore be explained in this section. The header of the IP protocol, IPv4, has a length of between 20 bytes and 60 bytes [Tanenbaum, 2003, p. 433]. This includes a source and destination IP address of each 32 bits, identifying the source and destination uniquely. Furthermore, the header contains a checksum of the header, meaning that no checksum of the data is included in the IP header, and must therefore be provided by layers above the Network layer. Thus, the IP provides a datagram service, which is a service that does not return an acknowledgment to the sender [Tanenbaum, 2003, p. 33]. The maximum length of an IP datagram is 65,535 bytes. Transport Layer As protocol of the transport layer only Transmission Control Protocol, TCP, and User Datagram Protocol, UDP, are examined and compared in this section. These are both widely used protocols. UDP is a connectionless protocol, which basically is IP with a short header [Tanenbaum, 2003, p. 524]. The header of UDP is 8 bytes long, and consists of four 2 bytes fields containing the source port, destination port, UDP length, and UDP checksum. The source port is used by the destination to reply the source while the destination port and UDP length is necessary for the receiver to be able to receive the message. The UDP checksum is not required to be used. This means that there is no error control in UDP. UDP is primarily used in real-time transport protocols, e.g. to send Internet radio [Tanenbaum, 2003, p. 529]. TCP is a connection-oriented protocol, which uses a three-way handshake to establish connection [Tanenbaum, 2003, p. 539]. TCP segments have a header of minimum 20 bytes, which includes a 32 bit sequence number used for acknowledgements [Tanenbaum, 2003, p. 535]. The TCP segment size is limited by the size of the data field in the IP, but TCP software can split data into multiple segments if necessary. Furthermore, TCP software can put several data segments into one TCP segment. The connection between the computers running Image Analysis and Robot Control needs to be as fast as possible, because Control Center has to control the robot in real-time. The project group has chosen to test the transmission time of the two protocols. This is accomplished using two Java sockets; one echo receiver and one echo sender connected between two computers. The source code for the echo clients can be found on the CD at the location [Test/Network_Test/]. The time is saved before 1000 messages of 64 byte each is send from the echo sender to the echo receiver. Similar, the time is registered when the last echo is returned. By comparing these times, Page 116 of 124

117 D.3 Interface between Robot Control and the Robot the transmission delay is calculated. The difference between the two times is assumed to be 2000 times the transmission delay. The test results are shown in Table D.1. Value TCP UDP Average time µs 79.5 µs Table D.1: The average time of sending an UDP datagram and a TCP segment. Both the theory and a test have shown that UDP gives a significantly smaller transmission delay than TCP. Thus, the project group has chosen to use UDP. Using UDP/IP there is no need of session and presentation layers, because they are not included in UDP/IP [Tanenbaum, 2003, p. 43]. Application Layer The application layer is defined in INTF1 and INTF2. discussed further in this section. The application layer will not be D.3 Interface between Robot Control and the Robot The interface between the computer running Robot Control and the robot is the wireless communication form Bluetooth. Hence, the computer network structure used is not specified using the OSI Reference Model. The reason is that Bluetooth does not fit very well into the OSI Reference Model [Tanenbaum, 2003, p. 313]. Therefore, it is described in a similar but not equal way. The Bluetooth Structure Figure D.4: The Bluetooth structure. One master and up to seven active slaves can form a piconet. Two or more piconets can be connected via one or more bridge slaves and by that form a scatternet. Bluetooth works on a master/slave basis with a basic structure as shown in Figure D.4. Some Bluetooth units, e.g. those in computers, are capable of acting both as master and slave, while others, like the one in a mouse, can act only as slaves. When two units capable of being both slave and master connect, the one initiating the connection becomes the master and the other one the slave. Slaves in a piconet cannot communicate with each other, since all communication must take place between a master and a slave. Page 117 of 124

118 Network The Bluetooth Protocol Stack The following sections are based on the sources [Tanenbaum, 2003, p ] and [Daid, xxxx]. The layers in the Bluetooth protocol stack can be seen in Figure D.5. The right side of the figure shows what each part correspond to in the OSI Reference Model. It is important to keep in mind that the Bluetooth specifications are not very stringent, and a lot is up to the individual implementation, which means that the model presented is not definitive. Figure D.5: The Bluetooth protocol stack with all main layers. The figure is inspired by [Tanenbaum, 2003, Fig. 4-37]. Physical Radio The radio layer handles the actual transmission of bits between the nodes. Most Bluetooth units used today are of class 2, which means that they are capable of communicating with other units within a radius of about 10 m. Bluetooth uses the license free 2.4 GHz ISM band; the band is divided into 79 channels with a spacing of 1 MHz. The used modulation is Gaussian Frequency Shift Keying (GFSK), with 1 bit per µs. GFSK uses shift where a binary one is represented by a positive frequency deviation and a binary zero by a negative frequency deviation. Because of the frequency band being license free, in most places there is a lot of noise from other electronic devices. Bluetooth tries to overcome this problem by using a frequency hopping spread spectrum with 1600 hops per second, i.e. it changes the carrier frequency 1600 times per second. All nodes in a piconet hop simultaneously with a pseudo-random pattern, algorithmically determined by certain parts of the address of the master Bluetooth device. The timing of the hopping follows the clock of the master. Baseband This layer turns the raw bit stream into frames with a header and also handles the time division between master and slave. As written earlier, the master in each piconet defines the hopping scheme, and thereby defines a series of time slots with a length of 625 µs. When the master starts a transmission, it is done in an even time slot while the slaves start in the odd ones. This time division multiplexing ensures that the master gets half of the time to transmit while the slaves must share the other half. Each frame can have a length of 1, 3, or 5 time slots and the start of a frame must be aligned with start of a time slot. When a single time slot frame is used, a settling time of µs is allowed for the radio to become stable, before transmission starts. When the settling time has passed, about 366 bits are left in that time slot. Of these bits, 126 bits are used for the header and access code. This leaves 240 bits for data. To get a higher rate of payload data, a 3 or 5 time slot frame can be used. When a 5 time slot frame is used, only a single settling time is needed. This leaves about 2744 bits to the payload data which makes it more efficient than using single slot frames. The baseband layer can use two different types of connections; either Synchronous Connection- Oriented (SCO) or Asynchronous Connection-Less (ACL). The SCO is a point-to-point link between Page 118 of 124

119 D.3 Interface between Robot Control and the Robot the master and a slave, and is mainly used for transporting audio information. The ACL is a pointto-multipoint link between a master and all slaves in a piconet. The baseband layer also handles error correction. There are 3 kinds of error correction used by the layer; these are 1/3 rate FEC, 2/3 rate FEC, and ARQ. The 1/3 rate FEC simply repeats every bit three times for redundancy and the receiver uses majority ruling to determine which bit to use if the three bits do not match. The 2/3 rate FEC uses a polynomial to encode 10 bit as 15 bit. In an ARQ method certain types of the Bluetooth frames are retransmitted until acknowledge is received. Link Manager (LM) The Link Manager handles link set-up, authentication, and link configuration, which means that everything concerning the actual connections are handled by the Link Manager. Logical Link Control Adaptation Protocol (L2CAP) This layer has three important functionalities. The first is splitting packets from the higher layers into frames for the baseband layer; it can accept packets of up to 64 kb. The second functionality is the handling of multiplexing and demultiplexing of multiple packets. The last functionality is to handle the quality of service requirements. RFCOMM This is a simple transport protocol that emulates a standard RS-232 serial port. This is the protocol used in this project, and thereby it is decided not to explain the other possible protocols like Telephony and Audio. Application Layer The application layer used in this project is defined by LEGO, and the commands used are shown in Enclosure i. The application layer will not be discussed further in this section. Page 119 of 124

120 i Direct Commands for LEGO Mindstorms NXT Direct Commands for LEGO Mindstorms NXT This is excerpted from the direct command descriptions from LEGO [LEGO, 2006a, app. 2]. )*+($$ '. 1 ( 8 * All response packages include a status byte, where 0x00 means success and any non-zero value indicates a!" specific error condition. 2&$*$9$$$9>$ All single byte values are unsigned, unless specifically stated. Internal data type is listed for all multi-byte 2&+*$9$- values and all are assumed to be little-endian as in the LEGO MINDSTORMS NXT Communication 2&#/-*8?!& Protocol. I 347 J H *#$$<+6$$$I 35 2&6/B*! If a legal range is not specified 47 here J explicitly, H *GGG5 it is generally documented in the relevant module documents and/or code.!" Variable * length packet fields are specified as in this example: Byte 4 N: Message data, where N is the 2&$*$9$# variable size of a given field plus command overhead. N may not exceed the Maximum Command Length 2&+*$9$- mentioned above, minus 1. 2&#*!2&!! 2&$*$9$$$9>$ 2&+*$9$$ 2&+*$9$6 2&#/#+*8 2&#*!"!" 4 (8 *$<#H$988" *= C/1 '! 9! 3D+B(-EF " "!"! 5 2&-*0 1 " 4 */+$$<+$$5 2&6*!" *.&42/ 5 2&$*$9$# 2&B*! 47 2LH! 5 2&%*! 2&+*$9$$ 42LH/+$$<+$$5 2&M*!47 2&#*!2& 2LH! 5 2&><+#* 47 H$*! '5!! 2&$*$9$$$9>$!" * 2&+*$9$+ 2&$*$9$# 2&+*$9$6 2&#*!2&!" * 2&$*$9$# 2&+*$9$+ ) 2&#*!2&! : ;* $9$+!"!" 2 = N $9$# 7!O.!O 0 J 7 = $9$6!! 2&$*$9$$$9>$ 2&+*$9$# )! :! ;* 2&#*" 7= G42H P P 7 *"! $9$$ &8=!1.. *0 & &5 2&-/##*8 7= P P (8 *= P C/1 0 $9$ ! 3D+B(-EF! "!"!!" 7= * P P P L $9$# &31.. 2&$*$9$# 4. 1!"!5 2&+*$9$# ) 2&#*!2&! :!;* P 7 P =P $9$$!"!1. P 7 P =P = 070 $9+$!"!1 "/!" P 7 P =P 7 $9#$!"!1.! P 7 P =P = 0 J $96$!"!1 "/1 Page 120 of 124 )*+($$, #$$%!" B

121 )*+($$ )*+($$! 1. "..!7 2!! 2! 2&$*$9$$$9>$!1.!! 2&+*$9$B "( 2&#*"!" 4 *$<-5 2&-*&" 8!9"722! 4! 5!" 2&6* 4! 5!"! (!"!. * 1 1!* 2&$*$9$# 2&+*$9$B 2&#*!2& Figure 1: General protocol architecture )! :&" ;* 2&$* &".&!" P $9$$ )*+($$ " ( "!"! &: ;:" & J I $9$+ ; "<:& ;" 0=7 $9$#!"! ( 8 $9$- =! $9$6 0x00: Direct command telegram, response required 2&$*$9$$$9>$ IP =) $9$B 0x01: System command telegram, response required 2&+*$9$% IP =) $9$% 2&#* 7 P!"!" 2 4 *$<#5 $9$M 0x02: Reply telegram 7 P 2= $9$> 7!" 0x80: * Direct command telegram, $9$Q no response 2&$*$9$# J 0 0x81: System command telegram, $9$= no response 2&+*$9$% J 0P Q) $9$2 2&#*!2& 2&+< P 8P * PL0 " $9$ &" &" 2&-*!"!" 4 *$<#5 2&6*0 )! 1 " 4/+$$/+$$5 : ;* =J 2&B* 4. / 5 $9$$ 2 2&%*!& =! 47 2LH! $9#$ 5 3 %6.&! &".&.'(=" = 2&M*! 42LH/+$$<+$$5 $96$!"! 0 2&>*!47 2! "'1.& 7 2LH! $9%$ 5 3.! 2&Q<+#* 087=! ( 47 $9>$ H! ' " &5 2&+-<+%*!4 7 $9= H!( $!.!!5 8=I I $9$ 2&+M<#$*2!4 =9".'2! = 0 $9$ '1.& H!" '" ' 5 2&#+<#6* =N!4 &.& $9+8 H!"!( ' "!1! 1 5.&( =N $9$ &!.112!!"!. *!! (! 2&$*$9$$$9>$ 2&+*$9$M Figure 2: Bluetooth protocol packages 2&#*"!" 4 *$<-5!!!" *!"" & 2&$*$9$# " &. 1 $9>$ &".& 2&+*$9$M " 9" ( " &!!&?! 2&#*!2& "!!" "" 9 &%$ &(!" 2&-*"!" 4 < 9 " " : "!)!;1!?! 2&6*) G42H 7 1 '!!. '5 " 1!A!. 1 ( 2&B*. G42H 7.!! :. )!; &%*&" 4! 5 2&M* 4! 5 2&><Q*, 1 = O'!47 J H'" 5 #$$%!" 6 2&+$<++* 3= O'!47 J H&" " H *$/+$#-5 2&+#<+-*'!4J, #$$%!" H " 5 M 2&+6<+B*. '!4J H)!. (7 L7 7 (5 &"!.!! (! 2&$*$9$$$9>$ 2&+*$9$> 2&#*"!" 4 *$<-5!" * Page 121 of 124

122 2&#*. 9!. 4$<Q5 2&-* 3 2&6/ * 1 R 3F- H!!!. " (= & Direct Commands for LEGO Mindstorms NXT 3!!!.&( 3!. "" BQ ". 7 2S!" * 2&$*$9$# 2&+*$9$Q 2&#*!2& )*+($$! 1 2&$*$9$$$9>$. "..!7 2!! 2! 2&+*$9$=!1.!! 2&#* "(!"!" 4 *$<#5 2&-* 'G42H 7 *" ' ' 8= *.!" 5 8!9"722!!"!" *!"! ( 2&$*$9$# 2&+*$9$=!. 1 1!* )*+($$ 2&#*!2& " Figure 1: General protocol architecture 2&$*$9$$$9>$ 2&$* &".&!" 2&+*$9$2 $ % " ( "!"! &: ;:" & H. " " ; "<:& ;"!" * ( 1.!1!" (!"! ( 2&$*$9$# 2&+*$9$2!" * 0x00: Direct command telegram, response required 2&#*!2& 2&$*$9$# 0x01: System command telegram, response required 2&-/6*) 2&+* ' '47 J " 5 & 2&#!*$?!!( $ 1 '! 0x02: Reply telegram! (!"$ 2&$*$9$$$9>$ 0x80: Direct command telegram, no response 1 &. '!* 2&+*$9$ 0x81: System command telegram, no response Pending communication transaction in progress 0x20 2&+<!" * * " &" &" Specified mailbox queue is empty 0x40 2&$*$9$# Request failed (i.e. specified file not found) 0xBD 2&+*$9$ Unknown command opcode 0xBE 2&#*!2&!& Insane packet 3 %6.&! 0xBF &".&.'(=" Data contains out-of-range values 0xC0!"! 2! Communication "'1.& bus error 3 0xDD.!! No free memory ( in communication buffer Specified channel/connection is not valid 0xDE 0xDF Specified channel/connection not configured or busy 0xE0 =9".'2! No active program '1.& 0xEC 1 1 Illegal size specified &.&!( 0xED "!1! 1.&( Illegal mailbox queue ID specified 0xEE Attempted to access invalid field of a structure 0xEF!.112! Bad input or output specified * 0xF0 Insufficient memory available 0xFB, #$$% Bad arguments!" 0xFF Q!! Figure 2: Bluetooth protocol packages!"" & " &. 1 $9>$ &".& " 9" ( " &!!&?! "!!" "" 9 &%$ < 9 " " : "!)!;1!?! " 1!A!. 1 (, #$$%!" 6 Page 122 of 124