Remote Automatic Control of Robots

via Wireless Communication Networks

Intersection Coordination of Robots

Bachelor’s thesis in Electrical Engineering

Arsam ShokrianSebastian Ylander MikkelsenOlof DüsterdieckErik Higbie

Supervisor: Mohammad Ali NazariCo-supervisor: Markus FröhleExaminer: Henk Wymeersch

Department of Electrical EngineeringCHALMERS UNIVERSITY OF TECHNOLOGYGothenburg, Sweden 2017

Bachelor’s thesis 2017:SSYX02-17-02

Remote Automatic Control of Robots viaWireless Communication Networks

Intersection Coordination of Robots

Arsam ShokrianSebastian Ylander Mikkelsen

Olof DüsterdieckErik Higbie

Department of Electrical EngineeringChalmers University of Technology

Gothenburg, Sweden 2017

Remote Automatic Control of Robots via Wireless Communication NetworksIntersection Coordination of RobotsARSAM SHOKRIANSEBASTIAN YLANDER MIKKELSENOLOF DÜSTERDIECKERIK HIGBIE

© ARSAM SHOKRIAN, 2017.© SEBASTIAN YLANDER MIKKELSEN, 2017.© OLOF DÜSTERDIECK, 2017.© ERIK HIGBIE, 2017.

Supervisor: Mohammad Ali Nazari, Department of Electrical EngineeringCo-Supervisor: Markus Fröhle, Department of Electrical EngineeringExaminer: Henk Wymeersch, Department of Electrical Engineering

Bachelor’s Thesis 2017:SSYX02-17-02Department of Electrical EngineeringChalmers University of TechnologySE-412 96 GothenburgTelephone +46 31 772 1000

Cover: Two robots moving in a path with an intersection, simultaneously commu-nicating with the UWB sensors. The continuity of the path is visualized with theorange arrows.

Typeset in LATEXGothenburg, Sweden 2017

iii

Remote Automatic Control of Robots via Wireless Communication NetworksIntersection Coordination of RobotsARSAM SHOKRIANSEBASTIAN YLANDER MIKKELSENOLOF DÜSTERDIECKERIK HIGBIEDepartment of Electrical EngineeringChalmers University of Technology

Abstract

The purpose of the project was to create a system for the coordination of two vehi-cles in an intersection. The system should prevent a collision between the vehiclesas they pass the intersection with the help of a controller. It was decided that 50intersections without collision would prove the correctness of the algorithms. Thecontroller should use filtered position data in order to calculate its commands. Thecontrol commands should be optimized in order to increase the e�ciency and com-fort.

To develop and test the finished product, two Pioneer P-3DX robots together withfive PulsOn 400 ’Ultra-wideband’ sensors were used. Three computers with ’RobotOperating System’ installed were used where two of them were connected to therobots. On the third computer, the central-computer, the programs used to controlthe robots were ran. The communication of information between the robots andbetween the positioning system and the central-computer was done through Wi-Fi.

A robust and flexible system was developed that made the robots travel through aself-intersecting path and that satisfied the goal of 50 intersections without collisionusing optimized control commands.

Keywords: Robots, intersection, autonomous, collision avoidance

iv

Sammandrag

Syftet med projektet var att skapa ett system för koordinering av två fordon i en ko-rsning. Systemet skulle förhindra kollision av fordonen med hjälp av en beräknaremedan de passerade korsningen. Det bestämdes att 50 korsningar utan kollisionskulle påvisa att algoritmerna var korrekt. Beräknaren skulle använda filtrerad po-sitionsdata för att beräkna passande kommandon. Utöver detta skulle styrsignalernaoptimeras för att öka e�ektivitet och komfort.

För att utveckla och testa den färdiga produkten användes två Pioneer P-3DX rob-otar tillsammans med fem PulsOn 400 ’Ultra-wideband’ sensorer. Tre datorer med’Robot Operating System’ installerade användes där två av datorerna koppladestill robotarna. På den tredje datorn, den centrala datorn, kördes programmen somkontrollerade robotarna. Kommunikationen mellan robotarna och mellan positioner-ingssystemet och den centrala datorn skedde genom Wi-Fi.

Ett robust och flexibelt system utvecklades som körde robotarna genom en självkor-sande bana och som uppfyllde målet av 50 korsningar utan kollision med optimeradestyrsignaler.

Nyckelord: Robotar, korsning, autonom, kollisionsundvikning

vi

Acknowledgements

We extend our deepest gratitude to our main supervisor Mohammad Ali Nazariand co-supervisor Markus Fröhle for guiding us through this project and alwaysproviding us with assistance and help. We would also like to thank our examinerHenk Wymeersch for enabling and administrating this project.

Arsam Shokrian, Sebastian Ylander Mikkelsen, Olof Düsterdieck, Erik Higbie,Gothenburg, May 2017

viii

Contents

List of Figures xiii

List of Tables xvii

Glossary of Acronyms and Terms xviii

1 Introduction 11.1 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Problem statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Scope & Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.4 Similar projects and their relevance . . . . . . . . . . . . . . . . . . . 3

2 Theory 52.1 Trilateration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 Gradient Descent . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.3 Extended Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . 72.4 Optimal Coordination Problem . . . . . . . . . . . . . . . . . . . . . 9

2.4.1 Definition of robustness . . . . . . . . . . . . . . . . . . . . . 92.4.2 Definition of e�ciency . . . . . . . . . . . . . . . . . . . . . . 102.4.3 Ruled based solutions . . . . . . . . . . . . . . . . . . . . . . . 10

2.4.3.1 Reservation solution . . . . . . . . . . . . . . . . . . 102.4.3.2 Priority solution . . . . . . . . . . . . . . . . . . . . 10

2.4.4 Optimization solution . . . . . . . . . . . . . . . . . . . . . . 112.4.5 Hybrid solutions . . . . . . . . . . . . . . . . . . . . . . . . . 11

3 Equipment 123.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.1.1 Ultra-wideband range sensor . . . . . . . . . . . . . . . . . . . 123.1.2 Wireless Communication link . . . . . . . . . . . . . . . . . . 133.1.3 Pioneer 3-DX . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.1.4 Computers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3.2 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.2.1 ROS and ROSARIA . . . . . . . . . . . . . . . . . . . . . . . 14

3.2.1.1 Nodes . . . . . . . . . . . . . . . . . . . . . . . . . . 143.2.1.2 Topics and Services . . . . . . . . . . . . . . . . . . . 153.2.1.3 RosAria/cmd_vel . . . . . . . . . . . . . . . . . . . . 15

x

Contents

3.2.1.4 Odometry . . . . . . . . . . . . . . . . . . . . . . . . 153.2.1.5 MobileSim . . . . . . . . . . . . . . . . . . . . . . . . 16

3.2.2 Python and Rospy . . . . . . . . . . . . . . . . . . . . . . . . 16

4 Methods 174.1 Initial Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.2 Determining the state of the robots . . . . . . . . . . . . . . . . . . . 18

4.2.1 Robot state . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184.2.2 Setup of components for determining the state . . . . . . . . . 194.2.3 Implementation of trilateration . . . . . . . . . . . . . . . . . 194.2.4 Implementation of the extended Kalman filter . . . . . . . . . 224.2.5 Generating noise for Pose-data . . . . . . . . . . . . . . . . . . 254.2.6 Calculating an intersection point . . . . . . . . . . . . . . . . 25

4.3 ROS structure and data handling . . . . . . . . . . . . . . . . . . . . 264.4 Communicating instructions to the Robot nodes . . . . . . . . . . . . 28

4.4.1 Basic movement missions . . . . . . . . . . . . . . . . . . . . . 284.4.2 Angular control mission . . . . . . . . . . . . . . . . . . . . . 294.4.3 More complex movement missions . . . . . . . . . . . . . . . . 294.4.4 Polling robot for state . . . . . . . . . . . . . . . . . . . . . . 29

4.5 Description and function of controller algorithms . . . . . . . . . . . . 304.5.1 Implementation of reservation solution . . . . . . . . . . . . . 304.5.2 Implementation of priority solution . . . . . . . . . . . . . . . 324.5.3 Implementation of hybrid solution . . . . . . . . . . . . . . . . 35

4.6 The setup of the experiment . . . . . . . . . . . . . . . . . . . . . . . 36

5 Results 385.1 Results of the experiment . . . . . . . . . . . . . . . . . . . . . . . . 41

5.1.1 Controller results from a single measurement . . . . . . . . . . 445.2 System robustness for di�erent scenarios . . . . . . . . . . . . . . . . 455.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

5.3.1 Controller in simulation . . . . . . . . . . . . . . . . . . . . . 475.3.2 Controller in simulation with di�erent noise levels . . . . . . . 485.3.3 Controller in simulation with di�erent velocity intervals . . . . 505.3.4 Controller in simulation with decreased starting distances . . . 50

6 Discussion 526.1 General analysis of the system . . . . . . . . . . . . . . . . . . . . . . 52

6.1.1 The performance of the UWB sensor grid . . . . . . . . . . . . 536.1.2 Positioning and angle calculation during filtering . . . . . . . . 546.1.3 Evaluation of hybrid algorithm . . . . . . . . . . . . . . . . . 56

6.2 Simulation analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 576.3 System robustness analysis . . . . . . . . . . . . . . . . . . . . . . . . 586.4 Further development on the intersection controller . . . . . . . . . . . 59

6.4.1 Improved robustness . . . . . . . . . . . . . . . . . . . . . . . 596.4.2 Further solution possibilities . . . . . . . . . . . . . . . . . . . 59

7 Conclusion 61

xi

Contents

Bibliography 62

A Appendix IA.1 Start-up code and instructions . . . . . . . . . . . . . . . . . . . . . . IA.2 Images of robot setup and UWB . . . . . . . . . . . . . . . . . . . . . IIIA.3 API and Code documentation . . . . . . . . . . . . . . . . . . . . . . IV

A.3.1 RobotHandler . . . . . . . . . . . . . . . . . . . . . . . . . . . IVA.3.1.1 Subscribed topics . . . . . . . . . . . . . . . . . . . . IVA.3.1.2 Provided services . . . . . . . . . . . . . . . . . . . . IV

A.4 Additional measurements . . . . . . . . . . . . . . . . . . . . . . . . . VII

xii

List of Figures

2.1 This figure describes how the distance of the point of interest fromeach anchor all cross to form a fixed location. . . . . . . . . . . . . . 5

2.2 This figure depicts the stages in finding a location using trilaterationstarting with one anchor and moving up to three. . . . . . . . . . . . 6

2.3 The blue dot and its accompanying error ellipsiod is the measuredposition and its associated noise. The red dot and haze representsthe predicted position and its associated noise respectively. The greendot represents the estimated position after filtering this data, whilethe yellow dot represents the actual position. . . . . . . . . . . . . . . 8

4.1 The figure shows the coordinate system and angle of the robot, whichare used to create a model of movement. It also shows the iterativestates of the system from X

k

to Xk≠2

. . . . . . . . . . . . . . . . . . 184.2 The leftmost subfigure describes the possibility of overlap causing

a residual error. The middle one describes the opposite with zerooverlap. The rightmost is the ideal case with perfect overlap . . . . . 20

4.3 This figure describes the following procedure for one of the sectionsof the sum for the total residual. The red vector is the equivalent ofa. The light blue vector is the r vector and the green vector describesthe vector whose magnitude is f(x) or the residual of the system. . . 21

4.4 This figure describes how the criteria are met for the tolerances ofthe residual in the descent function. In the case above the chosentolerance A was met before the secondary criteria R was met. . . . . 22

4.5 Riemann sum when using continuous angular velocity changes (ac-cording to a regulator). The area under the curve represents the ac-tual angle of the robot, while the rectangles represent the predictedangle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.6 Riemann sum when using three specific angular velocities. The greencurve represents the actual angle of the robot where the rectanglesare predictions of the angle. . . . . . . . . . . . . . . . . . . . . . . . 24

4.7 The figure shows an example of the robots, their position and angledata, and their resulting intersection point. x

1

, y

1

, ◊

1

describes thedata from the first robot, x

2

, y

2

, ◊

2

is for the second robot and IPx

and IPy

is the intersection point. . . . . . . . . . . . . . . . . . . . . 25

xiii

List of Figures

4.8 A figure modeling the basic structure of the code in the program thatruns the robots, location system, and intersection control algorithms.The blue boxes represent ROS nodes, the circles inside these repre-sent topics inside each of these nodes while the solid lines representpublisher and subscriber relationships between these di�erent topics.The dashed lines represent services and the hexagon shape representsa service proxy that creates and handles these services. . . . . . . . . 27

4.9 The figure depicts the two intersecting robots at di�erent positionsand times throughout the intersection, matching the time slots chosenby the user. In this case, both the robots are able to accelerate anddecelerate and in general act more independently. . . . . . . . . . . . 30

4.10 The figure depicts the two intersecting robots at di�erent positionsand times in the intersection. The yellow robot is the stable robotwhile the red one is the variable robot. The figure also shows theconcept behind the guard distance and how the robots are at di�er-ent positions when the uncontrollable robot is in the middle of theintersection. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.11 The figure shows the concept behind the hybrid algorithm betweenthe priority and the optimization solutions. The arrows show thevelocities of the robots, and as in the priority solution the yellow oneis the stable robot while the red one is the variable robot. Importantto note is that a

k

is the smallest acceleration solution that the variablerobot can find. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.12 This figure represents the setup of the experiment to scale. The an-chors are represented by antenna symbols and are placed in three lo-cations around the intersection. Each robot has its path also mappedout using the dashed lines in the figure showing the complete. Thefigure does not depict the controller or the Wi-Fi network as theirpositions are not important to the experiment. . . . . . . . . . . . . . 37

5.1 The figure shows the resulting x- and y-coordinates following 10000location measurements from the UWB sensors. . . . . . . . . . . . . . 38

5.2 The distribution of these 10000 location measurements in the x-axisand a best fit normal distribution curve overlayed. . . . . . . . . . . . 39

5.3 The distribution of these 10000 location measurements in the y-axisand a best fit normal distribution curve overlayed. . . . . . . . . . . . 39

5.4 Measurements of the position from the predicted state, sensors andfilter as the robot moves with constant angular and linear velocity.Note that the axis are shifted which deforms the otherwise circularmotion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.5 Measurements of the predicted state, sensors and filter as the robotmoves in a square. When it reached a point, it stood still for a secondbefore moving on, resulting in some point clusters around the pointsof the square. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

xiv

List of Figures

5.6 The figure shows the distances travelled for each robot, calculatedfrom their respective starting points. In this case Robot 1 acceleratesin order to avoid collision whilst Robot 0 maintains constant velocity. 41

5.7 The figure shows the distances travelled for each robot, calculatedfrom their respective starting points. In this case Robot 1 deceleratesin order to avoid collision whilst Robot 0 maintains constant velocity. 41

5.8 The figure plots the velocities of the robots against time. The greendata represents the stable robot whilst the blue data is the variablerobot. The initial velocities are randomized and both travel to theends of the intersection. After the possibility of a collision disappears,the optimization of acceleration is disregarded. . . . . . . . . . . . . . 42

5.9 The figure shows data gathered from three di�erent measurements.The data itself is the length between the robots plotted against time.The blue and green data were collected when the variable robot de-celerated whilst the red data was from when it accelerated. . . . . . . 43

5.10 The figures 5.10a, 5.10b and 5.10c show di�erent data collected fromthe same measurement. Figure 5.10a plots the distance covered, 5.10bis velocity of the robots and 5.10c is the distance between them. Theblue lines represent Robot 1 which is the one that is controlled, whilethe green lines represent Robot 0 which is the stable robot and travelswith constant velocity. In this case the variable robot mostly accel-erated. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

5.11 Simulation results of hybrid algorithm using 4 meters distance tocenter, 1 meter negative guard distance and 2 meter positive. Ran-domized starting velocities in interval 0.1 to 0.5. Figure 5.11a showsthe case in the simulation were the variable robot crosses the inter-section first while optimizing its acceleration. Figure 5.11b shows thesame thing but with deceleration and the stable robot crossing first. . 47

5.12 Simulation results of hybrid algorithm using 4 meters distance tocenter, 1 meter negative guard distance and 2 meter positive. Ran-domized starting velocities in interval 0.1 to 0.5 and di�erent noiselevels. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

5.13 The figure shows the hybrid algorithm traversing through the inter-section 20 times with di�erent noise levels. The guard distance was1.0 in the negative direction and 1.5 in the positive, whilst the veloc-ity interval was from 0.1 to 0.5. In the case with standard deviationof noise 0.15, shown in figure 5.13d, only 5 intersections were recordedsince collision could not be avoided with the selected guard distance. 49

5.14 The figure depicts the path travelled by the two robots when thestarting velocity interval goes from 0.1 to 0.8 m/s. The pathing is aresult of 10 intersections. . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.15 The figure shows the pathing of the two robots as their distancesto the intersection point are set to 2 meters. The starting velocityinterval is from 0.1 to 0.5 and the figure shows the results from 10intersections. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

xv

6.1 The figure highlights an area of where the predicted state preventsthe observed state from recording an incorrect action. . . . . . . . . . 55

6.2 This is an example of a calibration mission that the robot can runafter the intersection in order to fix faulty orientation data in therobot. The red dotted vector shows the angle of orientation that therobot believes it has when starting on its trajectory as shown by theblack vector. Over time this vector is show to be fixed as the robotaccumulates more accurate position data to compare its orientationwith. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

A.1 The figure shows two images, one of the robot setup and one of theUWB. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . III

A.2 The figures A.2a, A.2b and A.2c show di�erent data collected fromthe same measurement. Figure A.2a plots the distance covered, A.2bis velocity of the robots and A.2c is the distance between them. Inthis data set, the controllable robot decelerates and reaches the endof the path after the uncontrollable robot. . . . . . . . . . . . . . . . VIII

A.3 The figure shows the speeds of the robots as they travel to andthrough the intersection. . . . . . . . . . . . . . . . . . . . . . . . . . VIII

List of Tables

4.1 Table defining the di�erent missions the robot is able to execute. . . . 284.2 The table shows the values specified by the user for the reservation

based system. These values are in the form of time slots, intersectionvalues and the total length of the travelled path. . . . . . . . . . . . . 30

4.3 The table shows the values required from the user in the implemen-tation of the priority solution. These are categorized into a velocityinterval, a guard distance and the total size of the system. . . . . . . 33

Glossary of Acronyms and Terms

ROS Robot Operating SystemARIA Advanced Robot Interface for ApplicationsUWB Ultra-wide bandEKF Extended Kalman Filter

Gradient The multi-variable generalization of the slope of a curveat a given point

Riemann sum An estimation of the area under the curveGuard distance Used when describing the priority protocol. Is the

distance from the middle of the intersection to a displacedpoint in the y-axis were the controllable robot should notmove farther than if the uncontrollable robot is in theintersection

Stable robot Robot with constant velocity. Used in Priority andHybrid solution

Variable robot Robot that receives control actions thus changing itsacceleration and its velocity. Used in Priority and Hybridsolution

1Introduction

In a world with continuously increasing automation, many tasks that could onlybe done by humans can now be accomplished by machines. A clear area of thisadvancement is in transportation systems, and most relevantly, the autonomousvehicle. This can be seen even today with the widespread adoption of active safetysystems and driver aids in vehicles, making not only travel safer but easier and moree�cient. As these automated systems become more commonplace in vehicles andhave a greater responsibility for aspects of driving, the time may come when thesevehicles take over tra�c coordination from their human occupants. This will make itpossible for vehicles to either partially or entirely drive themselves on the roads of thefuture. Before that can happen however, there are a variety of challenges that mustbe addressed and solved. One of them is how to handle intersections of vehicles. Ashuman drivers are removed from the intersection problem, the possibility for moreoptimal solutions to this problem appears.

There are di�erent ways to handle intersections today. Some are purely human-based and were created to optimize safety, like the ‘priority to the right’ systemwhere, when two vehicles meet at an intersection, the one on the right has priority.Tra�c lights are another such rule based solution which try to optimize throughputby maintaining tra�c flow through the intersection. Other intersection techniquesare more structural and priority based, like the roundabout, created to try to givebetter throughput while maintaining high safety.

Unfortunately, these solutions may be far from optimal in regards to energy con-sumption, passenger comfort, time and foremost safety. In many situations, vehi-cles may have to wait for other vehicles by slowing down or coming to a completestop, while waiting for the intersection to clear or a tra�c light to turn green. Incombination with human responsiveness, reflexes and movement capabilities, thesefactors lay the basis for many forms of tra�c congestion and potential collisions.Autonomous vehicles can minimize or even eliminate these shortcomings by havingvastly superior response time and the ability to communicate to each other wire-lessly [1]. These abilities could solve the coordination problem that intersectionspose in a faster and far more e�cient way.

The idea of optimal tra�c flow is not only specific for intersections but is one of themain arguments for autonomous transportation. Enhanced safety, e�ciency, pollu-tion, and comfort are among the clear advantages of automating the transportation

1

1. Introduction

process [2]. These advantages allow the vehicles in use to enhance the driving expe-rience in a way that is di�cult to implement in the human controlled vehicles usedtoday.

1.1 Purpose

The goal of this project is to have a completely implemented solution for controllingautonomous vehicles crossing an intersection, emulating a real world scenario. Thesolution is able to prevent a collision between two vehicles that are passing throughthe intersection. In addition to this, it performs optimization through minimizingcontrol commands, i.e. acceleration/deceleration, and also moves as smoothly aspossible.

The algorithm was developed with the goal of solving the intersection coordinationproblem robustly enough for the given scenario yet flexibly enough to be imple-mentable on di�erent intersection problems involving two vehicles. It is thereforestructured in a way that its modules are as interchangeable as possible and allowfor changes to be made without the need to make larger alterations to the programitself. These changes can include, but are not limited to, di�erent sensors, vehicles,processing units and so on.

1.2 Problem statement

The main problem is to develop and implement a functioning solution for au-tonomous robots to travel from one side of the intersection to the other withoutcolliding with another vehicle. As this problem is broad and complicated, it isdivided into smaller tasks.

• Program the vehicles to move in a shape containing an intersection. It isimportant to be able to vary the speed of the vehicles along the path, so thatthe actual controller can later optimize the movements in a later stage.

• Measure and estimate the states of the vehicles around the intersection withthe aid of sensors and accuracy-enhancing technology such as data filters.This sets a basis for sensing the positions of the vehicles as well as accuratelycontrolling them in the intersection area. Robustness of the intersection coor-dination algorithm partially depends on the accuracy of the location of eachvehicle.

• To have a central-computer that calculates as well as adjusts the course andspeed of the vehicles near the intersection to avoid collision. This is done usingthe states such as location and speed measured in the previous task.

• Establish a communication link between the central-computer and the vehiclesso that they can communicate to each other.

2

1. Introduction

• Evaluate di�erent types of algorithms being used by the controller and deter-mine their characteristics.

• The final solution should be able to compute and perform 50 successful cross-ings with di�erent starting parameters.

The final problem is to combine the solutions to all these smaller solutions intoone system. The vehicle paths and positions will be pinpointed by the controllerusing the accurate location data. The controller will then be able to coordinate themovement of the vehicles in an optimal way and that avoids collisions. Once therisk of collision is over, the optimization demand is removed in order to expeditethe run-time of the experiments. The controller should communicate these controlcommands to the vehicles over the communication link. Finally the robots shouldbe able to implement these commands accurately and then give feedback from theirnew state to the controller to start the process over again.

1.3 Scope & Limitations

In the project, the focus is on two robots in a one lane intersection. The robots willhave predefined paths that they should follow. The only requirement is that thepath contains an intersection and that it is continuous. Therefore scalability willnot be tested in this project.

UWB sensors will be used to get location data and accordingly the trajectory ofeach robot. There will however be measurement errors. Because of this a filter willbe used to increase the accuracy. Other ways of locating the robot will not be usedin order to simplify potential solutions and emulate a more realistic scenario.

The controller will optimize control commands sent to the robots to make the pass-ing of the intersection smooth. As there exists uncertainties and randomness inthe scenario, an optimization problem guaranteeing collision avoidance with an ac-ceptable probability will be implemented. The acceptable probability is set 1% for50 intersections, using solvable starting parameters. Several ma constraints are alsoplaced on the robot to mimic comfort requirements, these being a maximum velocityof 1 m/s and a maximum acceleration of 1 m/s2.

1.4 Similar projects and their relevance

The project that was done here is a part of larger area of study both for the institu-tion but also the wider world. Not only are there other bachelor projects that havedeveloped other areas of automating the robots but there are a variety of papersthat theoretically describe potential solutions for intersection control. These arequite advanced but provided significant inspiration for the types of controllers thatwere implemented and emulated in this project.

3

1. Introduction

For instance, the two ideas presented in the report on how to build up the controlalgorithms were influenced by two papers [3] [1] which describes potential optimiza-tion of similar setups and the implementation of model predictive control in concertwith these types of algorithms respectively. Additionally, setting constraints on thecontrol action sets that are carried out by the robots and further along, the con-cept of guard distances in the controller algorithms was spawned by another paperon intersection control using a convex optimization, something well outside of thebounds of this report [4]. The way this paper di�ers from many papers of this typethat from the onset have solved the problem of intersection control and in a muchmore e�cient way is that these are highly theoretical whereas the paper at hand isdealing with an emulation of the system and all the challenges that can provide.

It is important to mention that a similar type of project has been attempted in theprevious year at the electrical engineering institution that handled these robots andgave a solid base upon which this report builds upon, particularly in regards to theUWB sensor system and the ROS utilization [5].

4

2Theory

The theoretical basis for the project is undoubtedly broad and covers di�erent ar-eas of kinematics, positioning, filtering and optimization. Ignoring the depth ofthese fields and the possibility of delving into all the minute details, the necessaryknowledge for understanding the project is presented in this section.

2.1 Trilateration

Trilateration is a concept that is used in a variety of localization applications such asthe global positioning system[6]. In the scenario presented in the project, the worldthat is being mapped is in two dimensions. To describe the concept of trilateration,a theoretical structure of how the method works is presented.

Figure 2.1: This figure describes how the distance of the point of interest fromeach anchor all cross to form a fixed location.

Suppose a scenario where there are at least three fixed anchors with known positionsplaced in the area where another object is also placed but whose position is unknown[6]. Assume that the object has the ability to accurately measure the distance

5

2. Theory

between itself and any known and fixed anchor in this area. This allows that object,using these measurements, to accurately determine where it is in relation to theanchors, whose position is known. This is shown in figure 2.1. In the figure 2.2a,the distance of the object is known but not the location as it can exist on infinitelymany locations along the distance circle from the first anchor. In the next figure2.2b, however, this uncertainty has been reduced to only two position where the twodistance circles intersect. This is unless the two distance circles only intersect atone point. This, however, is considered unlikely enough to be a negligible concern.The location of the object is then made certain due to the intersection of all threedistance circles resulting in only one possible location as shown in figure 2.2c. Thistechnique in combination with gradient descent is what will be used to providemeasurement data for the robot as it moves through the scenario.

(a) Single anchor active, infinitepoints possible

(b) Two anchors active and twopotential points found.

(c) All three anchors active andpoint found

Figure 2.2: This figure depicts the stages in finding a location using trilaterationstarting with one anchor and moving up to three.

6

2. Theory

2.2 Gradient Descent

Due to potentially inaccurate data that fails to produce an exact result, it is nec-essary to have an algorithm that is able to find a best fit solution. The gradientdescent algorithm can be used for this purpose.

Start by considering a problem P which describes a function that ideally needs tobe minimized [7].

P : min(fx

), (2.1)

where x œ Ÿn. It is assumed that this function is continuously di�erentiable in Ÿn

and therefore the gradient can be found for all points. Starting at a point x

k

thegradient of the function at this point can be described as Òf(x

k

). The gradientdescent is then described as follows:

f(xk+1

) = x

k

+ ⁄ · Òf(xk

). (2.2)

This iterative process will then approach a local minimum of function f

x

as longas ⁄ < 0 and Òf(x

k

) ”= 0. The minimum of the equation 2.1 is found due to thenegative descent of the function. This means that when Òf(x

0

) approaches zero, theproblem moves closer to its minimum x

k

as long as the descent ⁄ ·Òf(xk

) maintainsa negative value.

It is important to note that using this method and depending on the value of ⁄ willmake the function approach its minimum more rapidly at first. It will then becomeslower the more iterations are done, making the function e�cient if the precision ofthe solution is not of the highest priority[8].

2.3 Extended Kalman filter

The Extended Kalman filter (EKF) is an extension of the Kalman filter that can beapplied on non-linear state models[9]. After linearizing the state-space model, theEKF will act like a regular Kalman filter. Both the linearization and Kalman filterwill be described later in this section.

When using a Kalman filter it is important to note that while using a model of asystem to calculate its states, errors will occur due to external factors. It is thereforenot possible to model a system with absolute certainty and it is this uncertainly thatis called process noise. The purpose of the filter is to reduce the process noise thatcomes from predicting new states each iteration from the model of the system [10].This can be done by using an observation about known states such as a positionand the covariance, considered to be the error, of the observations together with theprediction.

This is graphically described by the figure 2.3 where it is possible to see that erroris reduced due to the fact that it has been overlayed with the possible position and

7

2. Theory

noise of the observation. This gives the final position a significantly lower value forits noise than either the model prediction or the observation would have had ontheir own.

Figure 2.3: The blue dot and its accompanying error ellipsiod is the measuredposition and its associated noise. The red dot and haze represents the predictedposition and its associated noise respectively. The green dot represents the estimatedposition after filtering this data, while the yellow dot represents the actual position.

In order to apply the equations corresponding to the Kalman filter, a linear modelneeds to be obtained. To linearize the model, the Jacobian matrix of the non-linear functions is derived at a linearization point. With the non-linear state-spacedi�erential equations on the form

x

1

= F

1

(x1

, ..., x

n

, u

1

, ..., u

m

),...

x

n

= F

n

(x1

, ..., x

n

, u

1

, ..., u

m

),(2.3)

where x

1

, ..., x

n

are the states of the system and u

1

, ..., u

m

are the inputs, the lin-earized model can be written as

xk|k≠1

= Axk≠1|k≠1

+ Bu (2.4)

where

A =

S

WWU

ˆF1ˆx1

|xs,us · · · ˆF1

ˆxn|xs,us

... . . . ...ˆFnˆx1

|xs,us · · · ˆFn

ˆxn|xs,us

T

XXV , (2.5)

B =

S

WWU

ˆF1ˆu1

|xs,us · · · ˆF1

ˆum|xs,us

... . . . ...ˆFnˆu1

|xs,us · · · ˆFn

ˆum|xs,us

T

XXV (2.6)

[11].

The filter first creates a prediction of the new state xk|k≠1

using the old state vectorx

k≠1|k≠1

and the linearized model of the system, the A and B matrices, as well as

8

2. Theory

the input signals, u. The error covariance matrix Pk|k≠1

is also predicted using thesame model, the previous error covariance matrix P

k≠1|k≠1

and the process noisecovariance matrix Q. This is done according to the following equations

xk|k≠1

= Axk≠1|k≠1

+ Bu, (2.7)P

k|k≠1

= APk≠1|k≠1

AT + Q. (2.8)

After the state vector and error covariance matrix have been predicted, the Kalmangain will be used to correct the errors in the predicted state by adding the scaleddi�erence between the observed states (z

k

) and the predicted states according tothe equation

xk|k = x

k|k≠1

+ Kk

(zk

≠ Axk|k≠1

). (2.9)The error covariance matrix is also updated by taking the previously predicted errorcovariance and subtracting with itself scaled with the Kalman gain and the modelmatrix A, resulting in

Pk|k = P

k|k≠1

≠ Kk

APk|k≠1

. (2.10).

The Kalman gain is calculated as follows:

Kk

= Pk|k≠1

AT (APk|k≠1

AT + Rk

)≠1 (2.11)

where Rk

is the observed measurement’s error covariance matrix which describesthe inaccuracies in the observed states.

After the state has been updated according to the above calculations, it shouldoutput a more accurate state (x

k|k) and its error covariance matrix (Pk|k) than from

the prediction or the observed state by themselves.

2.4 Optimal Coordination Problem

There are a variety of di�erent control schemes that can be used to prevent vehiclesfrom colliding when approaching an intersection. Each of these schemes have theiradvantages and disadvantages with the major factors being the robustness of thesystem, the e�ciency of the system, and the comfort of potential passengers in avehicle. [1]

2.4.1 Definition of robustness

The main purpose of the controller is to avoid collision. This goal falls under theconcept of how robust the system is. While it might seem ideal to create a systemthat prevents collisions at all possible times, realistically this is not feasible due toconstraints on both the maximum control actions, acceleration and deceleration,that can be taken by the cars.

9

2. Theory

For the purpose of this report, robustness is defined as the probability of a crash.Simply put the average number of intersection without a collision.

2.4.2 Definition of e�ciency

E�ciency in the terms of intersection control have several aspects. Firstly, e�ciencycan refer to the control actions, which require power, taken by each vehicle andideally minimize the total control actions taken. Additionally the e�ciency canrefer to the entire system’s e�ciency in regards to how rapidly the intersection iscleared.

For the purpose of this report, e�ciency is only considered during optimized solu-tions and is defined as minimizing control actions.

2.4.3 Ruled based solutions

Many solutions use a clear set of rules to define how vehicles are able to movethrough the intersection [1]. These focus mainly on guaranteeing a clear and safepath for all vehicles through the intersections and can therefore sometimes neglecte�ciency.

2.4.3.1 Reservation solution

The reservation solution is built upon the idea that both intersecting robots reservetime slots in which they are present at the intersection. By choosing non-overlappingtime slots, the robots are able to be in the intersection at di�erent times henceavoiding a collision. A controller is used to tailor the control actions of each vehiclein order to both enter and exit the intersection during each vehicle’s reserved timeslot. This system has the advantage of simplicity as both vehicles can have individualpaths and control action set to make clearing the intersection as safe as possible.Another advantage of the system is that it can be easily and e�ectively scaled aseach vehicle needs only to know its assigned time slot and how to get there.

2.4.3.2 Priority solution

The priority solution is where two vehicles approach an intersection and one is givenpriority over the other. For example, car A and car B approach the intersection.Car A enters first and has a certain acceleration and speed. Due to the lack of anyother cars that could possibly create a collision, the controller takes no action. CarB then enters the intersection and queries the controller on whether or not the carcan cross collision free. The controller then looks at this situation and determineswhether or not a collision would occur. If so, car B will then brake or acceleratedepending on constraints such as passenger comfort, limits on control actions, ande�ciency. If collision is unavoidable with control actions on car B, the controllercould then attempt a solution for Car A.

10

2. Theory

2.4.4 Optimization solution

These are solutions that find the most e�cient control action set for all vehiclespassing through the intersection. They have no rules but are able to calculate thesolution to the problem resulting in the most e�cient solution for sending bothvehicles through the intersection without collision [1].

These solutions tend to be incredibly di�cult to solve particularly with increasingamounts of vehicles in the problem requiring more processing power and longercomputation times. This is not ideal in the small window for computation that isa�orded in an intersection control problem [4].

2.4.5 Hybrid solutions

There are solutions that use rules to build a solution for the vehicles to followbut then optimizes how each vehicle completes its assigned solution. This has theadvantage of possibly being more e�cient than a strictly rule based system but isnot as di�cult to calculate as a purely optimization based approach. This is theapproach we follow in this project.

11

3Equipment

In this chapter the equipment used is presented and described. Overall this equip-ment is modern and relevant with the possibility of other more advanced or complexsystems replacing them if the experiment were to be scaled up. The equipment isclassified into two major categories, hardware and software, in which it is the soft-ware where the majority of the work in the project is conducted.

3.1 Hardware

In this section the hardware used for the project is presented. This includes hard-ware for positioning, the robots themselves as well as standard equipment such ascomputers and routers.

3.1.1 Ultra-wideband range sensor

The UWB ranging sensoring is a system used for measuring the distance between twopoints using a wide radio frequency band. This sensors are designated the PulsON400 Ranging and Communications Module and are made by the United States basedcompany TimeDomain [12]. They feature a built-in specialized high frequency chip-set, ethernet NIC port, and ultra-wideband transceiver antenna. This allows forlocalization, limited radar and communications for a wide variety of applications.For this project, the localization aspect of the unit will be used which accomplishesthis via a Two-Way Time-Of-Flight method [13]. The base will start by transmittinga range request to one anchor, which is a fixed sensor in the room. This requestwill be a wave-form pulse that propagates out from the antenna in all directions[14].The base then starts an internal timer on time T

1

. The anchor, after listening forsuch a pulse, will then reply with a wave-form back to the base which will arriveat time T

2

. The total time between send and receive will be T

r

as shown in theequation

T

r

= T

2

≠ T

1

. (3.1)Assuming the medium the wave travels through is air, the speed of the wave isextremely close to the speed of light in vacuum c

0

. The distance d traveled can then

12

3. Equipment

be calculated byd = c

0

· Tr

2 (3.2)

where you divide by 2 because the wave has traveled both to the anchor and back.

Using these times, the base can calculate the distance traveled by the wave, leadingto a measurement accuracy of 3.5cm with line of sight, e�ectively. It is importantto note that for this project, the measurement units used were in varying stages ofperformance due to their age and could have a�ected the accuracy of the locationdata, but this is disregarded. Newer versions of these antennas have an increasedaccuracy of up to 2 cm with line of sight [15].

3.1.2 Wireless Communication link

The fact that the project utilizes multiple robots necessitates the need for a datalink between these robots and the controller. To cover this, a Wi-Fi router is used.Wi-Fi is well within the requirements of this project giving low latency and high datarate with acceptable stability [16]. It is this network that allows the Robot Operat-ing System (ROS) to function across several di�erent computers and consequentlydi�erent robots simultaneously.

3.1.3 Pioneer 3-DX

The robots used for this project are Pioneer 3-DX mobile robots [17]. These arecompact, robust, wheel mounted robots that are suitable for emulating vehicles inthe constrained testing environments used for this project. They have two drivewheels giving two degrees of freedom with a third wheel to give stabilization thathas a free floating axis. It additionally has a front and side facing sonar system butthese are not used in this project.

3.1.4 Computers

In order to connect all the various pieces of equipment and control them, severallaptops (Dell Latitude E5450) where used running Ubuntu Linux version 14.04 or16.04 Trusty Thar [18]. Each robot has an on board laptop on board coordinatingvarious measurements and control commands. There is another laptop that servesas the remote controller, which runs all of the coordination software.

3.2 Software

The software package used is called ROS and a variety of programs coupled withthis operating system.

13

3. Equipment

3.2.1 ROS and ROSARIA

ROS is an operating system that allows the smooth operation of each robot andcommunication between them and the main controller [19]. This is a highly flexiblesystem allowing the smooth flow of information between many decoupled entities,subsequently called nodes. It is licensed under the Creative Commons Attribute 3.0[20]. Communication between the Pioneer robots and ROS is handled using a librarycalled ROSARIA. It merges the robot’s manufacturer’s Advanced Robot Interface forApplications software development kit, which allows the direct control of the robot’ssystems with the ROS. This allows for high level commands to be performed by therobot such as velocity and angle changes. ROS has multiple packages allowing forthe operating system to run di�erent programming languages such as C++, Lisp,and the language which this project is programmed in, Python. In order to runthe latest stable version of ROS, Kinetic Kame Linux is required and can be runthrough either an Debian or Ubuntu operating system, the later of which is used asspecified earlier. ROS can be found on ROS.org and the ROSARIA library can befound and installed from here [21]. Without ROSARIA, running the robot wouldbe significantly more di�cult and would require directly controlling all of the robotssystems, for example its wheel motors.

To give the reader a basic understanding of the implementation of ROS and sub-sequently the software behind the controller, a brief overview of key concepts arecontained below. This have been primarily sourced from the ROS Wiki which con-tains all the documentation used in this project and has been the primary guide forthe utilization of ROS. The sections below and further on in the paper refer to thisWiki.

3.2.1.1 Nodes

ROS divides up its active functions into packages called nodes. These nodes arewhat can be considered the “face” of a certain set of programs in the larger ROSnetwork. For instance a node may be programmed to accomplish a certain goal, suchas driving a robot, and as such will handle all the code devoted to fulfilling thatfunction. It will also communicate with other nodes for information on such thingsas positioning data, desired control actions, or status updates that the other nodeseither give or require for their continued function. In this way the code structureof ROS becomes highly distributed allowing nodes to be added and removed asnecessary for the project. All of these nodes are held together by the roscore. This isthe master program that orchestrates the running of all the other nodes to preventconflicts between them. It is this flexibility that makes ROS useful as a codingstructure, especially for a project such as this with multiple computers runningdi�erent equipment.

14

3. Equipment

3.2.1.2 Topics and Services

Topics and Services are predefined methods used by ROS nodes to communicate viathe ROS master, roscore, or with each other. The services and topics, which areboth used in the project, operate in slightly di�erent ways.

The Service is a function that is set up and connected to the node that initializesit. As long as the service is not called, the function will not be executed. If anothernode needs to use this service and is located on the same master, it can call on itto execute. Usually some variable or other data is used in combination with callingon a service. The service will then execute the function and optionally return somedata depending on the function. To specify what data needs to be transferred witheach service call, a service file (.srv) will define the variables that are sent with eachcall as well as the variables that will be returned.

Topics are similarly used by two or more nodes where one of the nodes is a publisher.The publisher will send data onto this virtual data bus that can be accessed by allother nodes on the same ROS master. Other nodes can then subscribe to therelevant topics. When data then is published, the subscriber node will use the datain combination with a callback to execute a defined function. The data publishedon these topics are called messages and are defined with the message files (.msg).Topics are unidirectional, meaning that a node does not usually publish to the samenode it subscribes to.

3.2.1.3 RosAria/cmd_vel

In ROSARIA, a predefined topic is created with each RosAria node, called /cmd_vel.The topic is subscribed to by the robots, and allows other nodes to send messagesof the type Twist, containing control commands in the form of linear speeds (x, y, z)and rotational speeds (x, y, z) to the RosAria nodes. The message used additionallycontains the possibility to send covariances of the processes that are being performedto give a greater detail of the accuracy of the robot’s actions. The RosAria nodetranslates all published messages on the topic into low-level movement-commandsthat can be understood by the robot.

3.2.1.4 Odometry

Similar to setting up the topic /cmd_vel, the RosAria node will also create anOdometry (RosAria/odom) topic where it will publish information about the cur-rent states of the robots in the form of Odometry messages. These messages includedata about the robot’s position, directional angle, speed and angular speed etc. TheOdometry message type sent on this topic is a combination of Pose and Twist mes-sages. Twist, as described earlier, contains data about the robots control actions.The Pose messages are calculations of the robots state made from the wheel rota-tions. When first set-up, the position (x- and y-coordinate) as well as the angle ofthe robot is 0, meaning the robot starts in the origin facing the x-axis.

15

3. Equipment

3.2.1.5 MobileSim

To be able to accurately simulate and test the systems and algorithms used inthis report, MobileSim was used. MobileSim is a simulation application that workstogether with RosAria and allows RosAria to be run in the same way as outside thesimulation [22]. If a MobileSim process is running when a RosAria node is startedall commands will be sent to the simulation instead. It is also possible to run severalrobots in the same simulation, using separate RosAria nodes.

3.2.2 Python and Rospy

Python is a free and open source programming language. Together with the Rospypacket, Python can be used to create programs that control the ROS nodes or readinformation published on the various topics. This can be for instance reading theOdom messages, or writing twist messages, etc.

Other additional packages are available for Python that can be used to solve matrixequations for the extended Kalman filter and also to solve trigonometric equationsand optimization problems and other similar mathematical problems.

16

4Methods

At the start of the project, the core problem was broken down into smaller tasks.An iterative mindset was then used throughout the project to tackle these issues oneby one until they were solved and combined to form a solution to the final problem.

The first step was to form the most basic solution to the problem and see whatwas necessary to solve it. This was determined to be two robots simply crossingeach other without collision, i.e. no demands on acceleration. In order to makethe robots cross, it was decided that two things were required: knowledge of theirpositions and being able to give them general commands. After retrieving knowledgeof their positions through the UWB sensors, and sending commands through ROSand RosAria, it was decided that a filter was needed to improve the positioning data.At the same time, more sophisticated algorithms were developed to not only sendgeneral commands, but to implement the control algorithms presented in section 2.In this section, these implementations will be presented in greater detail as well asthe methods used to test the entire system.

4.1 Initial Setup

The wireless communication system is used to connect the controller with the robots.Over this connection, ROS is able to operate and send information including locationand state data about the robots to the controller as well as control commands fromthe controller to the robots.

In order for the controller and ROS to communicate with each node, a simple router(Thomson TG784) was used as a base station. This router requires Internet con-nection and is used to enable the devices to communicate with the master over theWLAN, and vice versa1. To allow the nodes to communicate with the base sta-tion and the roscore of the controller, they need to be told the IP assigned to thecontroller as well as the host name of the ROS master.

1A mobile hotspot was used in place of the router because it lacked internet access.

17

4. Methods

4.2 Determining the state of the robots

This section describes the steps taken to find the state, primarily position andvelocity, of the robots at any given moment during execution.

4.2.1 Robot state

The state of the robot in any given moment in time is made up of its two-dimensionalposition (x- and y-coordinates), the angle of the robot in relation to its originalorientation ◊ as well as both its linear velocity v and angular velocity Ê. Thesestates are visualized in figure 4.1

Figure 4.1: The figure shows the coordinate system and angle of the robot, whichare used to create a model of movement. It also shows the iterative states of thesystem from X

k

to Xk≠2

.

The equations that describe the dynamics of the system from any moment k ≠ 1 tok, are

x

k

= x

k≠1

+ v · cos(◊k≠1

) · dt (4.1)y

k

= y

k≠1

+ v · sin(◊k≠1

) · dt (4.2)◊

k

= ◊

k≠1

+ Ê · dt (4.3)

where dt is the time step between each iteration of the filter. This state connects

18

4. Methods

the robots linear and rotational velocity to the robots x and y position as well asorientation, making sure that all measurable attributes are accounted for.

4.2.2 Setup of components for determining the state

The robot’s position will be determined by using a sensor grid that is comprised ofthree anchors, each with a UWB radio transceiver that will be pinged by a similardevice on each robot, allowing the robot to gather a time of flight reading from eachanchor. With these time of flight readings the sensor on each robot will determine thedistance to each anchor in the grid. These measurements will be used to determinethe position of the robot, which will be discussed in the section below. Howeverthese measurements are not perfect and therefore require estimation algorithms anda filter to be of a usable quality for the robot and the controller.

A Kalman filter is used in order to make the measurements of use to the controller.As described in section 2.3, it predicts the robot’s next possible state using a modeland then fixes this model using measurement data for use in predicting the nextstate. This allows for much more accurate location data that can be sent to thecontroller for use in solving the intersection problem.

4.2.3 Implementation of trilateration

In order for the position of the robots to be used, the data needs to be in a referencesystem which in the case of this project is an x- and y-coordinate system. Thisis possible to accomplish using a minimum of two anchors, however the systemimplemented here uses three, one as the origin of the system, and the other twomeasuring out the x- and the y-axis respectively. This is due to the fact that forthe robot to use trilateration to determine its position in this coordinate system, itmust have three points of reference to refer to.

An issue with the trilateration as it is described in the theory is that it requires allthree distance circles to overlap perfectly in one point in order to gain a location.When the robot pings an anchor and gets back a measurement, this measurementhas an inherent amount of noise. This can result in the possibility of overlappingcircles creating a triangle of uncertainty or worse still, the circles do not overlap atall. This is described by the figure 4.2.

19

4. Methods

Figure 4.2: The leftmost subfigure describes the possibility of overlap causinga residual error. The middle one describes the opposite with zero overlap. Therightmost is the ideal case with perfect overlap

It is therefore important to reduce this residual that is left between the distancecircles from each anchor. This is done using an iterative process that takes thepreviously measured point and moves this reference point such that it is reasonablyclose to a center point between the distance circles. The equation of this iterativeprocess is

f(x) =Nÿ

i=0

(ai

≠ r

i

)2 (4.4)

where the a vector describes the distance between the anchor and its measured pointand the r vector describes the vector between the anchor and the reference pointthat is being moved in each iteration to decrease the value of the function. This isdescribed in the figure 4.3.

20

4. Methods

Figure 4.3: This figure describes the following procedure for one of the sections ofthe sum for the total residual. The red vector is the equivalent of a. The light bluevector is the r vector and the green vector describes the vector whose magnitude isf(x) or the residual of the system.

Unfortunately the value that is given by the UWB radio antennas is only a magni-tude, not a vector so the vector a must be rewritten to use its magnitude instead.This is done by the following equation that puts a in terms of r

a = r

|r| |a|. (4.5)

Following this, the principle of gradient descent is used to minimize the value of theresidual. However in order to optimize the runtime of the program, the derivation ofthe gradient at each time step was abandoned and the gradient was determined tobe a function of the length of the residual vector divided by 2. The program startsby taking the previous point of measurement and then using that as the referencepoint. Then by using the methods described above, this point is drawn toward theplace that minimizes the residual of the function.

This function would then continue to run until the residual would satisfy

f(x) < A, (4.6)

with A being a chosen tolerance. To prevent a problem where the residual nevermet A due to exceptionally bad spacing between the distance measurements, theprogram would also see if

f

k≠1

(x) ≠ f

k

(x)) < R (4.7)

21

4. Methods

was satisfied, where R was another chosen tolerance and both f(x) were the residualsbetween the previous step and the current one. This allowed the system to determinean accurate location regardless of how uncertain the measurements were. The figure4.4 visually represents this system and how it works through each iteration of thedescent function.

Figure 4.4: This figure describes how the criteria are met for the tolerances of theresidual in the descent function. In the case above the chosen tolerance A was metbefore the secondary criteria R was met.

4.2.4 Implementation of the extended Kalman filter

Despite the optimization process for the trilateration to find the robot’s position,the measurement data is still noisy and of little use for the controller in accuratelymodeling the intersection. This noise modeled in a covariance matrix in section 5.1.To mitigate this noise and make the location data usable, a Kalman filter has beenimplemented. How this filter is generally built is described in section 2.3.

In order to apply the filter, the equations describing the robots movement (section4.2.1) need to be linearized in order to obtain the A matrix that describes the modelof the system. The calculated matrix is

A =

S

WWU

ˆxkˆxk≠1

|xs,us

ˆxkˆyk≠1

|xs,us

ˆxkˆ◊k

|xs,us

ˆykˆxk≠1

|xs,us

ˆykˆyk≠1

|xs,us

ˆykˆ◊k≠1

|xs,us

ˆ◊kˆxk≠1

|xs,us

ˆ◊kˆyk≠1

|xs,us

ˆ◊kˆ◊k≠1

|xs,us

T

XXV =

S

WU1 0 ≠v · sin(◊

k≠1

) · dt

0 1 v · cos(◊k≠1

) · dt

0 0 1

T

XV . (4.8)

The prediction of the new states can be calculated using the non-linear equations in4.1. In addition the B matrix is not an input in any further steps in the filter andis therefore not needed to run the Kalman filter.

22

4. Methods

In this implementation, the filter uses the previous state of the robot, control actionssent from the controller (the linear and rotational velocity), current measurementdata (x- and y-position) along with noise values for all of these inputs to estimatethe current state of the robot in the emulation. The process noise w

k

can be assumedto be zero mean Gaussian noise where w

k

≥ N (0, Q) and the covariance matrix

Q =

S

WU0.052 0 0

0 0.0252 00 0 0

T

XV . (4.9)

Similarly, the observed state also contains zero mean Gaussian noise vk

≥ N (0, R)where

R =

S

WUVar(x) Cov(x, y) 0

Cov(y, x) Var(y) 00 0 0

T

XV . (4.10)

The covariance values are based on a number of n readings where the covariance ma-trix is calculated from the sample measurements, see section 5. Lastly, the observedstate vector z

k

and the observation matrix H are

zk

= Hxk

+ v, (4.11)

H =

S

WU1 0 00 1 00 0 0

T

XV (4.12)

where xk

is a vector containing the observed x and y coordinates from the UWBsensor grid. Using all these equations together as explained in section 2.3 the statesthat are calculated in the filter should be more accurate due to a decrease in totaluncertainty and help the robots determine a more exact position.

However there is no observation made on the angle of the robot, so the resultingstate will therefore be purely based on the prediction made in equation 4.1. Therobots angular velocity will continuously and non-linearly increase/decrease betweenthe given values, necessitating that the resulting angle be calculated according toa Riemann sum, see figure 4.5, with width dt, decided from the iteration time ofthe filter. This will therefore deviate from the actual angle, which is the area underthe angular velocity curve. To minimize these errors, only three specific angularvelocities are used (depending on the angle o�set) to turn the robot rather than aregulator. The resulting method, visualized in figure 4.6 contains less errors due tothese more discrete angular velocities and will lead to a more accurate prediction.Despite the discrete angular velocity inputs, the system is not instantaneous andhas some lag resulting in the error seen in the visual representation below in figure4.6

23

4. Methods

Figure 4.5: Riemann sum when using continuous angular velocity changes (ac-cording to a regulator). The area under the curve represents the actual angle of therobot, while the rectangles represent the predicted angle.

Figure 4.6: Riemann sum when using three specific angular velocities. The greencurve represents the actual angle of the robot where the rectangles are predictionsof the angle.

24

4. Methods

The filter was tested by moving one robot in a known path. The observed position,as well as the predicted position and filtered positions are plotted out, and the datais observed to see if the filter manages to correct the location as the robot movesalong the path. The results are presented in section 5

4.2.5 Generating noise for Pose-data

For various reasons, including testing changes in the algorithm, the problem is sim-ulated in MobileSim as described in section 3.2.1.5. In order to more accuratelysimulate the real world scenario, where the positioning system has noisy measure-ments, an option for adding noise to the Pose-data used to simulate the robots wascreated. The noise is Gaussian in nature with the user specifying the standard de-viation. Since the UWB sensors have an estimated error of around 2 centimeters,a similar value was chosen for the standard deviation for the setup used to mostaccurately emulate the real scenario.

4.2.6 Calculating an intersection point

Figure 4.7: The figure shows an example of the robots, their position and angledata, and their resulting intersection point. x

1

, y

1

, ◊

1

describes the data from thefirst robot, x

2

, y

2

, ◊

2

is for the second robot and IPx

and IPy

is the intersection point.

In an ideal scenario the intersection point of the robots will be in the origin of thecoordinate system. Given that there is always errors for the robots as they travel andtry to aim themselves, a more general approach was taken. This approach was tocalculate the intersection points of both the robots using their positions and angles.

25

4. Methods

Figure 4.7 shows the general situation and the parameters that the calculation uses,those being the first and second robot’s position and angle, x

1

, y

1

, ◊

1

and x

2

, y

2

, ◊

2

.

Using this information the lines describing the trajectory of the robots can be found.Considering the first robot, its slope will be tan(◊

1

). The equation describing theline is then y = tan(◊

1

) ·x+m. The constant m is found by using the robots positiondata, resulting in m = y

1

≠ tan(◊1

) · x

1

. This gives the equation for the line in theform of

y = tan(◊1

) · x + (y1

≠ tan(◊1

) · x

1

). (4.13)

Using the same method, the equation describing the line for the second robot is

y = tan(fi ≠ ◊

2

) · x + (y2

≠ tan(fi ≠ ◊

2

) · x

2

). (4.14)

Setting these two equations equal to each other and solving for the x values, willgive the intersecting point IP

x

. The result is

IPx

= y

2

≠ y

1

≠ tan(fi ≠ ◊

2

) · x

2

+ tan(◊1

) · x

1

tan(◊1

) ≠ tan(fi ≠ ◊

2

) . (4.15)

Putting this x value in one of the equations describing the trajectories of the robotswill give the corresponding value IP

y

. This becomes

IPy

= tan(◊1

) ·3

y

2

≠ y

1

≠ tan(fi ≠ ◊

2

) · x

2

+ tan(◊1

) · x

1

tan(◊1

) ≠ tan(fi ≠ ◊

2

)

4+ y

1

≠ tan(◊1

) · x

1

,

(4.16)

and together with IPx

gives the intersection point.

4.3 ROS structure and data handling

The ROS structure for this project is divided into several di�erent packages that runon di�erent computers on di�erent robots. Roughly speaking, ROS runs in threephysical locations, the master computer running the main controller that organizesthe intersection, and two slave computers, each running separate versions of theprogram that handles location data, and the program controlling the robots controlactions (RosAria). Each of these computers run a node or combination of nodes thataccomplish various tasks. Together these nodes work together to send informationfrom robot to controller and allow the emulation of the intersection control. Thiscode structure is explained in greater detail in the figure 4.8.

26

4. Methods

Figure 4.8: A figure modeling the basic structure of the code in the program thatruns the robots, location system, and intersection control algorithms. The blueboxes represent ROS nodes, the circles inside these represent topics inside each ofthese nodes while the solid lines represent publisher and subscriber relationshipsbetween these di�erent topics. The dashed lines represent services and the hexagonshape represents a service proxy that creates and handles these services.

The simplest way to describe the flow of the code is to start from the controller andgo through the system step by step. The figure above will be referred to give a betteridea of how everything connects. The controller sends commands to the rest of theprogram via services that are handled via the Service Proxy. These allow directionsto be easily communicated to each robot since publishing to a topic is unnecessaryfor these command type messages. These service messages are then taken by therobot node which then interprets these commands and publishes in a topic whatboth the filter node and RosAria node need to know to fulfill their objectives. Theinterpretation that the robot node does is detailed in the following section.

Once the interpreted commands are sent to RosAria, the robot needs to update thecontroller with information on where it is or in other words, the robots state at thatcurrent time. To accomplish this the filter function, which includes the Kalmanfilter, builds up a topic for the state. It requires data however to compute the stateof the robot and this is taken from the sensor node. This node takes two di�erenttypes of information, location data from the UWB radios and linear and angularvelocity from the telemetry module on board the robot. This telemetry data comesfrom the RosAria node.

27

4. Methods

Once all this data has come together, been filtered and sent from the robot to thecontroller, the controller can then use this new location data to create new controlactions for the robot to follow. It is this loop that allows the robots to accuratelynavigate both the scripted paths but also the intersection crossing with help ofalgorithms detailed in section 4.5.

4.4 Communicating instructions to the Robot nodes

Table 4.1: Table defining the di�erent missions the robot is able to execute.

Basic Angular ComplexStop Aim at Point Go to Point

Set velocity Follow PathSet angular velocity Steer towards point

Set acceleration

Each robot is able to move around using the RosAria code but this requires an inputof both a linear and angular velocity. The controllers however work using more highlevel concepts such as a Go to Point-function. The Service proxy and Robot nodeare the programs that are used to translate these high level commands to the inputsthat RosAria needs to move the robots.

The handler states all the di�erent types of commands that can be outputted by thecontroller and translated for RosAria. These commands are called in this project,missions and there are six in the program that fulfill roughly all the necessary mo-tions for the controllers to accurately and safely get the robots to navigate an inter-section.

The possible missions are described in Table 4.1. A full API can be found in Ap-pendix A.3.1 of this report. The following sections (4.4.1, 4.4.2 and 4.4.3) will givea short overview of all the missions and short description of how to use them.

The simplest and most universal of the missions is the Stop mission. This missionwill end any currently active mission and make the robots movement come to a stop.

4.4.1 Basic movement missions

The first three missions define the most basic instructions you can send to the robot.They let you freely maneuver the robot using any of the defined methods for therobot. Each of the missions are called with a float-variable as the argument, denotingeither the speed or acceleration that the motion should be executed with.

Setting either acceleration or speed is exclusive of the other. If an acceleration is setthe robot will move with that acceleration, regardless of whether a speed was setpreviously. The same does not apply to the angular velocity. Angular movement

28

4. Methods

can be combined with any of the other basic missions and the robot will execute acombination of the movements asked.

4.4.2 Angular control mission

Currently there is only one mission solely controlling the angle of the robot. Thismission takes a point as an argument and rotates the robot so as to face the point.

As described in figure 4.5 and 4.6 and at the end of section 4.2.4, the predictionsof the angle gets better if discrete steps for the angular velocity is used. As suchthe aim at point mission used three di�erent velocities depending on how large thedi�erence in angle is between the robot and the point to be aimed at.

4.4.3 More complex movement missions

These missions build upon the previous ones and let the controller ask for morecomplex actions of the robot.

The Go to Point mission is the simplest version of asking the robot to move to apoint.

Next the Follow Path mission is an abstraction over several Go to Point call. Givena Path (a list of points) it will iteratively call Go to Point for each point one afteranother.

The last complex mission, Steer towards point is used to be able to abstract awayangular movement on a path between two points, as such allowing the travel to behandled as one-dimensional. After calling this with a point, the robot will continu-ously and automatically orient itself toward as to follow the path between it’s currentposition and the point. After this the user is free to call the previously describedmissions Set velocity and Set acceleration to move the robot over the path.

4.4.4 Polling robot for state

Besides the previously defined missions there are two other services provided by theRobot node. One lets you ask whether the robot is executing a mission currently andthe other lets you ask for the current positional and dynamic state of the system.

29

4. Methods

4.5 Description and function of controller algo-rithms

In this section the di�erent implementations of controller algorithms are presented.The one used to gather the final results is the hybrid version encompassing thepriority and optimization solutions.

4.5.1 Implementation of reservation solution

Figure 4.9: The figure depicts the two intersecting robots at di�erent positionsand times throughout the intersection, matching the time slots chosen by the user.In this case, both the robots are able to accelerate and decelerate and in general actmore independently.

Table 4.2: The table shows the values specified by the user for the reservationbased system. These values are in the form of time slots, intersection values and thetotal length of the travelled path.

User input for reservation solutionTime slots t

1

t

2

t

3

t

4

Intersection values x

A1

y

A1

x

A2

y

A2

x

B1

y

B1

x

B2

y

B2

Total length L

x

L

y

30

4. Methods

As explained in section 2.4.3.1, the reservation solution makes sure the robots arein the intersection at certain times. The chosen implementation of this systemrequires the user to specify three forms of parameters: the di�erent time slots forthe robots, the general intersection parameters i.e. when it starts and ends, and alsothe total length. The distance from the starting point to the intersection and fromthe intersection to the ending point also needs to be specified. Figure 4.9 and table4.2 shows the required values, with the table showing the time slots, intersectionvalues and total size that the user inputs. The t, x or y denotes whether theparameter is for time or position with corresponding coordinate axis. A or B isgiven depending on the robot and 1 or 2 represents the start and the end of theintersection respectively. The time slots t

1

and t

2

belong to robot A while t

3

andt

4

are for robot 2. The intersection itself is represented by the dashed lines in thefigure. The total length of the path is L

x

for the x-axis and L

y

for the y-axis. Thevalues x

A0

, y

A0

, x

B0

and y

B0

do not need to be specified by the user.

The algorithm contains two instances of the same code that run simultaneously; onefor each robot. The algorithm first calculates the necessary velocity for a robot toreach the start of the intersection at the specified start of the reserved time interval.For example if we regard robot A, the yellow robot in figure 4.9 traveling in the x

direction, the velocity sent to the robot would be

v

A

=

Ò(y

A1

≠ y

A0

)2 + (xA1

≠ x

A0

)2

t

1

. (4.17)

This velocity needs to be smaller than 1.0 but greater than 0, because of the fact thatthe robots cannot travel faster than 1 meter per second due to comfort restraintsplaced on the vehicle and since we do not want negative velocity. If a velocity iscalculated that does not meet those requirements, the user needs to input new timeslots or intersection values.

At the same time that the velocity is sent to the robot, a timer is initiated. Aftert

1

seconds has passed, the robot has arrived at the start of the intersection. Anacceleration is calculated by

a

A

=2

3Ò(y

A2

≠ y

A1

)2 + (xA2

≠ x

A1

)2 ≠ v

A

· t

2

4

t

2

2

. (4.18)

In theory, this acceleration value will then ensure that robot A has reached the endof the intersection at the end of the time interval. However, because of the fact thata possible acceleration that places the robot at the correct position at the correcttime is one that moves the robot forward past the point and then backwards, it needsto be guaranteed that the velocity does not become negative. The final velocity v

fA

is for that purpose calculated by

v

fA

= v

A

+ a

A

· t

2

. (4.19)

If v

fA

is negative, then x

A2

will be increased by 0.05 and a

A

recalculated with thenew distance. If v

fA

is still negative, x

A2

will incrementally increase. If one such

31

4. Methods

solution is found, but v

fA

is greater than the robots maximum velocity 1.0, then therobot will brake. If there is no distance that fulfills the requirement, robot A willalso brake in order to avoid collision.

One can impose restrictions on how much displacement is allowed, depending onwhat is considered an appropriate definition for the intersection. Such a value canbe found through experimental procedures and tests.

The same calculations described above are replicated for robot B with appropriatevariables changed to their respective names in accordance with table 4.2.

4.5.2 Implementation of priority solution

Figure 4.10: The figure depicts the two intersecting robots at di�erent positionsand times in the intersection. The yellow robot is the stable robot while the red oneis the variable robot. The figure also shows the concept behind the guard distanceand how the robots are at di�erent positions when the uncontrollable robot is in themiddle of the intersection.

32

4. Methods

Table 4.3: The table shows the values required from the user in the implementationof the priority solution. These are categorized into a velocity interval, a guarddistance and the total size of the system.

User input for priority solutionVelocity interval v

minA

v

maxA

v

minB

v

maxB

Guard distance g

d1

g

d2

Total size L

x

L

y

As described in section 2.4.3.2, this problem assumes one robot can be controlledand the other cannot. The initial state of the problem is shown by figure 4.10, wherethe parameters given by the user is shown in table 4.3.

The implementation first randomizes the initial velocities of the robots based onan interval ranging from v

minA

to v

maxA

for robot A and v

minB

to v

maxB

for robotB. The randomized initial velocity becomes v

A

and v

B

respectively in figure 4.10.The intersection point of the robots is calculated, in accordance to the description insection 4.2.6. After the robots have reached the given velocities, the time T requiredfor the stable robot to reach the intersection is calculated. This becomes

T =

Ò(IP

x

≠ x

A

)2 + (IPy

≠ y

A

)2

v

A

. (4.20)

When the intersection time has been determined, the acceleration required to reachthe distance IP

y

≠g

d1

at time T is calculated and given to the variable robot. This isdone by the standard formula correlating acceleration, velocity, distance and time,and results in

a =2

1Òx

2

B

+ (IPy

≠ g

d1

≠ y

B

)2 ≠ v

B

· T

2

T

2

, (4.21)

using the notations from figure 4.10. g

d1

used here is called the guard distanceand must be su�ciently large for the robots to not inhabit the intersection at thesame time. This does not preclude the further increase of the guard distance overthe minimum necessary to prevent collision and this concept will be used later toadjust the robots control actions, ensuring a successful crossing. The usage of g

d2

isexplained in the next paragraph, but the same principles applies to it as well.

This implementation should avoid collision between the two robots since they willbe at di�erent positions at di�erent times. Nevertheless there are limitations thatmust be imposed on the algorithm in order to accurately represent the real worldscenario and the demands placed on it. For instance, the calculated acceleration thatis given to the variable robot could be an acceleration that achieves the requirementof being at distance IP

y

≠g

d1

at time T by driving past the point with a decelerationand thereby later incurring a negative velocity in order to reach the point at thedesired time. Another limitation is the fact that the robots cannot drive faster than1.0 meters per second due to comfort constraints. Additionally, they cannot handle

33

4. Methods

acceleration values smaller than 0.006 m/s2 due to physical limitations of the robot.In order to deal with the aforementioned limitations, an adjusted algorithm hasbeen developed. It takes di�erent steps based on two di�erent scenarios; if the finalvelocity is negative and the acceleration is smaller than 0.006 m/s2 or if the finalvelocity is greater than 1.0. The final velocity is calculated through

v

f

= v

B

+ a · T. (4.22)

Since T and v

B

are constant when the problem initiates, it becomes obvious thatthe acceleration needs to be altered in order to change the value of the final velocity.The acceleration itself is dependent on the same constant variables, except it alsodepends on the guard distance, which can be seen in equation (4.21). By alteringthe value of the g

d1

, either with a positive or negative factor, the acceleration willhave decreased or increased.

If it is the case that the final velocity is greater than 1 m/s2, it needs to be lowered.That in turn means that the acceleration needs to be decreased, which is achievedby increasing g

d1

with 0.05 m. After g

d1

has been altered, the acceleration is recalcu-lated. If the new final velocity value is still too large, g

d1

will once again be increasedby 0.05. This process will repeat itself until the final velocity is lesser than 1. Thereis a potential issue where increasing the guard distance too much, placing the pointbehind the position of the robot, results in negative velocity. If this happens, thereis no proper value for the acceleration and the robot brakes to zero velocity almostimmediately in order to avoid collision.

If it is the opposite case where the final velocity is negative, the acceleration needsto be increased and in order to achieve this, the relevant guard distance needs tobe increased. However since the guard distance g

d1

is at what is deemed a safedistance from the intersection, increasing it will bypass that safety and place thevariable robot too close to the intersection. To combat this the algorithm will usethe flipped guard distance g

d2

instead of g

d1

when the final velocity is negative,shown in figure 4.10. The guard distance g

d2

will increase with 0.05 m and theacceleration will be recalculated, and if the final velocity still is negative, it willcontinue to increase with the same amount. Since g

d2

is on the other side of theintersection, its value can be increased without limitation since the robot will stoponce it reaches the total length L

y

defined by the user at the start of the problem.If the acceleration is smaller than 0.006 m/s2, the same process will be performed,since its value needs to be increased.

The mentioned solutions to the dangers of the problem introduce another problem inthemselves. The problem is that an acceleration that for example initially satisfiesthe first scenario but not the second, might not satisfy that scenario when it isrecalculated to fit the second. This only in principle though, since both methodsapply calculations that work against each other. However since there is a possibilityof a solution that satisfies both conditions, the calculations are placed in a loopwhere the algorithm tries to find a solution in 50 calculations where both scenariosare satisfied. This value is arbitrarily chosen, but should be su�cient to guarantee

34

4. Methods

a solution if there exists one. If it is unsuccessful, the variable robot will brake toensure collision avoidance.

4.5.3 Implementation of hybrid solution

Figure 4.11: The figure shows the concept behind the hybrid algorithm betweenthe priority and the optimization solutions. The arrows show the velocities of therobots, and as in the priority solution the yellow one is the stable robot while the redone is the variable robot. Important to note is that a

k

is the smallest accelerationsolution that the variable robot can find.

It is possible to optimize the algorithms for the priority solution as presented in theprevious section in regards to the control actions chosen. The resulting algorithmis a hybrid between an optimization and a rule based solution, and is the one thatthe real world experiment was conducted with.

The idea that is implemented here is that the problem reevaluates continuously astime passes, with updated parameters from the various inputs to the system suchas location data, creating a closed-loop control. There is a possibility that theacceleration found at the previous time step was not the smallest absolute solutionfor the system. This because of the fact that the algorithm does not incrementallyincrease in infinitesimal steps as well as the fact that it performs a limited numberof calculations before being stopped. On top of this there is also the fact thatthe positioning is a�ected by noise. This means that the trajectory of the robotcan change slightly, and by extension also changing the intersection point and thus

35

4. Methods

the distance to it, resulting in a new acceleration. The absolute value of the newcalculated acceleration is compared to the previous one, and the smaller one ofthe two is given to the system. This is done for each time step, resulting in anoptimization in regards to minimizing the acceleration.

The figure 4.11 demonstrates the concept behind the hybrid algorithm. Here thestable robot maintains a constant acceleration a

0

= 0 and arrives at the middle ofthe intersection at T . Simultaneously the variable robot calculates an acceleration.Initially at t

0

it finds the acceleration a

k≠n

that satisfies the demands in accordanceto the priority solution. However later at time t

k≠2

it finds a smaller absoluteacceleration a

k≠1

and therefore changes its acceleration value. At time T it willhave avoided collision using the smallest absolute acceleration solution it could find.Note that it does not need to stop at g

d

since the guard distance can be alteredin order to find new acceleration solutions, as described in the previous section.The figure also shows one case, there is also the other case where it drives past theintersection before the stable robot. The important part to note is simply that.

a

k≠n

> . . . > a

k≠1

> a

k

. (4.23)

Besides the fact that the acceleration becomes optimized, this approach also o�ersimprovements in the sense that if a solution is not found, and the robot does notneed to emergency brake, it can use the previous acceleration value and try againat a later time with di�erent initial values. One needs to be careful in this situationsince the previous acceleration value can be one that is results in a negative finalvelocity. The algorithm therefore disregards old acceleration values if the robot haspassed the guard distance and the old acceleration value was aiming behind theintersection.

In general this results in a greater possibility of finding solutions that satisfy allconditions and results in a successful intersection. It can however also present adanger if it takes too many tries to find a solution and the robot comes dangerouslyclose to the intersection point. There is also a risk that there is too much noise andan erroneous acceleration is calculated that results in a collision. The emergencybrake therefore becomes crucial in this implementation to decrease the probabilitiesof collisions. It is also of utmost importance to choose an appropriate guard distanceto further reduce the risks if there is substantial noise in the positioning.

4.6 The setup of the experiment

The setup of the experiment is critical as this is how the intersection controller istested in the way that fulfills the goals of the project. By testing the controller inthis way, the system is exposed to the true noise of both the robots and how theyexecute control actions but also the noise/error of the location system which di�ersfrom the simulation software.

36

4. Methods

Figure 4.12: This figure represents the setup of the experiment to scale. Theanchors are represented by antenna symbols and are placed in three locations aroundthe intersection. Each robot has its path also mapped out using the dashed linesin the figure showing the complete. The figure does not depict the controller or theWi-Fi network as their positions are not important to the experiment.

In figure 4.12 the full setup and paths that the robots are programmed to followcan be seen. The three antenna symbols match where the anchors are placed forthe location system with each corresponding location. The paths of each robot aredepicted by the dotted lines that go through the intersection and then back to astart point. This setup allows the robots to run the emulation over and over again,measuring the real world robustness of the controller in preventing collision. Inthe setup, the calculations are done on a centralized computer which communicatesits instructions to the individual robots through the Wi-Fi network. At each startpoint the robots are given a random velocity between 0 and 0.3 m/s to replicatean intersection and the randomness of the vehicles entering the intersection. Thetop robot is programmed to hold its velocity as per the hybrid priority algorithmdescribed in 4.5.3. The bottom robot is then controlled to avoid collision with thetop robot as it travels through the intersection. Following the intersection crossing,the robots then travel to start positions, switching places and continuing with a newintersection crossing.

37

5Results

The results of the di�erent scenarios previously discussed are put forward. Thesescenarios aim to determine the quality of the algorithms, hardware, software andgeneral functionality of the entire system. As aforementioned, the controller al-gorithm being tested is the hybrid priority optimization algorithm presented anddescribed in section 4.5.3.

The following figures show the results of 10000 position measurements done by theUWB system around a single static point, which was placed as near to the centerof the intersection of the following tests as possible. This test was meant to give areasonable measurement of the accuracy of the positioning system around the areaof greatest importance to the experiment, the intersection.

Figure 5.1: The figure shows the resulting x- and y-coordinates following 10000location measurements from the UWB sensors.

Additionally, the covariance matrix that was derived from this test is the same usedto represent the level of uncertainty for the observations used by the EKF. Thecovariance of the observed data was determined from the reading of UWB sensor at

38

5. Results

the stationary point. The resulting covariance matrix is:

R =

S

WU0.00031 ≠0.0000149 0

≠0.0000149 0.0000689 00 0 0

T

XV . (5.1)

The standard error that results from this ideal sample is 1.76 cm of the x-coordinateand 0.83 cm for the y-coordinate. The measurement data is also visualized in fig-ure 5.2 to 5.3 where it is possible to see that the data fits well within a normaldistribution curve.

Figure 5.2: The distribution of these 10000 location measurements in the x-axisand a best fit normal distribution curve overlayed.

Figure 5.3: The distribution of these 10000 location measurements in the y-axisand a best fit normal distribution curve overlayed.

39

5. Results

Figure 5.4: Measurements of the position from the predicted state, sensors andfilter as the robot moves with constant angular and linear velocity. Note that theaxis are shifted which deforms the otherwise circular motion.

To test the filter, one robot was first given a constant angular and linear velocity.This made the robot move in a circle. The position from the predicted states x

k|k≠1

(predicted from a previously filtered state xk≠1|k≠1

), the position from the filteredstate x

k|k and the observed position zk

are plotted out in figure 5.4.

Figure 5.5: Measurements of the predicted state, sensors and filter as the robotmoves in a square. When it reached a point, it stood still for a second before movingon, resulting in some point clusters around the points of the square.

40

5. Results

A similar measurement was made when the robot moves in a rectangular shapewhere it is programmed to move to each corner in the rectangle. This result isshown in figure 5.5.

5.1 Results of the experiment

Figure 5.6: The figure shows the distances travelled for each robot, calculatedfrom their respective starting points. In this case Robot 1 accelerates in order toavoid collision whilst Robot 0 maintains constant velocity.

Figure 5.7: The figure shows the distances travelled for each robot, calculatedfrom their respective starting points. In this case Robot 1 decelerates in order toavoid collision whilst Robot 0 maintains constant velocity.

41

5. Results

To show that the robots accelerate and decelerate in the real world setup, graphswere produced showing distance they travelled from their starting points. Figures 5.6and 5.7 show the results of running the hybrid algorithm with 2 meter guard distance.Figure 5.6 shows the variable robot accelerating whilst 5.7 shows it decelerating.

Figure 5.8: The figure plots the velocities of the robots against time. The greendata represents the stable robot whilst the blue data is the variable robot. The initialvelocities are randomized and both travel to the ends of the intersection. After thepossibility of a collision disappears, the optimization of acceleration is disregarded.

Figure 5.8 shows the di�erent velocities of the two robots as they travel through theintersection. It is important to remember that as soon as the risk of collision is zero,the variable robot will disregard the demand of minimizing its acceleration and willinstead travel with a high acceleration to the end of the path. For the measurementsfrom the opposite situation where the variable robot reaches the intersection first,and therefore does not need to accelerate with a high constant value, see figure A.3in appendix A.4.

42

5. Results

Figure 5.9: The figure shows data gathered from three di�erent measurements.The data itself is the length between the robots plotted against time. The blue andgreen data were collected when the variable robot decelerated whilst the red datawas from when it accelerated.

In order to verify that the robots do not collide with each other, data was gatheredthat shows the distance between the robots. Several measurements were taken andthree of these are presented in figure 5.9. As shown the distance never gets smallerthan 1 meter. The blue and green colors represent data gathered from when thevariable robot decelerates, whilst the red data is from when it accelerates.

43

5. Results

5.1.1 Controller results from a single measurement

(a) The graph shows the total travelled distance of the robotsas a function of time.

(b) The graph shows the di�erent speeds of the robots as afunction of time.

44

5. Results

(c) The graph shows the length between the robots as theytraverse the path.

Figure 5.10: The figures 5.10a, 5.10b and 5.10c show di�erent data collected fromthe same measurement. Figure 5.10a plots the distance covered, 5.10b is velocity ofthe robots and 5.10c is the distance between them. The blue lines represent Robot1 which is the one that is controlled, while the green lines represent Robot 0 whichis the stable robot and travels with constant velocity. In this case the variable robotmostly accelerated.

The figures and results presented in the previous section are all important andrelevant in their own regard, however showing those interpretations from the samemeasurement becomes an interesting option as well. This is presented in figure 5.10and 5.10a, 5.10b and 5.10c. This measurement covers the case where the variablerobot crosses the intersection first. For measurements regarding the other case wherethe stable robot crosses first, see figure A.2 in appendix A.4.

5.2 System robustness for di�erent scenarios

Multiple tests were conducted in order to test the robustness of the system in the realworld. First and foremost, the initial goal of 50 intersections without collisions wasachieved. This with randomized initial velocities from 0.1 to 0.3 m/s and positiveand negative guard distance of 2 meters. The test was repeated several times, andalthough 50 intersections were not observed each time because of time limitations,the observed intersections did not result in collisions.

Furthermore, the initial velocity interval was changed to 0.1 to 0.5 m/s with the sameguard distance of 2 meters, in order to test the robustness. Although a collision wasnot recorded over several continuous tests, it was still resolved that a larger guard

45

5. Results

distance was required to have more comfortable margins.

5.3 Simulation results

Using MobileSim, as presented in section 3.2.1.5, more results were able to be gath-ered covering some areas that were not tested with the real robots. This includestesting the robustness of the algorithms without having to consider real world limi-tations such as space, equipment and setup time. The optimization of the controlleralgorithm was tested, as well as di�erent noise levels and initial parameters. In thereal experiment, the goal was to achieve at least 50 intersections without collision,but in the simulation it was downsized to 20 in order to gather more results.

46

5. Results

5.3.1 Controller in simulation

(a) Optimized acceleration

(b) Optimized deceleration

Figure 5.11: Simulation results of hybrid algorithm using 4 meters distance tocenter, 1 meter negative guard distance and 2 meter positive. Randomized startingvelocities in interval 0.1 to 0.5. Figure 5.11a shows the case in the simulation were thevariable robot crosses the intersection first while optimizing its acceleration. Figure5.11b shows the same thing but with deceleration and the stable robot crossing first.

Figure 5.11 shows two primary scenarios; one where the variable robot passes theintersection first, and one where the stable robot passes first. This is shown infigures 5.11a and 5.11b respectively. The robots are shown at several di�erent timesteps in the simulation, and together with the specific time step their velocity andacceleration at that time is also shown. Through this information, the optimizationof their accelerations becomes clear. The data was collected using the parameters of4 meters distance to center, 1 meter negative guard distance and 2 meter positive.Randomized starting velocities in interval 0.1 to 0.5 were given to the robots.

47

5. Results

5.3.2 Controller in simulation with di�erent noise levels

(a) Noise 0.04 and no col-lision

(b) Noise 0.1 and no colli-sion

(c) Noise 0.15 and colli-sion

Figure 5.12: Simulation results of hybrid algorithm using 4 meters distance tocenter, 1 meter negative guard distance and 2 meter positive. Randomized startingvelocities in interval 0.1 to 0.5 and di�erent noise levels.

Simulating di�erent noise levels becomes an important feature of the simulator asreplicating a similar situation in the real world is di�cult. Figure 5.12 was generatedwith the motivation of testing the robustness of the system in a di�erent way. Itshows how the di�erent values for the standard deviation of the noise a�ect theintersection crossing drastically. Figure 5.12a shows a typical path of the robotwhen the standard deviation is at 0.04, while 5.12b shows the same thing withstandard deviation at 0.1. Figure 5.12c with 0.15 standard deviation follows thepattern but unlike the others, shows a way for a collision to manifest itself.

48

5. Results

(a) Noise 0 and 10 inter-sections

(b) Noise 0.04 and 10 in-tersections

(c) Noise 0.1 and 10 inter-sections

(d) Noise 0.15 and 5 inter-sections

Figure 5.13: The figure shows the hybrid algorithm traversing through the in-tersection 20 times with di�erent noise levels. The guard distance was 1.0 in thenegative direction and 1.5 in the positive, whilst the velocity interval was from 0.1to 0.5. In the case with standard deviation of noise 0.15, shown in figure 5.13d, only5 intersections were recorded since collision could not be avoided with the selectedguard distance.

Figure 5.13 shows what was recorded after 20 intersections of the controller fordi�erent noise levels and using the hybrid algorithm. In the case with 0.15 standarddeviation, shown in 5.13d, only 5 intersections were recorded since collision avoidancecould not be guaranteed for more crossings.

49

5. Results

5.3.3 Controller in simulation with di�erent velocity inter-vals

Figure 5.14: The figure depicts the path travelled by the two robots when thestarting velocity interval goes from 0.1 to 0.8 m/s. The pathing is a result of 10intersections.

Another aspect of interest is testing di�erent velocity intervals. This because of thefact that travelling faster can result in a small angular error not correcting itselfquickly enough, resulting in a higher probability of collision. On top of that it canalso lead to overshooting which can lead to aggregated errors in the calculated angle.Figure 5.14 shows the results of a simulation with an initial velocity interval rangingfrom 0.1 to 0.8 m/s. The plotted path is a result of 10 intersections.

5.3.4 Controller in simulation with decreased starting dis-tances

Figure 5.15: The figure shows the pathing of the two robots as their distances tothe intersection point are set to 2 meters. The starting velocity interval is from 0.1to 0.5 and the figure shows the results from 10 intersections.

50

5. Results

It is also worthwhile to try to test the controller with decreased distance to theintersection point. Increasing the distance would only improve the controller sinceit would have more time to correct itself from the filters point of view, and itwould also be more likely to find better acceleration solutions. On the other handdecreasing the distance could prove to be troublesome since it could not have enoughtime to find a solution, or if its initial angle is inaccurate, it could take too long tocorrect it. The room for error also decreases given that the robots are always closeto each other. The results of a simulation with decreased distance of 2 meters tothe intersection point, is shown in figure 5.15. The velocity interval was from 0.1 to0.5 and it shows the result of 10 intersections.

51

6Discussion

This chapter contains an analysis of the gathered results. Di�erent reasons andcauses for the successes and failures of the system are discussed and potential errorsources are presented.

6.1 General analysis of the system

The system and the implementations used proved to be as e�cient, flexible androbust as desired. Several limitations arose as consequences of the equipment andsoftware, regardless these were solved by specific algorithms.

Several interesting conclusions can be reached from reviewing the gathered results.Looking for the guard distance in the results is a way of evaluating the quality of thealgorithm as its magnitude should never be less than the chosen value of 2 meters.Figures 5.6 and 5.7 show that, in the case with deceleration, the guard distance isfound exactly at 25 seconds. The reason why it is not visible in the same way whenit accelerates is that the robot stops when it reaches it destination on the other sideof the intersection, which in this case was placed before the guard distance. It is alsoworth remembering that because of the possibility of iterative decrease or increaseof guard distance, that it is not certain that the robot will always land on the initialguard distance, in spite of where the final destination is. Regardless, the fact thatthe di�erences between the robots are not smaller than the magnitude of the guarddistance shows that the acceleration values chosen by the algorithm are desirable.

To clearly show that the robots do not collide with each other as they travel throughthe intersection, graphs showing the length between them were produced and pre-sented in the results. Looking at figure 5.9, that shows this from three measure-ments, it is clear that running multiple measurements still presents reliable andsimilar results, also regardless if the variable robot accelerates or decelerates. Thelines showing data from the case of the stable robot passing the intersection firstare relatively straightforward. The distance between the robots decrease and thenplain out when the stable robot reaches the intersection point, until they both reachtheir end points and lead to the increase in distance between them. Looking at thered line representing the case where the variable robot accelerates, one notices a soft

52

6. Discussion

spike at 28 seconds before the distance is again decreased followed by an increase,like in the other measurements. The valley most likely represents the case that thecontrollable robot passes the intersection point, leading to the robots being as closeas possible before distancing themselves again until the controllable robot reachesits final point. This would be the spike at 28 seconds, and afterwards the uncontrol-lable robot will have moved through the intersection, again decreasing the distance,until it passes it and reaches its final point leading to the increase of length betweenthem.

Also notable is the fact that the smallest distance between the robots is less thanthe guard distance of 2 meters. This is however due to the fact that the guarddistance does not result in the smallest distance between the robots. Dependingon the di�erence in speed between the robots after one has crossed the intersectionpoint, the distance between them might get smaller.

Having analyzed the graph mentioned in the previous paragraphs, it becomes clearthat having other information of the intersection could be helpful in figuring out whattranspired at certain times. This was the reason figure 5.10 was created and shown.Having the same times present in all of the graphs makes it tremendously easierto link them together. The measurement covers the case where the variable robotpasses the intersection first, and immediately it is possible to verify the explanationgiven for the spike in the red line from figure 5.9. Here it is clear that at 20 seconds,the controllable robot has reached its end point since 5.10a shows that it does notcover more distance after that time, and 5.10b shows that it has 0 velocity afterthat time as well. Now looking at figure 5.10c it can be seen that the graph showsa soft spike at 20 seconds, precisely the same as with the red line in 5.9.

6.1.1 The performance of the UWB sensor grid

As is shown in the results and particularly figure 5.1, the precision of the sensorgrid is lacking especially when considering the distances that are being measured.Having such an error for the x- and y-coordinates over a distance of only few meterscan be seen as disappointing when considering the needs of the project. There aretwo factors that make this system poor.

The first factor is the issue of error in the x- and y-coordinates. These in the idealscenario measured were around ±1.5 cm as seen in figure 5.1. It is possible that thiscan become worse as presented in the diagrams 5.2 and 5.3. This error can be anissue when compared to the size of each robot, which is 38 cm, resulting in this errorbeing 8%. When compared to the guard distance however this is well covered bythe 2 meter boundary.

A second factor that e�ects the robots is the inability of the location system todetermine an accurate orientation of the robots. This is referred to and discussedin section 6.1.2.

53

6. Discussion

It is interesting to note the di�erence between the uncertainty on the x-axis andy-axis. The spread of the points in the negative x- and positive y-direction happensto be the same direction where an anchor was not placed. This is likely explained bythe positioning of the anchor antennas in a triangle resulting in a greater precisionin certain directions due to the unequal orientation of the anchors and the robot’santenna.

Despite these limitations, the UWB system still has advantages. When lookingoutside of this emulation, into larger and more practical aspects of implementinga system such as intersection control, the most common areas of usage are tra�cflow on public roads and other forms of transport and logistics systems. Due tothe larger scale that these could be implemented with, the positioning systems alsoneeds to scale with said systems. These ranging from similar radio based positioningsystems all the way to the GPS system. Additionally, these systems need to have arobust way of handling positioning data in anticipation of unexpected changes suchas environmental obstacles. In this way, the tools that are used in this project arehighly applicable to a real world scenario.

Many of these systems build upon the same concepts and functions, e.g. trilat-eration, using similar algorithms, such as gradient descent, to solve these locationproblems. To look at GPS more closely, the standard measurement error can beupwards of 3 meters [23]. As a matter of scale this makes it comparable to the sys-tem implemented in this emulation. Since these comparisons can be made, solutionsfound in this report could be more generally applicable as well.

6.1.2 Positioning and angle calculation during filtering

The results gathered in section 5 compare the sensor data with the predicted stateand the filtered state that is used to more accurately obtain the location of therobots. In figure 5.4 and figure 5.5 it is clearly seen that the all of the states(x, y) plotted follow the actual path that is travelled by the robot. The pointsrepresenting the filtered states also appear to be a mean of the predicted and theobserved position, both which often di�er from each other.

The purpose of the observation is to restrict the noise from the predictions to ag-gregate to unrealistic values. While the predicted state seems to deviate from theactual path at times, the observed state corrects this error. Likewise, the predictionwill prevent the observed state from recording an incorrect action such as a suddensideways movement. This is seen in figure 6.1, where the observed measurementshows a sideways movement, most likely due to a noisy measurement.

54

6. Discussion

Figure 6.1: The figure highlights an area of where the predicted state prevents theobserved state from recording an incorrect action.

The angle of the robot, which is the third state that needs to be calculated, does nothave any correction in the filter, but instead is an estimation based on the previousangle, a time step and the robot’s angular velocity. Therefore this state allows forerrors to aggregate, where eventually the angle deviates too much for the systemto work properly, ex. find it’s target point. To correct this, a way to observe therobots actual angle would be ideal, such as an electronic compass. This could thenbe sent to the filter and prevent the aggregated errors from occurring. An alternativemethod for correcting the angle is by using the positioning data. If introducing acalibration mission, the actual angle of the robot can be derived by comparing thecurrent position with the previous position using trigonometry. This has the e�ectof zeroing the system’s accumulated orientation error as shown in the figure 6.2.

Figure 6.2: This is an example of a calibration mission that the robot can runafter the intersection in order to fix faulty orientation data in the robot. The reddotted vector shows the angle of orientation that the robot believes it has whenstarting on its trajectory as shown by the black vector. Over time this vector isshow to be fixed as the robot accumulates more accurate position data to compareits orientation with.

55

6. Discussion

The solution implemented and described in section 4.2.4 e�ectively minimized theerrors that came from using only the predicted angle of the robot in the system,however this requires the robot to be started in a known angle at the start of anyrun. Also, the more the robot changes between the fixed angular velocities, the moreerrors will aggregate in the system.

6.1.3 Evaluation of hybrid algorithm

The hybrid algorithm used to gather the results and that is the primary algorithmused throughout the project can be evaluated in many ways. The first and mostbasic evaluation is whether or not it results in collision avoidance, which was an-swered in section 6.1. Another more subtle aspect is whether or not there is avisible optimization, and if that optimization is desired and leads to an improvedsystem. Looking at figure 5.8, which shows the speed of the robot, one can see theoptimization algorithm minimizing the absolute acceleration value. At the start ofthe measurement the variable robot has a relatively high acceleration which leadsto a rapid increase of velocity. However a couple of seconds later it finds a smalleracceleration value which it then applies until the risk of collision is gone, which leadsto an end to the optimization and it simply assumes a constant high acceleration.Comparing the slope of the graph from 10 to 12 seconds to the slope from 12 to 27seconds, makes it clear that the new acceleration value is smaller than the initialone. A similar occurrence can be noted in figure 5.10b. Here the variable robotswitches acceleration values at about 10 seconds and once again a smaller absoluteacceleration value was found, leading to the change in velocity.

A tangible advantage of using the hybrid algorithm as opposed to solely the priorityprotocol, is apparent when considering the advantage of reevaluating the accelerationcontinuously. Before heading to the intersections, the robots aim themselves to thepoints after the intersection. Due to noise and miscalculations, they can believe thatthey are aimed correctly and start moving, resulting in paths shown with the highernoise levels in figure 5.13. The algorithm will attempt to correct their movement,but if the acceleration is not recalculated, the distance travelled will not matchwhat the robot calculated at the start of the problem, e�ectively meaning that thecalculated acceleration value that would avoid collision does not necessarily do so.With the hybrid algorithm, the acceleration is recalculated as the robot travels toits end point, meaning that it considers new distances all the time. There can stillbe errors with this method, but the risk is reduced.

After having optimized the algorithm for the priority based protocol, one possibilityworth considering is whether or not the same thinking could be applied to thereservation based system. Initially this might seem feasible and even preferable, butconsiderable di�culties arise when taking into account that acceleration commandsare only given and calculated once the robot is in fact inside of the intersection. Thismeans that it cannot choose to stop earlier in order to decrease the final velocity. Ifthe final velocity is greater than 1.0 then you cannot decrease it and if you use theemergency brake, you will stop in the middle of the intersection, causing a collision.

56

6. Discussion

Doing an initial calculation at the start of the intersection is possible since if youstop immediately there then a collision is impossible, however in order to optimizethe control action you need to continuously evaluate the acceleration, which leadsto the previously mentioned problem. This was a considerable factor when choosingthe hybrid of the optimization and priority solutions as opposed to the reservationsolution, for the algorithm used in this project.

6.2 Simulation analysis

The simulation results presented in section 5.3 can give more information regardingthe algorithms that the gathered results from the real-world experiments cannot.Glancing at 5.11a, the optimization becomes even more evident. Rememberingthat a predefined lower limit of the acceleration is 0.006 m/s2, it becomes ratherimpressive of the system to first find a solution 0.006007 that close to the minimalvalue, but then to minimize that to 0.006003 truly shows the optimization workingas intended. The optimization is also seen in 5.11b where it continuously decreasesat all three time steps.

The simulations with the di�erent noise levels is where the simulation completelyseparates itself from the real world experiment in regards to what is easily possibleto test or not. Looking at the results gathered and shown with figure 5.12 and figure5.13, the limits of the controller become apparent. For instance, trying to preventan intersection with noise standard deviation at 0.15 becomes near impossible whenthe trajectory is as uneven and unpredictable as shown in figure 5.13d. Noise levelsranging from 0.04 to 0.1 seem however to be acceptable, and seeing as the UWBproduces errors at about 2 centimeters, it should not present any impossible scenar-ios for the controller. This is also what was seen in the real world experiments sinceno collision was recorded and no path like that from figure 5.13d. If for instancesuch a path was present, it would definitely show in the measurement from 5.9 asthey would not be similar at all. Notice however that the results gathered here arefrom 20 intersections, excluding the one from 5.13d, and that the pathing would de-teriorate with more runs. Nevertheless, the general pattern shown in the simulationresults should persist and definitely represent a generous estimate of what would beexpected at 50 intersections as well.

Besides noise disrupting the system, there is another source of error that leads toerroneous pathing. That is overshooting and is shown quite accurately in figure5.14, where an increased starting velocity interval leads to higher velocities andacceleration values on top of those lead to overshooting. Overshooting in turnleads to miscalculated angles which leads to more overshooting and in general a lessrobust system. A comparison between the two scenarios where the system becomesless robust, that is between this figure and figure 5.13d, shows several similarities.Foremost, neither of them avoid collision for 20 intersections, even if the one with aninterval containing higher velocities is still more robust than the noisy model. Thiscan be seen from looking at the figures and comparing their paths or from a moretheoretical point of view; in the case with the interval containing higher velocities it

57

6. Discussion

only overshoots if certain conditions are met whilst the noisy model is always noisy.

A very important factor that cannot be neglected when discussing these scenarios,is the guard distance. The guard distance’s primary use is to adjust the system forhow much deviation the robot has from its ideal path when it travels through theintersection. These simulations were all ran with the same primary settings, meaningthat they had a guard distance adjusted for relatively low noise and a lower velocityinterval. Changing the constants used for the angular velocity when the robots areaiming too far away from the point they are going to, could also reduce the risk ofcollision at high velocities. Those constants being too low could result in the robotnot having enough time to correct its direction while its traveling to and throughthe intersection. Adjusting the guard distance for the problem parameters woulddefinitely increase the robustness of the system. However regarding the case withthe high noise, the model is very unpredictable which lessens the e�ect of the guarddistance.

A similar mindset should be taken when decreasing the distance from the startingpoints of the robots to the intersection. The results from doing this is shown infigure 5.15, where the distance was reduced from the standard 4 meters to 2 meters.This lead to an increased probability of collision, mainly due to the same problemas with the high velocities, that the robot does not have enough time to adjust itsdirection when its travelling to its end point. An adjusted guard distance wouldalso lead to much greater robustness in this case.

6.3 System robustness analysis

From both the simulations and the real world experiments, it was clear that theparameters we give the algorithm are co-dependant. For instance if the system isto have an interval with higher velocities, it is necessary that the guard distanceis increased. In general the system will become more robust the greater the guarddistance chosen, since the possibility of collision will decrease due to greater distancesbetween the robots. However if this was the sole purpose of the controller, it iseasiest to instantaneously give the variable robot zero acceleration and always avoidcollision. This obviously is not a desired algorithm and we can add flexibility to thedemands of our algorithm. The smaller the guard distance, the more e�cient thesystem and algorithm will appear. Therefore one wants to choose the smallest valuefor the guard distance that does not result in a collision. It is also in our intereststo have the largest possible starting velocity interval since that shows the flexibilityof the system in the way that it can handle a greater array of scenarios. In thisequation one also needs to consider the fact that the higher the velocities of therobot at the start, the greater the possibility of a collision. Hence it is up to theuser to decide its desirable to have a large starting velocity interval and then alsolarge guard distances and less fluidity, or if the opposite of having smaller guarddistances and a more fluid system and sacrifice flexibility.

58

6. Discussion

6.4 Further development on the intersection con-troller

The algorithms and methods deployed can be improved upon in the future givenmore resources and time. Several of these possible improvements are presentedbelow.

6.4.1 Improved robustness

The system, although meeting the desired robustness, could still be improved upon.The algorithm theoretically guarantees that there will be no collisions in the inter-section at the set velocities used in the tests. The robustness, however, was reducedover time due to the poor calculation and correction of the states, specifically therobot’s angle. If further tests and optimization were to be made on system, thisproblem needs to be able to be corrected. This will allow tests to see if the the-oretical zero-collision robustness actually is achievable, meaning more consecutiveintersections than previously obtained is possible. A larger variety of algorithmscould also be tested on the system to compare robustness or various optimizationmethods. Gathering data from the implementation of the reservation and prioritysolutions and comparing that to the hybrid algorithms could also further supportthe superiority of the hybrid algorithm.

To create a more real-world applicable scenario, the system can be equipped withmore sensors that are of a higher accuracy. This includes cameras, radars, lasers andother equipment that are currently being used in the development of autonomousvehicles. With this extra data, the system would be able to determine its state withgreater certainty. This would allow for opportunities such as emergency brakes,obstacle avoidance, lane detection and other functions that will help navigate thevehicles through an intersection, as well as further prevent any accidents from occur-ring. Additionally, this increased robustness could allow for greater liberties beingtaken in regards to optimization and thus e�ciency.

6.4.2 Further solution possibilities

In a real scenario, an intersection will have to handle more than two vehicles tryingto cross at the same time. An intersection will most likely also contain additionallanes and more intersecting roads. It would therefore be reasonable to improve thesystem to be able to handle this. The current algorithm used would not be directlyapplicable for such scenarios, since it assumes that there are two vehicles going intothe two way intersection and only one robot needs to adjust its speed to the robotwith priority.

Previous studies have been made on more appropriate approaches for handling thesevarious scenarios. One approach is creating a grid of sectors in the intersection thatwill be reserved by vehicles according to their lane or route, if they need to turn for

59

6. Discussion

example [3].

The final solution used in this report is as described a hybrid of a optimized andrule based solution. Pure optimization solutions are however possible [1]. Thesesolutions would not only optimize the control actions of one vehicle but for thewhole system, finding the most e�cient solution possible. The complexity of thesesolutions compared to the ones in this report are significantly increased however.

60

7Conclusion

In conclusion, the purpose of the project was to look at ways that fully automatedvehicles can deal with the problems of road intersections. The demands were arobust system that prevents collision and also optimizes the movement of the vehiclethrough the intersection. The limitation of the problem is that it only applies to atwo-vehicle intersection with only one lane on each road.

The system developed consists of the vehicles, where two programmable robots wereused, as well as a location system and computers running the ROS code. The robotswere then programmed to travel in a self intersecting continuous path. An algorithmwas developed which provides priority to one of the robots that travels into theintersection, where the other robot’s velocity would be optimized to minimize thecontrol actions and to avoid collision.

The resulting system allowed for the robots to achieve at least 50 consecutive in-tersections in each trial. Furthermore, proof of the optimization of the movementcould be recorded to prove that the algorithm worked as expected.

61

Bibliography

[1] R. Hult, G. R. Campos, E. Steinmetz, L. Hammarstrand, P. Falcone, and H.Wymeersch, “Coordination of cooperative autonomous vehicles: Toward saferand more e�cient road transportation”, IEEE Signal Processing Magazine,vol. 33, no. 6, pp. 74–84, Nov. 2016, issn: 1053-5888. doi: 10.1109/MSP.2016.2602005.

[2] C. Thompson. (Mar. 2017). The 3 biggest ways self-driving cars will im-prove our lives, [Online]. Available: http://www.businessinsider.com/advantages-of-driverless-cars-2016-6?r=US&IR=T&IR=T/#roads-will-be-safer-1 (visited on 03/11/2017).

[3] P. Dai, K. Liu, Q. Zhuge, E. H. M. Sha, V. C. S. Lee, and S. H. Son, “Aconvex optimization based autonomous intersection control strategy in vehic-ular cyber-physical systems”, in 2016 Intl IEEE Conferences on UbiquitousIntelligence Computing, Advanced and Trusted Computing, Scalable Comput-ing and Communications, Cloud and Big Data Computing, Internet of Peo-ple, and Smart World Congress (UIC/ATC/ScalCom/CBDCom/IoP/Smart-World), Jul. 2016, pp. 203–210. doi: 10.1109/UIC-ATC-ScalCom-CBDCom-IoP-SmartWorld.2016.0050.

[4] P. Dai, K. Liu, Q. Zhuge, E. H.-M. Sha, V. C. S. Lee, and S. H. Son, “Aconvex optimization based autonomous intersection control strategy in vehic-ular cyber-physical systems”, in Ubiquitous Intelligence & Computing, Ad-vanced and Trusted Computing, Scalable Computing and Communications,Cloud and Big Data Computing, Internet of People, and Smart World Congress(UIC/ATC/ScalCom/CBDCom/IoP/SmartWorld), 2016 Intl IEEE Confer-ences, IEEE, 2016, pp. 203–210.

[5] O. Beronius, E. Lundén, M. Malmquist, and A. Rohlin, “Coordination ofrobots via a wireless network”, unpublished.

[6] (n.d.). Trilateration exercise. Accessed March, 2017, U.S. Air Force, [Online].Available: http://www.gps.gov/multimedia/tutorials/trilateration/.

[7] Y.-x. Yuan, “Step-sizes for the gradient method”, Chinese Academy of Sci-ences, P.O.Box 2719, Beijing, P.R. China, Tech. Rep.

[8] A. Croeze, L. Pittman, and W. Reynolds, “Solving nonlinear least-squaresproblems with the gauss-newton and levenberg-marquardt methods”, LSU,Baton Rouge, LA, USA, Tech. Rep.

62

Bibliography

[9] C. Keatmanee, J. Baber, and M. Bakhtyar, Simple example of applying ex-tended kalman filter, 1st International Electrical Engineering Congress, Chi-angmai city, Thailand, 2013.

[10] Understanding the basis of the kalman filter via a simple and intuitive deriva-tion, Lecture notes, United kingdom: IEEE, September 2012.

[11] B. Lennartson, “Tillståndsmodeller”, in Reglerteknikens grunder, B. Lennart-son, Ed., vol. 4, Lund, Sweden: Studentlitteratur, 2002, pp. 95–98.

[12] “Rangenet user guide pulson 400 series”, Time Domain, Huntsville, AL USA,Tech. Rep. 320-0320C, May 2016.

[13] M. Yavari and B. G. Nickerson. (Mar. 2014). Ultra wideband wireless posi-tioning systems, [Online]. Available: http://thetoolchain.com/mirror/dw1000/uwb_wireless_positioning_systems_technical_report.pdf.

[14] Pulson ranging and communications, Time Domain, Huntsville, Alabama, 2012,pp. 7–8.

[15] (n.d.). Pulson 440. Accessed Feb 3,2017, Time Domain, [Online]. Available:http://www.timedomain.com/products/pulson-440/.

[16] (n.d.). Wifi alliance. Accessed May 11, 2017, Wifi, [Online]. Available: http://www.wi-fi.org/.

[17] (n.d.). Pioneer 3dx. Accessed May. 12, 2017, Adept Mobile Robots, [On-line]. Available: http://www.mobilerobots.com/Libraries/Downloads/Pioneer3DX-P3DX-RevA.sflb.ashx.

[18] (n.d.). Ubuntu wiki. Accessed Feb 7, 2017, Ubuntu, [Online]. Available: https://wiki.ubuntu.com/.

[19] Robotic operating system, www.ros.org, Accessed May 12, 2017, Open SourceRobotics Foundation, May 2016.

[20] (Feb. 2017). Terms of use, Creative Commons, [Online]. Available: https://creativecommons.org/terms/.

[21] Rosaria, http://wiki.ros.org/ROSARIA, Accessed May 12, 2017, Open SourceRobotics Foundation, Mar. 2017.

[22] A. Technology, Mobilesim, http://robots.mobilerobots.com/wiki/MobileSim,Accessed May 12, 2017, 2016.

[23] “Global positioning system (gps) standard positioning service (sps) perfor-mance analysis report”, William J. Hughes Technical Center WAAS T&ETeam, Atlantic City International Airport, NJ, Tech. Rep. 96, Jan. 2017.

63

AAppendix

A.1 Start-up code and instructions

In this section, the necessary steps needed to run the intersection problem are de-scribed.

First three computers are needed with ROS and ROSARIA installed, which meansthey need to run or emulate Ubuntu. In order to install these, look at the re-spective wikis. On these computers, open the terminal and change directory tocatkin_ws/src/. Next use the command git clone https://github.com/Olodus/SSYX02Group2.git and then go to the catkin_ws directory and use catkin_make.

In the real world place the anchors on appropriate positions and add the measure-ments to the Measure.py file in the directory controller/last_year_code/. Connecttwo computers to the robots that are going to cross the intersections. Place a UWBon each robot and connect them to the computers. Connect all the computers tothe same Wi-Fi network and make sure that they all have the correct bash-filesin regards to the IP-numbers. Then connect the computers that are linked to therobots to the central computer by changing the bash-file and writing

export ROS_MASTER_URI=http://#IP of central Computer#:11311

Start a roscore on the centralized computer and then start the servers and RosArianodes on the computers linked to the robots. Start the RosAria node through

rosrun rosaria RosAria __name:=RosAriaN

and to start the server, write

rosrun controller get_coord_server.py __name:=get_coord_serverNget_coord:=get_coordN _ip_of_uwb:=#IP of UWB#

The N should be set to 1 for the robot starting in the center of the coordinate

I

A. Appendix

problem and 2 for the one starting displaced.

Now the bash-files can be ran on the central computer to initiate the algorithms andprograms. To get the priority problem running, simply use the command

bash robot_run.bash -priority

II

A. Appendix

A.2 Images of robot setup and UWB

(a) Image of the robot setup (b) Image of a UWB sensor

Figure A.1: The figure shows two images, one of the robot setup and one of theUWB.

III

A. Appendix

A.3 API and Code documentation

In these sections there will be instructions for how the implemented code is usedand what it does.

A.3.1 RobotHandler

The RobotHandler is an abstraction adding additional and more complex commandsover the ones already o�ered by RosAria.

RobotHandler provides a couple of Services representing the di�erent missions.

A.3.1.1 Subscribed topics

Filter/measurements

Topic outputting nav_msgs/Odometry messages describing the state of thesystem. Uses this data for maneuvering.

A.3.1.2 Provided services

/get_state

Returns the current state of the robot as a Odometry message.

/is_ready

Returns whether the robot is currently executing a command or not. Is used topoll if a new mission can be sent.

/stop

Ends the mission currently executed by the robot and makes the robot stopany movement.

/set_speed

Sets the linear speed of the robot. Provides free movement paired togetherwith /set_ang_vel and /set_acc.

Input arguments:

std_msgs/Float

IV

A. Appendix

Returns:

Returns an error message if the robot is already executing a mission (except/set_ang_vel and /set_acc). Can be called while executing the/steer_towards mission A.3.1.2 to control movement along given path.

/set_acc

Sets the acceleration/deceleration of the robot. Provides free movement pairedtogether with /set_ang_vel and /set_speed.

Input arguments:

std_msgs/Float

Returns:

Returns an error message if the robot is already executing a mission (except/set_ang_vel and /set_speed). Can be called while executing the/steer_towards mission A.3.1.2 to control movement along given path.

/set_ang_vel

Sets the angular speed of the robot. Provides free movement paired togetherwith /set_speed and /set_acc.

Input arguments:

std_msgs/Float

Returns: Returns an error message if the robot is already executing a mission(except /set_speed and /set_acc).

/aim_at_point

Aims the robot towards a given point.

Input arguments:

geometry_msgs/Point

Returns:

Returns an error message if the robot is already executing a mission.

V

A. Appendix

/go_to_point

Moves the robot towards to a given point.

Input arguments: geometry_msgs/Point

Returns:

Returns an error message if the robot is already executing a mission.

/follow_path

Moves the robot following to a given path.

Input arguments:

nav_msgs/Path

Returns:

Returns an error message if the robot is already executing a mission.

/steer_towards

Puts the robot into "steer"-mode. During this mode, the robot willcontinuously aim itself as to follow the path formed between the location of therobot when the mission was initiated and the point given as argument. Usercan call set_speed and set_acc to freely control the speed of the robot alongthe path.

Input arguments:

geometry_msgs/Point

Returns:

Returns an error message if the robot is already executing a mission.

VI

A. Appendix

A.4 Additional measurements

(a) The graph shows the total travelled distance of the robotsas a function of time. The blue line represents Robot 1 whichis the one that is controlled.

(b) The graph shows the di�erent speeds of the robots as afunction of time.

VII

A. Appendix

(c) The graph shows the length between the robots as theytraverse the path.

Figure A.2: The figures A.2a, A.2b and A.2c show di�erent data collected fromthe same measurement. Figure A.2a plots the distance covered, A.2b is velocity ofthe robots and A.2c is the distance between them. In this data set, the controllablerobot decelerates and reaches the end of the path after the uncontrollable robot.

Figure A.3: The figure shows the speeds of the robots as they travel to and throughthe intersection.

VIII